From a5ebf36425de260a8d709de86f3e7f0de0fe46eb Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Sun, 21 Sep 2014 16:38:52 +0200 Subject: [PATCH 1/3] OP-1499 - Replay the configuration sequence in case of gps disconnection, add retry logic in case of errors --- flight/modules/GPS/GPS.c | 4 ++- flight/modules/GPS/UBX.c | 8 ++++++ flight/modules/GPS/inc/ubx_autoconfig.h | 18 ++++++++---- flight/modules/GPS/ubx_autoconfig.c | 37 ++++++++++++++++++++++--- 4 files changed, 56 insertions(+), 11 deletions(-) diff --git a/flight/modules/GPS/GPS.c b/flight/modules/GPS/GPS.c index 9729c5189..2517aef98 100644 --- a/flight/modules/GPS/GPS.c +++ b/flight/modules/GPS/GPS.c @@ -245,7 +245,9 @@ static void gpsTask(__attribute__((unused)) void *parameters) if (gpsSettings.DataProtocol == GPSSETTINGS_DATAPROTOCOL_UBX) { char *buffer = 0; uint16_t count = 0; - ubx_autoconfig_run(&buffer, &count); + uint8_t status; + GPSPositionSensorStatusGet(&status); + ubx_autoconfig_run(&buffer, &count, status != GPSPOSITIONSENSOR_STATUS_NOGPS); // Something to send? if (count) { PIOS_COM_SendBuffer(gpsPort, (uint8_t *)buffer, count); diff --git a/flight/modules/GPS/UBX.c b/flight/modules/GPS/UBX.c index ba417b5ac..c883e7a39 100644 --- a/flight/modules/GPS/UBX.c +++ b/flight/modules/GPS/UBX.c @@ -515,6 +515,14 @@ uint32_t parse_ubx_message(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosi GPSPositionSensorSet(GpsPosition); msgtracker.msg_received = NONE_RECEIVED; id = GPSPOSITIONSENSOR_OBJID; + } else { + uint8_t status; + GPSPositionSensorStatusGet(&status); + if (status == GPSPOSITIONSENSOR_STATUS_NOGPS) { + // Some ubx thing has been received so GPS is there + status = GPSPOSITIONSENSOR_STATUS_NOFIX; + GPSPositionSensorStatusSet(&status); + } } return id; } diff --git a/flight/modules/GPS/inc/ubx_autoconfig.h b/flight/modules/GPS/inc/ubx_autoconfig.h index e01391a7d..25a5857b0 100644 --- a/flight/modules/GPS/inc/ubx_autoconfig.h +++ b/flight/modules/GPS/inc/ubx_autoconfig.h @@ -33,12 +33,18 @@ // defines // TODO: NEO8 max rate is for Rom version, flash is limited to 10Hz, need to handle that. -#define UBX_MAX_RATE_VER8 18 -#define UBX_MAX_RATE_VER7 10 -#define UBX_MAX_RATE 5 +#define UBX_MAX_RATE_VER8 18 +#define UBX_MAX_RATE_VER7 10 +#define UBX_MAX_RATE 5 -#define UBX_REPLY_TIMEOUT (500 * 1000) -#define UBX_MAX_RETRIES 5 +// time to wait before reinitializing the fsm due to disconnection +#define UBX_CONNECTION_TIMEOUT (2000 * 1000) +// times between retries in case an error does occurs +#define UBX_ERROR_RETRY_TIMEOUT (1000 * 1000) +// timeout for ack reception +#define UBX_REPLY_TIMEOUT (500 * 1000) +// max retries in case of timeout +#define UBX_MAX_RETRIES 5 // types typedef enum { @@ -179,7 +185,7 @@ typedef union { } message; } __attribute__((packed)) UBXSentPacket_t; -void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send); +void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send, bool gps_connected); void ubx_autoconfig_set(ubx_autoconfig_settings_t config); int32_t ubx_autoconfig_get_status(); #endif /* UBX_AUTOCONFIG_H_ */ diff --git a/flight/modules/GPS/ubx_autoconfig.c b/flight/modules/GPS/ubx_autoconfig.c index 74aa89179..fbecfa9f7 100644 --- a/flight/modules/GPS/ubx_autoconfig.c +++ b/flight/modules/GPS/ubx_autoconfig.c @@ -46,6 +46,7 @@ typedef enum { typedef struct { initSteps_t currentStep; // Current configuration "fsm" status uint32_t lastStepTimestampRaw; // timestamp of last operation + uint32_t lastConnectedRaw; // timestamp of last time gps was connected UBXSentPacket_t working_packet; // outbound "buffer" ubx_autoconfig_settings_t currentSettings; int8_t lastConfigSent; // index of last configuration string sent @@ -279,15 +280,38 @@ static void enable_sentences(__attribute__((unused)) uint16_t *bytes_to_send) } } -void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send) +void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send, bool gps_connected) { *bytes_to_send = 0; *buffer = (char *)status->working_packet.buffer; - if (!status || !enabled) { + if (!status || !enabled || status->currentStep == INIT_STEP_DISABLED) { return; // autoconfig not enabled } + + // when gps is disconnected it will replay the autoconfig sequence. + if (!gps_connected) { + if (status->currentStep == INIT_STEP_DONE) { + // if some operation is in progress and it is not running into issues it maybe that + // the disconnection is only due to wrong message rates, so reinit only when done. + // errors caused by disconnection are handled by error retry logic + if (PIOS_DELAY_DiffuS(status->lastConnectedRaw) > UBX_CONNECTION_TIMEOUT) { + status->currentStep = INIT_STEP_START; + return; + } + } + } else { + // reset connection timer + status->lastConnectedRaw = PIOS_DELAY_GetRaw(); + } + switch (status->currentStep) { - case INIT_STEP_ERROR: // TODO: what to do? retries after a while? maybe gps was disconnected (this can be detected)? + case INIT_STEP_ERROR: + if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) > UBX_ERROR_RETRY_TIMEOUT) { + status->currentStep = INIT_STEP_START; + status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); + } + return; + case INIT_STEP_DISABLED: case INIT_STEP_DONE: return; @@ -348,6 +372,7 @@ void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send) status->retryCount++; if (status->retryCount > UBX_MAX_RETRIES) { status->currentStep = INIT_STEP_ERROR; + status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); return; } } @@ -374,12 +399,16 @@ void ubx_autoconfig_set(ubx_autoconfig_settings_t config) status->currentSettings = config; status->currentStep = INIT_STEP_START; enabled = true; + } else { + if (!status) { + status->currentStep = INIT_STEP_DISABLED; + } } } int32_t ubx_autoconfig_get_status() { - if (!status) { + if (!status || !enabled) { return UBX_AUTOCONFIG_STATUS_DISABLED; } switch (status->currentStep) { From 0885fda9263a8c1da60c73c4d378d9643051cc8c Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Mon, 22 Sep 2014 00:56:49 +0200 Subject: [PATCH 2/3] OP-1499 - CFG-CFG mask was incorrect (nav is 0x02, not 0x01) --- flight/modules/GPS/ubx_autoconfig.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/modules/GPS/ubx_autoconfig.c b/flight/modules/GPS/ubx_autoconfig.c index fbecfa9f7..bf9b41cf7 100644 --- a/flight/modules/GPS/ubx_autoconfig.c +++ b/flight/modules/GPS/ubx_autoconfig.c @@ -228,7 +228,7 @@ void config_save(uint16_t *bytes_to_send) { memset(status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_cfg_t)); // mask LSB=ioPort|msgConf|infMsg|navConf|rxmConf|||||rinvConf|antConf|....|= MSB - status->working_packet.message.payload.cfg_cfg.saveMask = 0x01 | 0x08; // msgConf + navConf + status->working_packet.message.payload.cfg_cfg.saveMask = 0x02 | 0x08; // msgConf + navConf status->working_packet.message.payload.cfg_cfg.deviceMask = UBX_CFG_CFG_ALL_DEVICES_MASK; *bytes_to_send = prepare_packet(&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_CFG, sizeof(ubx_cfg_cfg_t)); status->requiredAck.clsID = UBX_CLASS_CFG; From 78756f6873a8e40debb6d09d5baf39c58340b77f Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Wed, 24 Sep 2014 06:12:48 +0200 Subject: [PATCH 3/3] OP-1499 - Failed Autoconfig will raise an Error Alarm. When AC does start it will reset any previous ack. Force update to Autoconfig Status in GPSPositionSensor UAVO. Add a small pause between each config step --- flight/modules/GPS/GPS.c | 9 ++++++++- flight/modules/GPS/inc/ubx_autoconfig.h | 3 ++- flight/modules/GPS/ubx_autoconfig.c | 8 +++++++- 3 files changed, 17 insertions(+), 3 deletions(-) diff --git a/flight/modules/GPS/GPS.c b/flight/modules/GPS/GPS.c index 2517aef98..e8524fd2e 100644 --- a/flight/modules/GPS/GPS.c +++ b/flight/modules/GPS/GPS.c @@ -268,11 +268,17 @@ static void gpsTask(__attribute__((unused)) void *parameters) { #if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) int32_t ac_status = ubx_autoconfig_get_status(); + static uint8_t lastStatus = 0; + gpspositionsensor.AutoConfigStatus = ac_status == UBX_AUTOCONFIG_STATUS_DISABLED ? GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DISABLED : ac_status == UBX_AUTOCONFIG_STATUS_DONE ? GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DONE : ac_status == UBX_AUTOCONFIG_STATUS_ERROR ? GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_ERROR : GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_RUNNING; + if (gpspositionsensor.AutoConfigStatus != lastStatus) { + GPSPositionSensorAutoConfigStatusSet(&gpspositionsensor.AutoConfigStatus); + lastStatus = gpspositionsensor.AutoConfigStatus; + } #endif res = parse_ubx_stream(c, gps_rx_buffer, &gpspositionsensor, &gpsRxStats); } @@ -292,7 +298,8 @@ static void gpsTask(__attribute__((unused)) void *parameters) // Check for GPS timeout timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS; - if ((timeNowMs - timeOfLastUpdateMs) >= GPS_TIMEOUT_MS) { + if ((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. uint8_t status = GPSPOSITIONSENSOR_STATUS_NOGPS; diff --git a/flight/modules/GPS/inc/ubx_autoconfig.h b/flight/modules/GPS/inc/ubx_autoconfig.h index 25a5857b0..734c54537 100644 --- a/flight/modules/GPS/inc/ubx_autoconfig.h +++ b/flight/modules/GPS/inc/ubx_autoconfig.h @@ -45,7 +45,8 @@ #define UBX_REPLY_TIMEOUT (500 * 1000) // max retries in case of timeout #define UBX_MAX_RETRIES 5 - +// pause between each configuration step +#define UBX_STEP_WAIT_TIME (10 * 1000) // types typedef enum { UBX_AUTOCONFIG_STATUS_DISABLED = 0, diff --git a/flight/modules/GPS/ubx_autoconfig.c b/flight/modules/GPS/ubx_autoconfig.c index bf9b41cf7..1671b601a 100644 --- a/flight/modules/GPS/ubx_autoconfig.c +++ b/flight/modules/GPS/ubx_autoconfig.c @@ -287,7 +287,9 @@ void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send, bool gps_connec if (!status || !enabled || status->currentStep == INIT_STEP_DISABLED) { return; // autoconfig not enabled } - + if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) < UBX_STEP_WAIT_TIME) { + return; + } // when gps is disconnected it will replay the autoconfig sequence. if (!gps_connected) { if (status->currentStep == INIT_STEP_DONE) { @@ -318,6 +320,10 @@ void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send, bool gps_connec case INIT_STEP_START: case INIT_STEP_ASK_VER: + // clear ack + ubxLastAck.clsID = 0x00; + ubxLastAck.msgID = 0x00; + status->lastStepTimestampRaw = PIOS_DELAY_GetRaw(); build_request(&status->working_packet, UBX_CLASS_MON, UBX_ID_MON_VER, bytes_to_send); status->currentStep = INIT_STEP_WAIT_VER;