Exemple #1
0
static bool
devInitOne(PDeviceDescriptor_t dev, int index, const TCHAR *port,
           DWORD speed, PDeviceDescriptor_t &nmeaout)
{
  TCHAR DeviceName[DEVNAMESIZE];

  if (is_simulator())
    return FALSE;

  ReadDeviceSettings(index, DeviceName);

  const struct DeviceRegister *Driver = devGetDriver(DeviceName);

  if (Driver) {
    ComPort *Com = new ComPort(dev);

    if (!Com->Initialize(port, speed))
      return FALSE;

    memset(dev->Name, 0, sizeof(dev->Name));
    _tcsncpy(dev->Name, Driver->Name, DEVNAMESIZE);

    dev->Driver = Driver;

    dev->Com = Com;

    devOpen(dev, index);

    if (devIsBaroSource(dev)) {
      if (pDevPrimaryBaroSource == NULL) {
        pDevPrimaryBaroSource = dev;
      } else if (pDevSecondaryBaroSource == NULL) {
        pDevSecondaryBaroSource = dev;
      }
    }

    if (nmeaout == NULL && Driver->Flags & (1l << dfNmeaOut)) {
      nmeaout = dev;
    }
  }
  return TRUE;
}
//
// 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;

}
Exemple #3
0
BOOL devInit(LPCTSTR CommandLine) {
    TCHAR DeviceName[DEVNAMESIZE + 1];
    PDeviceDescriptor_t pDevNmeaOut = NULL;

    TCHAR Port[MAX_PATH] = {_T('\0')};
    DWORD SpeedIndex = 2;
    DWORD BitIndex = (BitIndex_t) bit8N1;

    static bool doinit = true;

    pDevPrimaryBaroSource = NULL;
    pDevSecondaryBaroSource = NULL;

    std::set<std::wstring> UsedPort; // list of already used port
    
    for (unsigned i = 0; i < NUMDEV; i++) {
        DeviceList[i].InitStruct(i);

        ComPortStatus[i] = CPS_UNUSED; // 100210
        ComPortHB[i] = 0; // counter
        if (doinit) {
            ComPortRx[i] = 0;
            ComPortTx[i] = 0;
            ComPortErrTx[i] = 0;
            ComPortErrRx[i] = 0;
            ComPortErrors[i] = 0;

            doinit = false;
        }
        
        if (SIMMODE){
            continue;
        }
        
        ReadDeviceSettings(i, DeviceName);
        DeviceList[i].Disabled = (wcscmp(DeviceName, _T(DEV_DISABLED_NAME)) == 0);
        if (DeviceList[i].Disabled) {
            StartupStore(_T(". Device %c is DISABLED.%s"), (_T('A') + i), NEWLINE);
            continue;
        }

        DeviceRegister_t* pDev = std::find_if(&DeviceRegister[0], &DeviceRegister[DeviceRegisterCount], devNameCompare(DeviceName));
        if (pDev == &DeviceRegister[DeviceRegisterCount]) {
            DeviceList[i].Disabled = true;
            StartupStore(_T(". Device %c : invalide drivers name <%s>%s"), (_T('A') + i), DeviceName, NEWLINE);
            continue;
        }
        if(_tcscmp(pDev->Name,TEXT("Internal")) == 0) {
            _tcscpy(Port, _T("GPSID"));
        } else { 
            Port[0] = _T('\0');
            SpeedIndex = 2;
            BitIndex = (BitIndex_t) bit8N1;
            ReadPortSettings(i, Port, &SpeedIndex, &BitIndex);
        }
        // remember: Port1 is the port used by device A, port1 may be Com3 or Com1 etc

        if(std::find(UsedPort.begin(), UsedPort.end(), Port) != UsedPort.end()) {
            StartupStore(_T(". Port <%s> Already used, Device %c Disabled ! %s"), Port, (_T('A') + i), NEWLINE);
            continue;
        }
        UsedPort.insert(Port);
        
        // remember: Port1 is the port used by device A, port1 may be Com3 or Com1 etc
        StartupStore(_T(". Device %c is <%s> Port=%s%s"), (_T('A') + i), DeviceName, Port, NEWLINE);
        
        ComPort *Com = NULL;
        if (_tcsncmp(Port, _T("BT:"), 3) == 0) {
            CBtHandler* pBtHandler = CBtHandler::Get();
            StartupStore(_T(".. Initialise Bluetooth Device %s%s"), Port, NEWLINE);
            if (pBtHandler && pBtHandler->IsOk()) {
                if (pBtHandler->StartHW()) {
                    Com = new BthPort(i, &Port[3]);
                }
            }
        } else if (_tcscmp(Port, _T("GPSID")) == 0) {
            Com = new GpsIdPort(i, Port);
        } else {
            Com = new SerialPort(i, Port, dwSpeed[SpeedIndex], (BitIndex_t)BitIndex, PollingMode);
        }

        if (Com && Com->Initialize()) {
            ComPortStatus[i] = CPS_OPENOK;
            pDev->Installer(&DeviceList[i]);

            if ((pDevNmeaOut == NULL) && (pDev->Flags & (1l << dfNmeaOut))) {
                pDevNmeaOut = &DeviceList[i];
            }

            DeviceList[i].Com = Com;

            devInit(&DeviceList[i]);
            devOpen(&DeviceList[i], i);

            if (devIsBaroSource(&DeviceList[i])) {
                if (pDevPrimaryBaroSource == NULL) {
                    pDevPrimaryBaroSource = &DeviceList[i];
                } else if (pDevSecondaryBaroSource == NULL) {
                    pDevSecondaryBaroSource = &DeviceList[i];
                }
            }
        } else {
            delete Com;
            ComPortStatus[i] = CPS_OPENKO;
        }
    }

    if (pDevNmeaOut != NULL) {
        if (pDevNmeaOut == devA()) {
            devB()->pDevPipeTo = devA();
        }
        if (pDevNmeaOut == devB()) {
            devA()->pDevPipeTo = devB();
        }
    }

    return (TRUE);
}
Exemple #4
0
BOOL devInit(LPTSTR CommandLine){
  int i;
  TCHAR DeviceName[DEVNAMESIZE+1];
  PDeviceDescriptor_t pDevNmeaOut = NULL;
  static bool doinit=true;

  for (i=0; i<NUMDEV; i++){
    DeviceList[i].Port = -1;
    DeviceList[i].fhLogFile = NULL;
    DeviceList[i].Name[0] = '\0';
    DeviceList[i].ParseNMEA = NULL;
    DeviceList[i].PutMacCready = NULL;
    DeviceList[i].DirectLink = NULL;
    DeviceList[i].PutBugs = NULL;
    DeviceList[i].PutBallast = NULL;
    DeviceList[i].Open = NULL;
    DeviceList[i].Close = NULL;
    DeviceList[i].Init = NULL;
    DeviceList[i].LinkTimeout = NULL;
    DeviceList[i].Declare = NULL;
    DeviceList[i].IsLogger = devIsFalseReturn;
    DeviceList[i].IsGPSSource = devIsFalseReturn;
    DeviceList[i].IsBaroSource = devIsFalseReturn;
    DeviceList[i].IsRadio = devIsFalseReturn;

    DeviceList[i].PutVoice = (int (*)(struct DeviceDescriptor_t *,TCHAR *))devIsFalseReturn;
    DeviceList[i].PortNumber = i;
    DeviceList[i].PutQNH = NULL;
    DeviceList[i].OnSysTicker = NULL;

    DeviceList[i].pDevPipeTo = NULL;
    DeviceList[i].PutVolume = NULL;
    DeviceList[i].PutFreqActive = NULL;
    DeviceList[i].PutFreqStandby = NULL;
    DeviceList[i].IsCondor = devIsFalseReturn;
    DeviceList[i].Disabled = true;

    ComPortStatus[i]=CPS_UNUSED; // 100210
    ComPortHB[i]=0; // counter
    if (doinit) {
	ComPortRx[i]=0;
	ComPortTx[i]=0;
	ComPortErrTx[i]=0;
	ComPortErrRx[i]=0;
	ComPortErrors[i]=0;

	doinit=false;
    }
  }

  pDevPrimaryBaroSource = NULL;
  pDevSecondaryBaroSource=NULL;

  ReadDeviceSettings(0, DeviceName);
  #ifdef DEBUG_DEVSETTING
  StartupStore(_T(".......... ReadDeviceSetting 0, DeviceName=<%s>\n"),DeviceName);
  #endif
	
  PortIndex1 = 0; SpeedIndex1 = 2; Bit1Index=(BitIndex_t)bit8N1;
  ReadPort1Settings(&PortIndex1,&SpeedIndex1,&Bit1Index);

  //if (_tcslen(DeviceName)>0) // removed 110530
  if (wcscmp(DeviceName,_T(DEV_DISABLED_NAME))!=0) {
	DeviceList[0].Disabled=false;
	StartupStore(_T(". Device A is <%s> Port=%s%s"),DeviceName,COMMPort[PortIndex1],NEWLINE);
  } else {
	DeviceList[0].Disabled=true;
	StartupStore(_T(". Device A is DISABLED.%s"),NEWLINE);
  }

  for (i=DeviceRegisterCount-1; i>=0; i--) {
    if (DeviceList[0].Disabled) break;

    if ((_tcscmp(DeviceRegister[i].Name, DeviceName) == 0)) {

      ComPort *Com = new ComPort(0);

      // remember: Port1 is the port used by device A, port1 may be Com3 or Com1 etc
	// this is port 1, so index 0 for us. 
      if (!Com->Initialize(COMMPort[PortIndex1], dwSpeed[SpeedIndex1],Bit1Index,0)) {
	   	delete Com;
		ComPortStatus[0]=CPS_OPENKO;
        break;
      }
      ComPortStatus[0]=CPS_OPENOK;

      DeviceRegister[i].Installer(devA());

      if ((pDevNmeaOut == NULL) && 
	  (DeviceRegister[i].Flags & (1l << dfNmeaOut))){
        pDevNmeaOut = devA();
      }

      devA()->Com = Com;

      devInit(devA());
      devOpen(devA(), 0);

      if (devIsBaroSource(devA())) {
        if (pDevPrimaryBaroSource == NULL){
          pDevPrimaryBaroSource = devA();
        } else 
        if (pDevSecondaryBaroSource == NULL){
          pDevSecondaryBaroSource = devA();
        }
      }
      break;
    }
  }

  ReadDeviceSettings(1, DeviceName);
  #ifdef DEBUG_DEVSETTING
  StartupStore(_T(".......... ReadDeviceSetting 1, DeviceName=<%s>\n"),DeviceName);
  #endif

  PortIndex2 = 0; SpeedIndex2 = 2, Bit2Index=(BitIndex_t)bit8N1;
  ReadPort2Settings(&PortIndex2,&SpeedIndex2, &Bit2Index);

  //if (_tcslen(DeviceName)>0) // removed 110530
  if (wcscmp(DeviceName,_T(DEV_DISABLED_NAME))!=0) {
	DeviceList[1].Disabled=false;
	StartupStore(_T(". Device B is <%s> Port=%s%s"),DeviceName,COMMPort[PortIndex2],NEWLINE);
  } else {
	DeviceList[1].Disabled=true;
	StartupStore(_T(". Device B is DISABLED.%s"),NEWLINE);
  }

  for (i=DeviceRegisterCount-1; i>=0; i--) {
    if (PortIndex1 == PortIndex2) break;
    if (DeviceList[1].Disabled) break;

    if ((_tcscmp(DeviceRegister[i].Name, DeviceName) == 0)) {
      ComPort *Com = new ComPort(1);

	// this is port 2, so index 1 for us
      if (!Com->Initialize(COMMPort[PortIndex2], dwSpeed[SpeedIndex2],Bit2Index,1)) { // 100210
	delete Com;
	ComPortStatus[1]=CPS_OPENKO;
        break;
      }
      ComPortStatus[1]=CPS_OPENOK;

      DeviceRegister[i].Installer(devB());

      if ((pDevNmeaOut == NULL) && 
          (DeviceRegister[i].Flags & (1l << dfNmeaOut))){
        pDevNmeaOut = devB();
      }

      devB()->Com = Com;

      devInit(devB());
      devOpen(devB(), 1);

      if (devIsBaroSource(devB())) {
        if (pDevPrimaryBaroSource == NULL){
          pDevPrimaryBaroSource = devB();
        } else 
        if (pDevSecondaryBaroSource == NULL){
          pDevSecondaryBaroSource = devB();
        }
      }

      break;
    }
  }

  if (pDevNmeaOut != NULL){
    if (pDevNmeaOut == devA()){
      devB()->pDevPipeTo = devA();
    }
    if (pDevNmeaOut == devB()){
      devA()->pDevPipeTo = devB();
    }
  }

  return(TRUE);
}
Exemple #5
0
//#define DEBUGNPM 1
// run every 5 seconds, approx.
void NMEAParser::UpdateMonitor(void) 
{
  short active;
  static short lastactive=0;
  static bool  lastvalidBaro=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 {
		nmeaParser1.activeGPS = nmeaParser1.gpsValid;
		nmeaParser2.activeGPS = nmeaParser2.gpsValid;
		active= nmeaParser1.activeGPS ? 1 : 2;
	}
  } else {
	// assume device 1 is active
	nmeaParser2.activeGPS = false;
	nmeaParser1.activeGPS = true;
	active=1;
  }
 #if 1	// TODO better check if ok
  if (nmeaParser2.activeGPS==true && active==1) {
	StartupStore(_T("... GPS Update error: port 1 and 2 are active!%s"),NEWLINE);
	FailStore(_T("... GPS Update error: port 1 and 2 are active!%s"),NEWLINE);
	nmeaParser2.activeGPS=false; // force it off
	active=1; 
  }
 #endif

  // wait for some seconds before monitoring, after startup
  if (LKHearthBeats<20) return;
  // 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=%.0f CBHB=%.0f %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"),NEWLINE);
	}
	nmeaParser1.gpsValid=false;
	invalidGps=1;
	#if DUALBARO
	// 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.RMAAvailable || nmeaParser1.TASAvailable ) {
			invalidBaro=1;
		}
	}
	#endif
  } else {
	// We have hearth beats, is baro available?
	if ( devIsBaroSource(devA()) || nmeaParser1.RMZAvailable || nmeaParser1.RMAAvailable || nmeaParser1.TASAvailable ) // 100411
		validBaro++;
  }
  // now check also port 2
  if ( (LKHearthBeats-ComPortHB[1])>10 ) {
	#ifdef DEBUGNPM
	StartupStore(_T("... GPS Port 2 : no activity LKHB=%.0f CBHB=%.0f %s"),LKHearthBeats, ComPortHB[1],NEWLINE);
	#endif
	if ( (active==2) && (nmeaParser2.gpsValid) ) {
		StartupStore(_T("... GPS port 2 no hearthbeats, but still gpsValid: forced invalid%s"),NEWLINE);
	}
	nmeaParser2.gpsValid=false;
	invalidGps++;
	#if DUALBARO
	if (GPS_INFO.BaroAltitudeAvailable==TRUE) {
		if ( devB() == pDevPrimaryBaroSource || nmeaParser2.RMZAvailable 
		  || nmeaParser2.RMAAvailable || nmeaParser2.TASAvailable ) {
			invalidBaro++;
		}
	}
	#endif
  } else {
	// We have hearth beats, is baro available?
	if ( devIsBaroSource(devB()) || nmeaParser2.RMZAvailable || nmeaParser2.RMAAvailable || nmeaParser2.TASAvailable   )  // 100411
		validBaro++;
  }

  #ifdef DEBUGNPM
  if (invalidGps==2) {
	StartupStore(_T("... GPS no gpsValid available on port 1 and 2, active=%d%s"),active,NEWLINE);
  }
  if (invalidBaro>0) {
	StartupStore(_T("... Baro altitude just lost, current status=%d%s"),GPS_INFO.BaroAltitudeAvailable,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"),NEWLINE);
		if (EnableNavBaroAltitude) {
	// LKTOKEN  _@M122_ = "BARO ALTITUDE NOT AVAILABLE, USING GPS ALTITUDE" 
			DoStatusMessage(gettext(TEXT("_@M122_")));
			PortMonitorMessages++;	// 100911
		} else
	// LKTOKEN  _@M121_ = "BARO ALTITUDE NOT AVAILABLE" 
			DoStatusMessage(gettext(TEXT("_@M121_")));
		GPS_INFO.BaroAltitudeAvailable=false;
		GPS_INFO.AirspeedAvailable=false;
		GPS_INFO.VarioAvailable=false;
		GPS_INFO.NettoVarioAvailable=false;
		lastvalidBaro=false;
	}
  } else {
	if ( lastvalidBaro==false) {
		StartupStore(_T("... GPS baro source back available%s"),NEWLINE);
		if (EnableNavBaroAltitude)
	// LKTOKEN  _@M755_ = "USING AVAILABLE BARO ALTITUDE" 
			DoStatusMessage(gettext(TEXT("_@M755_")));
		else
	// LKTOKEN  _@M120_ = "BARO ALTITUDE IS AVAILABLE" 
			DoStatusMessage(gettext(TEXT("_@M120_")));
		lastvalidBaro=true;
	} 
	#if DUALBARO
	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) {
			GPS_INFO.BaroAltitudeAvailable=FALSE;
			#ifdef DEBUGNPM
			StartupStore(_T(".... We still have valid baro, resetting BaroAltitude OFF\n"));
			#endif
		}
	}
	#endif
  }

  // Following diagnostics only
  if (active == lastactive) return;
  if (lastactive==0) {
	lastactive=active;
	StartupStore(_T(". GPS NMEA init delegated to port %d%s"),active,NEWLINE);
	return;
  }

  lastactive=active;
  // in case of no gps at all, port 1 is selected but we dont want to tell unless really working
  if (PortMonitorMessages<10) { // 100221 do not overload pilot with messages!
	StartupStore(_T("... GPS NMEA source changed to port %d %s"),active,NEWLINE);
	if (nmeaParser1.gpsValid || nmeaParser2.gpsValid){
		TCHAR vbuf[100]; _stprintf(vbuf,_T("%s %d"),
	// LKTOKEN  _@M277_ = "FALLBACK USING GPS ON PORT" 
		gettext(TEXT("_@M277_")),active);
		DoStatusMessage(vbuf);
	}
	PortMonitorMessages++;
  } else {
	if (PortMonitorMessages==10) { // 100221
		StartupStore(_T("... GOING SILENT on too many Com reportings.%s"),NEWLINE);
	// LKTOKEN  _@M317_ = "GOING SILENT ON COM REPORTING" 
		DoStatusMessage(gettext(TEXT("_@M317_")));
		PortMonitorMessages++;
	} else
		PortMonitorMessages++;
  }


}