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;