|
|
@@ -205,6 +205,53 @@ static int handle_stale_gps_condition() {
|
|
|
return return_code;
|
|
|
}
|
|
|
|
|
|
+//------------
|
|
|
+
|
|
|
+// Standard c parsing and tokenization functions are having trouble with blank fields,
|
|
|
+// so these custom field parsing routines were developed.
|
|
|
+//
|
|
|
+// Each takes in a string and parses the first field, where appropriate.
|
|
|
+// It then advances to the next token by searching for the field delimiter (',')
|
|
|
+// and pushing the pointer one past it, prepping it for next use.
|
|
|
+//
|
|
|
+// A default value of '0' is assigned if no conversion has taken place.
|
|
|
+//
|
|
|
+// Return NULL on error or end of string.
|
|
|
+//
|
|
|
+static char *_convert_advance_nop(char *in) {
|
|
|
+ if (!in) { return NULL; }
|
|
|
+ if ((*in) == ',') { return in+1; }
|
|
|
+ in = strchr(in, ',');
|
|
|
+ if (in) { return in+1; }
|
|
|
+ return in;
|
|
|
+}
|
|
|
+
|
|
|
+static char *_convert_advance_float(char *in, float *f) {
|
|
|
+
|
|
|
+ *f = 0.0;
|
|
|
+
|
|
|
+ if (!in) { return NULL; }
|
|
|
+ if ((*in) != ',') { *f = strtof(in, NULL); }
|
|
|
+ if ((*in) == ',') { return in+1; }
|
|
|
+ in = strchr(in, ',');
|
|
|
+ if (in) { return in+1; }
|
|
|
+ return in;
|
|
|
+}
|
|
|
+
|
|
|
+static char *_convert_advance_char(char *in, char *ch) {
|
|
|
+
|
|
|
+ *ch = '\0';
|
|
|
+
|
|
|
+ if (!in) { return NULL; }
|
|
|
+ if ((*in) != ',') { *ch = *in; }
|
|
|
+ if ((*in) == ',') { return in+1; }
|
|
|
+ in = strchr(in, ',');
|
|
|
+ if (in) { return in+1; }
|
|
|
+ return in;
|
|
|
+}
|
|
|
+
|
|
|
+//----
|
|
|
+
|
|
|
int update_gps(char *in) {
|
|
|
// This will hold the number of matched variables populated by sscanf().
|
|
|
//
|
|
|
@@ -231,14 +278,66 @@ int update_gps(char *in) {
|
|
|
char f6=0;
|
|
|
float f7=0;
|
|
|
float f8=0;
|
|
|
- int f9=0;
|
|
|
+ float f9=0;
|
|
|
float f10=0;
|
|
|
char f11=0;
|
|
|
|
|
|
- //DEBUG
|
|
|
- printf(">> gprmc\n");
|
|
|
+ char *chp;
|
|
|
|
|
|
- num = sscanf(in,"$GPRMC,%f,%c,%f,%c,%f,%c,%f,%f,%d,%f,%c",&f1,&f2,&f3,&f4,&f5,&f6,&f7,&f8,&f9,&f10,&f11);
|
|
|
+ chp = in;
|
|
|
+ num=0;
|
|
|
+ do {
|
|
|
+
|
|
|
+ // ignore first token
|
|
|
+ //
|
|
|
+ chp = _convert_advance_nop(in);
|
|
|
+ if (!chp) { break; }
|
|
|
+
|
|
|
+ chp = _convert_advance_float(chp, &f1);
|
|
|
+ if (!chp) { break; }
|
|
|
+ num++;
|
|
|
+
|
|
|
+ chp = _convert_advance_char(chp, &f2);
|
|
|
+ if (!chp) { break; }
|
|
|
+ num++;
|
|
|
+
|
|
|
+ chp = _convert_advance_float(chp, &f3);
|
|
|
+ if (!chp) { break; }
|
|
|
+ num++;
|
|
|
+
|
|
|
+ chp = _convert_advance_char(chp, &f4);
|
|
|
+ if (!chp) { break; }
|
|
|
+ num++;
|
|
|
+
|
|
|
+ chp = _convert_advance_float(chp, &f5);
|
|
|
+ if (!chp) { break; }
|
|
|
+ num++;
|
|
|
+
|
|
|
+ chp = _convert_advance_char(chp, &f6);
|
|
|
+ if (!chp) { break; }
|
|
|
+ num++;
|
|
|
+
|
|
|
+ chp = _convert_advance_float(chp, &f7);
|
|
|
+ if (!chp) { break; }
|
|
|
+ num++;
|
|
|
+
|
|
|
+ chp = _convert_advance_float(chp, &f8);
|
|
|
+ if (!chp) { break; }
|
|
|
+ num++;
|
|
|
+
|
|
|
+ chp = _convert_advance_float(chp, &f9);
|
|
|
+ if (!chp) { break; }
|
|
|
+ num++;
|
|
|
+
|
|
|
+ chp = _convert_advance_float(chp, &f10);
|
|
|
+ if (!chp) { break; }
|
|
|
+ num++;
|
|
|
+
|
|
|
+ chp = _convert_advance_char(chp, &f11);
|
|
|
+ if (!chp) { break; }
|
|
|
+ num++;
|
|
|
+
|
|
|
+ } while (0);
|
|
|
|
|
|
// If we have a full GPRMC sentence we can consider setting the time
|
|
|
//
|
|
|
@@ -254,7 +353,7 @@ int update_gps(char *in) {
|
|
|
// (this routine also stores the utc timestamp derived from the GPS date and time fields so it can be passed to
|
|
|
// other modules that may have a need for this information).
|
|
|
//
|
|
|
- handle_gps_time(f1,f9);
|
|
|
+ handle_gps_time(f1,(int)f9);
|
|
|
|
|
|
}
|
|
|
}
|
|
|
@@ -281,10 +380,11 @@ int update_gps(char *in) {
|
|
|
my_gps_stat.stamp = time(NULL);
|
|
|
|
|
|
return_code |= 1;
|
|
|
+
|
|
|
}
|
|
|
}
|
|
|
else if(!strncmp(in,"$GPGGA",6)) {
|
|
|
- int f1=0;
|
|
|
+ float f1=0;
|
|
|
float f2=0;
|
|
|
char f3=0;
|
|
|
float f4=0;
|
|
|
@@ -297,12 +397,8 @@ int update_gps(char *in) {
|
|
|
float f11=0;
|
|
|
char f12=0;
|
|
|
|
|
|
- //DEBUG
|
|
|
- printf(">> gpgga\n");
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
- num=sscanf(in,"$GPGGA,%d,%f,%c,%f,%c,%d,%d,%f,%f,%c,%f,%c",&f1,&f2,&f3,&f4,&f5,&f6,&f7,&f8,&f9,&f10,&f11,&f12);
|
|
|
+ //num=sscanf(in,"$GPGGA,%d,%f,%c,%f,%c,%d,%d,%f,%f,%c,%f,%c",&f1,&f2,&f3,&f4,&f5,&f6,&f7,&f8,&f9,&f10,&f11,&f12);
|
|
|
+ num=sscanf(in,"$GPGGA,%f,%f,%c,%f,%c,%d,%d,%f,%f,%c,%f,%c",&f1,&f2,&f3,&f4,&f5,&f6,&f7,&f8,&f9,&f10,&f11,&f12);
|
|
|
|
|
|
if(num == 12) {
|
|
|
|