/**
* Parses a PFLAU sentence
* (Operating status and priority intruder and obstacle data)
* @param String Input string
* @param params Parameter array
* @param nparams Number of parameters
* @param GPS_INFO GPS_INFO struct to parse into
* @return Parsing success
* @see http://flarm.com/support/manual/FLARM_DataportManual_v4.06E.pdf
*/
bool
NMEAParser::PFLAU(NMEAInputLine &line, FLARM_STATE &flarm)
{
static int old_flarm_rx = 0;
flarm.FLARM_Available = true;
isFlarm = true;
// PFLAU,<RX>,<TX>,<GPS>,<Power>,<AlarmLevel>,<RelativeBearing>,<AlarmType>,
// <RelativeVertical>,<RelativeDistance>(,<ID>)
flarm.FLARM_RX = line.read(0);
flarm.FLARM_TX = line.read(0);
flarm.FLARM_GPS = line.read(0);
line.skip();
flarm.FLARM_AlarmLevel = line.read(0);
// process flarm updates
if (flarm.FLARM_RX && old_flarm_rx == 0)
// traffic has appeared..
InputEvents::processGlideComputer(GCE_FLARM_TRAFFIC);
if (flarm.FLARM_RX == 0 && old_flarm_rx)
// traffic has disappeared..
InputEvents::processGlideComputer(GCE_FLARM_NOTRAFFIC);
// TODO feature: add another event for new traffic.
old_flarm_rx = flarm.FLARM_RX;
return false;
}
bool
VegaDevice::PDVSC(NMEAInputLine &line, gcc_unused NMEAInfo &info)
{
char responsetype[10];
line.read(responsetype, 10);
char name[80];
line.read(name, 80);
if (strcmp(name, "ERROR") == 0)
// ignore error responses...
return true;
int value = line.read(0);
if (strcmp(name, "ToneDeadbandCruiseLow") == 0)
value = std::max(value, -value);
if (strcmp(name, "ToneDeadbandCirclingLow") == 0)
value = std::max(value, -value);
settings_mutex.Lock();
settings[name] = value;
settings_mutex.Unlock();
return true;
}
bool
NMEAParser::GSA(NMEAInputLine &line, NMEAInfo &info)
{
/*
* $--GSA,a,a,x,x,x,x,x,x,x,x,x,x,x,x,x,x,x.x,x.x,x.x*hh
*
* Field Number:
* 1) Selection mode
* M=Manual, forced to operate in 2D or 3D
* A=Automatic, 3D/2D
* 2) Mode (1 = no fix, 2 = 2D fix, 3 = 3D fix)
* 3) ID of 1st satellite used for fix
* 4) ID of 2nd satellite used for fix
* ...
* 14) ID of 12th satellite used for fix
* 15) PDOP
* 16) HDOP
* 17) VDOP
* 18) checksum
*/
line.skip();
if (line.read(0) == 1)
info.location_available.Clear();
// satellites are in items 4-15 of GSA string (4-15 is 1-indexed)
for (unsigned i = 0; i < GPSState::MAXSATELLITES; i++)
info.gps.satellite_ids[i] = line.read(0);
info.gps.satellite_ids_available.Update(info.clock);
return true;
}
// $PDVDS,nx,nz,flap,stallratio,netto
static bool
PDVDS(NMEAInputLine &line, NMEAInfo &info)
{
fixed AccelX = line.read(fixed_zero);
fixed AccelZ = line.read(fixed_zero);
int mag = (int)hypot(AccelX, AccelZ);
info.acceleration.ProvideGLoad(fixed(mag) / 100, true);
/*
double flap = line.read(0.0);
*/
line.skip();
info.stall_ratio = line.read(fixed_zero);
info.stall_ratio_available.Update(info.clock);
fixed value;
if (line.read_checked(value))
info.ProvideNettoVario(value / 10);
//hasVega = true;
return true;
}
static bool
PDAAV(NMEAInputLine &line, gcc_unused NMEAInfo &info)
{
gcc_unused unsigned short beepfrequency = line.read(0);
gcc_unused unsigned short soundfrequency = line.read(0);
gcc_unused unsigned char soundtype = line.read(0);
// Temporarily commented out - function as yet undefined
// audio_setconfig(beepfrequency, soundfrequency, soundtype);
return true;
}
void PDVSC_S(NMEAInputLine &line) {
char name[64], value[256];
line.read(name, ARRAY_SIZE(name));
line.read(value, ARRAY_SIZE(value));
settings[name] = value;
ConsoleOperationEnvironment env;
char buffer[512];
snprintf(buffer, ARRAY_SIZE(buffer), "PDVSC,A,%s,%s", name, value);
PortWriteNMEA(*port, buffer, env);
}
/**
* Parses a GSA sentence
*
* $--GSA,a,a,x,x,x,x,x,x,x,x,x,x,x,x,x,x,x.x,x.x,x.x*hh
*
* Field Number:
* 1) Selection mode
* M=Manual, forced to operate in 2D or 3D
* A=Automatic, 3D/2D
* 2) Mode (1 = no fix, 2 = 2D fix, 3 = 3D fix)
* 3) ID of 1st satellite used for fix
* 4) ID of 2nd satellite used for fix
* ...
* 14) ID of 12th satellite used for fix
* 15) PDOP
* 16) HDOP
* 17) VDOP
* 18) checksum
* @param String Input string
* @param params Parameter array
* @param nparams Number of parameters
* @param info NMEA_INFO struct to parse into
* @return Parsing success
*/
bool
NMEAParser::GSA(NMEAInputLine &line, NMEAInfo &info)
{
line.skip();
if (line.read(0) == 1)
info.location_available.Clear();
// satellites are in items 4-15 of GSA string (4-15 is 1-indexed)
for (unsigned i = 0; i < GPSState::MAXSATELLITES; i++)
info.gps.satellite_ids[i] = line.read(0);
return true;
}
/**
* Parses a PFLAU sentence
* (Operating status and priority intruder and obstacle data)
* @param String Input string
* @param params Parameter array
* @param nparams Number of parameters
* @param info NMEA_INFO struct to parse into
* @return Parsing success
* @see http://flarm.com/support/manual/FLARM_DataportManual_v5.00E.pdf
*/
bool
NMEAParser::PFLAU(NMEAInputLine &line, FLARM_STATE &flarm, fixed clock)
{
flarm.available.Update(clock);
// PFLAU,<RX>,<TX>,<GPS>,<Power>,<AlarmLevel>,<RelativeBearing>,<AlarmType>,
// <RelativeVertical>,<RelativeDistance>(,<ID>)
flarm.rx = line.read(0);
flarm.tx = line.read(false);
flarm.gps = (FLARM_STATE::GPSStatus)line.read(FLARM_STATE::GPS_NONE);
line.skip();
flarm.alarm_level = (FLARM_TRAFFIC::AlarmType)
line.read(FLARM_TRAFFIC::ALARM_NONE);
return true;
}
/**
* Parses a GLL sentence
*
* $--GLL,llll.ll,a,yyyyy.yy,a,hhmmss.ss,a,m,*hh
*
* Field Number:
* 1) Latitude
* 2) N or S (North or South)
* 3) Longitude
* 4) E or W (East or West)
* 5) Universal Time Coordinated (UTC)
* 6) Status A - Data Valid, V - Data Invalid
* 7) FAA mode indicator (NMEA 2.3 and later)
* 8) Checksum
* @param String Input string
* @param params Parameter array
* @param nparams Number of parameters
* @param info NMEA_INFO struct to parse into
* @return Parsing success
*/
bool
NMEAParser::GLL(NMEAInputLine &line, NMEAInfo &info)
{
GeoPoint location;
bool valid_location = ReadGeoPoint(line, location);
fixed ThisTime = TimeModify(line.read(fixed_zero), info.date_time_utc,
info.date_available);
ThisTime = TimeAdvanceTolerance(ThisTime);
bool gpsValid = !NAVWarn(line.read_first_char());
if (!TimeHasAdvanced(ThisTime, info))
return true;
if (!gpsValid)
info.location_available.Clear();
else if (valid_location)
info.location_available.Update(info.clock);
if (valid_location)
info.location = location;
info.gps.real = real;
#ifdef ANDROID
info.gps.android_internal_gps = false;
#endif
return true;
}
/**
* Parses a GLL sentence
*
* $--GLL,llll.ll,a,yyyyy.yy,a,hhmmss.ss,a,m,*hh
*
* Field Number:
* 1) Latitude
* 2) N or S (North or South)
* 3) Longitude
* 4) E or W (East or West)
* 5) Universal Time Coordinated (UTC)
* 6) Status A - Data Valid, V - Data Invalid
* 7) FAA mode indicator (NMEA 2.3 and later)
* 8) Checksum
* @param String Input string
* @param params Parameter array
* @param nparams Number of parameters
* @param GPS_INFO GPS_INFO struct to parse into
* @return Parsing success
*/
bool
NMEAParser::GLL(NMEAInputLine &line, NMEA_INFO *GPS_INFO)
{
GeoPoint location;
bool valid_location = ReadGeoPoint(line, location);
fixed ThisTime = TimeModify(line.read(fixed_zero), GPS_INFO->DateTime);
gpsValid = !NAVWarn(line.read_first_char());
if (!activeGPS)
return true;
if (GPS_INFO->gps.Replay)
// block actual GPS signal
return true;
GPS_INFO->gps.NAVWarning = !gpsValid;
if (!TimeHasAdvanced(ThisTime, GPS_INFO))
return false;
if (valid_location)
GPS_INFO->Location = location;
else
GPS_INFO->gps.NAVWarning = true;
return true;
}
/**
* Parses a RMC sentence
*
* $--RMC,hhmmss.ss,A,llll.ll,a,yyyyy.yy,a,x.x,x.x,xxxx,x.x,a,m,*hh
*
* Field Number:
* 1) UTC Time
* 2) Status, V=Navigation receiver warning A=Valid
* 3) Latitude
* 4) N or S
* 5) Longitude
* 6) E or W
* 7) Speed over ground, knots
* 8) Track made good, degrees true
* 9) Date, ddmmyy
* 10) Magnetic Variation, degrees
* 11) E or W
* 12) FAA mode indicator (NMEA 2.3 and later)
* 13) Checksum
* @param String Input string
* @param params Parameter array
* @param nparams Number of parameters
* @param info NMEA_INFO struct to parse into
* @return Parsing success
*/
bool
NMEAParser::RMC(NMEAInputLine &line, NMEAInfo &info)
{
fixed ThisTime = line.read(fixed_zero);
bool gpsValid = !NAVWarn(line.read_first_char());
GeoPoint location;
bool valid_location = ReadGeoPoint(line, location);
GPSState &gps = info.gps;
fixed speed;
bool GroundSpeedAvailable = line.read_checked(speed);
fixed track;
bool track_available = line.read_checked(track);
// JMW get date info first so TimeModify is accurate
if (ReadDate(line, info.date_time_utc))
info.date_available = true;
ThisTime = TimeModify(ThisTime, info.date_time_utc, info.date_available);
ThisTime = TimeAdvanceTolerance(ThisTime);
if (!TimeHasAdvanced(ThisTime, info))
return true;
if (!gpsValid)
info.location_available.Clear();
else if (valid_location)
info.location_available.Update(info.clock);
if (valid_location)
info.location = location;
if (GroundSpeedAvailable) {
info.ground_speed = Units::ToSysUnit(speed, unKnots);
info.ground_speed_available.Update(info.clock);
}
if (track_available && info.MovementDetected()) {
// JMW don't update bearing unless we're moving
info.track = Angle::degrees(track).as_bearing();
info.track_available.Update(info.clock);
}
if (!GGAAvailable) {
// update SatInUse, some GPS receiver don't emit GGA sentence
gps.satellites_used = -1;
}
info.gps.real = real;
#ifdef ANDROID
info.gps.android_internal_gps = false;
#endif
return true;
}
void PDVSC(NMEAInputLine &line) {
char command[4];
line.read(command, ARRAY_SIZE(command));
if (strcmp(command, "S") == 0)
PDVSC_S(line);
else if (strcmp(command, "R") == 0)
PDVSC_R(line);
}
bool
NMEAParser::PFLAU(NMEAInputLine &line, FlarmState &flarm, fixed time)
{
flarm.available.Update(time);
// PFLAU,<RX>,<TX>,<GPS>,<Power>,<AlarmLevel>,<RelativeBearing>,<AlarmType>,
// <RelativeVertical>,<RelativeDistance>(,<ID>)
flarm.rx = line.read(0);
flarm.tx = line.read(false);
flarm.gps = (FlarmState::GPSStatus)
line.read((int)FlarmState::GPSStatus::NONE);
line.skip();
flarm.alarm_level = (FlarmTraffic::AlarmType)
line.read((int)FlarmTraffic::AlarmType::NONE);
return true;
}
/**
* Parses a GSA sentence
*
* $--GSA,a,a,x,x,x,x,x,x,x,x,x,x,x,x,x,x,x.x,x.x,x.x*hh
*
* Field Number:
* 1) Selection mode
* M=Manual, forced to operate in 2D or 3D
* A=Automatic, 3D/2D
* 2) Mode (1 = no fix, 2 = 2D fix, 3 = 3D fix)
* 3) ID of 1st satellite used for fix
* 4) ID of 2nd satellite used for fix
* ...
* 14) ID of 12th satellite used for fix
* 15) PDOP
* 16) HDOP
* 17) VDOP
* 18) checksum
* @param String Input string
* @param params Parameter array
* @param nparams Number of parameters
* @param GPS_INFO GPS_INFO struct to parse into
* @return Parsing success
*/
bool
NMEAParser::GSA(NMEAInputLine &line, NMEA_INFO *GPS_INFO)
{
if (GPS_INFO->gps.Replay)
return true;
line.skip(2);
// satellites are in items 4-15 of GSA string (4-15 is 1-indexed)
for (unsigned i = 0; i < MAXSATELLITES; i++)
GPS_INFO->gps.SatelliteIDs[i] = line.read(0);
GSAAvailable = true;
return true;
}
void PDVSC_R(NMEAInputLine &line) {
char name[64];
line.read(name, ARRAY_SIZE(name));
auto i = settings.find(name);
if (i == settings.end())
return;
const char *value = i->second.c_str();
ConsoleOperationEnvironment env;
char buffer[512];
snprintf(buffer, ARRAY_SIZE(buffer), "PDVSC,A,%s,%s", name, value);
PortWriteNMEA(*port, buffer, env);
}
bool
NMEAParser::GLL(NMEAInputLine &line, NMEAInfo &info)
{
/*
* $--GLL,llll.ll,a,yyyyy.yy,a,hhmmss.ss,a,m,*hh
*
* Field Number:
* 1) Latitude
* 2) N or S (North or South)
* 3) Longitude
* 4) E or W (East or West)
* 5) Universal Time Coordinated (UTC)
* 6) Status A - Data Valid, V - Data Invalid
* 7) FAA mode indicator (NMEA 2.3 and later)
* 8) Checksum
*/
GeoPoint location;
bool valid_location = ReadGeoPoint(line, location);
fixed this_time = TimeModify(line.read(fixed_zero), info.date_time_utc,
info.date_available);
this_time = TimeAdvanceTolerance(this_time);
bool gps_valid = !NAVWarn(line.read_first_char());
if (!TimeHasAdvanced(this_time, info))
return true;
if (!gps_valid)
info.location_available.Clear();
else if (valid_location)
info.location_available.Update(info.clock);
if (valid_location)
info.location = location;
info.gps.real = real;
#ifdef ANDROID
info.gps.android_internal_gps = false;
#endif
return true;
}
static bool
LXWP0(NMEAInputLine &line, NMEA_INFO *GPS_INFO, bool enable_baro)
{
/*
$LXWP0,Y,222.3,1665.5,1.71,,,,,,239,174,10.1
0 loger_stored (Y/N)
1 IAS (kph) ----> Condor uses TAS!
2 baroaltitude (m)
3-8 vario (m/s) (last 6 measurements in last second)
9 heading of plane
10 windcourse (deg)
11 windspeed (kph)
*/
line.skip();
fixed airspeed;
GPS_INFO->AirspeedAvailable = line.read_checked(airspeed);
fixed alt = line.read(fixed_zero);
if (GPS_INFO->AirspeedAvailable) {
GPS_INFO->TrueAirspeed = Units::ToSysUnit(airspeed, unKiloMeterPerHour);
GPS_INFO->IndicatedAirspeed =
GPS_INFO->TrueAirspeed / AtmosphericPressure::AirDensityRatio(alt);
}
if (enable_baro) {
GPS_INFO->BaroAltitudeAvailable = true;
GPS_INFO->BaroAltitude = alt; // ToDo check if QNH correction is needed!
}
GPS_INFO->TotalEnergyVarioAvailable =
line.read_checked(GPS_INFO->TotalEnergyVario);
line.skip(6);
GPS_INFO->ExternalWindAvailable = ReadSpeedVector(line, GPS_INFO->wind);
TriggerVarioUpdate();
return true;
}
static bool
PZAN5(NMEAInputLine &line, NMEAInfo &info)
{
// $PZAN5,VA,MUEHL,123.4,KM,T,234*cc
char state[3];
line.read(state, 3);
if (strcmp(state, "SF") == 0) {
info.switch_state.flight_mode = SwitchInfo::FlightMode::CRUISE;
info.switch_state.speed_command = true;
} else if (strcmp(state, "VA") == 0) {
info.switch_state.flight_mode = SwitchInfo::FlightMode::CIRCLING;
info.switch_state.speed_command = false;
} else
return false;
info.switch_state_available = true;
return true;
}
static bool
PDVDV(NMEAInputLine &line, NMEAInfo &info)
{
fixed value;
if (line.read_checked(value))
info.ProvideTotalEnergyVario(value / 10);
bool ias_available = line.read_checked(value);
fixed tas_ratio = line.read(fixed(1024)) / 1024;
if (ias_available)
info.ProvideBothAirspeeds(value / 10, value / 10 * tas_ratio);
//hasVega = true;
if (line.read_checked(value))
info.ProvidePressureAltitude(value);
return true;
}
static bool
PZAN5(NMEAInputLine &line, NMEA_INFO *GPS_INFO)
{
// $PZAN5,VA,MUEHL,123.4,KM,T,234*cc
char state[3];
line.read(state, 3);
if (strcmp(state, "SF") == 0) {
GPS_INFO->SwitchState.FlightMode = SWITCH_INFO::MODE_CRUISE;
GPS_INFO->SwitchState.SpeedCommand = true;
} else if (strcmp(state, "VA") == 0) {
GPS_INFO->SwitchState.FlightMode = SWITCH_INFO::MODE_CIRCLING;
GPS_INFO->SwitchState.SpeedCommand = false;
} else
return false;
GPS_INFO->SwitchStateAvailable = true;
return true;
}
// RMN: Volkslogger
// Source data:
// $PGCS,1,0EC0,FFF9,0C6E,02*61
// $PGCS,1,0EC0,FFFA,0C6E,03*18
static bool
vl_PGCS1(NMEAInputLine &line, NMEA_INFO *GPS_INFO, bool enable_baro)
{
GPS_STATE &gps = GPS_INFO->gps;
if (line.read(1) != 1)
return false;
/* pressure sensor */
line.skip();
// four characers, hex, barometric altitude
long altitude = line.read_hex(0L);
if (enable_baro) {
if (altitude > 60000)
/* Assuming that altitude has wrapped around. 60 000 m occurs
at QNH ~2000 hPa */
altitude -= 65535;
GPS_INFO->BaroAltitude =
GPS_INFO->pressure.AltitudeToQNHAltitude(fixed(altitude));
GPS_INFO->BaroAltitudeAvailable = true;
}
// ExtractParameter(String,ctemp,3);
// four characters, hex, constant. Value 1371 (dec)
// nSatellites = (int)(min(12,HexStrToDouble(ctemp, NULL)));
if (gps.SatellitesUsed <= 0) {
gps.SatellitesUsed = 4;
// just to make XCSoar quit complaining. VL doesn't tell how many
// satellites it uses. Without this XCSoar won't do wind
// measurements.
}
return false;
}
/**
* Parses an angle in the form "DDDMM.SSS". Minutes are 0..59, and
* seconds are 0..999.
*/
static bool
ReadPositiveAngle(NMEAInputLine &line, Angle &a)
{
char buffer[32], *endptr;
line.read(buffer, sizeof(buffer));
char *dot = strchr(buffer, '.');
if (dot < buffer + 3)
return false;
double x = strtod(dot - 2, &endptr);
if (x < 0 || x >= 60 || *endptr != 0)
return false;
dot[-2] = 0;
long y = strtol(buffer, &endptr, 10);
if (y < 0 || endptr == buffer || *endptr != 0)
return false;
a = Angle::degrees(fixed(y) + fixed(x) / 60);
return true;
}
static bool
ReadDate(NMEAInputLine &line, BrokenDate &date)
{
char buffer[9];
line.read(buffer, 9);
if (strlen(buffer) != 6)
return false;
BrokenDate new_value;
new_value.year = atoi(buffer + 4) + 2000;
buffer[4] = '\0';
new_value.month = atoi(buffer + 2);
buffer[2] = '\0';
new_value.day = atoi(buffer);
if (!new_value.Plausible())
return false;
date = new_value;
return true;
}
/**
* Parses a PFLAA sentence
* (Data on other moving objects around)
* @param String Input string
* @param params Parameter array
* @param nparams Number of parameters
* @param GPS_INFO GPS_INFO struct to parse into
* @return Parsing success
* @see http://flarm.com/support/manual/FLARM_DataportManual_v4.06E.pdf
*/
bool
NMEAParser::PFLAA(NMEAInputLine &line, NMEA_INFO *GPS_INFO)
{
FLARM_STATE &flarm = GPS_INFO->flarm;
isFlarm = true;
// calculate relative east and north projection to lat/lon
Angle delta_lat = Angle::degrees(fixed(0.01));
Angle delta_lon = Angle::degrees(fixed(0.01));
GeoPoint plat = GPS_INFO->Location;
plat.Latitude += delta_lat;
GeoPoint plon = GPS_INFO->Location;
plon.Longitude += delta_lon;
fixed dlat = Distance(GPS_INFO->Location, plat);
fixed dlon = Distance(GPS_INFO->Location, plon);
fixed FLARM_NorthingToLatitude(0);
fixed FLARM_EastingToLongitude(0);
if (positive(fabs(dlat)) && positive(fabs(dlon))) {
FLARM_NorthingToLatitude = delta_lat.value_degrees() / dlat;
FLARM_EastingToLongitude = delta_lon.value_degrees() / dlon;
}
// PFLAA,<AlarmLevel>,<RelativeNorth>,<RelativeEast>,<RelativeVertical>,
// <IDType>,<ID>,<Track>,<TurnRate>,<GroundSpeed>,<ClimbRate>,<AcftType>
FLARM_TRAFFIC traffic;
traffic.AlarmLevel = line.read(0);
traffic.RelativeNorth = line.read(fixed_zero);
traffic.RelativeEast = line.read(fixed_zero);
traffic.RelativeAltitude = line.read(fixed_zero);
traffic.IDType = line.read(0);
// 5 id, 6 digit hex
char id_string[16];
line.read(id_string, 16);
traffic.ID.parse(id_string, NULL);
traffic.TrackBearing = Angle::degrees(line.read(fixed_zero));
traffic.TurnRate = line.read(fixed_zero);
traffic.Speed = line.read(fixed_zero);
traffic.ClimbRate = line.read(fixed_zero);
traffic.Type = (FLARM_TRAFFIC::AircraftType)line.read(0);
FLARM_TRAFFIC *flarm_slot = flarm.FindTraffic(traffic.ID);
if (flarm_slot == NULL) {
flarm_slot = flarm.AllocateTraffic();
if (flarm_slot == NULL)
// no more slots available
return false;
flarm_slot->ID = traffic.ID;
flarm.NewTraffic = true;
InputEvents::processGlideComputer(GCE_FLARM_NEWTRAFFIC);
}
// set time of fix to current time
flarm_slot->Time_Fix = GPS_INFO->Time;
// PFLAA,<AlarmLevel>,<RelativeNorth>,<RelativeEast>,<RelativeVertical>,
// <IDType>,<ID>,<Track>,<TurnRate>,<GroundSpeed>,<ClimbRate>,<AcftType>
flarm_slot->AlarmLevel = traffic.AlarmLevel;
flarm_slot->RelativeNorth = traffic.RelativeNorth;
flarm_slot->RelativeEast = traffic.RelativeEast;
flarm_slot->RelativeAltitude = traffic.RelativeAltitude;
flarm_slot->IDType = traffic.IDType;
flarm_slot->TrackBearing = traffic.TrackBearing;
flarm_slot->TurnRate = traffic.TurnRate;
flarm_slot->Speed = traffic.Speed;
flarm_slot->ClimbRate = traffic.ClimbRate;
flarm_slot->Type = traffic.Type;
// 1 relativenorth, meters
flarm_slot->Location.Latitude = Angle::degrees(flarm_slot->RelativeNorth
* FLARM_NorthingToLatitude) + GPS_INFO->Location.Latitude;
// 2 relativeeast, meters
flarm_slot->Location.Longitude = Angle::degrees(flarm_slot->RelativeEast
* FLARM_EastingToLongitude) + GPS_INFO->Location.Longitude;
// alt
flarm_slot->Altitude = flarm_slot->RelativeAltitude + GPS_INFO->GPSAltitude;
flarm_slot->Average30s = flarmCalculations.Average30s(flarm_slot->ID,
GPS_INFO->Time,
flarm_slot->Altitude);
// QUESTION TB: never returns true?!
return false;
}
bool
NMEAParser::RMC(NMEAInputLine &line, NMEAInfo &info)
{
/*
* $--RMC,hhmmss.ss,A,llll.ll,a,yyyyy.yy,a,x.x,x.x,xxxx,x.x,a,m,*hh
*
* Field Number:
* 1) UTC Time
* 2) Status, V=Navigation receiver warning A=Valid
* 3) Latitude
* 4) N or S
* 5) Longitude
* 6) E or W
* 7) Speed over ground, knots
* 8) Track made good, degrees true
* 9) Date, ddmmyy
* 10) Magnetic Variation, degrees
* 11) E or W
* 12) FAA mode indicator (NMEA 2.3 and later)
* 13) Checksum
*/
fixed this_time = line.read(fixed_zero);
bool gps_valid = !NAVWarn(line.read_first_char());
GeoPoint location;
bool valid_location = ReadGeoPoint(line, location);
fixed speed;
bool ground_speed_available = line.read_checked(speed);
fixed track;
bool track_available = line.read_checked(track);
// JMW get date info first so TimeModify is accurate
if (ReadDate(line, info.date_time_utc))
info.date_available = true;
this_time = TimeModify(this_time, info.date_time_utc, info.date_available);
this_time = TimeAdvanceTolerance(this_time);
if (!TimeHasAdvanced(this_time, info))
return true;
if (!gps_valid)
info.location_available.Clear();
else if (valid_location)
info.location_available.Update(info.clock);
if (valid_location)
info.location = location;
if (ground_speed_available) {
info.ground_speed = Units::ToSysUnit(speed, unKnots);
info.ground_speed_available.Update(info.clock);
}
if (track_available && info.MovementDetected()) {
// JMW don't update bearing unless we're moving
info.track = Angle::Degrees(track).AsBearing();
info.track_available.Update(info.clock);
}
info.gps.real = real;
#ifdef ANDROID
info.gps.android_internal_gps = false;
#endif
return true;
}
/**
* Parses a GGA sentence
*
* $--GGA,hhmmss.ss,llll.ll,a,yyyyy.yy,a,x,xx,x.x,x.x,M,x.x,M,x.x,xxxx*hh
*
* Field Number:
* 1) Universal Time Coordinated (UTC)
* 2) Latitude
* 3) N or S (North or South)
* 4) Longitude
* 5) E or W (East or West)
* 6) GPS Quality Indicator,
* 0 - fix not available,
* 1 - GPS fix,
* 2 - Differential GPS fix
* (values above 2 are 2.3 features)
* 3 = PPS fix
* 4 = Real Time Kinematic
* 5 = Float RTK
* 6 = estimated (dead reckoning)
* 7 = Manual input mode
* 8 = Simulation mode
* 7) Number of satellites in view, 00 - 12
* 8) Horizontal Dilution of precision (meters)
* 9) Antenna Altitude above/below mean-sea-level (geoid) (in meters)
* 10) Units of antenna altitude, meters
* 11) Geoidal separation, the difference between the WGS-84 earth
* ellipsoid and mean-sea-level (geoid), "-" means mean-sea-level
* below ellipsoid
* 12) Units of geoidal separation, meters
* 13) Age of differential GPS data, time in seconds since last SC104
* type 1 or 9 update, null field when DGPS is not used
* 14) Differential reference station ID, 0000-1023
* 15) Checksum
* @param String Input string
* @param params Parameter array
* @param nparams Number of parameters
* @param GPS_INFO GPS_INFO struct to parse into
* @return Parsing success
*/
bool
NMEAParser::GGA(NMEAInputLine &line, NMEA_INFO *GPS_INFO)
{
GPS_STATE &gps = GPS_INFO->gps;
if (gps.Replay)
return true;
GGAAvailable = true;
fixed ThisTime = TimeModify(line.read(fixed_zero), GPS_INFO->DateTime);
GeoPoint location;
bool valid_location = ReadGeoPoint(line, location);
gps.FixQuality = line.read(0);
if (gps.FixQuality != 1 && gps.FixQuality != 2)
gpsValid = false;
int nSatellites = min(16, line.read(0));
if (nSatellites == 0)
gpsValid = false;
if (!activeGPS)
return true;
gps.SatellitesUsed = nSatellites;
if (!TimeHasAdvanced(ThisTime, GPS_INFO))
return false;
if (valid_location)
GPS_INFO->Location = location;
else
GPS_INFO->gps.NAVWarning = true;
gps.HDOP = line.read(fixed_zero);
if (RMZAvailable) {
GPS_INFO->BaroAltitudeAvailable = true;
GPS_INFO->BaroAltitude = RMZAltitude;
} else if (RMAAvailable) {
GPS_INFO->BaroAltitudeAvailable = true;
GPS_INFO->BaroAltitude = RMAAltitude;
}
// VENTA3 CONDOR ALTITUDE
// "Altitude" should always be GPS Altitude.
bool altitude_available = ReadAltitude(line, GPS_INFO->GPSAltitude);
if (!altitude_available)
GPS_INFO->GPSAltitude = fixed_zero;
fixed GeoidSeparation;
if (ReadAltitude(line, GeoidSeparation)) {
// No real need to parse this value,
// but we do assume that no correction is required in this case
if (!altitude_available)
/* Some devices, such as the "LG Incite Cellphone" seem to be
severely bugged, and report the GPS altitude in the Geoid
column. That sucks! */
GPS_INFO->GPSAltitude = GeoidSeparation;
} else {
// need to estimate Geoid Separation internally (optional)
// FLARM uses MSL altitude
//
// Some others don't.
//
// If the separation doesn't appear in the sentence,
// we can assume the GPS unit is giving ellipsoid height
//
if (!HaveCondorDevice()) {
// JMW TODO really need to know the actual device..
GeoidSeparation = LookupGeoidSeparation(GPS_INFO->Location);
GPS_INFO->GPSAltitude -= GeoidSeparation;
}
}
return true;
}
/**
* Parses a RMC sentence
*
* $--RMC,hhmmss.ss,A,llll.ll,a,yyyyy.yy,a,x.x,x.x,xxxx,x.x,a,m,*hh
*
* Field Number:
* 1) UTC Time
* 2) Status, V=Navigation receiver warning A=Valid
* 3) Latitude
* 4) N or S
* 5) Longitude
* 6) E or W
* 7) Speed over ground, knots
* 8) Track made good, degrees true
* 9) Date, ddmmyy
* 10) Magnetic Variation, degrees
* 11) E or W
* 12) FAA mode indicator (NMEA 2.3 and later)
* 13) Checksum
* @param String Input string
* @param params Parameter array
* @param nparams Number of parameters
* @param GPS_INFO GPS_INFO struct to parse into
* @return Parsing success
*/
bool
NMEAParser::RMC(NMEAInputLine &line, NMEA_INFO *GPS_INFO)
{
fixed ThisTime = TimeModify(line.read(fixed_zero), GPS_INFO->DateTime);
gpsValid = !NAVWarn(line.read_first_char());
GeoPoint location;
bool valid_location = ReadGeoPoint(line, location);
GPS_STATE &gps = GPS_INFO->gps;
gps.Connected = 2;
if (!activeGPS)
return true;
fixed speed = line.read(fixed_zero);
gps.MovementDetected = speed > fixed_two;
if (gps.Replay)
// block actual GPS signal if not moving and a log is being replayed
return true;
gps.NAVWarning = !gpsValid;
fixed TrackBearing = line.read(fixed_zero);
// JMW get date info first so TimeModify is accurate
char date_buffer[9];
line.read(date_buffer, 9);
GPS_INFO->DateTime.year = atoi(&date_buffer[4]) + 2000;
date_buffer[4] = '\0';
GPS_INFO->DateTime.month = atoi(&date_buffer[2]);
date_buffer[2] = '\0';
GPS_INFO->DateTime.day = atoi(&date_buffer[0]);
if (!TimeHasAdvanced(ThisTime, GPS_INFO))
return false;
if (valid_location)
GPS_INFO->Location = location;
else
GPS_INFO->gps.NAVWarning = true;
GPS_INFO->GroundSpeed = Units::ToSysUnit(speed, unKnots);
if (GPS_INFO->GroundSpeed > fixed_one) {
// JMW don't update bearing unless we're moving
GPS_INFO->TrackBearing = Angle::degrees(TrackBearing).as_bearing();
}
if (!gps.Replay) {
if (RMZAvailable) {
// JMW changed from Altitude to BaroAltitude
GPS_INFO->BaroAltitudeAvailable = true;
GPS_INFO->BaroAltitude = RMZAltitude;
} else if (RMAAvailable) {
// JMW changed from Altitude to BaroAltitude
GPS_INFO->BaroAltitudeAvailable = true;
GPS_INFO->BaroAltitude = RMAAltitude;
}
}
if (!GGAAvailable) {
// update SatInUse, some GPS receiver don't emit GGA sentence
if (!gpsValid)
gps.SatellitesUsed = 0;
else
gps.SatellitesUsed = -1;
}
// say we are updated every time we get this,
// so infoboxes get refreshed if GPS connected
TriggerGPSUpdate();
return true;
}
bool
NMEAParser::GGA(NMEAInputLine &line, NMEAInfo &info)
{
/*
* $--GGA,hhmmss.ss,llll.ll,a,yyyyy.yy,a,x,xx,x.x,x.x,M,x.x,M,x.x,xxxx*hh
*
* Field Number:
* 1) Universal Time Coordinated (UTC)
* 2) Latitude
* 3) N or S (North or South)
* 4) Longitude
* 5) E or W (East or West)
* 6) GPS Quality Indicator,
* 0 - fix not available,
* 1 - GPS fix,
* 2 - Differential GPS fix
* (values above 2 are 2.3 features)
* 3 = PPS fix
* 4 = Real Time Kinematic
* 5 = Float RTK
* 6 = estimated (dead reckoning)
* 7 = Manual input mode
* 8 = Simulation mode
* 7) Number of satellites in view, 00 - 12
* 8) Horizontal Dilution of precision (meters)
* 9) Antenna Altitude above/below mean-sea-level (geoid) (in meters)
* 10) Units of antenna altitude, meters
* 11) Geoidal separation, the difference between the WGS-84 earth
* ellipsoid and mean-sea-level (geoid), "-" means mean-sea-level
* below ellipsoid
* 12) Units of geoidal separation, meters
* 13) Age of differential GPS data, time in seconds since last SC104
* type 1 or 9 update, null field when DGPS is not used
* 14) Differential reference station ID, 0000-1023
* 15) Checksum
*/
GPSState &gps = info.gps;
fixed this_time = TimeModify(line.read(fixed_zero), info.date_time_utc,
info.date_available);
this_time = TimeAdvanceTolerance(this_time);
GeoPoint location;
bool valid_location = ReadGeoPoint(line, location);
gps.fix_quality = line.read(0);
gps.satellites_used_available.Update(info.clock);
gps.satellites_used = min(16, line.read(-1));
if (!TimeHasAdvanced(this_time, info))
return true;
(void)valid_location;
/* JMW: note ignore location updates from GGA -- definitive frame is GPRMC sentence
if (!gpsValid)
info.LocationAvailable.Clear();
else if (valid_location)
info.LocationAvailable.Update(info.clock);
if (valid_location)
info.Location = location;
*/
info.gps.real = real;
#ifdef ANDROID
info.gps.android_internal_gps = false;
#endif
gps.hdop = line.read(fixed_zero);
// VENTA3 CONDOR ALTITUDE
// "Altitude" should always be GPS Altitude.
bool altitude_available = ReadAltitude(line, info.gps_altitude);
if (altitude_available)
info.gps_altitude_available.Update(info.clock);
else
info.gps_altitude_available.Clear();
fixed geoid_separation;
if (ReadAltitude(line, geoid_separation)) {
// No real need to parse this value,
// but we do assume that no correction is required in this case
if (!altitude_available) {
/* Some devices, such as the "LG Incite Cellphone" seem to be
severely bugged, and report the GPS altitude in the Geoid
column. That sucks! */
info.gps_altitude = geoid_separation;
info.gps_altitude_available.Update(info.clock);
}
} else {
// need to estimate Geoid Separation internally (optional)
// FLARM uses MSL altitude
//
// Some others don't.
//
// If the separation doesn't appear in the sentence,
// we can assume the GPS unit is giving ellipsoid height
//
if (use_geoid) {
// JMW TODO really need to know the actual device..
geoid_separation = EGM96::LookupSeparation(info.location);
info.gps_altitude -= geoid_separation;
}
}
return true;
}
/**
* Parses a PFLAA sentence
* (Data on other moving objects around)
* @param String Input string
* @param params Parameter array
* @param nparams Number of parameters
* @param info NMEA_INFO struct to parse into
* @return Parsing success
* @see http://flarm.com/support/manual/FLARM_DataportManual_v5.00E.pdf
*/
bool
NMEAParser::PFLAA(NMEAInputLine &line, NMEAInfo &info)
{
FLARM_STATE &flarm = info.flarm;
// PFLAA,<AlarmLevel>,<RelativeNorth>,<RelativeEast>,<RelativeVertical>,
// <IDType>,<ID>,<Track>,<TurnRate>,<GroundSpeed>,<ClimbRate>,<AcftType>
FLARM_TRAFFIC traffic;
traffic.alarm_level = (FLARM_TRAFFIC::AlarmType)
line.read(FLARM_TRAFFIC::ALARM_NONE);
fixed value;
bool stealth = false;
if (!line.read_checked(value))
// Relative North is required !
return true;
traffic.relative_north = value;
if (!line.read_checked(value))
// Relative East is required !
return true;
traffic.relative_east = value;
if (!line.read_checked(value))
// Relative Altitude is required !
return true;
traffic.relative_altitude = value;
line.skip(); /* id type */
// 5 id, 6 digit hex
char id_string[16];
line.read(id_string, 16);
traffic.id.parse(id_string, NULL);
traffic.track_received = line.read_checked(value);
if (!traffic.track_received) {
// Field is empty in stealth mode
stealth = true;
traffic.track = Angle::zero();
} else
traffic.track = Angle::degrees(value);
traffic.turn_rate_received = line.read_checked(value);
if (!traffic.turn_rate_received) {
// Field is empty in stealth mode
traffic.turn_rate = fixed_zero;
} else
traffic.turn_rate = value;
traffic.speed_received = line.read_checked(value);
if (!traffic.speed_received) {
// Field is empty in stealth mode
stealth = true;
traffic.speed = fixed_zero;
} else
traffic.speed = value;
traffic.climb_rate_received = line.read_checked(value);
if (!traffic.climb_rate_received) {
// Field is empty in stealth mode
stealth = true;
traffic.climb_rate = fixed_zero;
} else
traffic.climb_rate = value;
traffic.stealth = stealth;
traffic.type = (FLARM_TRAFFIC::AircraftType)line.read(0);
FLARM_TRAFFIC *flarm_slot = flarm.FindTraffic(traffic.id);
if (flarm_slot == NULL) {
flarm_slot = flarm.AllocateTraffic();
if (flarm_slot == NULL)
// no more slots available
return true;
flarm_slot->Clear();
flarm_slot->id = traffic.id;
flarm.NewTraffic = true;
}
// set time of fix to current time
flarm_slot->valid.Update(info.clock);
flarm_slot->Update(traffic);
return true;
}
/**
* Parses a GGA sentence
*
* $--GGA,hhmmss.ss,llll.ll,a,yyyyy.yy,a,x,xx,x.x,x.x,M,x.x,M,x.x,xxxx*hh
*
* Field Number:
* 1) Universal Time Coordinated (UTC)
* 2) Latitude
* 3) N or S (North or South)
* 4) Longitude
* 5) E or W (East or West)
* 6) GPS Quality Indicator,
* 0 - fix not available,
* 1 - GPS fix,
* 2 - Differential GPS fix
* (values above 2 are 2.3 features)
* 3 = PPS fix
* 4 = Real Time Kinematic
* 5 = Float RTK
* 6 = estimated (dead reckoning)
* 7 = Manual input mode
* 8 = Simulation mode
* 7) Number of satellites in view, 00 - 12
* 8) Horizontal Dilution of precision (meters)
* 9) Antenna Altitude above/below mean-sea-level (geoid) (in meters)
* 10) Units of antenna altitude, meters
* 11) Geoidal separation, the difference between the WGS-84 earth
* ellipsoid and mean-sea-level (geoid), "-" means mean-sea-level
* below ellipsoid
* 12) Units of geoidal separation, meters
* 13) Age of differential GPS data, time in seconds since last SC104
* type 1 or 9 update, null field when DGPS is not used
* 14) Differential reference station ID, 0000-1023
* 15) Checksum
* @param String Input string
* @param params Parameter array
* @param nparams Number of parameters
* @param info NMEA_INFO struct to parse into
* @return Parsing success
*/
bool
NMEAParser::GGA(NMEAInputLine &line, NMEAInfo &info)
{
GPSState &gps = info.gps;
GGAAvailable = true;
fixed ThisTime = TimeModify(line.read(fixed_zero), info.date_time_utc,
info.date_available);
ThisTime = TimeAdvanceTolerance(ThisTime);
GeoPoint location;
bool valid_location = ReadGeoPoint(line, location);
gps.fix_quality = line.read(0);
gps.satellites_used = min(16, line.read(-1));
if (!TimeHasAdvanced(ThisTime, info))
return true;
(void)valid_location;
/* JMW: note ignore location updates from GGA -- definitive frame is GPRMC sentence
if (!gpsValid)
info.LocationAvailable.Clear();
else if (valid_location)
info.LocationAvailable.Update(info.clock);
if (valid_location)
info.Location = location;
*/
info.gps.real = real;
#ifdef ANDROID
info.gps.android_internal_gps = false;
#endif
gps.hdop = line.read(fixed_zero);
// VENTA3 CONDOR ALTITUDE
// "Altitude" should always be GPS Altitude.
bool altitude_available = ReadAltitude(line, info.gps_altitude);
if (altitude_available)
info.gps_altitude_available.Update(info.clock);
else {
info.gps_altitude = fixed_zero;
info.gps_altitude_available.Clear();
}
fixed GeoidSeparation;
if (ReadAltitude(line, GeoidSeparation)) {
// No real need to parse this value,
// but we do assume that no correction is required in this case
if (!altitude_available) {
/* Some devices, such as the "LG Incite Cellphone" seem to be
severely bugged, and report the GPS altitude in the Geoid
column. That sucks! */
info.gps_altitude = GeoidSeparation;
info.gps_altitude_available.Update(info.clock);
}
} else {
// need to estimate Geoid Separation internally (optional)
// FLARM uses MSL altitude
//
// Some others don't.
//
// If the separation doesn't appear in the sentence,
// we can assume the GPS unit is giving ellipsoid height
//
if (use_geoid) {
// JMW TODO really need to know the actual device..
GeoidSeparation = LookupGeoidSeparation(info.location);
info.gps_altitude -= GeoidSeparation;
}
}
return true;
}