void NMEAParser::Reset(void) { // clear status nmeaParser1._Reset(); nmeaParser2._Reset(); // trigger updates TriggerGPSUpdate(); TriggerVarioUpdate(); }
// // 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(); }