From 0ce92f991c47d6e572edd783a41de16e8930820e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Niccol=C3=B2=20Izzo?= Date: Mon, 8 Feb 2021 17:25:11 +0100 Subject: [PATCH] Add GPS NMEA parsing task Add dedicated task to parse NMEA sentences received by the GPS module. The parsed data is moved to the radio state, to be read by the user interface. --- meson.build | 3 +- .../GPS.h => openrtx/include/interfaces/gps.h | 1 + openrtx/include/state.h | 26 +++-- openrtx/include/threads.h | 5 + openrtx/src/gps.c | 102 ++++++++++++++++++ openrtx/src/threads.c | 85 +++++++++++++++ platform/drivers/GPS/GPS_MDx.c | 2 +- tests/platform/gps_test_MDx.c | 2 - 8 files changed, 211 insertions(+), 15 deletions(-) rename platform/drivers/GPS/GPS.h => openrtx/include/interfaces/gps.h (99%) create mode 100644 openrtx/src/gps.c diff --git a/meson.build b/meson.build index c2a74837..33f73b1c 100644 --- a/meson.build +++ b/meson.build @@ -21,7 +21,8 @@ openrtx_src = ['openrtx/src/bootstrap.c', 'openrtx/src/graphics.c', 'openrtx/src/input.c', 'openrtx/src/calibUtils.c', - 'openrtx/src/rtx.c'] + 'openrtx/src/rtx.c', + 'openrtx/src/gps.c'] ## Replace main executable with platform test diff --git a/platform/drivers/GPS/GPS.h b/openrtx/include/interfaces/gps.h similarity index 99% rename from platform/drivers/GPS/GPS.h rename to openrtx/include/interfaces/gps.h index 3b7eba98..d6c486f9 100644 --- a/platform/drivers/GPS/GPS.h +++ b/openrtx/include/interfaces/gps.h @@ -23,6 +23,7 @@ #include #include +#include /** * Low-level driver for interfacing with radio's on-board GPS module. diff --git a/openrtx/include/state.h b/openrtx/include/state.h index 233d986b..54a8793a 100644 --- a/openrtx/include/state.h +++ b/openrtx/include/state.h @@ -31,10 +31,10 @@ */ typedef struct { - uint8_t id; - uint8_t elevation; - uint16_t azimuth; - uint8_t snr; + uint8_t id; // ID of the satellite + uint8_t elevation; // Elevation in degrees + uint16_t azimuth; // Azimuth in degrees + uint8_t snr; // Quality of the signal in range 0-99 } sat_t; @@ -43,13 +43,17 @@ sat_t; */ typedef struct { - uint8_t fix_quality; - curTime_t timestamp; - sat_t satellites[12]; - float latitude; - float longitude; - float speed; - float tmg; + curTime_t timestamp; // Timestamp of the latest GPS update + uint8_t fix_quality; // 0: no fix, 1: GPS, 2: GPS SPS, 3: GPS PPS + uint8_t fix_type; // 0: no fix, 1: 2D, 2: 3D + uint8_t satellites_tracked; // Number of tracked satellites + uint8_t satellites_in_view; // Satellites in view + sat_t satellites[12]; // Details about satellites in view + float latitude; // Latitude coordinates + float longitude; // Longitude coordinates + float speed; // Ground speed in km/h + float tmg_mag; // Course over ground, degrees, magnetic + float tmg_true; // Course over ground, degrees, true } gps_t; diff --git a/openrtx/include/threads.h b/openrtx/include/threads.h index 997bdba9..ca01029a 100644 --- a/openrtx/include/threads.h +++ b/openrtx/include/threads.h @@ -50,6 +50,11 @@ void create_threads(); */ #define RTX_TASK_STKSIZE 512 +/** + * Stack size for GPS task, in bytes. + */ +#define GPS_TASK_STKSIZE 4096 + #else /* __arm__ */ #define UI_TASK_STKSIZE 4096 diff --git a/openrtx/src/gps.c b/openrtx/src/gps.c new file mode 100644 index 00000000..8cd15c58 --- /dev/null +++ b/openrtx/src/gps.c @@ -0,0 +1,102 @@ +/*************************************************************************** + * Copyright (C) 2020 by Federico Amedeo Izzo IU2NUO, * + * Niccolò Izzo IU2KIN, * + * Silvano Seva IU2KWO * + * * + * This program is free software; you can redistribute it and/or modify * + * it under the terms of the GNU General Public License as published by * + * the Free Software Foundation; either version 3 of the License, or * + * (at your option) any later version. * + * * + * This program is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU General Public License for more details. * + * * + * You should have received a copy of the GNU General Public License * + * along with this program; if not, see * + ***************************************************************************/ + +#include +#include +#include +#include + +/** + * This function parses a GPS NMEA sentence and updates radio state + */ +void gps_taskFunc(char *line, int len, gps_t *state) +{ + switch (minmea_sentence_id(line, false)) { + case MINMEA_SENTENCE_RMC: + { + struct minmea_sentence_rmc frame; + if (minmea_parse_rmc(&frame, line)) { + state->latitude = minmea_tocoord(&frame.latitude); + state->longitude = minmea_tocoord(&frame.longitude); + state->speed = minmea_tofloat(&frame.speed); + state->timestamp.hour = frame.time.hours; + state->timestamp.minute = frame.time.minutes; + state->timestamp.second = frame.time.seconds; + state->timestamp.day = 0; + state->timestamp.date = frame.date.day; + state->timestamp.month = frame.date.month; + state->timestamp.year = frame.date.year; + } + } break; + + case MINMEA_SENTENCE_GGA: + { + struct minmea_sentence_gga frame; + if (minmea_parse_gga(&frame, line)) { + state->fix_quality = frame.fix_quality; + state->satellites_tracked = frame.satellites_tracked; + } + } break; + + case MINMEA_SENTENCE_GSA: + { + struct minmea_sentence_gsa frame; + if (minmea_parse_gsa(&frame, line)) { + state->fix_type = frame.fix_type; + } + } break; + + case MINMEA_SENTENCE_GSV: + { + struct minmea_sentence_gsv frame; + if (minmea_parse_gsv(&frame, line)) { + state->satellites_in_view = frame.total_sats; + for (int i = 0; i < 4; i++) { + int index = 4 * (frame.msg_nr - 1) + i; + state->satellites[index].id = frame.sats[i].nr; + state->satellites[index].elevation = frame.sats[i].elevation; + state->satellites[index].azimuth = frame.sats[i].azimuth; + state->satellites[index].snr = frame.sats[i].snr; + } + } + } break; + + case MINMEA_SENTENCE_VTG: + { + struct minmea_sentence_vtg frame; + if (minmea_parse_vtg(&frame, line)) { + state->speed = minmea_tofloat(&frame.speed_kph); + state->tmg_mag = minmea_tofloat(&frame.magnetic_track_degrees); + state->tmg_true = minmea_tofloat(&frame.true_track_degrees); + } + + } break; + + // Ignore this message as we take data from RMC + case MINMEA_SENTENCE_GLL: break; + + // These messages are never sent by the Jumpstar JS-M710 Module + case MINMEA_SENTENCE_GST: break; + case MINMEA_SENTENCE_ZDA: break; + + // Error handling + case MINMEA_INVALID: break; + case MINMEA_UNKNOWN: break; + } +} diff --git a/openrtx/src/threads.c b/openrtx/src/threads.c index cdb8d241..3fcb5b4f 100644 --- a/openrtx/src/threads.c +++ b/openrtx/src/threads.c @@ -26,9 +26,11 @@ #include #include #include +#include #include #include #include +#include /* Mutex for concurrent access to state variable */ static OS_MUTEX state_mutex; @@ -71,6 +73,10 @@ static CPU_STK dev_stk[DEV_TASK_STKSIZE/sizeof(CPU_STK)]; static OS_TCB rtx_tcb; static CPU_STK rtx_stk[RTX_TASK_STKSIZE/sizeof(CPU_STK)]; +/* GPS task control block and stack */ +static OS_TCB gps_tcb; +static CPU_STK gps_stk[GPS_TASK_STKSIZE/sizeof(CPU_STK)]; + /** * \internal Task function in charge of updating the UI. */ @@ -283,6 +289,69 @@ static void rtx_task(void *arg) } } +/** + * \internal Task function for parsing GPS data and updating radio state. + */ +static void gps_task(void *arg) +{ + (void) arg; + OS_ERR os_err; + char line[MINMEA_MAX_LENGTH*10]; + + if (!gps_detect(5000)) + return; + + gps_init(9600); + gps_enable(); + + while(1) + { + int len = gps_getNmeaSentence(line, MINMEA_MAX_LENGTH*10); + if(len != -1) + { + // Lock mutex and update internal state + OSMutexPend(&state_mutex, 0u, OS_OPT_PEND_BLOCKING, 0u, &os_err); + + // GPS readout is blocking, no need to delay here + gps_taskFunc(line, len, &state.gps_data); + + // Unlock state mutex + OSMutexPost(&state_mutex, OS_OPT_POST_NONE, &os_err); + + // Debug prints + printf("Timestamp: %d:%d:%d %d/%d/%d\n\r", + state.gps_data.timestamp.hour, + state.gps_data.timestamp.minute, + state.gps_data.timestamp.second, + state.gps_data.timestamp.date, + state.gps_data.timestamp.month, + state.gps_data.timestamp.year); + printf("Fix quality: %d - %d\n\r", + state.gps_data.fix_quality, + state.gps_data.fix_type); + printf("Satellites tracked: %d/%d\n\r", + state.gps_data.satellites_tracked, + state.gps_data.satellites_in_view); + for(int i = 0; i < state.gps_data.satellites_in_view; i++) + { + printf("%d - elevation: %d azimuth: %d snr: %d\n\r", + state.gps_data.satellites[i].id, + state.gps_data.satellites[i].elevation, + state.gps_data.satellites[i].azimuth, + state.gps_data.satellites[i].snr); + } + printf("Coordinates: %f %f\n\r", + state.gps_data.latitude, + state.gps_data.longitude); + printf("Speed: %f km/h TMGM: %f deg TMGT: %f deg\n\r", + state.gps_data.speed, + state.gps_data.tmg_mag, + state.gps_data.tmg_true); + printf("\n\r\n\r"); + } + } +} + /** * \internal This function creates all the system tasks and mutexes. */ @@ -359,6 +428,21 @@ void create_threads() (OS_OPT ) (OS_OPT_TASK_STK_CHK | OS_OPT_TASK_STK_CLR), (OS_ERR *) &os_err); + // Create GPS thread + OSTaskCreate((OS_TCB *) &gps_tcb, + (CPU_CHAR *) "GPS Task", + (OS_TASK_PTR ) gps_task, + (void *) 0, + (OS_PRIO ) 25, + (CPU_STK *) &gps_stk[0], + (CPU_STK ) 0, + (CPU_STK_SIZE) GPS_TASK_STKSIZE/sizeof(CPU_STK), + (OS_MSG_QTY ) 0, + (OS_TICK ) 0, + (void *) 0, + (OS_OPT ) (OS_OPT_TASK_STK_CHK | OS_OPT_TASK_STK_CLR), + (OS_ERR *) &os_err); + // Create state thread OSTaskCreate((OS_TCB *) &dev_tcb, (CPU_CHAR *) "Device Task", @@ -373,4 +457,5 @@ void create_threads() (void *) 0, (OS_OPT ) (OS_OPT_TASK_STK_CHK | OS_OPT_TASK_STK_CLR), (OS_ERR *) &os_err); + } diff --git a/platform/drivers/GPS/GPS_MDx.c b/platform/drivers/GPS/GPS_MDx.c index 99f49a83..f1522bab 100644 --- a/platform/drivers/GPS/GPS_MDx.c +++ b/platform/drivers/GPS/GPS_MDx.c @@ -20,10 +20,10 @@ #include #include +#include #include #include #include -#include "GPS.h" static int8_t detectStatus = -1; size_t bufPos = 0; diff --git a/tests/platform/gps_test_MDx.c b/tests/platform/gps_test_MDx.c index d77d692a..5d56a21b 100644 --- a/tests/platform/gps_test_MDx.c +++ b/tests/platform/gps_test_MDx.c @@ -40,7 +40,6 @@ int main() gps_init(9600); gps_enable(); - int i = 0; while(1) { int len = gps_getNmeaSentence(line, MINMEA_MAX_LENGTH*10); @@ -117,7 +116,6 @@ int main() printf("Error: Unsupported NMEA sentence!\n\r"); } break; } - i = 0; } }