Added state management for gps read failure4.0.20

1 files changed, 59 insertions, 5 deletions

diff --git a/lora_pkt_fwd/src/lora_pkt_fwd.c b/lora_pkt_fwd/src/lora_pkt_fwd.c
index 885f80e..145b717 100644
--- a/lora_pkt_fwd/src/lora_pkt_fwd.c
+++ b/lora_pkt_fwd/src/lora_pkt_fwd.c
@@ -312,6 +312,9 @@ static uint32_t tx_freq_max[LGW_RF_CHAIN_NB]; /* highest frequency supported by
 
 static uint32_t rx_rf_freq[LGW_RF_CHAIN_NB]; /* center frequency of the radio in Hz */
 
+static char lora_port[5] = "lora"; /* path mapping for mts-io */
+
+
 /* -------------------------------------------------------------------------- */
 /* --- PRIVATE FUNCTIONS DECLARATION ---------------------------------------- */
 
@@ -1701,7 +1704,6 @@ int main(int argc, char** argv)
         exit(EXIT_FAILURE);
     }
 
-    char lora_port[5] = "lora"; /* path mapping for mts-io */
     /* set custom SPI device path if configured */
     if (strlen(spi_device_path) > 0) {
         if (strcmp(spi_device_path, LORA_PATH_AP1) == 0 ||
@@ -3589,16 +3591,68 @@ void thread_gps(void) {
     /* initialize some variables before loop */
     memset(serial_buff, 0, sizeof serial_buff);
 
+    uint8_t read_fail_count = 0;
+    enum gps_state state = GPS_RUNNING;
+    int i;
+
     while (!exit_sig && !quit_sig) {
         size_t rd_idx = 0;
         size_t frame_end_idx = 0;
+        ssize_t nb_char = 0;
 
-        /* blocking non-canonical read on serial port */
-        ssize_t nb_char = read(gps_tty_fd, serial_buff + wr_idx, LGW_GPS_MIN_MSG_SIZE);
-        if (nb_char <= 0) {
-            MSG_DEBUG(DEBUG_PKT_FWD, "WARNING: [gps] read() returned value %zd\n", nb_char);
+        switch (state) {
+        case GPS_LOST: {
+            i = lgw_gps_disable(gps_tty_fd);
+            if (i == LGW_HAL_SUCCESS) {
+                MSG("INFO: GPS closed successfully\n");
+            } else {
+                MSG("WARNING: failed to close GPS successfully\n");
+            }
+            read_fail_count = 0;
+            state = GPS_RETRYING;
+            wait_ms(5000);
+            continue;
+        }
+        case GPS_RETRYING: {
+            i = lgw_gps_enable(gps_tty_path, "ubx7", 0, &gps_tty_fd, ((strcmp(lora_port, "ap2") == 0) ? 2 : 1));
+            if (i != LGW_GPS_SUCCESS) {
+                printf("WARNING: [main] impossible to open for GPS sync (Check GPSD)\n");
+                gps_enabled = false;
+                gps_ref_valid = false;
+            } else {
+                printf("INFO: [main] GPSD polling open for GPS synchronization\n");
+                gps_enabled = true;
+                gps_ref_valid = false;
+            }
+            nb_char = read(gps_tty_fd, serial_buff + wr_idx, LGW_GPS_MIN_MSG_SIZE);
+            if (nb_char <= 0) {
+                printf("WARNING: [gps] reconnect failed\n");
+                wait_ms(2000);
+                continue;
+            } else {
+                printf("INFO: [gps] reconnected\n");
+                state = GPS_RUNNING;
+                break;
+            }
+        }
+        case GPS_RUNNING: {
+            nb_char = read(gps_tty_fd, serial_buff + wr_idx, LGW_GPS_MIN_MSG_SIZE);
+            if (nb_char <= 0) {
+                printf("WARNING: [gps] read() returned value %zd\n", nb_char);
+                read_fail_count++;
+                if (read_fail_count > 9) {
+                    state = GPS_LOST;
+                    continue;
+                }
+            }
+            break;
+        }
+        default:
+            printf("ERROR: Unknown GPS state, resetting\n");
+            state = GPS_LOST;
             continue;
         }
+
         wr_idx += (size_t)nb_char;
 
         /*******************************************