From efa16e5b3d8a12c43a169415c3aa0640f72e733f Mon Sep 17 00:00:00 2001 From: Tom Dilatush Date: Thu, 9 Nov 2017 15:09:09 -0700 Subject: [PATCH] Satellites query is now working. --- .idea/dictionaries/tom.xml | 1 + gdbserver.cmd | 2 +- gpsctl.c | 154 +++++++++++++++++++++++++++++++------ remote_options.txt | 2 +- sl_general.c | 33 ++++++-- sl_general.h | 3 + sl_serial.c | 2 +- ublox.c | 90 ++++++++++++++++++++++ ublox.h | 38 +++++++++ 9 files changed, 292 insertions(+), 33 deletions(-) diff --git a/.idea/dictionaries/tom.xml b/.idea/dictionaries/tom.xml index 13b3872..88e01ff 100644 --- a/.idea/dictionaries/tom.xml +++ b/.idea/dictionaries/tom.xml @@ -7,6 +7,7 @@ endiness gpsctl longitudnal + pseudorange synchronizer diff --git a/gdbserver.cmd b/gdbserver.cmd index 84c4cfd..9b74fdd 100644 --- a/gdbserver.cmd +++ b/gdbserver.cmd @@ -1 +1 @@ -nohup gdbserver :1234 ./gpsctl/gpsctl --query config -j > ~/gpsctl/gpsctl.out 2>&1 & +nohup gdbserver :1234 ./gpsctl/gpsctl -vv --query satellites > ~/gpsctl/gpsctl.out 2>&1 & diff --git a/gpsctl.c b/gpsctl.c index 54b55ce..0c10e9d 100644 --- a/gpsctl.c +++ b/gpsctl.c @@ -39,6 +39,7 @@ // TODO: troll all headers, inserting parameter names and return value comments // TODO: add reset GPS command // TODO: add query for satellite ID and location +// TODO: when no baud rate is specified, leave it the hell alone // these defines allow compiling on both an OS X development machine and the target Raspberry Pi. If different // development environments or target machines are needed, these will likely need to be tweaked. @@ -62,7 +63,7 @@ #include "ublox.h" #include "cJSON.h" -typedef enum { fixQuery, versionQuery, configQuery } queryType; +typedef enum { fixQuery, versionQuery, configQuery, satelliteQuery } queryType; typedef enum { syncNone, syncASCII, syncNMEA, syncUBX } syncType; typedef struct { char* name; syncType type; } syncTypeRec; @@ -312,6 +313,84 @@ static slReturn doVersionQuery( const clientData_slOptions* clientData ) { } +// comparison function for satellite search... +int cmpSatellite( const void* a, const void* b ) { + + ubxSatellite* c = (ubxSatellite*) a; + ubxSatellite* d = (ubxSatellite*) b; + + if( c->used && !d->used ) return -1; + if( d->used && !c->used ) return 1; + return d->cno - c->cno; +} + + +// Outputs a satellite query, in English or JSON. +static slReturn doSatelliteQuery( const clientData_slOptions* clientData ) { + + // get the satellite data from the GPS... + ubxSatellites satellites = {0}; // zeroes all elements... + slReturn ugsResp = ubxGetSatellites( clientData->fdPort, clientData->verbosity, &satellites ); + if( isErrorReturn( ugsResp ) ) + return makeErrorMsgReturn( ERR_CAUSE( ugsResp ), "Problem obtaining satellites information from GPS" ); + + // sort the result by used or not, then in descending order of CNo... + qsort( satellites.satellites, (unsigned) satellites.numberOfSatellites, sizeof( ubxSatellite ), cmpSatellite ); + + if( clientData->json ) { + } + else { + /* u = used + * d = diffCorr + * s = smoothed + * e = have ephemeris + * a = have almanac + * S = sbasCorrUsed + * R = rtcmCorrUsed + * P = pseudorange corrections used + * C = carrier range corrections used + * D = range rate (Doppler) corrections used + */ + // GNSS ID CNO ELV AZI Res Qual Hlth Src Flags + // 8 4 4 4 4 6 20 8 15 + + // print out our column headers... + printf( "%-8s%3s %3s %3s %3s %5s %-20s%-8s%-15sFlags\n", "GNSS", "ID", "CNo", "El", "Azi", "PRr", "Signal qual", "Sat Hlt", "Orbit Src" ); + + // now print out all our satellites... + for( int i = 0; i < satellites.numberOfSatellites; i++ ) { + + // build up our flags string... + ubxSatellite* s = satellites.satellites + i; + char* flags = NULL; + if( s->used ) append( &flags, "u" ); + if( s->diffCorr ) append( &flags, "d" ); + if( s->smoothed ) append( &flags, "s" ); + if( s->haveEphemeris ) append( &flags, "e" ); + if( s->haveAlmanac ) append( &flags, "a" ); + if( s->sbasCorrUsed ) append( &flags, "S" ); + if( s->rtcmCorrUsed ) append( &flags, "R" ); + if( s->prCorrUsed ) append( &flags, "P" ); + if( s->crCorrUsed ) append( &flags, "C" ); + if( s->doCorrUsed ) append( &flags, "D" ); + if( flags == NULL ) append( &flags, "" ); + + // print our satellite line... + printf( "%-8s%3d %3d %3d %3d %5.1f %-20s%-8s%-15s%s\n", + getGnssName( s->gnssID ), s->satelliteID, s->cno, s->elevation, s->azimuth, s->pseudoRangeResidualM, + getSignalQuality( s->signalQuality ), getSatelliteHealth( s->health ), getOrbitSource( s->orbitSource ), flags ); + + free( flags ); + } + } + + // free up the memory we allocated... + free( satellites.satellites ); + + return makeOkReturn(); +} + + // Outputs a configuration query, in English or JSON. static slReturn doConfigQuery( const clientData_slOptions* clientData ) { @@ -525,7 +604,7 @@ static slReturn parseBaud( void* ptrArg, int intArg, const optionDef_slOptions* // Parse autobaud or sync, which have an optional argument of ascii, nmea, or ubx. static slReturn parseSyncMethod( void* ptrArg, int intArg, const optionDef_slOptions* def, const char* arg, clientData_slOptions* clientData ) { - // if we get here, we couldn't find the argument... + // set up the sync method... slReturn ssmResp = setSyncMethod( arg, clientData ); if( isErrorReturn( ssmResp ) ) return parseMsgHelper(ERR_CAUSE( ssmResp ), def, "the specified baud rate inference method (\"%s\") is unrecognizable; should be \"ascii\", \"nmea\", or \"ubx\"", arg ); @@ -579,9 +658,10 @@ static slReturn parseFlag( void* ptrArg, int intArg, const optionDef_slOptions* typedef struct { char* name; queryType queryType; } queryDef; static queryDef queryDefs[] = { - { "fix", fixQuery }, - { "version", versionQuery }, - { "config", configQuery } + { "fix", fixQuery }, + { "version", versionQuery }, + { "satellites", satelliteQuery }, + { "config", configQuery } }; @@ -685,14 +765,6 @@ static slReturn actionSetup( const optionDef_slOptions* defs, const psloConfig* } if( V2 ) printf( "Serial port (\"%s\") open...\n", CD->port ); - // now we set the correct options (the configured baud rate, 8 data bits, 1 stop bit, no parity) - // note that this baud rate may be incorrect, and we might try to infer it in a moment... - slReturn result = setTermOptions( fdPort, CD->baud, 8, 1, false, false ); - if( isErrorReturn( result ) ) { - close( fdPort ); - return makeErrorMsgReturn(ERR_CAUSE( result ), "Error when setting terminal options" ); - } - // stuff our file descriptor and return in victory... CD->fdPort = fdPort; if( V2 ) printf( "Serial port open and configured...\n" ); @@ -710,7 +782,7 @@ static slReturn actionTeardown( const optionDef_slOptions* defs, const psloConf // Autobaud action function, which infers the GPS baud rate by trying to synchronize at various baud rates. static slReturn actionAutoBaud( const optionDef_slOptions* defs, const psloConfig* config ) { - + // first we figure out what synchronization type we're going to use... baudRateSynchronizer* synchronizer; clientData_slOptions* cd = config->clientData; @@ -728,7 +800,14 @@ static slReturn actionAutoBaud( const optionDef_slOptions* defs, const psloConfi } if( V2 ) printf( "Automatically determining baud rate...\n"); - + + // now we set the correct options (the first baud rate, 8 data bits, 1 stop bit, no parity) + slReturn result = setTermOptions( CD->fdPort, 230400, 8, 1, false, false ); + if( isErrorReturn( result ) ) { + close(CD->fdPort ); + return makeErrorMsgReturn(ERR_CAUSE( result ), "Error when setting terminal options" ); + } + // now we do the actual work... slReturn abResp = autoBaudRate( cd->fdPort, cd->minBaud, synchronizer, cd->verbosity ); if( isErrorReturn( abResp ) ) @@ -741,6 +820,22 @@ static slReturn actionAutoBaud( const optionDef_slOptions* defs, const psloConfi } +// Baud action function, which sets the host's port to the specified baud rate. +static slReturn actionBaud( const optionDef_slOptions* defs, const psloConfig* config ) { + + if( V2 ) printf( "Setting baud rate to %d...\n", CD->baud ); + + // now we set the correct options (the specified baud rate, 8 data bits, 1 stop bit, no parity) + slReturn result = setTermOptions( CD->fdPort, CD->baud, 8, 1, false, false ); + if( isErrorReturn( result ) ) { + close(CD->fdPort ); + return makeErrorMsgReturn(ERR_CAUSE( result ), "Error when setting terminal options" ); + } + + return makeOkReturn(); +} + + // New baud rate action function, which changes both the U-Blox GPS baud rate and the host baud rate. static slReturn actionNewBaud( const optionDef_slOptions* defs, const psloConfig* config ) { @@ -796,9 +891,11 @@ static slReturn actionQuery( const optionDef_slOptions* defs, const psloConfig slReturn resp; switch( clientData->queryType ) { - case fixQuery: resp = doFixQuery( clientData ); break; - case versionQuery: resp = doVersionQuery( clientData ); break; - case configQuery: resp = doConfigQuery( clientData ); break; + case fixQuery: resp = doFixQuery( clientData ); break; + case versionQuery: resp = doVersionQuery( clientData ); break; + case configQuery: resp = doConfigQuery( clientData ); break; + case satelliteQuery: resp = doSatelliteQuery( clientData ); break; + default: return makeErrorFmtMsgReturn(ERR_ROOT, "invalid query type: %d", clientData->queryType ); } if( isErrorReturn( resp ) ) @@ -921,9 +1018,9 @@ static optionDef_slOptions* getOptionDefs( const clientData_slOptions* clientDat 1, "baud", 'b', argRequired, // max, long, short, arg parseBaud, (void*) &clientData->baud, 0, // parser, ptrArg, intArg constrainBaud, // constrainer - NULL, // action + actionBaud, // action "baud rate", // argument name - "specify host baud rate (default is '9600'); any standard rate is allowed" + "specify different host baud rate; any standard rate is allowed" }; @@ -983,6 +1080,8 @@ static optionDef_slOptions* getOptionDefs( const clientData_slOptions* clientDat }; optionDef_slOptions optionDefs[] = { + + // initialization elements... verboseDef, quietDef, portDef, @@ -990,13 +1089,15 @@ static optionDef_slOptions* getOptionDefs( const clientData_slOptions* clientDat autobaudDef, baudDef, minBaudDef, + + // all the above MUST come before those below, or port won't be initialized correctly... syncDef, newbaudDef, nmeaDef, queryDef, saveConfigDef, echoDef, - { 0 } + { 0 } // terminator; MUST be at the end of this list... }; // allocate memory for our option definitions, and copy them in... @@ -1034,7 +1135,7 @@ int main( int argc, char *argv[] ) { // initialize our client data structure, and set defaults... clientData_slOptions clientData = { 0 }; - clientData.baud = 9600; + clientData.baud = 0; // triggers actionSetup to keep existing terminal options, including baud rate clientData.minBaud = 9600; clientData.port = "/dev/serial0"; clientData.syncMethod = syncUBX; @@ -1054,7 +1155,14 @@ int main( int argc, char *argv[] ) { slReturn resp = process_slOptions(argc, (const char **) argv, &config ); - if( isErrorReturn( resp ) ) printReturn( resp, true, true ); + if( isErrorReturn( resp ) ) + switch( clientData.verbosity ) { + case 0: printf( "Errors occurred!\n" ); break; + case 1: printReturn( resp, false, false ); break; + case 2: printReturn( resp, true, false ); break; + case 3: printReturn( resp, true, true ); break; + default: printReturn( resp, true, true ); break; + } freeReturn( resp ); diff --git a/remote_options.txt b/remote_options.txt index 8750930..1ee8fa0 100644 --- a/remote_options.txt +++ b/remote_options.txt @@ -1 +1 @@ - --query config -j \ No newline at end of file + -vv --query satellites \ No newline at end of file diff --git a/sl_general.c b/sl_general.c index 036b2e1..29a116c 100644 --- a/sl_general.c +++ b/sl_general.c @@ -61,6 +61,17 @@ extern bool issgraph( const char* str ) { } +// Appends the second given string to the first. If the first string is NULL, makes a new copy of the second into +// the first. Otherwise, allocates memory for the concatenation of the two strings, concatenates them, and frees +// the original first string. This mechanism allows a series of appends to be performed while ending up with only +// one unfreed allocated block of memory: the one with the result. +extern void append( char** s1, const char* s2 ) { + char* r = concat( *s1, s2 ); + free( *s1 ); + *s1 = r; +} + + // Return the concatenation of the two given string in a newly allocated memory on the heap. If the two // given strings are null, a null is returned. If one of the given strings is null, then a COPY (in newly allocated // memory) of the non-null given string is returned. Note that this behavior means that ANY non-null return value @@ -68,19 +79,22 @@ extern bool issgraph( const char* str ) { // and the program is aborted. extern char* concat( const char *s1, const char *s2 ) { - // get the lengths just once (optimization)... - const size_t len1 = s1 ? strlen(s1) : 0; - const size_t len2 = s2 ? strlen(s2) : 0; + // if both arguments are NULL, return a NULL... + if( (s1 == NULL) && (s2 == NULL) ) return NULL; - // if both arguments were null, return with a null... - if( len1 + len2 == 0 ) return NULL; + // get the lengths just once (optimization)... + const size_t len1 = s1 ? strlen( s1 ) : 0; + const size_t len2 = s2 ? strlen( s2 ) : 0; // get the memory required for the concatenated strings plus the terminator... char *result = safeMalloc( len1 + len2 + 1 ); // build our result... - memcpy(result, s1, len1); - memcpy(result + len1, s2, len2 + 1 ); //+1 to copy the null-terminator + if( s1 != NULL ) memcpy(result, s1, len1); + if( s2 != NULL ) memcpy(result + len1, s2, len2 ); + + // insert the terminator... + *(result + len1 + len2) = 0; return result; } @@ -97,6 +111,11 @@ extern long long max_ll( long long a, long long b ) { } +extern int min_i( int a, int b ) { + return (a < b) ? a : b; +} + + // Returns the time since the epoch in milliseconds. extern long long currentTimeMs() { struct timespec tp; diff --git a/sl_general.h b/sl_general.h index 3ffbc54..1a1bce3 100644 --- a/sl_general.h +++ b/sl_general.h @@ -37,6 +37,8 @@ bool issgraph( const char* str ); char* concat( const char *s1, const char *s2 ); +void append( char** s1, const char* s2 ); + void sleep_ns( int ns ); long long currentTimeMs(); @@ -44,6 +46,7 @@ long long currentTimeMs(); bool strempty( const char* str ); long long max_ll( long long a, long long b ); +int min_i( int a, int b); int hex2int( char c ); diff --git a/sl_serial.c b/sl_serial.c index 7be0bcc..0eded2d 100644 --- a/sl_serial.c +++ b/sl_serial.c @@ -36,7 +36,7 @@ extern slReturn getSpeedInfo( int fdPort, speedInfo* result ) { result->nsBit = 1000000000 / result->baudRate; int stopBits = (options.c_cflag & CSTOPB) ? 2 : 1; int dataBits = (int) getBitField_slBits( options.c_cflag, CSIZE ); - int bits = stopBits + dataBits; + int bits = stopBits + dataBits + 1; // the +1 is for the ever-present start bit... result->nsChar = bits * result->nsBit; return makeOkReturn(); } diff --git a/ublox.c b/ublox.c index b88be50..ef0336d 100644 --- a/ublox.c +++ b/ublox.c @@ -34,8 +34,10 @@ #define UBX_MON_VER 0x04 #define UBX_NAV 0x01 #define UBX_NAV_PVT 0x07 +#define UBX_NAV_SAT 0x35 #define NAV_PVT_MAX_MS 2000 +#define NAV_SAT_MAX_MS 2000 #define CFG_NAV5_MAX_MS 2000 #define MON_VER_MAX_MS 2000 #define MON_VER_SYNC_MAX_MS 1200 @@ -437,6 +439,94 @@ extern slReturn ubxGetVersion( int fdPort, int verbosity, ubxVersion* version ) } +extern char* getSignalQuality( signalQuality qual ) { + switch( qual ) { + case signalNone: return "None"; + case signalAcquired: return "Acquired"; + case signalCodeCarrierLocked: return "Code/carrier locked"; + case signalCodeLocked: return "Code locked"; + case signalSearching: return "Searching"; + case signalUnusable: return "Unusable"; + default: return "Unknown"; + } +} + + +extern char* getSatelliteHealth( satelliteHealth health ) { + switch( health ) { + case healthUnknown: return "Unknown"; + case healthOk: return "Ok"; + case healthBad: return "Bad"; + default: return "Unknown"; + } +} + + +extern char* getOrbitSource( orbitSource source ) { + switch( source ) { + case osNone: return "None"; + case osAlmanac: return "Almanac"; + case osAssistNowAutonomous: return "AssistNow auto"; + case osAssistNowOffline: return "AssistNow off"; + case osEphemeris: return "Ephemeris"; + case osOther: return "Other"; + default: return "Unknown"; + } +} + + +// Fills the ubxSatellites structure at the given pointer with information about the GPS satellites. +extern slReturn ubxGetSatellites( int fdPort, int verbosity, ubxSatellites* satellites ) { + + ubxType type = { UBX_NAV, UBX_NAV_SAT }; + slBuffer* body = create_slBuffer( 0, LittleEndian ); + ubxMsg msg = createUbxMsg( type, body ); + ubxMsg satMsg; + slReturn nsResp = pollUbx( fdPort, msg, MON_VER_MAX_MS, &satMsg ); + if( isErrorReturn( nsResp ) ) return nsResp; + + // now decode our message, starting with the number of satellites... + void* b = satMsg.body; + ubxSatellites* ss = satellites; + ss->numberOfSatellites = get_uint8_slBuffer( b, 5 ); + + // make a place to store our results... + ss->satellites = safeMalloc( ss->numberOfSatellites * sizeof( ubxSatellite ) ); + + // now examine each of our satellites... + for( int i = 0; i < ss->numberOfSatellites; i++ ) { + ubxSatellite* s = ss->satellites + i; + size_t o = (unsigned) (8 + 12 * i); + + s->gnssID = get_uint8_slBuffer( b, o + 0 ); + s->satelliteID = get_uint8_slBuffer( b, o + 1 ); + s->cno = get_uint8_slBuffer( b, o + 2 ); + s->elevation = get_int8_slBuffer( b, o + 3 ); + s->azimuth = get_int16_slBuffer( b, o + 4 ); + s->pseudoRangeResidualM = get_int8_slBuffer( b, o + 6 ) * 0.1; + + uint32_t flags = get_uint32_slBuffer( b, o + 8 ); + s->signalQuality = (signalQuality) min_i( (int) getBitField_slBits( flags, 0x07 ), 5 ); + s->used = isBitSet_slBits( flags, 3 ); + s->health = (satelliteHealth) getBitField_slBits( flags, 0x30 ); + s->diffCorr = isBitSet_slBits( flags, 6 ); + s->smoothed = isBitSet_slBits( flags, 7 ); + s->orbitSource = (orbitSource) min_i( (int) getBitField_slBits( flags, 0x700 ), 5 ); + s->haveEphemeris = isBitSet_slBits( flags, 11 ); + s->haveAlmanac = isBitSet_slBits( flags, 12 ); + s->haveAssistNowOff = isBitSet_slBits( flags, 13 ); + s->haveAssistNowAuto = isBitSet_slBits( flags, 14 ); + s->sbasCorrUsed = isBitSet_slBits( flags, 16 ); + s->rtcmCorrUsed = isBitSet_slBits( flags, 17 ); + s->prCorrUsed = isBitSet_slBits( flags, 20 ); + s->crCorrUsed = isBitSet_slBits( flags, 21 ); + s->doCorrUsed = isBitSet_slBits( flags, 22 ); + } + + return makeOkReturn(); +} + + extern char* getGnssName( gnssID id ) { switch( id ) { case GPS: return "GPS"; diff --git a/ublox.h b/ublox.h index 4eb4e55..621e7f1 100644 --- a/ublox.h +++ b/ublox.h @@ -66,6 +66,7 @@ typedef struct { int number_of_extensions; } ubxVersion; + typedef enum { GPS, SBAS, Galileo, BeiDou, IMES, QZSS, GLONASS } gnssID; typedef enum { Portable, Stationary = 2, Pedestrian, Automotive, Sea, Air1G, Air2G, Air4G, Watch } dynModel; typedef enum { Only2D = 1, Only3D, Auto2D3D } fixMode; @@ -73,6 +74,39 @@ typedef enum { AutoUTC, USNO_UTC = 3, GLONASS_UTC = 6, BeiDou_UTC } utcType; typedef enum { tgUTC, tgGPS, tgGLONASS, tgBeiDou, tgGalileo } timegridType; typedef enum { fixUTC, fixGPS, fixGLONASS, fixBeiDou, fixGalileo } fixTimeRefType; typedef enum { pmFull, pmBalanced, pmInterval, pmAggressive1Hz, pmAggressive2Hz, pmAggressive4Hz, pmInvalid=0xFF } powerModeType; +typedef enum { signalNone, signalSearching, signalAcquired, signalUnusable, signalCodeLocked, signalCodeCarrierLocked } signalQuality; +typedef enum { healthUnknown, healthOk, healthBad } satelliteHealth; +typedef enum { osNone, osEphemeris, osAlmanac, osAssistNowOffline, osAssistNowAutonomous, osOther } orbitSource; + +typedef struct { + gnssID gnssID; // GNSS that this satellite belongs to + uint8_t satelliteID; // the GNSS-assigned satellite number + uint8_t cno; // the carrier-to-noise signal ratio in dBHz + int8_t elevation; // elevation in degrees (0 is horizon, +/- 90 degrees + int16_t azimuth; // azimuth in degrees (0-360, 0 is due north) + double pseudoRangeResidualM; // pseudo range residual, in meters + signalQuality signalQuality; // signal quality + bool used; // true if this satellite is being used for navigation + satelliteHealth health; // current state of satellite health + bool diffCorr; // true if differential correction data is available for this satellite + bool smoothed; // true if a carrier-smoothed pseudorange is being used for this satellite + orbitSource orbitSource; // the source for the orbital information for this satellite + bool haveEphemeris; // true if the GPS has an ephemeris for this satellite + bool haveAlmanac; // true if the GPS has an almanac for this satellite + bool haveAssistNowOff; // true if the GPS has AssistNow offline data for this satellite + bool haveAssistNowAuto; // true if the GPS has AssistNow autonomous data for this satellite + bool sbasCorrUsed; // true if the GPS used SBAS corrections for this satellite + bool rtcmCorrUsed; // true if the GPS used RTCM corrections for this satellite + bool prCorrUsed; // true if the GPS used pseudorange corrections for this satellite + bool crCorrUsed; // true if the GPS used carrier range corrections for this satellite + bool doCorrUsed; // true if the GPS used range rate (Doppler) corrections for this satellite +} ubxSatellite; + +typedef struct { + int numberOfSatellites; + ubxSatellite* satellites; +} ubxSatellites; + typedef struct { gnssID id; int minChnnls; @@ -135,8 +169,12 @@ char* getUTCTypeName( utcType utc ); char* getTimeGridTypeName( timegridType type ); char* getFixTimeRefName( fixTimeRefType type ); char* getPowerModeName( powerModeType type ); +char* getSignalQuality( signalQuality qual ); +char* getSatelliteHealth( satelliteHealth health ); +char* getOrbitSource( orbitSource source ); slReturn ubxGetVersion( int fdPort, int verbosity, ubxVersion* version ); +slReturn ubxGetSatellites( int fdPort, int verbosity, ubxSatellites* satellites ); slReturn ubxSaveConfig( int fdPort, int verbosity ); slReturn ubxGetFix( int fdPort, int verbosity, ubxFix* fix ); slReturn ubxGetConfig( int fdPort, int verbosity, ubxConfig* config );