rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-03-16 08:29:15 +01:00
Code Issues Releases Activity

LP-145 CC3D GPS reparse and general GPS system health

Browse Source
This commit is contained in:
Cliff Geerdes 2015-11-15 03:44:28 -05:00
parent 8c3beaa28c
commit f0ef594cdb
3 changed files with 62 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
flight/modules/GPS/GPS.c
View File

@ -77,7 +77,9 @@ void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev);
// **************** // ****************
// Private constants // 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 // delay from detecting HomeLocation.Set == False before setting new homelocation
// this prevent that a save with homelocation.Set = false triggered by gps ends saving // this prevent that a save with homelocation.Set = false triggered by gps ends saving
// the new location with Set = true. // the new location with Set = true.
@ -120,7 +122,6 @@ static xTaskHandle gpsTaskHandle;
static char *gps_rx_buffer; static char *gps_rx_buffer;
static uint32_t timeOfLastCommandMs;
static uint32_t timeOfLastUpdateMs; static uint32_t timeOfLastUpdateMs;
#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER) || defined(PIOS_INCLUDE_GPS_UBX_PARSER) #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 #endif
GPSPositionSensorData gpspositionsensor; GPSPositionSensorData gpspositionsensor;
timeOfLastUpdateMs = timeNowMs; timeOfLastUpdateMs = timeNowMs;
timeOfLastCommandMs = timeNowMs;
GPSPositionSensorGet(&gpspositionsensor); GPSPositionSensorGet(&gpspositionsensor);
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) #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) */ #endif /* if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) */
uint16_t cnt; uint16_t cnt;
int res;
// This blocks the task until there is something on the buffer (or 100ms? passes) // This blocks the task until there is something on the buffer (or 100ms? passes)
cnt = PIOS_COM_ReceiveBuffer(gpsPort, c, GPS_READ_BUFFER, xDelay); cnt = PIOS_COM_ReceiveBuffer(gpsPort, c, GPS_READ_BUFFER, xDelay);
res = PARSER_INCOMPLETE;
if (cnt > 0) { if (cnt > 0) {
PERF_TIMED_SECTION_START(counterParse); PERF_TIMED_SECTION_START(counterParse);
PERF_TRACK_VALUE(counterBytesIn, cnt); PERF_TRACK_VALUE(counterBytesIn, cnt);
PERF_MEASURE_PERIOD(counterRate); PERF_MEASURE_PERIOD(counterRate);
int res;
switch (gpsSettings.DataProtocol) { switch (gpsSettings.DataProtocol) {
#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER) #if defined(PIOS_INCLUDE_GPS_NMEA_PARSER)
case GPSSETTINGS_DATAPROTOCOL_NMEA: case GPSSETTINGS_DATAPROTOCOL_NMEA:
@ -339,20 +340,23 @@ static void gpsTask(__attribute__((unused)) void *parameters)
if (res == PARSER_COMPLETE) { if (res == PARSER_COMPLETE) {
timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS; timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
timeOfLastUpdateMs = timeNowMs; timeOfLastUpdateMs = timeNowMs;
timeOfLastCommandMs = timeNowMs;
} }
} }
// if there is any error at all, set status to NOGPS
// Check for GPS timeout // Check for GPS timeout
timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS; 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)) { (gpsSettings.DataProtocol == GPSSETTINGS_DATAPROTOCOL_UBX && gpspositionsensor.AutoConfigStatus == GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_ERROR)) {
// we have not received any valid GPS sentences for a while. // 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. // either the GPS is not plugged in or a hardware problem or the GPS has locked up.
GPSPositionSensorStatusOptions status = GPSPOSITIONSENSOR_STATUS_NOGPS; GPSPositionSensorStatusOptions status = GPSPOSITIONSENSOR_STATUS_NOGPS;
GPSPositionSensorStatusSet(&status); GPSPositionSensorStatusSet(&status);
AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_ERROR); 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 // we appear to be receiving GPS sentences OK, we've had an update
// criteria for GPS-OK taken from this post... // criteria for GPS-OK taken from this post...
// http://forums.openpilot.org/topic/1523-professors-insgps-in-svn/page__view__findpost__p__5220 // http://forums.openpilot.org/topic/1523-professors-insgps-in-svn/page__view__findpost__p__5220

8
flight/modules/GPS/NMEA.c
View File

@ -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 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 uint8_t rx_count = 0;
static bool start_flag = false; static bool start_flag = false;
static bool found_cr = 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 { } else {
i = len; i = len;
} }
// if no more data, we can return an error
ret = PARSER_ERROR;
// loop to restart at the $ if there is one // loop to restart at the $ if there is one
continue; 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. // Flush the buffer and note the overflow event.
gpsRxStats->gpsRxOverflow++; gpsRxStats->gpsRxOverflow++;
start_flag = false; start_flag = false;
ret = PARSER_OVERRUN;
continue; continue;
} else { } else {
gps_rx_buffer[rx_count++] = c; 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); // PIOS_DEBUG_PinHigh(2);
gpsRxStats->gpsRxChkSumError++; gpsRxStats->gpsRxChkSumError++;
// PIOS_DEBUG_PinLow(2); // PIOS_DEBUG_PinLow(2);
ret = PARSER_ERROR;
} else { // Valid checksum, use this packet to update the GPS position } else { // Valid checksum, use this packet to update the GPS position
if (!NMEA_update_position(&gps_rx_buffer[1], GpsData)) { if (!NMEA_update_position(&gps_rx_buffer[1], GpsData)) {
// PIOS_DEBUG_PinHigh(2); // PIOS_DEBUG_PinHigh(2);
gpsRxStats->gpsRxParserError++; gpsRxStats->gpsRxParserError++;
// PIOS_DEBUG_PinLow(2); // PIOS_DEBUG_PinLow(2);
ret = PARSER_ERROR;
} else { } else {
gpsRxStats->gpsRxReceived++; gpsRxStats->gpsRxReceived++;
goodParse = true; 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 // might think the GPS was offline
return PARSER_COMPLETE; return PARSER_COMPLETE;
} else { } else {
return ret; return PARSER_INCOMPLETE;
} }
} }

