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;