kopia lustrzana https://github.com/OpenRTX/OpenRTX
214 wiersze
7.1 KiB
C
214 wiersze
7.1 KiB
C
/***************************************************************************
|
|
* Copyright (C) 2020 - 2023 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/platform.h>
|
|
#include <peripherals/gps.h>
|
|
#include <gps.h>
|
|
#include <minmea.h>
|
|
#include <stdio.h>
|
|
#include <state.h>
|
|
#include <string.h>
|
|
#include <stdbool.h>
|
|
|
|
#define KNOTS2KMH(x) ((((int) x) * 1852) / 1000)
|
|
|
|
static char sentence[2*MINMEA_MAX_LENGTH];
|
|
static bool gpsEnabled = false;
|
|
static bool readNewSentence = true;
|
|
#ifdef CONFIG_RTC
|
|
static bool isRtcSyncronised = false;
|
|
#endif
|
|
|
|
void gps_task()
|
|
{
|
|
// No GPS, return
|
|
if(state.gpsDetected == false)
|
|
return;
|
|
|
|
// Handle GPS turn on/off
|
|
if(state.settings.gps_enabled != gpsEnabled)
|
|
{
|
|
gpsEnabled = state.settings.gps_enabled;
|
|
|
|
if(gpsEnabled)
|
|
gps_enable();
|
|
else
|
|
gps_disable();
|
|
}
|
|
|
|
// GPS disabled, nothing to do
|
|
if(gpsEnabled == false)
|
|
return;
|
|
|
|
// Acquire a new NMEA sentence from GPS
|
|
if(readNewSentence)
|
|
{
|
|
int status = gps_getNmeaSentence(sentence, 2*MINMEA_MAX_LENGTH);
|
|
if(status != 0) return;
|
|
readNewSentence = false;
|
|
}
|
|
|
|
// Waiting for a sentence...
|
|
if(gps_nmeaSentenceReady() == false)
|
|
return;
|
|
|
|
// Discard all non-GPS sentences
|
|
if((sentence[0] != '$') || (sentence[1] != 'G'))
|
|
{
|
|
readNewSentence = true;
|
|
return;
|
|
}
|
|
|
|
// Parse the sentence. Work on a local state copy to minimize the time
|
|
// spent with the state mutex locked
|
|
gps_t gps_data;
|
|
pthread_mutex_lock(&state_mutex);
|
|
gps_data = state.gps_data;
|
|
pthread_mutex_unlock(&state_mutex);
|
|
|
|
int32_t sId = minmea_sentence_id(sentence, false);
|
|
switch(sId)
|
|
{
|
|
case MINMEA_SENTENCE_RMC:
|
|
{
|
|
struct minmea_sentence_rmc frame;
|
|
if (minmea_parse_rmc(&frame, sentence))
|
|
{
|
|
gps_data.latitude = minmea_tofixedpoint(&frame.latitude);
|
|
gps_data.longitude = minmea_tofixedpoint(&frame.longitude);
|
|
gps_data.timestamp.hour = frame.time.hours;
|
|
gps_data.timestamp.minute = frame.time.minutes;
|
|
gps_data.timestamp.second = frame.time.seconds;
|
|
gps_data.timestamp.day = 0;
|
|
gps_data.timestamp.date = frame.date.day;
|
|
gps_data.timestamp.month = frame.date.month;
|
|
gps_data.timestamp.year = frame.date.year;
|
|
}
|
|
|
|
gps_data.tmg_true = minmea_tofloat(&frame.course);
|
|
gps_data.speed = KNOTS2KMH(minmea_toint(&frame.speed));
|
|
}
|
|
break;
|
|
|
|
case MINMEA_SENTENCE_GGA:
|
|
{
|
|
struct minmea_sentence_gga frame;
|
|
if (minmea_parse_gga(&frame, sentence))
|
|
{
|
|
gps_data.fix_quality = frame.fix_quality;
|
|
gps_data.satellites_tracked = frame.satellites_tracked;
|
|
gps_data.altitude = minmea_toint(&frame.altitude);
|
|
}
|
|
}
|
|
break;
|
|
|
|
case MINMEA_SENTENCE_GSA:
|
|
{
|
|
gps_data.active_sats = 0;
|
|
struct minmea_sentence_gsa frame;
|
|
if (minmea_parse_gsa(&frame, sentence))
|
|
{
|
|
gps_data.fix_type = frame.fix_type;
|
|
for (int i = 0; i < 12; i++)
|
|
{
|
|
if (frame.sats[i] != 0)
|
|
{
|
|
gps_data.active_sats |= 1 << (frame.sats[i] - 1);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
break;
|
|
|
|
case MINMEA_SENTENCE_GSV:
|
|
{
|
|
// Parse only sentences 1 - 3, maximum 12 satellites
|
|
struct minmea_sentence_gsv frame;
|
|
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)
|
|
{
|
|
memset(&gps_data.satellites[0], 0x00, 12 * sizeof(gpssat_t));
|
|
}
|
|
|
|
gps_data.satellites_in_view = frame.total_sats;
|
|
for (int i = 0; i < 4; i++)
|
|
{
|
|
int index = 4 * (frame.msg_nr - 1) + i;
|
|
gps_data.satellites[index].id = frame.sats[i].nr;
|
|
gps_data.satellites[index].elevation = frame.sats[i].elevation;
|
|
gps_data.satellites[index].azimuth = frame.sats[i].azimuth;
|
|
gps_data.satellites[index].snr = frame.sats[i].snr;
|
|
}
|
|
}
|
|
}
|
|
break;
|
|
|
|
case MINMEA_SENTENCE_VTG:
|
|
{
|
|
struct minmea_sentence_vtg frame;
|
|
if (minmea_parse_vtg(&frame, sentence))
|
|
{
|
|
gps_data.speed = minmea_toint(&frame.speed_kph);
|
|
gps_data.tmg_mag = minmea_tofloat(&frame.magnetic_track_degrees);
|
|
gps_data.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;
|
|
}
|
|
|
|
// Update GPS data inside radio state
|
|
pthread_mutex_lock(&state_mutex);
|
|
state.gps_data = gps_data;
|
|
pthread_mutex_unlock(&state_mutex);
|
|
|
|
// Synchronize RTC with GPS UTC clock, only when fix is done
|
|
#ifdef CONFIG_RTC
|
|
if(state.gps_set_time)
|
|
{
|
|
if((sId == MINMEA_SENTENCE_RMC) &&
|
|
(gps_data.fix_quality > 0) &&
|
|
(isRtcSyncronised == false))
|
|
{
|
|
platform_setTime(gps_data.timestamp);
|
|
isRtcSyncronised = true;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
isRtcSyncronised = false;
|
|
}
|
|
#endif
|
|
|
|
// Finally, trigger the acquisition of a new NMEA sentence
|
|
readNewSentence = true;
|
|
}
|