diff --git a/flight/modules/GPS/GPS.c b/flight/modules/GPS/GPS.c index 74166d6cf..fea271921 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 @@ -547,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/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..262c72ab6 100644 --- a/flight/modules/GPS/UBX.c +++ b/flight/modules/GPS/UBX.c @@ -122,49 +122,70 @@ 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; - int i = 0; + 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) { 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 + restart_state = RESTART_NO_ERROR; + break; } - 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) + restart_state = RESTART_NO_ERROR; + break; +#else + restart_state = RESTART_WITH_ERROR; + break; +#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 +193,50 @@ 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; + // 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); - 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; + restart_state = RESTART_WITH_ERROR; + break; } - 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 + 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; + proto_state = START; } return ret; @@ -528,7 +567,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; @@ -538,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); } @@ -546,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 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 */