From 45d919f50f9f5a0e8be96bfe1ee17c19169fcc1a Mon Sep 17 00:00:00 2001 From: Silvano Seva Date: Tue, 14 Jun 2022 19:20:42 +0200 Subject: [PATCH] Reorganised GPS driver to allow for non-blocking acquisition of NMEA sentences --- openrtx/include/core/gps.h | 2 +- openrtx/include/interfaces/gps.h | 19 ++++++++- openrtx/src/core/gps.c | 4 +- openrtx/src/core/threads.c | 8 ++-- platform/drivers/GPS/GPS_MDx.cpp | 69 +++++++++++++++++--------------- platform/drivers/GPS/GPS_linux.c | 27 +++++++++---- 6 files changed, 81 insertions(+), 48 deletions(-) diff --git a/openrtx/include/core/gps.h b/openrtx/include/core/gps.h index c9b009e5..232a4c46 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, int len); +void gps_taskFunc(char *line); #endif /* GPS_H */ diff --git a/openrtx/include/interfaces/gps.h b/openrtx/include/interfaces/gps.h index 2365e378..d98534b4 100644 --- a/openrtx/include/interfaces/gps.h +++ b/openrtx/include/interfaces/gps.h @@ -36,6 +36,7 @@ extern "C" { /** * Initialise the GPS driver. * This function does not turn on the GPS module. + * * @param baud: baud rate of GPS serial interface. */ void gps_init(const uint16_t baud); @@ -58,19 +59,35 @@ void gps_disable(); /** * Detect if a GPS module is present in the system, it can be called also * when driver is not initialised. + * * @param timeout: timeout for GPS detection, in milliseconds. * @return true if a GPS module is present, false otherwise. */ bool gps_detect(uint16_t timeout); /** - * Read a NMEA sentence from the GPS module, blocking function. + * Start the acquisition of a new NMEA sentence from the GPS module. + * The function returns immediately and acquisition is stopped when a complete + * sentence is received or maximum buffer length is reached. + * * @param buf: buffer to which the NMEA sentence is written. * @param maxLength: maximum writable length inside the buffer. * @return number of characters written in the buffer or -1 on error. */ int gps_getNmeaSentence(char *buf, const size_t maxLength); +/** + * Check if a newly acquired NMEA sentence is available. + * + * @return true if a NMEA sentence has been read from the GPS. + */ +bool gps_nmeaSentenceReady(); + +/** + * Wait until a new NMEA sentence is ready, blocking the execution flow. + */ +void gps_waitForNmeaSentence(); + #ifdef __cplusplus } #endif diff --git a/openrtx/src/core/gps.c b/openrtx/src/core/gps.c index 538c7730..56bd6519 100644 --- a/openrtx/src/core/gps.c +++ b/openrtx/src/core/gps.c @@ -30,10 +30,8 @@ /** * This function parses a GPS NMEA sentence and updates radio state */ -void gps_taskFunc(char *line, int len) +void gps_taskFunc(char *line) { - (void) len; - char nmea_id[3] = { 0 }; // Little mechanism to ensure that RTC is synced with GPS time only once. diff --git a/openrtx/src/core/threads.c b/openrtx/src/core/threads.c index e8ea9e3f..937237fe 100644 --- a/openrtx/src/core/threads.c +++ b/openrtx/src/core/threads.c @@ -242,14 +242,16 @@ void *gps_task(void *arg) while(1) { - int len = gps_getNmeaSentence(line, MINMEA_MAX_LENGTH*10); - if(len != -1) + int status = gps_getNmeaSentence(line, MINMEA_MAX_LENGTH*10); + gps_waitForNmeaSentence(); + + 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, len); + gps_taskFunc(line); // Unlock state mutex pthread_mutex_unlock(&state_mutex); diff --git a/platform/drivers/GPS/GPS_MDx.cpp b/platform/drivers/GPS/GPS_MDx.cpp index 8a143226..ed499edf 100644 --- a/platform/drivers/GPS/GPS_MDx.cpp +++ b/platform/drivers/GPS/GPS_MDx.cpp @@ -26,15 +26,14 @@ #include #include -int8_t detectStatus = -1; -size_t bufPos = 0; -size_t maxPos = 0; -char *dataBuf; -bool receiving = false; -uint8_t status = 0; +static int8_t detectStatus = -1; +static size_t bufPos = 0; +static size_t maxPos = 0; +static char *dataBuf; +static bool receiving = false; using namespace miosix; -Thread *gpsWaiting = 0; +static Thread *gpsWaiting = 0; #ifdef PLATFORM_MD3x0 #define PORT USART3 @@ -74,7 +73,8 @@ void __attribute__((used)) GpsUsartImpl() if((receiving == false) && (bufPos != 0)) { - status = (bufPos < maxPos) ? 0x01 : 0x02; + // NMEA sentence received, turn off serial port + PORT->CR1 &= ~USART_CR1_UE; if(gpsWaiting) { @@ -127,10 +127,8 @@ void gps_init(const uint16_t baud) | USART_CR1_RXNEIE; #ifdef PLATFORM_MD3x0 - NVIC_ClearPendingIRQ(USART3_IRQn); NVIC_SetPriority(USART3_IRQn, 14); #else - NVIC_ClearPendingIRQ(USART1_IRQn); NVIC_SetPriority(USART1_IRQn, 14); #endif } @@ -149,7 +147,15 @@ void gps_terminate() void gps_enable() { gpio_setPin(GPS_EN); - PORT->CR1 |= USART_CR1_UE; + + // Enable IRQ + #ifdef PLATFORM_MD3x0 + NVIC_ClearPendingIRQ(USART3_IRQn); + NVIC_EnableIRQ(USART3_IRQn); + #else + NVIC_ClearPendingIRQ(USART1_IRQn); + NVIC_EnableIRQ(USART1_IRQn); + #endif } void gps_disable() @@ -162,6 +168,9 @@ void gps_disable() #else NVIC_DisableIRQ(USART1_IRQn); #endif + + receiving = false; + bufPos = 0; } bool gps_detect(uint16_t timeout) @@ -199,16 +208,23 @@ int gps_getNmeaSentence(char *buf, const size_t maxLength) if(detectStatus != 1) return -1; memset(buf, 0x00, maxLength); - bufPos = 0; - maxPos = maxLength; + bufPos = 0; + maxPos = maxLength; dataBuf = buf; - #ifdef PLATFORM_MD3x0 - NVIC_EnableIRQ(USART3_IRQn); - #else - NVIC_EnableIRQ(USART1_IRQn); - #endif + // Enable serial port + PORT->CR1 |= USART_CR1_UE; + return 0; +} + +bool gps_nmeaSentenceReady() +{ + return (receiving == false) && (bufPos > 0); +} + +void gps_waitForNmeaSentence() +{ /* * Put the calling thread in waiting status until a complete sentence is ready. */ @@ -222,20 +238,7 @@ int gps_getNmeaSentence(char *buf, const size_t maxLength) FastInterruptEnableLock eLock(dLock); Thread::yield(); } - } while(gpsWaiting); + } + while(gpsWaiting); } - - #ifdef PLATFORM_MD3x0 - NVIC_DisableIRQ(USART3_IRQn); - #else - NVIC_DisableIRQ(USART1_IRQn); - #endif - - if(status & 0x01) - { - return bufPos; - } - - return -1; } - diff --git a/platform/drivers/GPS/GPS_linux.c b/platform/drivers/GPS/GPS_linux.c index 0bb17d4d..ca895fda 100644 --- a/platform/drivers/GPS/GPS_linux.c +++ b/platform/drivers/GPS/GPS_linux.c @@ -26,7 +26,8 @@ #define MAX_NMEA_LEN 80 #define NMEA_SAMPLES 8 -char test_nmea_sentences [NMEA_SAMPLES][MAX_NMEA_LEN] = { +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", "$GPGSA,A,3,01,02,03,04,05,06,07,08,09,10,11,12,1.0,1.0,1.0*30", "$GPGSV,3,1,12,30,79,066,27,05,63,275,21,07,42,056,,13,40,289,13*76", @@ -36,28 +37,30 @@ char test_nmea_sentences [NMEA_SAMPLES][MAX_NMEA_LEN] = { "$GPVTG,92.15,T,,M,0.15,N,0.28,K,A*0C" }; -void gps_init(__attribute__((unused)) const uint16_t baud) +void gps_init(const uint16_t baud) { - ; + (void) baud; + return; } void gps_terminate() { - ; + return; } void gps_enable() { - ; + return; } void gps_disable() { - ; + return; } -bool gps_detect(__attribute__((unused)) uint16_t timeout) +bool gps_detect(uint16_t timeout) { + (void) timeout; return true; } @@ -77,3 +80,13 @@ int gps_getNmeaSentence(char *buf, const size_t maxLength) return len; } +bool gps_nmeaSentenceReady() +{ + return true; +} + +void gps_waitForNmeaSentence() +{ + return; +} +