Reorganised GPS task to make it a non-blocking task to be called periodically

openrtx/include/core/gps.h

@@ -61,6 +61,6 @@ gps_t;
  * if available, enabled and ready, decode NMEA sentences and update
  * the radio state with the retrieved data.
  */
-void gps_taskFunc(char *line);
+void gps_taskFunc();
 
 #endif /* GPS_H */

openrtx/src/core/gps.c

@@ -27,33 +27,46 @@
 
 #define KNOTS2KMH 1.852f
 
-/**
+static char sentence[MINMEA_MAX_LENGTH + 1];
- * This function parses a GPS NMEA sentence and updates radio state
+static bool isRtcSyncronised  = false;
- */
+static bool gpsEnabled        = false;
-void gps_taskFunc(char *line)
+static bool readNewSentence   = true;
-{
-    char nmea_id[3] = { 0 };
 
-    // Little mechanism to ensure that RTC is synced with GPS time only once.
+void gps_taskFunc()
-    static bool isRtcSyncronised = false;
+{
-    if(!state.gps_set_time)
+    // Handle GPS turn on/off
+    if(state.settings.gps_enabled != gpsEnabled)
     {
-        isRtcSyncronised = false;
+        gpsEnabled = state.settings.gps_enabled;
+
+        if(gpsEnabled)
+            gps_enable();
+        else
+            gps_disable();
     }
 
-    if (!minmea_talker_id(nmea_id, line))
+    // GPS disabled, nothing to do
+    if(gpsEnabled == false) return;
+
+    if(readNewSentence)
+    {
+        // Acquire a new NMEA sentence from GPS
+        int status = gps_getNmeaSentence(sentence, MINMEA_MAX_LENGTH + 1);
+        if(status != 0) return;
+        readNewSentence = false;
+    }
+
+    if(gps_nmeaSentenceReady() == false)
         return;
 
-    // Discard BeiDou sentences as we currently don't support it
+    // Parse the sentence and request a new one
-    if (!strncmp(nmea_id, "BD", 3))
+    readNewSentence = true;
-        return;
+    switch(minmea_sentence_id(sentence, false))
-
-    switch (minmea_sentence_id(line, false))
     {
         case MINMEA_SENTENCE_RMC:
         {
             struct minmea_sentence_rmc frame;
-            if (minmea_parse_rmc(&frame, line))
+            if (minmea_parse_rmc(&frame, sentence))
             {
                 state.gps_data.latitude = minmea_tocoord(&frame.latitude);
                 state.gps_data.longitude = minmea_tocoord(&frame.longitude);
@@ -76,24 +89,26 @@ void gps_taskFunc(char *line)
 
             state.gps_data.tmg_true = minmea_tofloat(&frame.course);
             state.gps_data.speed = minmea_tofloat(&frame.speed) * KNOTS2KMH;
-        } break;
+        }
+        break;
 
         case MINMEA_SENTENCE_GGA:
         {
             struct minmea_sentence_gga frame;
-            if (minmea_parse_gga(&frame, line))
+            if (minmea_parse_gga(&frame, sentence))
             {
                 state.gps_data.fix_quality = frame.fix_quality;
                 state.gps_data.satellites_tracked = frame.satellites_tracked;
                 state.gps_data.altitude = minmea_tofloat(&frame.altitude);
             }
-        } break;
+        }
+        break;
 
         case MINMEA_SENTENCE_GSA:
         {
             state.gps_data.active_sats = 0;
             struct minmea_sentence_gsa frame;
-            if (minmea_parse_gsa(&frame, line))
+            if (minmea_parse_gsa(&frame, sentence))
             {
                 state.gps_data.fix_type = frame.fix_type;
                 for (int i = 0; i < 12; i++)
@@ -104,13 +119,14 @@ void gps_taskFunc(char *line)
                     }
                 }
             }
-        } break;
+        }
+        break;
 
         case MINMEA_SENTENCE_GSV:
         {
             // Parse only sentences 1 - 3, maximum 12 satellites
             struct minmea_sentence_gsv frame;
-            if (minmea_parse_gsv(&frame, line) && (frame.msg_nr < 3))
+            if (minmea_parse_gsv(&frame, sentence) && (frame.msg_nr < 3))
             {
                 // When the first sentence arrives, clear all the old data
                 if (frame.msg_nr == 1)
@@ -128,18 +144,20 @@ void gps_taskFunc(char *line)
                     state.gps_data.satellites[index].snr = frame.sats[i].snr;
                 }
             }
