Merge remote-tracking branch 'origin/amorale/OP-1499_autoconfig_retries' into next

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);
@@ -266,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);
             }
@@ -290,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;

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;
 }

flight/modules/GPS/inc/ubx_autoconfig.h

@@ -33,13 +33,20 @@
 
 // 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_VER8       18
-#define UBX_MAX_RATE_VER7 10
+#define UBX_MAX_RATE_VER7       10
-#define UBX_MAX_RATE      5
+#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
+// pause between each configuration step
+#define UBX_STEP_WAIT_TIME      (10 * 1000)
 // types
 typedef enum {
     UBX_AUTOCONFIG_STATUS_DISABLED = 0,
@@ -179,7 +186,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_ */

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
@@ -227,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;
@@ -279,21 +280,50 @@ 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
     }
+    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) {
+            // 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;
 
     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;
@@ -348,6 +378,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 +405,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) {