From 82549a0039197695eddc2889cdc7c8437790b9bf Mon Sep 17 00:00:00 2001 From: Harsh Sharma Date: Tue, 25 Oct 2022 11:06:02 -0500 Subject: Fixed gps rmc message parsing --- libloragw/inc/loragw_gps.h | 2 +- libloragw/src/loragw_gps.c | 75 +++++++++++++++++++++++++++------------------- 2 files changed, 45 insertions(+), 32 deletions(-) diff --git a/libloragw/inc/loragw_gps.h b/libloragw/inc/loragw_gps.h index 606bd15..cf40b7f 100644 --- a/libloragw/inc/loragw_gps.h +++ b/libloragw/inc/loragw_gps.h @@ -148,7 +148,7 @@ lgw_gps_get function. If the lgw_parse_nmea and lgw_gps_get are used in different threads, a mutex lock must be acquired before calling either function. */ -enum gps_msg lgw_parse_nmea(const char* serial_buff, int buff_size); +enum gps_msg lgw_parse_nmea(const char* serial_buff, size_t buff_size); /** @brief Parse Ublox proprietary messages coming from the GPS system diff --git a/libloragw/src/loragw_gps.c b/libloragw/src/loragw_gps.c index 8395108..5b400bf 100644 --- a/libloragw/src/loragw_gps.c +++ b/libloragw/src/loragw_gps.c @@ -87,7 +87,7 @@ static short gps_alt = 0; /* altitude */ static bool gps_pos_ok = false; static bool gps_lock_ok = false; -static char gps_mod = 'N'; /* GPS mode (N no fix, A autonomous, D differential) */ +static char gps_valid = 'V'; /* GPS valid (A = data valid or V = data not valid ) */ static short gps_sat = 0; /* number of satellites used for fix */ static uint32_t leap_seconds = 18; /* Adjustment that is applied to Coordinated Universal Time, @@ -96,13 +96,13 @@ static uint32_t leap_seconds = 18; /* Adjustment that is applied to Coordinated /* -------------------------------------------------------------------------- */ /* --- PRIVATE FUNCTIONS DECLARATION ---------------------------------------- */ -static int nmea_checksum(const char *nmea_string, int buff_size, char *checksum); +static int nmea_checksum(const char *nmea_string, size_t buff_size, char *checksum); static char nibble_to_hexchar(uint8_t a); -static bool validate_nmea_checksum(const char *serial_buff, int buff_size); +static bool validate_nmea_checksum(const char *serial_buff, size_t buff_size); -static bool match_label(const char *s, char *label, int size, char wildcard); +static bool match_label(const char *s, char *label, size_t size, char wildcard); static int str_chop(char *s, int buff_size, char separator, int *idx_ary, int max_idx); @@ -116,7 +116,7 @@ reached (or buff_size exceeded). Checksum must point to a 2-byte (or more) char array. Return position of the checksum in the string */ -static int nmea_checksum(const char *nmea_string, int buff_size, char *checksum) { +static int nmea_checksum(const char *nmea_string, size_t buff_size, char *checksum) { int i = 0; uint8_t check_num = 0; @@ -167,7 +167,7 @@ Calculate the checksum of a NMEA frame and compare it to the checksum that is present at the end of it. Return true if it matches */ -static bool validate_nmea_checksum(const char *serial_buff, int buff_size) { +static bool validate_nmea_checksum(const char *serial_buff, size_t buff_size) { int checksum_index; char checksum[2]; /* 2 characters to calculate NMEA checksum */ @@ -200,11 +200,12 @@ static bool validate_nmea_checksum(const char *serial_buff, int buff_size) { Return true if the "label" string (can contain wildcard characters) matches the begining of the "s" string */ -static bool match_label(const char *s, char *label, int size, char wildcard) { +static bool match_label(const char *s, char *label, size_t size, char wildcard) { int i; for (i=0; i < size; i++) { if (label[i] == wildcard) continue; + if (!s[i] || !label[i]) return false; if (label[i] != s[i]) return false; } return true; @@ -273,7 +274,7 @@ int lgw_gps_enable() { /* initialize global variables */ gps_time_ok = false; gps_pos_ok = false; - gps_mod = 'N'; + gps_valid = 'N'; return LGW_GPS_SUCCESS; } @@ -447,37 +448,42 @@ enum gps_msg lgw_parse_ubx(const char *serial_buff, size_t buff_size, size_t *ms /* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */ -enum gps_msg lgw_parse_nmea(const char *serial_buff, int buff_size) { +enum gps_msg lgw_parse_nmea(const char *serial_buff, size_t buff_size) { int i, j, k; int str_index[30]; /* string index from the string chopping */ int nb_fields; /* number of strings detected by string chopping */ char parser_buf[256]; /* parsing modifies buffer so need a local copy */ + char rmc_time[64] = {0}; /* string containing the time */ + char rmc_date[64] = {0}; /* string containing the date */ + memset(parser_buf, 0, sizeof parser_buf); /* check input parameters */ if (serial_buff == NULL) { return UNKNOWN; } - if(buff_size > (int)(sizeof(parser_buf) - 1)) { + if(buff_size > (size_t)(sizeof(parser_buf) - 1)) { DEBUG_MSG("Note: input string to big for parsing\n"); return INVALID; + } else if (buff_size < 8) { + DEBUG_MSG("ERROR: TOO SHORT TO BE A VALID NMEA SENTENCE\n"); + return UNKNOWN; } + memcpy(parser_buf, serial_buff, buff_size); + parser_buf[buff_size] = '\0'; + /* look for some NMEA sentences in particular */ - if (buff_size < 8) { - DEBUG_MSG("ERROR: TOO SHORT TO BE A VALID NMEA SENTENCE\n"); - return UNKNOWN; - } else if (!validate_nmea_checksum(serial_buff, buff_size)) { + if (!validate_nmea_checksum(parser_buf, buff_size)) { DEBUG_MSG("Warning: invalid NMEA sentence (bad checksum)\n"); return INVALID; - } else if (match_label(serial_buff, "$G?RMC", 6, '?')) { + } else if (match_label(parser_buf, "$G?RMC", 6, '?')) { /* NMEA sentence format: $xxRMC,time,status,lat,NS,long,EW,spd,cog,date,mv,mvEW,posMode*cs Valid fix: $GPRMC,083559.34,A,4717.11437,N,00833.91522,E,0.004,77.52,091202,,,A*00 No fix: $GPRMC,,V,,,,,,,,,,N*00 */ - memcpy(parser_buf, serial_buff, buff_size); - parser_buf[buff_size] = '\0'; + nb_fields = str_chop(parser_buf, buff_size, ',', str_index, ARRAY_SIZE(str_index)); if (nb_fields != 12) { DEBUG_MSG("Warning: invalid RMC sentence (number of fields)\n"); @@ -485,36 +491,44 @@ enum gps_msg lgw_parse_nmea(const char *serial_buff, int buff_size) { } /* parse GPS status */ - if (str_index[12] < 0) { - gps_mod = 'N'; - } else { - gps_mod = (char)*(parser_buf + str_index[12]); /* get first character, no need to bother with sscanf */ - if ((gps_mod != 'N') && (gps_mod != 'A') && (gps_mod != 'D')) { - gps_mod = 'N'; + if (parser_buf + str_index[2]) { + gps_valid = *(parser_buf + str_index[2]); + if ((gps_valid != 'A')) { + gps_valid = 'V'; } } - /* parse complete time */ - i = sscanf(parser_buf + str_index[1], "%2hd%2hd%2hd%4f", &gps_hou, &gps_min, &gps_sec, &gps_fra); - j = sscanf(parser_buf + str_index[9], "%2hd%2hd%2hd", &gps_day, &gps_mon, &gps_yea); + /* parse GPS time */ + if (parser_buf + str_index[1]) { + strncat(rmc_time, parser_buf + str_index[1], 63); + i = sscanf(rmc_time, "%2hd%2hd%2hd%4f", &gps_hou, &gps_min, &gps_sec, &gps_fra); + } + + /* parse GPS date */ + if (parser_buf + str_index[9]) { + strncat(rmc_date, parser_buf + str_index[9], 63); + j = sscanf(rmc_date, "%2hd%2hd%2hd", &gps_day, &gps_mon, &gps_yea); + } + + /* if rmc status, hours, mins, secs, frac, day, mon, & yea is valid, set gps_time_ok to true */ if ((i == 4) && (j == 3)) { - if ((gps_mod == 'A') || (gps_mod == 'D')) { + if (gps_valid == 'A') { gps_time_ok = true; DEBUG_MSG("Note: Valid RMC sentence, GPS locked, date: 20%02d-%02d-%02dT%02d:%02d:%06.3fZ\n", gps_yea, gps_mon, gps_day, gps_hou, gps_min, gps_fra + (float)gps_sec); } else { gps_time_ok = false; - gps_pos_ok = false + gps_pos_ok = false; DEBUG_MSG("Note: Valid RMC sentence, no satellite fix, estimated date: 20%02d-%02d-%02dT%02d:%02d:%06.3fZ\n", gps_yea, gps_mon, gps_day, gps_hou, gps_min, gps_fra + (float)gps_sec); return IGNORED; } } else { /* could not get a valid hour AND date */ gps_time_ok = false; - DEBUG_MSG("Note: Valid RMC sentence, mode %c, no date\n", gps_mod); + DEBUG_MSG("Note: Valid RMC sentence, mode %c, no date\n", gps_valid); return IGNORED; } return NMEA_RMC; - } else if (match_label(serial_buff, "$G?GGA", 6, '?')) { + } else if (match_label(parser_buf, "$G?GGA", 6, '?')) { /* NMEA sentence format: $xxGGA,time,lat,NS,long,EW,quality,numSV,HDOP,alt,M,sep,M,diffAge,diffStation*cs Valid fix: $GPGGA,092725.00,4717.11399,N,00833.91590,E,1,08,1.01,499.6,M,48.0,M,,*5B @@ -549,7 +563,6 @@ enum gps_msg lgw_parse_nmea(const char *serial_buff, int buff_size) { } } - /* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */ int lgw_gps_get(struct timespec *utc, struct timespec *gps_time, struct coord_s *loc, struct coord_s *err) { -- cgit v1.2.3