-        } break;
+        }
+        break;
 
         case MINMEA_SENTENCE_VTG:
         {
             struct minmea_sentence_vtg frame;
-            if (minmea_parse_vtg(&frame, line))
+            if (minmea_parse_vtg(&frame, sentence))
             {
                 state.gps_data.speed = minmea_tofloat(&frame.speed_kph);
                 state.gps_data.tmg_mag = minmea_tofloat(&frame.magnetic_track_degrees);
                 state.gps_data.tmg_true = minmea_tofloat(&frame.true_track_degrees);
             }
-        } break;
+        }
+        break;
 
         // Ignore this message as we take data from RMC
         case MINMEA_SENTENCE_GLL: break;

openrtx/src/core/threads.c

@@ -226,36 +226,17 @@ void *gps_task(void *arg)
 {
     (void) arg;
 
-    char line[MINMEA_MAX_LENGTH*10];
-
     if (!gps_detect(5000)) return NULL;
 
     gps_init(9600);
-    // Lock mutex to read internal state
-    pthread_mutex_lock(&state_mutex);
-    bool enabled = state.settings.gps_enabled;
-    pthread_mutex_unlock(&state_mutex);
-    if(enabled)
-        gps_enable();
-    else
-        gps_disable();
 
     while(1)
     {
-        int status = gps_getNmeaSentence(line, MINMEA_MAX_LENGTH*10);
+        pthread_mutex_lock(&state_mutex);
-        gps_waitForNmeaSentence();
+        gps_taskFunc();
+        pthread_mutex_unlock(&state_mutex);
 
-        if(status != -1)
+        sleepFor(0u, 100u);
-        {
-            // Lock mutex and update internal state
-            pthread_mutex_lock(&state_mutex);
-
-            // GPS readout is blocking, no need to delay here
-            gps_taskFunc(line);
-
-            // Unlock state mutex
-            pthread_mutex_unlock(&state_mutex);
-        }
     }
 }
 #endif

openrtx/src/ui/ui.c

@@ -1555,17 +1555,10 @@ void ui_updateFSM(event_t event, bool *sync_rtx)
                     switch(ui_state.menu_selected)
                     {
                         case G_ENABLED:
-                            // Disable or Enable GPS to stop or start GPS thread
                             if(state.settings.gps_enabled)
-                            {
                                 state.settings.gps_enabled = 0;
-                                gps_disable();
-                            }
                             else
-                            {
                                 state.settings.gps_enabled = 1;
-                                gps_enable();
-                            }
                             break;
                         case G_SET_TIME:
                             state.gps_set_time = !state.gps_set_time;

platform/drivers/GPS/GPS_linux.c

@@ -20,12 +20,15 @@
 
 #include <interfaces/gps.h>
 #include <interfaces/delays.h>
+#include <sys/time.h>
 #include <hwconfig.h>
 #include <string.h>
 
 #define MAX_NMEA_LEN 80
 #define NMEA_SAMPLES 8
 
+static long long startTime;
+
 char test_nmea_sentences [NMEA_SAMPLES][MAX_NMEA_LEN] =
 {
     "$GPGGA,223659.522,5333.735,N,00959.130,E,1,12,1.0,0.0,M,0.0,M,,*62",
@@ -71,18 +74,34 @@ int gps_getNmeaSentence(char *buf, const size_t maxLength)
     // Emulate GPS device by sending NMEA sentences every 1s
     if(i == 0)
         sleepFor(1u, 0u);
+
     size_t len = strnlen(test_nmea_sentences[i], MAX_NMEA_LEN);
+
     if (len > maxLength)
         return -1;
+
     strncpy(buf, test_nmea_sentences[i], maxLength);
     i++;
     i %= NMEA_SAMPLES;
-    return len;
+
+    // Save the current timestamp for sentence ready emulation
+    struct timeval te;
+    gettimeofday(&te, NULL);
+    startTime = te.tv_sec*1000LL + te.tv_usec/1000;
+
+    return 0;
 }
 
 bool gps_nmeaSentenceReady()
 {
-    return true;
+    // Return new sentence ready only after 1s from start
+    struct timeval te;
+    gettimeofday(&te, NULL);
+    long long currTime = te.tv_sec*1000LL + te.tv_usec/1000;
+
+    if((currTime -  startTime) > 1000) return true;
+
+    return false;
 }
 
 void gps_waitForNmeaSentence()