Skip to content

Commit

Permalink
Satellites query is now working.
Browse files Browse the repository at this point in the history
  • Loading branch information
@SlightlyLoony
SlightlyLoony committed Nov 9, 2017
1 parent 804a3a1 commit efa16e5
Show file tree
Hide file tree
Showing 9 changed files with 292 additions and 33 deletions.
1 change: 1 addition & 0 deletions .idea/dictionaries/tom.xml
View file

Some generated files are not rendered by default. Learn more about how customized files appear on GitHub.

2 changes: 1 addition & 1 deletion gdbserver.cmd
View file
@@ -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 &
154 changes: 131 additions & 23 deletions gpsctl.c
View file
Expand Up @@ -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.
Expand All @@ -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;

Expand Down Expand Up @@ -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 ) {

Expand Down Expand Up @@ -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 );
Expand Down Expand Up @@ -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 }
};


Expand Down Expand Up @@ -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" );
Expand All @@ -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;
Expand All @@ -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 ) )
Expand All @@ -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 ) {

Expand Down Expand Up @@ -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 ) )
Expand Down Expand Up @@ -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"
};


Expand Down Expand Up @@ -983,20 +1080,24 @@ static optionDef_slOptions* getOptionDefs( const clientData_slOptions* clientDat
};

optionDef_slOptions optionDefs[] = {

// initialization elements...
verboseDef,
quietDef,
portDef,
jsonDef,
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...
Expand Down Expand Up @@ -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;
Expand All @@ -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 );

Expand Down
2 changes: 1 addition & 1 deletion remote_options.txt
View file
@@ -1 +1 @@
--query config -j
-vv --query satellites
33 changes: 26 additions & 7 deletions sl_general.c
View file
Expand Up @@ -61,26 +61,40 @@ 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
// represents a block of memory that must be freed by the caller. If malloc fails, a stack trace is dumped to stderr
// 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;
}
Expand All @@ -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;
Expand Down
3 changes: 3 additions & 0 deletions sl_general.h
View file
Expand Up @@ -37,13 +37,16 @@ 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();

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 );

Expand Down
2 changes: 1 addition & 1 deletion sl_serial.c
View file
Expand Up @@ -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();
}
Expand Down

0 comments on commit efa16e5

Please sign in to comment.