OP-1499 - Replay the configuration sequence in case of gps disconnection, add retry logic in case of errors

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

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,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_ */

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) {