static void TestTasman() { NMEAParser parser; NMEAInfo nmea_info; nmea_info.Reset(); nmea_info.clock = fixed(1); ok1(parser.ParseLine("$PTAS1,200,200,02426,000*25", nmea_info)); ok1(nmea_info.total_energy_vario_available); ok1(equals(nmea_info.total_energy_vario, fixed(0))); ok1(nmea_info.pressure_altitude_available); ok1(equals(nmea_info.pressure_altitude, Units::ToSysUnit(fixed(426), Unit::FEET))); ok1(nmea_info.airspeed_available); ok1(equals(nmea_info.true_airspeed, fixed(0))); ok1(parser.ParseLine("$PTAS1,234,000,00426,062*26", nmea_info)); ok1(nmea_info.total_energy_vario_available); ok1(equals(nmea_info.total_energy_vario, Units::ToSysUnit(fixed(3.4), Unit::KNOTS))); ok1(nmea_info.pressure_altitude_available); ok1(equals(nmea_info.pressure_altitude, Units::ToSysUnit(fixed(-1574), Unit::FEET))); ok1(nmea_info.airspeed_available); ok1(equals(nmea_info.true_airspeed, Units::ToSysUnit(fixed(62), Unit::KNOTS))); }
/** * Redirects parsing of the String into GPS_INFO to the right device parser * @param device 0 or 1, depending on the port used * @param String NMEA string * @param GPS_INFO GPS_INFO struct that will be updated * @return Parsing success */ bool NMEAParser::ParseNMEAString(int device, const TCHAR *String, NMEA_INFO *GPS_INFO) { switch (device) { case 0: return nmeaParser1.ParseNMEAString_Internal(String, GPS_INFO); case 1: return nmeaParser2.ParseNMEAString_Internal(String, GPS_INFO); }; return false; }
void NMEAParser::Reset(void) { // clear status nmeaParser1._Reset(); nmeaParser2._Reset(); // trigger updates TriggerGPSUpdate(); TriggerVarioUpdate(); }
BOOL NMEAParser::ParseNMEAString(int device, TCHAR *String, NMEA_INFO *GPS_INFO) { switch (device) { case 0: return nmeaParser1.ParseNMEAString_Internal(String, GPS_INFO); case 1: return nmeaParser2.ParseNMEAString_Internal(String, GPS_INFO); }; return FALSE; }
BOOL NMEAParser::ParseGPS_POSITION(int Idx, const GPS_POSITION& loc, NMEA_INFO& GPSData) { LKASSERT(!ReplayLogger::IsEnabled()); switch (Idx) { case 0: ComPortHB[0]=LKHearthBeats; return nmeaParser1.ParseGPS_POSITION_internal(loc, GPSData); case 1: ComPortHB[1]=LKHearthBeats; return nmeaParser2.ParseGPS_POSITION_internal(loc, GPSData); }; return FALSE; }
BOOL NMEAParser::ParseNMEAString(int device, TCHAR *String, NMEA_INFO *pGPS) { LKASSERT(!ReplayLogger::IsEnabled()); switch (device) { case 0: return nmeaParser1.ParseNMEAString_Internal(String, pGPS); case 1: return nmeaParser2.ParseNMEAString_Internal(String, pGPS); }; return FALSE; }
/** * This function creates some simulated traffic for FLARM debugging * @param GPS_INFO Pointer to the NMEA_INFO struct */ void Simulator::GenerateFLARMTraffic(NMEAInfo &basic) { static int i = 90; i++; if (i > 255) i = 0; if (i > 80) return; const Angle angle = Angle::degrees(fixed((i * 360) / 255)).as_bearing(); Angle dangle = (angle + Angle::degrees(fixed(120))).as_bearing(); Angle hangle = dangle.flipped().as_bearing(); int alt = (angle.ifastsine()) / 7; int north = (angle.ifastsine()) / 2 - 200; int east = (angle.ifastcosine()) / 1.5; int track = -angle.as_bearing().value_degrees(); unsigned alarm_level = (i % 30 > 13 ? 0 : (i % 30 > 5 ? 2 : 1)); NMEAParser parser; char buffer[50]; // PFLAA,<AlarmLevel>,<RelativeNorth>,<RelativeEast>,<RelativeVertical>, // <IDType>,<ID>,<Track>,<TurnRate>,<GroundSpeed>,<ClimbRate>,<AcftType> sprintf(buffer, "$PFLAA,%d,%d,%d,%d,2,DDA85C,%d,0,35,0,1", alarm_level, north, east, alt, track); AppendNMEAChecksum(buffer); parser.ParseNMEAString_Internal(buffer, basic); alt = (angle.ifastcosine()) / 10; north = (dangle.ifastsine()) / 1.20 + 300; east = (dangle.ifastcosine()) + 500; track = hangle.value_degrees(); // PFLAA,<AlarmLevel>,<RelativeNorth>,<RelativeEast>,<RelativeVertical>, // <IDType>,<ID>,<Track>,<TurnRate>,<GroundSpeed>,<ClimbRate>,<AcftType> sprintf(buffer, "$PFLAA,0,%d,%d,%d,2,AA9146,,,,,1", north, east, alt); AppendNMEAChecksum(buffer); parser.ParseNMEAString_Internal(buffer, basic); // PFLAU,<RX>,<TX>,<GPS>,<Power>,<AlarmLevel>,<RelativeBearing>,<AlarmType>, // <RelativeVertical>,<RelativeDistance>(,<ID>) sprintf(buffer, "$PFLAU,2,1,2,1,%d", alarm_level); AppendNMEAChecksum(buffer); parser.ParseNMEAString_Internal(buffer, basic); }
int main(int argc, char **argv) { NarrowString<1024> usage; usage = "DRIVER\n\n" "Where DRIVER is one of:"; { const DeviceRegister *driver; for (unsigned i = 0; (driver = GetDriverByIndex(i)) != nullptr; ++i) { WideToUTF8Converter driver_name(driver->name); usage.AppendFormat("\n\t%s", (const char *)driver_name); } } Args args(argc, argv, usage); tstring driver_name = args.ExpectNextT(); args.ExpectEnd(); driver = FindDriverByName(driver_name.c_str()); if (driver == nullptr) { _ftprintf(stderr, _T("No such driver: %s\n"), driver_name.c_str()); return 1; } DeviceConfig config; config.Clear(); NullPort port; Device *device = driver->CreateOnPort != nullptr ? driver->CreateOnPort(config, port) : nullptr; NMEAParser parser; NMEAInfo data; data.Reset(); char buffer[1024]; while (fgets(buffer, sizeof(buffer), stdin) != nullptr) { StripRight(buffer); if (device == nullptr || !device->ParseNMEA(buffer, data)) parser.ParseLine(buffer, data); } Dump(data); return EXIT_SUCCESS; }
static void TestVega() { NullPort null; Device *device = vega_driver.CreateOnPort(dummy_config, null); ok1(device != NULL); NMEAInfo nmea_info; nmea_info.Reset(); nmea_info.clock = fixed(1); /* enable FLARM mode (switches the $PGRMZ parser to pressure altitude) */ NMEAParser parser; ok1(parser.ParseLine("$PFLAU,0,0,0,1,0,,0,,*63", nmea_info)); ok1(parser.ParseLine("$PGRMZ,2447,F,2*0F", nmea_info)); ok1(nmea_info.pressure_altitude_available); ok1(equals(nmea_info.pressure_altitude, 745.845)); ok1(device->ParseNMEA("$PDSWC,0,1002000,100,115*54", nmea_info)); ok1(nmea_info.settings.mac_cready_available); ok1(equals(nmea_info.settings.mac_cready, 0)); ok1(nmea_info.voltage_available); ok1(equals(nmea_info.voltage, 11.5)); ok1(device->ParseNMEA("$PDVDV,1,0,1062,762,9252,0*5B", nmea_info)); ok1(nmea_info.total_energy_vario_available); ok1(equals(nmea_info.total_energy_vario, 0.1)); ok1(nmea_info.airspeed_available); ok1(equals(nmea_info.true_airspeed, 0)); ok1(equals(nmea_info.indicated_airspeed, 0)); ok1(!nmea_info.static_pressure_available); ok1(!nmea_info.baro_altitude_available); ok1(nmea_info.pressure_altitude_available); ok1(equals(nmea_info.pressure_altitude, 762)); /* parse $PGRMZ again, it should be ignored */ ok1(parser.ParseLine("$PGRMZ,2447,F,2*0F", nmea_info)); ok1(nmea_info.pressure_altitude_available); ok1(equals(nmea_info.pressure_altitude, 762)); delete device; }
static void TestGeneric() { NMEAParser parser; NMEAInfo nmea_info; nmea_info.Reset(); nmea_info.clock = fixed(1); nmea_info.alive.Update(nmea_info.clock); /* no GPS reception */ ok1(parser.ParseLine("$GPRMC,082310,V,,,,,230610*3f", nmea_info)); ok1(nmea_info.alive); ok1(!nmea_info.location_available); ok1(nmea_info.date_time_utc.year == 2010); ok1(nmea_info.date_time_utc.month == 6); ok1(nmea_info.date_time_utc.day == 23); ok1(nmea_info.date_time_utc.hour == 8); ok1(nmea_info.date_time_utc.minute == 23); ok1(nmea_info.date_time_utc.second == 10); /* got a GPS fix */ ok1(parser.ParseLine("$GPRMC,082311,A,5103.5403,N,00741.5742,E,055.3,022.4,230610,000.3,W*6C", nmea_info)); ok1(nmea_info.alive); ok1(nmea_info.location_available); ok1(nmea_info.date_time_utc.hour == 8); ok1(nmea_info.date_time_utc.minute == 23); ok1(nmea_info.date_time_utc.second == 11); ok1(equals(nmea_info.location.longitude, 7.693)); ok1(equals(nmea_info.location.latitude, 51.059)); ok1(!nmea_info.baro_altitude_available); /* baro altitude (proprietary Garmin sentence) */ ok1(parser.ParseLine("$PGRMZ,100,m,3*11", nmea_info)); ok1(nmea_info.baro_altitude_available); ok1(equals(nmea_info.baro_altitude, 100)); }
bool DebugReplayNMEA::Next() { last_basic = basic; last_calculated = calculated; const char *line; while ((line = reader->read()) != NULL) { if (basic.time_available) basic.clock = basic.time; if (device == NULL || !device->ParseNMEA(line, basic)) parser.ParseLine(line, basic); if (basic.location_available != last_basic.location_available) { Compute(); return true; } } return false; }
void NMEAParser::TestRoutine(NMEA_INFO *GPS_INFO) { // QUESTION TB: should be moved to test folder!? #ifndef NDEBUG #ifndef GNAV static int i=90; static TCHAR t1[] = TEXT("1,1,1,1"); static TCHAR t2[] = TEXT("1,300,500,220,2,DD927B,0,-4.5,30,-1.4,1"); static TCHAR t3[] = TEXT("0,0,1200,50,2,DD9146,270,-4.5,30,-1.4,1"); // static TCHAR b50[] = TEXT("0,.1,.0,0,0,1.06,0,-222"); // static TCHAR t4[] = TEXT("-3,500,1024,50"); // nmeaParser1.ParseNMEAString_Internal(TEXT("$PTAS1,201,200,02583,000*2A"), GPS_INFO); // nmeaParser1.ParseNMEAString_Internal(TEXT("$GPRMC,082430.00,A,3744.09096,S,14426.16069,E,0.520294.90,301207,,,A*77"), GPS_INFO); // nmeaParser1.ParseNMEAString_Internal(TEXT("$GPGGA,082430.00,3744.09096,S,1426.16069,E,1,08,1.37,157.6,M,-4.9,M,,*5B"), GPS_INFO); QNH=1013.25; double h; double altraw= 5.0; h = AltitudeToQNHAltitude(altraw); QNH = FindQNH(altraw, 50.0); h = AltitudeToQNHAltitude(altraw); i++; if (i>100) { i=0; } if (i<50) { GPS_INFO->FLARM_Available = true; TCHAR ctemp[MAX_NMEA_LEN]; const TCHAR *params[MAX_NMEA_PARAMS]; size_t nr; nr = nmeaParser1.ExtractParameters(t1, ctemp, params, MAX_NMEA_PARAMS); nmeaParser1.PFLAU(t1, params, nr, GPS_INFO); nr = nmeaParser1.ExtractParameters(t2, ctemp, params, MAX_NMEA_PARAMS); nmeaParser1.PFLAA(t2, params, nr, GPS_INFO); nr = nmeaParser1.ExtractParameters(t3, ctemp, params, MAX_NMEA_PARAMS); nmeaParser1.PFLAA(t3, params, nr, GPS_INFO); } #endif #endif }
static void TestFLARM() { NMEAParser parser; NMEAInfo nmea_info; nmea_info.Reset(); nmea_info.clock = fixed(1); ok1(parser.ParseLine("$PFLAU,3,1,1,1,0*50", nmea_info)); ok1(nmea_info.flarm.status.rx == 3); ok1(nmea_info.flarm.status.tx); ok1(nmea_info.flarm.status.gps == FlarmStatus::GPSStatus::GPS_2D); ok1(nmea_info.flarm.status.alarm_level == FlarmTraffic::AlarmType::NONE); ok1(nmea_info.flarm.traffic.GetActiveTrafficCount() == 0); ok1(!nmea_info.flarm.traffic.new_traffic); ok1(parser.ParseLine("$PFLAA,0,100,-150,10,2,DDA85C,123,13,24,1.4,2*7f", nmea_info)); ok1(nmea_info.flarm.traffic.new_traffic); ok1(nmea_info.flarm.traffic.GetActiveTrafficCount() == 1); FlarmId id = FlarmId::Parse("DDA85C", NULL); FlarmTraffic *traffic = nmea_info.flarm.traffic.FindTraffic(id); if (ok1(traffic != NULL)) { ok1(traffic->valid); ok1(traffic->alarm_level == FlarmTraffic::AlarmType::NONE); ok1(equals(traffic->relative_north, 100)); ok1(equals(traffic->relative_east, -150)); ok1(equals(traffic->relative_altitude, 10)); ok1(equals(traffic->track, 123)); ok1(traffic->track_received); ok1(equals(traffic->turn_rate, 13)); ok1(traffic->turn_rate_received); ok1(equals(traffic->speed, 24)); ok1(traffic->speed_received); ok1(equals(traffic->climb_rate, 1.4)); ok1(traffic->climb_rate_received); ok1(traffic->type == FlarmTraffic::AircraftType::TOW_PLANE); ok1(!traffic->stealth); } else { skip(16, 0, "traffic == NULL"); } ok1(parser.ParseLine("$PFLAA,2,20,10,24,2,DEADFF,,,,,1*46", nmea_info)); ok1(nmea_info.flarm.traffic.GetActiveTrafficCount() == 2); id = FlarmId::Parse("DEADFF", NULL); traffic = nmea_info.flarm.traffic.FindTraffic(id); if (ok1(traffic != NULL)) { ok1(traffic->valid); ok1(traffic->alarm_level == FlarmTraffic::AlarmType::IMPORTANT); ok1(equals(traffic->relative_north, 20)); ok1(equals(traffic->relative_east, 10)); ok1(equals(traffic->relative_altitude, 24)); ok1(!traffic->track_received); ok1(!traffic->turn_rate_received); ok1(!traffic->speed_received); ok1(!traffic->climb_rate_received); ok1(traffic->type == FlarmTraffic::AircraftType::GLIDER); ok1(traffic->stealth); } else { skip(12, 0, "traffic == NULL"); } ok1(parser.ParseLine("$PFLAA,0,1206,574,21,2,DDAED5,196,,32,1.0,1*10", nmea_info)); ok1(nmea_info.flarm.traffic.GetActiveTrafficCount() == 3); id = FlarmId::Parse("DDAED5", NULL); traffic = nmea_info.flarm.traffic.FindTraffic(id); if (ok1(traffic != NULL)) { ok1(traffic->valid); ok1(traffic->alarm_level == FlarmTraffic::AlarmType::NONE); ok1(equals(traffic->relative_north, 1206)); ok1(equals(traffic->relative_east, 574)); ok1(equals(traffic->relative_altitude, 21)); ok1(equals(traffic->track, 196)); ok1(traffic->track_received); ok1(!traffic->turn_rate_received); ok1(equals(traffic->speed, 32)); ok1(traffic->speed_received); ok1(equals(traffic->climb_rate, 1.0)); ok1(traffic->climb_rate_received); ok1(traffic->type == FlarmTraffic::AircraftType::GLIDER); ok1(!traffic->stealth); } else { skip(15, 0, "traffic == NULL"); } }
// // Run every 5 seconds, approx. // This is the hearth of LK. Questions? Ask Paolo.. // THIS IS RUNNING WITH LockComm from ConnectionProcessTimer . // void NMEAParser::UpdateMonitor(void) { short active=0; // active port number for gps static short lastactive=0; static bool lastvalidBaro=false; static bool wasSilent[2]={false,false}; short invalidGps=0; short invalidBaro=0; short validBaro=0; // does anyone have GPS? if (nmeaParser1.gpsValid || nmeaParser2.gpsValid) { if (nmeaParser1.gpsValid && nmeaParser2.gpsValid) { // both valid, just use first nmeaParser2.activeGPS = false; nmeaParser1.activeGPS = true; active=1; } else { // only one valid, pick it up nmeaParser1.activeGPS = nmeaParser1.gpsValid; nmeaParser2.activeGPS = nmeaParser2.gpsValid; active= nmeaParser1.activeGPS ? 1 : 2; } } else { // No valid fix on any port. We use the first port with at least some data going through! // This will keep probably at least the time updated since the gps may still be receiving a // valid time, good for us. if ( (LKHearthBeats-ComPortHB[0])<10 ) { // It is not granted that devA is really a GPS source. // Very unlikely, but possible.. if (devIsGPSSource(devA())) active=1; } else { if ( (LKHearthBeats-ComPortHB[1])<10 ) { // portB is really active, although there is no valid fix on it. // Before electing it to gps, lets be sure it really has one! // LKEXT1 and other instruments do not provide GPS source in fact. if (devIsGPSSource(devB())) active=2; } else { // nothing coming in from any port, recently. if (devIsGPSSource(devA())) active=1; // lets keep waiting for the first port } } switch(active) { case 0: nmeaParser1.activeGPS = false; nmeaParser2.activeGPS = false; break; case 1: nmeaParser1.activeGPS = true; nmeaParser2.activeGPS = false; break; case 2: nmeaParser1.activeGPS = false; nmeaParser2.activeGPS = true; break; default: nmeaParser1.activeGPS = false; nmeaParser2.activeGPS = false; LKASSERT(0); break; } } if (nmeaParser2.activeGPS==true && active==1) { StartupStore(_T(".... GPS Update error: port 1 and 2 are active! %s%s"),WhatTimeIsIt(),NEWLINE); nmeaParser2.activeGPS=false; // force it off active=1; } // wait for some seconds before monitoring, after startup if (LKHearthBeats<20) return; /* check if Flarm disappeared after 30 seconds no activity */ if (GPS_INFO.FLARM_Available && ((GPS_INFO.Time -LastFlarmCommandTime)> 30) ) { static unsigned short MessageCnt =0; if(MessageCnt <10) { MessageCnt++; StartupStore(_T(". FLARM lost! Disable FLARM functions !%s"),NEWLINE); DoStatusMessage(gettext(TEXT("_@M947_"))); // _@M947_ "FLARM SIGNAL LOST" } GPS_INFO.FLARM_Available = false; GPS_INFO.FLARM_HW_Version =0.0; GPS_INFO.FLARM_SW_Version =0.0; } // Check Port 1 with no serial activity in last seconds if ( (LKHearthBeats-ComPortHB[0])>10 ) { #ifdef DEBUGNPM StartupStore(_T("... GPS Port 1 : no activity LKHB=%u CBHB=%u %s"),LKHearthBeats, ComPortHB[0],NEWLINE); #endif // if this is active and supposed to have a valid fix.., but no HB.. if ( (active==1) && (nmeaParser1.gpsValid) ) { StartupStore(_T("... GPS Port 1 no hearthbeats, but still gpsValid: forced invalid %s%s"),WhatTimeIsIt(),NEWLINE); } nmeaParser1.gpsValid=false; invalidGps=1; // We want to be sure that if this device is silent, and it was providing Baro altitude, // now it is set to off. if (GPS_INFO.BaroAltitudeAvailable==TRUE) { if ( devA() == pDevPrimaryBaroSource || nmeaParser1.RMZAvailable || nmeaParser1.TASAvailable ) { invalidBaro=1; } } nmeaParser1._Reset(); nmeaParser1.activeGPS=false; // because Reset is setting it to true // We reset some flags globally only once in case of device gone silent if (!devIsDisabled(0) && !wasSilent[0]) { GPS_INFO.AirspeedAvailable=false; GPS_INFO.VarioAvailable=false; GPS_INFO.NettoVarioAvailable=false; GPS_INFO.AccelerationAvailable = false; EnableExternalTriggerCruise = false; wasSilent[0]=true; } } else { wasSilent[0]=false; // We have hearth beats, is baro available? if ( devIsBaroSource(devA()) || nmeaParser1.RMZAvailable || nmeaParser1.TASAvailable ) // 100411 validBaro++; } // now check also port 2 if ( (LKHearthBeats-ComPortHB[1])>10 ) { #ifdef DEBUGNPM StartupStore(_T("... GPS Port 2 : no activity LKHB=%u CBHB=%u %s"),LKHearthBeats, ComPortHB[1],NEWLINE); #endif if ( (active==2) && (nmeaParser2.gpsValid) ) { StartupStore(_T("... GPS port 2 no hearthbeats, but still gpsValid: forced invalid %s%s"),WhatTimeIsIt(),NEWLINE); } nmeaParser2.gpsValid=false; invalidGps++; if (GPS_INFO.BaroAltitudeAvailable==TRUE) { if ( devB() == pDevPrimaryBaroSource || nmeaParser2.RMZAvailable || nmeaParser2.TASAvailable ) { invalidBaro++; } } nmeaParser2._Reset(); nmeaParser2.activeGPS=false; // because Reset is setting it to true if (!devIsDisabled(1) && !wasSilent[1]) { GPS_INFO.AirspeedAvailable=false; GPS_INFO.VarioAvailable=false; GPS_INFO.NettoVarioAvailable=false; GPS_INFO.AccelerationAvailable = false; EnableExternalTriggerCruise = false; wasSilent[1]=true; } } else { wasSilent[1]=false; // We have hearth beats, is baro available? if ( devIsBaroSource(devB()) || nmeaParser2.RMZAvailable || nmeaParser2.TASAvailable ) // 100411 validBaro++; } #ifdef DEBUGNPM if (invalidGps==2) { StartupStore(_T("... GPS no gpsValid available on port 1 and 2, active=%d @%s%s"),active,WhatTimeIsIt(),NEWLINE); } if (invalidBaro>0) { StartupStore(_T("... Baro altitude just lost, current status=%d @%s%s"),GPS_INFO.BaroAltitudeAvailable,WhatTimeIsIt(),NEWLINE); } #endif // do we really still have a baro altitude available? // If some baro source disappeared, let's reset it for safety. Parser will re-enable them immediately if available. // Assuming here that if no Baro is available, no airdata is available also // if (validBaro==0) { if ( GPS_INFO.BaroAltitudeAvailable ) { StartupStore(_T("... GPS no active baro source, and still BaroAltitudeAvailable, forced off %s%s"),WhatTimeIsIt(),NEWLINE); if (EnableNavBaroAltitude && active) { // LKTOKEN _@M122_ = "BARO ALTITUDE NOT AVAILABLE, USING GPS ALTITUDE" DoStatusMessage(MsgToken(122)); PortMonitorMessages++; } else { // LKTOKEN _@M121_ = "BARO ALTITUDE NOT AVAILABLE" DoStatusMessage(MsgToken(121)); } GPS_INFO.BaroAltitudeAvailable=FALSE; // We alse reset these values, just in case we are through a mux GPS_INFO.AirspeedAvailable=false; GPS_INFO.VarioAvailable=false; GPS_INFO.NettoVarioAvailable=false; GPS_INFO.AccelerationAvailable = false; EnableExternalTriggerCruise = false; nmeaParser1._Reset(); nmeaParser2._Reset(); // 120824 Check this situation better> Reset is setting activeGPS true for both devices! lastvalidBaro=false; } } else { if ( lastvalidBaro==false) { #if DEBUGBARO TCHAR devname[50]; if (pDevPrimaryBaroSource) { LK_tcsncpy(devname,pDevPrimaryBaroSource->Name,49); } else { _tcscpy(devname,_T("unknown")); } StartupStore(_T("... GPS baro source back available from <%s>%s"),devname,NEWLINE); #endif if (GotFirstBaroAltitude) { if (EnableNavBaroAltitude) { DoStatusMessage(MsgToken(1796)); // USING BARO ALTITUDE } else { DoStatusMessage(MsgToken(1795)); // BARO ALTITUDE IS AVAILABLE } StartupStore(_T("... GPS baro source back available %s%s"),WhatTimeIsIt(),NEWLINE); lastvalidBaro=true; } else { static bool said=false; if (!said) { StartupStore(_T("... GPS BARO SOURCE PROBLEM, umnanaged port activity. Wrong device? %s%s"),WhatTimeIsIt(),NEWLINE); said=true; } } } else { // last baro was Ok, currently we still have a validbaro, but no HBs... // Probably it is a special case when no gps fix was found on the secondary baro source. if (invalidBaro||!GotFirstBaroAltitude) { GPS_INFO.BaroAltitudeAvailable=FALSE; #ifdef DEBUGNPM StartupStore(_T(".... We still have valid baro, resetting BaroAltitude OFF %s\n"),WhatTimeIsIt()); #endif } } } // Very important check for multiplexers: if RMZ did not get through in the past seconds, we want to // be very sure that there still is one incoming, otherwise we shall be UpdatingBaroSource using the old one, // never really updated!! This is because GGA and RMC are using RMZAvailable to UpdateBaroSource, no matter if // there was a real RMZ in the NMEA stream lately. // Normally RMZAvailable, RMCAvailable, GGA etc.etc. are reset to false when the com port is silent. // But RMZ is special, because it can be sent through the multiplexer from a flarm box. if ( (nmeaParser1.RMZAvailable || nmeaParser2.RMZAvailable) && (LKHearthBeats > (LastRMZHB+5))) { #if DEBUGBARO StartupStore(_T(".... RMZ not updated recently, resetting HB\n")); #endif nmeaParser1.RMZAvailable = FALSE; nmeaParser2.RMZAvailable = FALSE; } // Check baro altitude problems. This can happen for several reasons: mixed input on baro on same port, // faulty device, etc. The important thing is that we shall not be using baro altitude for navigation in such cases. // So we do this check only for the case we are actually using baro altitude. // A typical case is: mixed devices on same port, baro altitude disappearing because of mechanical switch, // but other traffic still incoming, so hearthbeats are ok. static double lastBaroAltitude=-1, lastGPSAltitude=-1; static unsigned int counterSameBaro=0, counterSameHGPS=0; static unsigned short firstrecovery=0; if (GPS_INFO.BaroAltitudeAvailable && EnableNavBaroAltitude && !GPS_INFO.NAVWarning) { if (GPS_INFO.BaroAltitude==lastBaroAltitude) { counterSameBaro++; } else { lastBaroAltitude=GPS_INFO.BaroAltitude; counterSameBaro=0; } if (GPS_INFO.Altitude==lastGPSAltitude) { counterSameHGPS++; } else { lastGPSAltitude=GPS_INFO.Altitude; counterSameHGPS=0; } // This is suspicious enough, because the baro altitude is a floating value, should not be the same.. // but ok, lets assume it is filtered. // if HBAR is steady for some time ... and HGPS is not steady unsigned short timethreshold=15; // first three times, timeout at about 1 minute if (firstrecovery>=3) timethreshold=40; // then about every 3 minutes if ( ((counterSameBaro > timethreshold) && (counterSameHGPS<2)) && (fabs(GPS_INFO.Altitude-GPS_INFO.BaroAltitude)>100.0) && !CALCULATED_INFO.OnGround ) { DoStatusMessage(MsgToken(122)); // Baro not available, Using GPS ALTITUDE EnableNavBaroAltitude=false; StartupStore(_T("... WARNING, NavBaroAltitude DISABLED due to possible fault: baro steady at %f, HGPS=%f @%s%s"), GPS_INFO.BaroAltitude, GPS_INFO.Altitude,WhatTimeIsIt(),NEWLINE); lastBaroAltitude=-1; lastGPSAltitude=-1; counterSameBaro=0; counterSameHGPS=0; // We do only ONE attempt to recover a faulty device, to avoid flipflopping. // In case of big problems, we shall have disabled the use of the faulty baro altitude, and keep it // incoming sporadically. if (firstrecovery<3) { GPS_INFO.BaroAltitudeAvailable=FALSE; // We alse reset these values, just in case we are through a mux GPS_INFO.AirspeedAvailable=false; GPS_INFO.VarioAvailable=false; GPS_INFO.NettoVarioAvailable=false; GPS_INFO.AccelerationAvailable = false; EnableExternalTriggerCruise = false; nmeaParser1._Reset(); nmeaParser2._Reset(); // 120824 Check this situation better> Reset is setting activeGPS true for both devices! lastvalidBaro=false; GotFirstBaroAltitude=false; firstrecovery++; } } } // Set some fine tuning parameters here, depending on device/situation/mode if (ISCAR) trackbearingminspeed=0; // trekking mode/car mode, min speed >0 else trackbearingminspeed=1; // flymode, min speed >1 knot // // Following is for diagnostics only // // Nothing has changed? No need to give new alerts. We might have no active gps at all, also. // In this case, active and lastactive are 0, nothing we can do about it. if (active == lastactive) return; if (active!=0) StartupStore(_T(". GPS NMEA source changed to port %d %s%s"),active,WhatTimeIsIt(),NEWLINE); else StartupStore(_T("... GPS NMEA source PROBLEM, no active GPS! %s%s"),WhatTimeIsIt(),NEWLINE); if (PortMonitorMessages<15) { // do not overload pilot with messages! // do not say anything if we never got the first port, on startup essentially if ((lastactive!=0) && (nmeaParser1.gpsValid || nmeaParser2.gpsValid)){ TCHAR vbuf[100]; _stprintf(vbuf,_T("%s %d"), MsgToken(277),active); // FALLBACK USING GPS ON PORT .. DoStatusMessage(vbuf); PortMonitorMessages++; } } else { if (PortMonitorMessages==15) { StartupStore(_T("... GOING SILENT on too many Com reportings. %s%s"),WhatTimeIsIt(),NEWLINE); DoStatusMessage(MsgToken(317)); // GOING SILENT ON COM REPORTING PortMonitorMessages++; // we go to 16, and never be back here } } lastactive=active; }
/** * Resets both NMEAParsers (Port1 + Port2) */ void NMEAParser::Reset(void) { // clear status nmeaParser1._Reset(); nmeaParser2._Reset(); }