From 52ead401bdd7d2b1ad1c99828381bedc9ebe8c2c Mon Sep 17 00:00:00 2001 From: Silvano Seva Date: Thu, 16 Jun 2022 16:39:50 +0200 Subject: [PATCH] Reorganised GPS task to make it a non-blocking task to be called periodically --- openrtx/include/core/gps.h | 2 +- openrtx/src/core/gps.c | 70 ++++++++++++++++++++------------ openrtx/src/core/threads.c | 27 ++---------- openrtx/src/ui/ui.c | 7 ---- platform/drivers/GPS/GPS_linux.c | 23 ++++++++++- 5 files changed, 70 insertions(+), 59 deletions(-) diff --git a/openrtx/include/core/gps.h b/openrtx/include/core/gps.h index d8ab8c26..c6a975fb 100644 --- a/openrtx/include/core/gps.h +++ b/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 */ diff --git a/openrtx/src/core/gps.c b/openrtx/src/core/gps.c index 56bd6519..b8a5cd18 100644 --- a/openrtx/src/core/gps.c +++ b/openrtx/src/core/gps.c @@ -27,33 +27,46 @@ #define KNOTS2KMH 1.852f -/** - * This function parses a GPS NMEA sentence and updates radio state - */ -void gps_taskFunc(char *line) -{ - char nmea_id[3] = { 0 }; +static char sentence[MINMEA_MAX_LENGTH + 1]; +static bool isRtcSyncronised = false; +static bool gpsEnabled = false; +static bool readNewSentence = true; - // Little mechanism to ensure that RTC is synced with GPS time only once. - static bool isRtcSyncronised = false; - if(!state.gps_set_time) +void gps_taskFunc() +{ + // 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 - if (!strncmp(nmea_id, "BD", 3)) - return; - - switch (minmea_sentence_id(line, false)) + // Parse the sentence and request a new one + readNewSentence = true; + switch(minmea_sentence_id(sentence, 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; diff --git a/openrtx/src/core/threads.c b/openrtx/src/core/threads.c index 937237fe..eaba85ff 100644 --- a/openrtx/src/core/threads.c +++ b/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); - gps_waitForNmeaSentence(); + pthread_mutex_lock(&state_mutex); + gps_taskFunc(); + pthread_mutex_unlock(&state_mutex); - if(status != -1) - { - // 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); - } + sleepFor(0u, 100u); } } #endif diff --git a/openrtx/src/ui/ui.c b/openrtx/src/ui/ui.c index f50b3ffa..0f38ace8 100644 --- a/openrtx/src/ui/ui.c +++ b/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; diff --git a/platform/drivers/GPS/GPS_linux.c b/platform/drivers/GPS/GPS_linux.c index ca895fda..2d5b22f3 100644 --- a/platform/drivers/GPS/GPS_linux.c +++ b/platform/drivers/GPS/GPS_linux.c @@ -20,12 +20,15 @@ #include #include +#include #include #include #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()