mirror
/
OpenRTX
kopia lustrzana https://github.com/OpenRTX/OpenRTX
1
0
Forkuj 0
Kod Zgłoszenia Packages Projekty Wydania Wiki Aktywność

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.
replace/e1dc4454c9ae75fa36441db125bf4fe5412e2a23
Niccolò Izzo 2021-02-08 17:25:11 +01:00
rodzic 2e0ac1b1f6
commit 0ce92f991c
8 zmienionych plików z 211 dodań i 15 usunięć
Pokaż statystyki Ściągnij plik aktualizacji Ściągnij plik porównania Expand all files Collapse all files

3
meson.build
Wyświetl plik

@ -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

1
platform/drivers/GPS/GPS.h → openrtx/include/interfaces/gps.h
Wyświetl plik

@ -23,6 +23,7 @@
#include <stdbool.h>
#include <stdint.h>
#include <stddef.h>
/**
* Low-level driver for interfacing with radio's on-board GPS module.

26
openrtx/include/state.h
Wyświetl plik

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

5
openrtx/include/threads.h
Wyświetl plik

@ -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

102
openrtx/src/gps.c 100644
Wyświetl plik

@ -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 <http://www.gnu.org/licenses/> *
***************************************************************************/
#include <interfaces/gps.h>
#include <minmea.h>
#include <stdio.h>
#include <state.h>
/**
* 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;
}
}

85
openrtx/src/threads.c
Wyświetl plik

@ -26,9 +26,11 @@
#include <interfaces/keyboard.h>
#include <interfaces/graphics.h>
#include <interfaces/platform.h>
#include <interfaces/gps.h>
#include <hwconfig.h>
#include <event.h>
#include <rtx.h>
#include <minmea.h>
/* 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);
}

2
platform/drivers/GPS/GPS_MDx.c
Wyświetl plik

@ -20,10 +20,10 @@
#include <interfaces/delays.h>
#include <interfaces/gpio.h>
#include <interfaces/gps.h>
#include <hwconfig.h>
#include <string.h>
#include <os.h>
#include "GPS.h"
static int8_t detectStatus = -1;
size_t bufPos = 0;

2
tests/platform/gps_test_MDx.c
Wyświetl plik

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