From f0ef594cdb98acee8055430ef2e154668570dd84 Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Sun, 15 Nov 2015 03:44:28 -0500 Subject: [PATCH 1/5] LP-145 CC3D GPS reparse and general GPS system health --- flight/modules/GPS/GPS.c | 20 ++++++----- flight/modules/GPS/NMEA.c | 8 +---- flight/modules/GPS/UBX.c | 75 +++++++++++++++++++++++++-------------- 3 files changed, 62 insertions(+), 41 deletions(-) diff --git a/flight/modules/GPS/GPS.c b/flight/modules/GPS/GPS.c index 74166d6cf..9a7f5490d 100644 --- a/flight/modules/GPS/GPS.c +++ b/flight/modules/GPS/GPS.c @@ -77,7 +77,9 @@ void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev); // **************** // Private constants -#define GPS_TIMEOUT_MS 500 +// GPS timeout is greater than 1000ms so that a stock GPS configuration can be used without timeout errors +#define GPS_TIMEOUT_MS 1250 + // delay from detecting HomeLocation.Set == False before setting new homelocation // this prevent that a save with homelocation.Set = false triggered by gps ends saving // the new location with Set = true. @@ -120,7 +122,6 @@ static xTaskHandle gpsTaskHandle; static char *gps_rx_buffer; -static uint32_t timeOfLastCommandMs; static uint32_t timeOfLastUpdateMs; #if defined(PIOS_INCLUDE_GPS_NMEA_PARSER) || defined(PIOS_INCLUDE_GPS_UBX_PARSER) @@ -256,8 +257,7 @@ static void gpsTask(__attribute__((unused)) void *parameters) #endif GPSPositionSensorData gpspositionsensor; - timeOfLastUpdateMs = timeNowMs; - timeOfLastCommandMs = timeNowMs; + timeOfLastUpdateMs = timeNowMs; GPSPositionSensorGet(&gpspositionsensor); #if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) @@ -312,13 +312,14 @@ static void gpsTask(__attribute__((unused)) void *parameters) #endif /* if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) */ uint16_t cnt; + int res; // This blocks the task until there is something on the buffer (or 100ms? passes) cnt = PIOS_COM_ReceiveBuffer(gpsPort, c, GPS_READ_BUFFER, xDelay); + res = PARSER_INCOMPLETE; if (cnt > 0) { PERF_TIMED_SECTION_START(counterParse); PERF_TRACK_VALUE(counterBytesIn, cnt); PERF_MEASURE_PERIOD(counterRate); - int res; switch (gpsSettings.DataProtocol) { #if defined(PIOS_INCLUDE_GPS_NMEA_PARSER) case GPSSETTINGS_DATAPROTOCOL_NMEA: @@ -339,20 +340,23 @@ static void gpsTask(__attribute__((unused)) void *parameters) if (res == PARSER_COMPLETE) { timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS; timeOfLastUpdateMs = timeNowMs; - timeOfLastCommandMs = timeNowMs; } } + // if there is any error at all, set status to NOGPS // Check for GPS timeout timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS; - if ((timeNowMs - timeOfLastUpdateMs) >= GPS_TIMEOUT_MS || + if ((res == PARSER_ERROR) || + (timeNowMs - timeOfLastUpdateMs) >= GPS_TIMEOUT_MS || (gpsSettings.DataProtocol == GPSSETTINGS_DATAPROTOCOL_UBX && gpspositionsensor.AutoConfigStatus == GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_ERROR)) { // we have not received any valid GPS sentences for a while. // either the GPS is not plugged in or a hardware problem or the GPS has locked up. GPSPositionSensorStatusOptions status = GPSPOSITIONSENSOR_STATUS_NOGPS; GPSPositionSensorStatusSet(&status); AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_ERROR); - } else { + } + // if we parsed at least one packet successfully + else if (res == PARSER_COMPLETE) { // we appear to be receiving GPS sentences OK, we've had an update // criteria for GPS-OK taken from this post... // http://forums.openpilot.org/topic/1523-professors-insgps-in-svn/page__view__findpost__p__5220 diff --git a/flight/modules/GPS/NMEA.c b/flight/modules/GPS/NMEA.c index ab4733a83..13ca0555f 100644 --- a/flight/modules/GPS/NMEA.c +++ b/flight/modules/GPS/NMEA.c @@ -107,7 +107,6 @@ static const struct nmea_parser nmea_parsers[] = { int parse_nmea_stream(uint8_t *rx, uint8_t len, char *gps_rx_buffer, GPSPositionSensorData *GpsData, struct GPS_RX_STATS *gpsRxStats) { - int ret = PARSER_INCOMPLETE; static uint8_t rx_count = 0; static bool start_flag = false; static bool found_cr = false; @@ -139,8 +138,6 @@ int parse_nmea_stream(uint8_t *rx, uint8_t len, char *gps_rx_buffer, GPSPosition } else { i = len; } - // if no more data, we can return an error - ret = PARSER_ERROR; // loop to restart at the $ if there is one continue; } @@ -150,7 +147,6 @@ int parse_nmea_stream(uint8_t *rx, uint8_t len, char *gps_rx_buffer, GPSPosition // Flush the buffer and note the overflow event. gpsRxStats->gpsRxOverflow++; start_flag = false; - ret = PARSER_OVERRUN; continue; } else { gps_rx_buffer[rx_count++] = c; @@ -182,13 +178,11 @@ int parse_nmea_stream(uint8_t *rx, uint8_t len, char *gps_rx_buffer, GPSPosition // PIOS_DEBUG_PinHigh(2); gpsRxStats->gpsRxChkSumError++; // PIOS_DEBUG_PinLow(2); - ret = PARSER_ERROR; } else { // Valid checksum, use this packet to update the GPS position if (!NMEA_update_position(&gps_rx_buffer[1], GpsData)) { // PIOS_DEBUG_PinHigh(2); gpsRxStats->gpsRxParserError++; // PIOS_DEBUG_PinLow(2); - ret = PARSER_ERROR; } else { gpsRxStats->gpsRxReceived++; goodParse = true; @@ -205,7 +199,7 @@ int parse_nmea_stream(uint8_t *rx, uint8_t len, char *gps_rx_buffer, GPSPosition // might think the GPS was offline return PARSER_COMPLETE; } else { - return ret; + return PARSER_INCOMPLETE; } } diff --git a/flight/modules/GPS/UBX.c b/flight/modules/GPS/UBX.c index d34a59195..0ff32f4a4 100644 --- a/flight/modules/GPS/UBX.c +++ b/flight/modules/GPS/UBX.c @@ -126,45 +126,56 @@ int parse_ubx_stream(uint8_t *rx, uint16_t len, char *gps_rx_buffer, GPSPosition static enum proto_states proto_state = START; static uint16_t rx_count = 0; struct UBXPacket *ubx = (struct UBXPacket *)gps_rx_buffer; - int i = 0; + uint16_t i = 0; + uint16_t restart_index = 0; while (i < len) { c = rx[i++]; switch (proto_state) { case START: // detect protocol if (c == UBX_SYNC1) { // first UBX sync char found - proto_state = UBX_SY2; + proto_state = UBX_SY2; + // restart here, at byte after SYNC1, if we fail to parse + restart_index = i; } - break; + continue; case UBX_SY2: if (c == UBX_SYNC2) { // second UBX sync char found proto_state = UBX_CLASS; } else { - proto_state = START; // reset state + goto RESTART; // declare a packet error and reparse packet } - break; + continue; case UBX_CLASS: ubx->header.class = c; proto_state = UBX_ID; - break; + continue; case UBX_ID: ubx->header.id = c; proto_state = UBX_LEN1; - break; + continue; case UBX_LEN1: ubx->header.len = c; proto_state = UBX_LEN2; - break; + continue; case UBX_LEN2: ubx->header.len += (c << 8); if (ubx->header.len > sizeof(UBXPayload)) { gpsRxStats->gpsRxOverflow++; - proto_state = START; +#if defined(PIOS_GPS_MINIMAL) + goto RESTART_NOERROR; // known issue that some packets are too long - reparse packet +#else + goto RESTART; // declare a packet error and reparse packet +#endif } else { - rx_count = 0; - proto_state = UBX_PAYLOAD; + if (ubx->header.len == 0) { + proto_state = UBX_CHK1; + } else { + proto_state = UBX_PAYLOAD; + rx_count = 0; + } } - break; + continue; case UBX_PAYLOAD: if (rx_count < ubx->header.len) { ubx->payload.payload[rx_count] = c; @@ -172,32 +183,44 @@ int parse_ubx_stream(uint8_t *rx, uint16_t len, char *gps_rx_buffer, GPSPosition proto_state = UBX_CHK1; } } - break; + continue; case UBX_CHK1: ubx->header.ck_a = c; proto_state = UBX_CHK2; - break; + continue; case UBX_CHK2: ubx->header.ck_b = c; if (checksum_ubx_message(ubx)) { // message complete and valid parse_ubx_message(ubx, GpsData); - proto_state = FINISHED; + gpsRxStats->gpsRxReceived++; + proto_state = START; + // pass PARSER_ERROR to be to caller if it happens even once + if (ret == PARSER_INCOMPLETE) { + ret = PARSER_COMPLETE; // message complete & processed + } } else { gpsRxStats->gpsRxChkSumError++; - proto_state = START; + goto RESTART; } - break; + continue; default: - break; + continue; } - if (proto_state == START) { - ret = (ret != PARSER_COMPLETE) ? PARSER_ERROR : PARSER_COMPLETE; // parser couldn't use this byte - } else if (proto_state == FINISHED) { - gpsRxStats->gpsRxReceived++; - proto_state = START; - ret = PARSER_COMPLETE; // message complete & processed - } + // this simple restart doesn't work across calls + // but it does work within a single call + // and it does the expected thing across calls + // if restarting due to error detected in 2nd call to this function (on split packet) + // then we just restart at index 0, which is mid-packet, not the second byte +RESTART: + ret = PARSER_ERROR; // inform caller that we found at least one error (along with 0 or more good packets) +#if defined(PIOS_GPS_MINIMAL) +RESTART_NOERROR: +#endif + rx += restart_index; // restart parsing just past the most recent SYNC1 + len -= restart_index; + i = 0; + proto_state = START; } return ret; @@ -528,7 +551,7 @@ uint32_t parse_ubx_message(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosi GpsPosition->SensorType = sensorType; if (msgtracker.msg_received == ALL_RECEIVED) { - // leave my new field alone! + // leave BaudRate field alone GPSPositionSensorBaudRateGet(&GpsPosition->BaudRate); GPSPositionSensorSet(GpsPosition); msgtracker.msg_received = NONE_RECEIVED; From 44a57cb308be5e5c701a514a2c7b37023c50128e Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Sun, 15 Nov 2015 18:13:51 -0500 Subject: [PATCH 2/5] LP-145 reduce UBX false positive red X from 1 in 256 NMEA packets to 1 in 64k --- flight/modules/GPS/UBX.c | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/flight/modules/GPS/UBX.c b/flight/modules/GPS/UBX.c index 0ff32f4a4..f6b6167e5 100644 --- a/flight/modules/GPS/UBX.c +++ b/flight/modules/GPS/UBX.c @@ -143,7 +143,7 @@ int parse_ubx_stream(uint8_t *rx, uint16_t len, char *gps_rx_buffer, GPSPosition if (c == UBX_SYNC2) { // second UBX sync char found proto_state = UBX_CLASS; } else { - goto RESTART; // declare a packet error and reparse packet + goto RESTART_NOERROR; // declare a packet error and reparse packet } continue; case UBX_CLASS: @@ -214,9 +214,7 @@ int parse_ubx_stream(uint8_t *rx, uint16_t len, char *gps_rx_buffer, GPSPosition // then we just restart at index 0, which is mid-packet, not the second byte RESTART: ret = PARSER_ERROR; // inform caller that we found at least one error (along with 0 or more good packets) -#if defined(PIOS_GPS_MINIMAL) RESTART_NOERROR: -#endif rx += restart_index; // restart parsing just past the most recent SYNC1 len -= restart_index; i = 0; From bb04d6275671e6ce73f4694d6fdb287d665133a1 Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Sun, 15 Nov 2015 18:42:47 -0500 Subject: [PATCH 3/5] LP-145 uncrustify and comments --- flight/modules/GPS/UBX.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/flight/modules/GPS/UBX.c b/flight/modules/GPS/UBX.c index f6b6167e5..5ac03b41f 100644 --- a/flight/modules/GPS/UBX.c +++ b/flight/modules/GPS/UBX.c @@ -143,7 +143,7 @@ int parse_ubx_stream(uint8_t *rx, uint16_t len, char *gps_rx_buffer, GPSPosition if (c == UBX_SYNC2) { // second UBX sync char found proto_state = UBX_CLASS; } else { - goto RESTART_NOERROR; // declare a packet error and reparse packet + goto RESTART_NOERROR; // reparse packet but don't declare error } continue; case UBX_CLASS: @@ -163,7 +163,7 @@ int parse_ubx_stream(uint8_t *rx, uint16_t len, char *gps_rx_buffer, GPSPosition if (ubx->header.len > sizeof(UBXPayload)) { gpsRxStats->gpsRxOverflow++; #if defined(PIOS_GPS_MINIMAL) - goto RESTART_NOERROR; // known issue that some packets are too long - reparse packet + goto RESTART_NOERROR; // known issue that some packets are too long on CC3D - reparse packet #else goto RESTART; // declare a packet error and reparse packet #endif @@ -213,9 +213,9 @@ int parse_ubx_stream(uint8_t *rx, uint16_t len, char *gps_rx_buffer, GPSPosition // if restarting due to error detected in 2nd call to this function (on split packet) // then we just restart at index 0, which is mid-packet, not the second byte RESTART: - ret = PARSER_ERROR; // inform caller that we found at least one error (along with 0 or more good packets) + ret = PARSER_ERROR; // inform caller that we found at least one error (along with 0 or more good packets) RESTART_NOERROR: - rx += restart_index; // restart parsing just past the most recent SYNC1 + rx += restart_index; // restart parsing just past the most recent SYNC1 len -= restart_index; i = 0; proto_state = START; From f4a088bcfad3c7834b32c66e375bdc901f614db5 Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Tue, 2 Feb 2016 09:17:56 -0500 Subject: [PATCH 4/5] change forward error gotos into breaks --- flight/modules/GPS/UBX.c | 52 ++++++++++++++++++++++++++-------------- 1 file changed, 34 insertions(+), 18 deletions(-) diff --git a/flight/modules/GPS/UBX.c b/flight/modules/GPS/UBX.c index 5ac03b41f..262c72ab6 100644 --- a/flight/modules/GPS/UBX.c +++ b/flight/modules/GPS/UBX.c @@ -122,13 +122,20 @@ int parse_ubx_stream(uint8_t *rx, uint16_t len, char *gps_rx_buffer, GPSPosition UBX_CHK2, FINISHED }; + enum restart_states { + RESTART_WITH_ERROR, + RESTART_NO_ERROR + }; uint8_t c; static enum proto_states proto_state = START; static uint16_t rx_count = 0; struct UBXPacket *ubx = (struct UBXPacket *)gps_rx_buffer; uint16_t i = 0; uint16_t restart_index = 0; + enum restart_states restart_state; + // switch continue is the normal condition and comes back to here for another byte + // switch break is the error state that branches to the end and restarts the scan at the byte after the first sync byte while (i < len) { c = rx[i++]; switch (proto_state) { @@ -143,7 +150,8 @@ int parse_ubx_stream(uint8_t *rx, uint16_t len, char *gps_rx_buffer, GPSPosition if (c == UBX_SYNC2) { // second UBX sync char found proto_state = UBX_CLASS; } else { - goto RESTART_NOERROR; // reparse packet but don't declare error + restart_state = RESTART_NO_ERROR; + break; } continue; case UBX_CLASS: @@ -163,9 +171,11 @@ int parse_ubx_stream(uint8_t *rx, uint16_t len, char *gps_rx_buffer, GPSPosition if (ubx->header.len > sizeof(UBXPayload)) { gpsRxStats->gpsRxOverflow++; #if defined(PIOS_GPS_MINIMAL) - goto RESTART_NOERROR; // known issue that some packets are too long on CC3D - reparse packet + restart_state = RESTART_NO_ERROR; + break; #else - goto RESTART; // declare a packet error and reparse packet + restart_state = RESTART_WITH_ERROR; + break; #endif } else { if (ubx->header.len == 0) { @@ -190,6 +200,12 @@ int parse_ubx_stream(uint8_t *rx, uint16_t len, char *gps_rx_buffer, GPSPosition continue; case UBX_CHK2: ubx->header.ck_b = c; + // OP GPSV9 sends data with bad checksums this appears to happen because it drops data + // this has been proven by running it without autoconfig and testing: + // data coming from OPV9 "GPS+MCU" port the checksum errors happen roughly every 5 to 30 seconds + // same data coming from OPV9 "GPS Only" port the checksums are always good + // this also ocassionally causes parse_ubx_message() to issue alarms because not all the messages were received + // see OP GPSV9 comment in parse_ubx_message() for further information if (checksum_ubx_message(ubx)) { // message complete and valid parse_ubx_message(ubx, GpsData); gpsRxStats->gpsRxReceived++; @@ -200,7 +216,8 @@ int parse_ubx_stream(uint8_t *rx, uint16_t len, char *gps_rx_buffer, GPSPosition } } else { gpsRxStats->gpsRxChkSumError++; - goto RESTART; + restart_state = RESTART_WITH_ERROR; + break; } continue; default: @@ -212,9 +229,10 @@ int parse_ubx_stream(uint8_t *rx, uint16_t len, char *gps_rx_buffer, GPSPosition // and it does the expected thing across calls // if restarting due to error detected in 2nd call to this function (on split packet) // then we just restart at index 0, which is mid-packet, not the second byte -RESTART: - ret = PARSER_ERROR; // inform caller that we found at least one error (along with 0 or more good packets) -RESTART_NOERROR: + if (restart_state == RESTART_WITH_ERROR) { + ret = PARSER_ERROR; // inform caller that we found at least one error (along with 0 or more good packets) + } + // else restart with no error rx += restart_index; // restart parsing just past the most recent SYNC1 len -= restart_index; i = 0; @@ -549,7 +567,7 @@ uint32_t parse_ubx_message(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosi GpsPosition->SensorType = sensorType; if (msgtracker.msg_received == ALL_RECEIVED) { - // leave BaudRate field alone + // leave BaudRate field alone! GPSPositionSensorBaudRateGet(&GpsPosition->BaudRate); GPSPositionSensorSet(GpsPosition); msgtracker.msg_received = NONE_RECEIVED; @@ -559,6 +577,10 @@ uint32_t parse_ubx_message(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosi GPSPositionSensorStatusGet(&status); if (status == GPSPOSITIONSENSOR_STATUS_NOGPS) { // Some ubx thing has been received so GPS is there + // + // OP GPSV9 will sometimes cause this NOFIX + // because GPSV9 drops data which causes checksum errors which causes GPS.c to set the status to NOGPS + // see OP GPSV9 comment in parse_ubx_stream() for further information status = GPSPOSITIONSENSOR_STATUS_NOFIX; GPSPositionSensorStatusSet(&status); } @@ -567,18 +589,12 @@ uint32_t parse_ubx_message(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosi } #if !defined(PIOS_GPS_MINIMAL) - -void load_mag_settings() +void op_gpsv9_load_mag_settings() { - auxmagsupport_reload_settings(); - - if (auxmagsupport_get_type() != AUXMAGSETTINGS_TYPE_GPSV9) { - useMag = false; - const uint8_t status = AUXMAGSENSOR_STATUS_NONE; - // next sample from other external mags will provide the right status if present - AuxMagSensorStatusSet((uint8_t *)&status); - } else { + if (auxmagsupport_get_type() == AUXMAGSETTINGS_TYPE_GPSV9) { useMag = true; + } else { + useMag = false; } } #endif From dae90526865adbdae05b0b9c98948729cd57c80f Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Tue, 2 Feb 2016 09:39:53 -0500 Subject: [PATCH 5/5] finish renaming load_mag_settings to include hardware type --- flight/modules/GPS/GPS.c | 2 +- flight/modules/GPS/inc/UBX.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/flight/modules/GPS/GPS.c b/flight/modules/GPS/GPS.c index 9a7f5490d..fea271921 100644 --- a/flight/modules/GPS/GPS.c +++ b/flight/modules/GPS/GPS.c @@ -551,7 +551,7 @@ static void updateHwSettings(UAVObjEvent __attribute__((unused)) *ev) #if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) void AuxMagSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { - load_mag_settings(); + op_gpsv9_load_mag_settings(); } diff --git a/flight/modules/GPS/inc/UBX.h b/flight/modules/GPS/inc/UBX.h index c4ea60357..8263da2ca 100644 --- a/flight/modules/GPS/inc/UBX.h +++ b/flight/modules/GPS/inc/UBX.h @@ -620,6 +620,6 @@ bool checksum_ubx_message(struct UBXPacket *); uint32_t parse_ubx_message(struct UBXPacket *, GPSPositionSensorData *); int parse_ubx_stream(uint8_t *rx, uint16_t len, char *, GPSPositionSensorData *, struct GPS_RX_STATS *); -void load_mag_settings(); +void op_gpsv9_load_mag_settings(); #endif /* UBX_H */