75
flight/modules/GPS/UBX.c
View File

@ -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 enum proto_states proto_state = START;
static uint16_t rx_count = 0; static uint16_t rx_count = 0;
struct UBXPacket *ubx = (struct UBXPacket *)gps_rx_buffer; struct UBXPacket *ubx = (struct UBXPacket *)gps_rx_buffer;
int i = 0; uint16_t i = 0;
uint16_t restart_index = 0;
while (i < len) { while (i < len) {
c = rx[i++]; c = rx[i++];
switch (proto_state) { switch (proto_state) {
case START: // detect protocol case START: // detect protocol
if (c == UBX_SYNC1) { // first UBX sync char found 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: case UBX_SY2:
if (c == UBX_SYNC2) { // second UBX sync char found if (c == UBX_SYNC2) { // second UBX sync char found
proto_state = UBX_CLASS; proto_state = UBX_CLASS;
} else { } else {
proto_state = START; // reset state goto RESTART; // declare a packet error and reparse packet
} }
break; continue;
case UBX_CLASS: case UBX_CLASS:
ubx->header.class = c; ubx->header.class = c;
proto_state = UBX_ID; proto_state = UBX_ID;
break; continue;
case UBX_ID: case UBX_ID:
ubx->header.id = c; ubx->header.id = c;
proto_state = UBX_LEN1; proto_state = UBX_LEN1;
break; continue;
case UBX_LEN1: case UBX_LEN1:
ubx->header.len = c; ubx->header.len = c;
proto_state = UBX_LEN2; proto_state = UBX_LEN2;
break; continue;
case UBX_LEN2: case UBX_LEN2:
ubx->header.len += (c << 8); ubx->header.len += (c << 8);
if (ubx->header.len > sizeof(UBXPayload)) { if (ubx->header.len > sizeof(UBXPayload)) {
gpsRxStats->gpsRxOverflow++; 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 { } else {
rx_count = 0; if (ubx->header.len == 0) {
proto_state = UBX_PAYLOAD; proto_state = UBX_CHK1;
} else {
proto_state = UBX_PAYLOAD;
rx_count = 0;
}
} }
break; continue;
case UBX_PAYLOAD: case UBX_PAYLOAD:
if (rx_count < ubx->header.len) { if (rx_count < ubx->header.len) {
ubx->payload.payload[rx_count] = c; 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; proto_state = UBX_CHK1;
} }
} }
break; continue;
case UBX_CHK1: case UBX_CHK1:
ubx->header.ck_a = c; ubx->header.ck_a = c;
proto_state = UBX_CHK2; proto_state = UBX_CHK2;
break; continue;
case UBX_CHK2: case UBX_CHK2:
ubx->header.ck_b = c; ubx->header.ck_b = c;
if (checksum_ubx_message(ubx)) { // message complete and valid if (checksum_ubx_message(ubx)) { // message complete and valid
parse_ubx_message(ubx, GpsData); 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 { } else {
gpsRxStats->gpsRxChkSumError++; gpsRxStats->gpsRxChkSumError++;
proto_state = START; goto RESTART;
} }
break; continue;
default: default:
break; continue;
} }
if (proto_state == START) { // this simple restart doesn't work across calls
ret = (ret != PARSER_COMPLETE) ? PARSER_ERROR : PARSER_COMPLETE; // parser couldn't use this byte // but it does work within a single call
} else if (proto_state == FINISHED) { // and it does the expected thing across calls
gpsRxStats->gpsRxReceived++; // if restarting due to error detected in 2nd call to this function (on split packet)
proto_state = START; // then we just restart at index 0, which is mid-packet, not the second byte
ret = PARSER_COMPLETE; // message complete & processed 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; return ret;
@ -528,7 +551,7 @@ uint32_t parse_ubx_message(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosi
GpsPosition->SensorType = sensorType; GpsPosition->SensorType = sensorType;
if (msgtracker.msg_received == ALL_RECEIVED) { if (msgtracker.msg_received == ALL_RECEIVED) {
// leave my new field alone! // leave BaudRate field alone
GPSPositionSensorBaudRateGet(&GpsPosition->BaudRate); GPSPositionSensorBaudRateGet(&GpsPosition->BaudRate);
GPSPositionSensorSet(GpsPosition); GPSPositionSensorSet(GpsPosition);
msgtracker.msg_received = NONE_RECEIVED; msgtracker.msg_received = NONE_RECEIVED;