示例#1
0
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)));
}
示例#2
0
文件: Parser.cpp 项目: scottp/xcsoar
/**
 * 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;
}
示例#3
0
文件: Parser.cpp 项目: rkalman/LK8000
void NMEAParser::Reset(void) {

  // clear status
  nmeaParser1._Reset();
  nmeaParser2._Reset();

  // trigger updates
  TriggerGPSUpdate();
  TriggerVarioUpdate();
}
示例#4
0
文件: Parser.cpp 项目: miza/LK8000
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;
}
示例#5
0
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;
}
示例#6
0
文件: Parser.cpp 项目: rkalman/LK8000
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;
}
示例#7
0
/**
 * 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);
}
示例#8
0
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;
}
示例#9
0
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;
}
示例#10
0
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));
}
示例#11
0
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;
}
示例#12
0
文件: Parser.cpp 项目: scottp/xcsoar
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
}
示例#13
0
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");
  }
}
示例#14
0
//
// 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;

}
示例#15
0
文件: Parser.cpp 项目: scottp/xcsoar
/**
 * Resets both NMEAParsers (Port1 + Port2)
 */
void NMEAParser::Reset(void) {
  // clear status
  nmeaParser1._Reset();
  nmeaParser2._Reset();
}