kopia lustrzana https://github.com/OpenRTX/OpenRTX
Reorganised GPS task to make it a non-blocking task to be called periodically
rodzic
d16eb04696
commit
52ead401bd
|
@ -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 */
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -20,12 +20,15 @@
|
|||
|
||||
#include <interfaces/gps.h>
|
||||
#include <interfaces/delays.h>
|
||||
#include <sys/time.h>
|
||||
#include <hwconfig.h>
|
||||
#include <string.h>
|
||||
|
||||
#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()
|
||||
|
|
Ładowanie…
Reference in New Issue