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; } }