Esempio n. 1
0
/**
 * Attempt to compute airspeed when it is not yet available from:
 * 1) dynamic pressure and air density derived from some altitude.
 * 2) pitot pressure and static pressure.
 * 3) ground speed and wind.
 */
static void
ComputeAirspeed(NMEAInfo &basic, const DerivedInfo &calculated)
{
  if (basic.airspeed_available && basic.airspeed_real)
    /* got it already */
    return;

  const auto any_altitude = basic.GetAnyAltitude();

  if (!basic.airspeed_available && any_altitude.first) {
    fixed dyn; bool available = false;
    if (basic.dyn_pressure_available) {
      dyn = basic.dyn_pressure.GetHectoPascal();
      available = true;
    } else if (basic.pitot_pressure_available && basic.static_pressure_available) {
      dyn = basic.pitot_pressure.GetHectoPascal() - basic.static_pressure.GetHectoPascal();
      available = true;
    }
    if (available) {
      basic.indicated_airspeed = sqrt(fixed(163.2653061) * dyn);
      basic.true_airspeed = basic.indicated_airspeed *
                            AirDensityRatio(any_altitude.second);

      basic.airspeed_available.Update(basic.clock);
      basic.airspeed_real = true; // Anyway not less real then any other method.
      return;
    }
  }

  if (!basic.ground_speed_available || !calculated.wind_available ||
      !calculated.flight.flying) {
    /* impossible to calculate */
    basic.airspeed_available.Clear();
    return;
  }

  fixed TrueAirspeedEstimated = fixed(0);

  const SpeedVector wind = calculated.wind;
  if (positive(basic.ground_speed) || wind.IsNonZero()) {
    fixed x0 = basic.track.fastsine() * basic.ground_speed;
    fixed y0 = basic.track.fastcosine() * basic.ground_speed;
    x0 += wind.bearing.fastsine() * wind.norm;
    y0 += wind.bearing.fastcosine() * wind.norm;

    TrueAirspeedEstimated = SmallHypot(x0, y0);
  }

  basic.true_airspeed = TrueAirspeedEstimated;

  basic.indicated_airspeed = TrueAirspeedEstimated;
  if (any_altitude.first)
    basic.indicated_airspeed /= AirDensityRatio(any_altitude.second);

  basic.airspeed_available.Update(basic.clock);
  basic.airspeed_real = false;
}
Esempio n. 2
0
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
/// Parses LXWP0 sentence.
///
/// @param d         device descriptor
/// @param sentence  received NMEA sentence
/// @param info      GPS info to be updated
///
/// @retval true if the sentence has been parsed
///
//static
bool DevLX::LXWP0(PDeviceDescriptor_t d, const TCHAR* sentence, NMEA_INFO* info)
{
  // $LXWP0,logger_stored, airspeed, airaltitude,
  //   v1[0],v1[1],v1[2],v1[3],v1[4],v1[5], hdg, windspeed*CS<CR><LF>
  //
  // 0 loger_stored : [Y|N] (not used in LX1600)
  // 1 IAS [km/h] ----> Condor uses TAS!
  // 2 baroaltitude [m]
  // 3-8 vario values [m/s] (last 6 measurements in last second)
  // 9 heading of plane (not used in LX1600)
  // 10 windcourse [deg] (not used in LX1600)
  // 11 windspeed [km/h] (not used in LX1600)
  //
  // e.g.:
  // $LXWP0,Y,222.3,1665.5,1.71,,,,,,239,174,10.1

  double alt=0, airspeed=0;

  if (ParToDouble(sentence, 1, &airspeed))
  {
    airspeed /= TOKPH;
    info->TrueAirspeed = airspeed;
    info->AirspeedAvailable = TRUE;
  }

  if (ParToDouble(sentence, 2, &alt))
  {
    if (airspeed>0) {
      LKASSERT(AirDensityRatio(alt)!=0);
      info->IndicatedAirspeed = airspeed / AirDensityRatio(alt);
    }

    UpdateBaroSource( info, 0,d,  AltitudeToQNHAltitude(alt));
  }

  if (ParToDouble(sentence, 8, &info->Vario)) /* take the last value to be more recent */
    info->VarioAvailable = TRUE;

  if (ParToDouble(sentence, 9, &info->MagneticHeading)) 
      info->MagneticHeadingAvailable=TRUE;

  if (ParToDouble(sentence, 10, &info->ExternalWindDirection) &&
      ParToDouble(sentence, 11, &info->ExternalWindSpeed))
  {
	info->ExternalWindSpeed /= TOKPH;  /* convert to m/s */
    info->ExternalWindAvailable = TRUE;
  }
  TriggerVarioUpdate();

  return(true);
} // LXWP0()
Esempio n. 3
0
static BOOL PZAN2(PDeviceDescriptor_t d, TCHAR *String, NMEA_INFO *apGPS)
{
  TCHAR ctemp[80];
  double vtas, wnet, vias;

  NMEAParser::ExtractParameter(String,ctemp,0);
  vtas = StrToDouble(ctemp,NULL)/3.6;
  
  NMEAParser::ExtractParameter(String,ctemp,1);
  wnet = (StrToDouble(ctemp,NULL)-10000)/100; // cm/s
  apGPS->Vario = wnet;


  if (apGPS->BaroAltitudeAvailable)
  {
    vias = vtas/AirDensityRatio(AltitudeToQNEAltitude(apGPS->BaroAltitude));

  } else {
    vias = 0.0;
  }

  apGPS->AirspeedAvailable = TRUE;
  apGPS->TrueAirspeed = vtas;
  apGPS->IndicatedAirspeed = vias;
  apGPS->VarioAvailable = TRUE;

  TriggerVarioUpdate();

  return TRUE;
}
Esempio n. 4
0
void
NMEAInfo::ProvideIndicatedAirspeedWithAltitude(double ias, double altitude)
{
  indicated_airspeed = ias;
  true_airspeed = indicated_airspeed * AirDensityRatio(altitude);
  airspeed_available.Update(clock);
  airspeed_real = true;
}
Esempio n. 5
0
void
NMEAInfo::ProvideTrueAirspeedWithAltitude(double tas, double altitude)
{
  true_airspeed = tas;
  indicated_airspeed = true_airspeed / AirDensityRatio(altitude);
  airspeed_available.Update(clock);
  airspeed_real = true;
}
Esempio n. 6
0
bool DevLXV7_EXP::PLXVF(PDeviceDescriptor_t d, const TCHAR* sentence, NMEA_INFO* info)
{

  double alt, airspeed;


  if (ParToDouble(sentence, 1, &info->AccelX))
    if (ParToDouble(sentence, 2, &info->AccelY))
      if (ParToDouble(sentence, 3, &info->AccelZ))
        info->AccelerationAvailable = true;

  if (ParToDouble(sentence, 5, &airspeed))
  {
//	airspeed = 135.0/TOKPH;
	info->IndicatedAirspeed = airspeed;
	info->AirspeedAvailable = TRUE;

  }

  if (ParToDouble(sentence, 6, &alt))
  {
	UpdateBaroSource( info, 0, d, AltitudeToQNHAltitude(alt));
    info->TrueAirspeed =  airspeed * AirDensityRatio(alt);
  }

  if (ParToDouble(sentence, 4, &info->Vario))
  {
	info->VarioAvailable = TRUE;
	TriggerVarioUpdate();
  }


  // Get STF switch
double fTmp;
if (ParToDouble(sentence, 7, &fTmp))
{
int  iTmp = (int)(fTmp+0.1);
EnableExternalTriggerCruise = true;

static int  iOldVarioSwitch=0;
if(iTmp != iOldVarioSwitch)
{
  iOldVarioSwitch = iTmp;
  if(iTmp==1)
  {
    ExternalTriggerCruise = true;
    ExternalTriggerCircling = false;
  }
  else
  {
    ExternalTriggerCruise = false;
    ExternalTriggerCircling = true;
  }
}
}

  return(true);
} // PLXVF()
Esempio n. 7
0
static BOOL VMVABD(PDeviceDescriptor_t d, TCHAR *String, NMEA_INFO *GPS_INFO)
{
/*
	$VMVABD,  
	0000.0 gps altitude,		0
	M,				1
	0000.0 bari altitude,		2
	M,				3
	-0.0 vario ms,,,		4
	MS,				7
	0.0 ias or tas,			8
	KH,				9
	22.4 temp,			10
	C*nn

	091129
	The Brauniger Compeo can send TAS if a pitot is connected (delta hang gliders) or IAS is rotary device for para.
	Since rotary is seldom used, we assume TAS is received. No indication from NMEA about IAS or TAS selected.
	100114 IAS or TAS???
*/

  TCHAR ctemp[80];
  double vtas, vias;

  NMEAParser::ExtractParameter(String,ctemp,0);
  GPS_INFO->Altitude = StrToDouble(ctemp,NULL);

  NMEAParser::ExtractParameter(String,ctemp,2);
  if (d == pDevPrimaryBaroSource) {
	  GPS_INFO->BaroAltitude = AltitudeToQNHAltitude(StrToDouble(ctemp,NULL));
	  GPS_INFO->BaroAltitudeAvailable = TRUE;
  }

  NMEAParser::ExtractParameter(String,ctemp,4);
  GPS_INFO->Vario = StrToDouble(ctemp,NULL);
  GPS_INFO->VarioAvailable = TRUE;

  NMEAParser::ExtractParameter(String,ctemp,8);
  if (ctemp[0] != '\0') { // 100209
	// we store m/s  , so we convert it from kmh
	vias = StrToDouble(ctemp,NULL)/3.6;
	GPS_INFO->IndicatedAirspeed = vias;
	// Check if zero?
	vtas = vias*AirDensityRatio(GPS_INFO->BaroAltitude);
	GPS_INFO->TrueAirspeed = vtas;

	if (GPS_INFO->IndicatedAirspeed >0) 
		GPS_INFO->AirspeedAvailable = TRUE;
	else 
		GPS_INFO->AirspeedAvailable = FALSE;
  } else
		GPS_INFO->AirspeedAvailable = FALSE;


  TriggerVarioUpdate();

  return TRUE;
}
Esempio n. 8
0
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
/// Parses LXWP0 sentence.
///
/// @param d         device descriptor
/// @param sentence  received NMEA sentence
/// @param info      GPS info to be updated
///
/// @retval true if the sentence has been parsed
///
//static
bool DevLXMiniMap::LXWP0(PDeviceDescriptor_t d, const TCHAR* sentence, NMEA_INFO* info)
{
  // $LXWP0,logger_stored, airspeed, airaltitude,
  //   v1[0],v1[1],v1[2],v1[3],v1[4],v1[5], hdg, windspeed*CS<CR><LF>
  //
  // 0 loger_stored : [Y|N] (not used in LX1600)
  // 1 IAS [km/h] ----> Condor uses TAS!
  // 2 baroaltitude [m]
  // 3-8 vario values [m/s] (last 6 measurements in last second)
  // 9 heading of plane (not used in LX1600)
  // 10 windcourse [deg] (not used in LX1600)
  // 11 windspeed [km/h] (not used in LX1600)
  //
  // e.g.:
  // $LXWP0,Y,222.3,1665.5,1.71,,,,,,239,174,10.1

  TICKER = GetTickCount();

  double alt, airspeed;

  if (ParToDouble(sentence, 1, &airspeed))
  {
    airspeed /= TOKPH;
    info->TrueAirspeed = airspeed;
    info->AirspeedAvailable = TRUE;


  }

  if (ParToDouble(sentence, 2, &alt))
  {
    info->IndicatedAirspeed = airspeed / AirDensityRatio(alt);

    if (d == pDevPrimaryBaroSource)
    {
      UpdateQNH(CalculateQNH(alt, alt + AltOffset));
      UpdateBaroSource(info, 0, d, alt + AltOffset);


     /* if(FirstCheckBaroAlt)
      {
      FirstCheckBaroAlt = false;
      InputEvents::eventSetup(_T("Basic"));
      }*/
    }
  }

  if (ParToDouble(sentence, 3, &info->Vario))
    info->VarioAvailable = TRUE;

  if (ParToDouble(sentence, 10, &info->ExternalWindDirection) &&
      ParToDouble(sentence, 11, &info->ExternalWindSpeed))
    info->ExternalWindAvailable = TRUE;

  TriggerVarioUpdate();

  return(true);
} // LXWP0()
Esempio n. 9
0
//
// 6 march 2014 by Bruno and Paolo
// This is to correct sinkrate adjusted for air density
//
double AirDensitySinkRate(double ias, double qnhaltitude) {

    double sinkias=0;

    sinkias=GlidePolar::SinkRate(ias)*AirDensityRatio(AltitudeToQNEAltitude(qnhaltitude));

    LKASSERT(sinkias<=0);
    if (sinkias>0) sinkias=0;
    return sinkias;
}
Esempio n. 10
0
double AirDensitySinkRate(double ias, double qnhaltitude, double gload) {

    double w0 = GlidePolar::SinkRate(GlidePolar::polar_a,GlidePolar::polar_b,GlidePolar::polar_c,0.0,0.0,ias);
    w0 *= AirDensityRatio(AltitudeToQNEAltitude(qnhaltitude));
    gload = max(0.1,fabs(gload));
    double v2 = GlidePolar::Vbestld()/max((double)GlidePolar::Vbestld()/2,ias);

    LKASSERT(GlidePolar::bestld!=0);
    if (GlidePolar::bestld==0) return -1; // UNMANAGED
    return w0-(ias/(2*GlidePolar::bestld))* (gload*gload-1)*(v2*v2);
}
Esempio n. 11
0
//
// 6 march 2014 by Bruno and Paolo
// This is to correct sinkrate adjusted for air density
//
double AirDensitySinkRate(double ias, double qnhaltitude) {

    double sinkias=0;

    sinkias=GlidePolar::SinkRate(ias)*AirDensityRatio(AltitudeToQNEAltitude(qnhaltitude));

    // this can actually happen with a bad polar file loaded!
    BUGSTOP_LKASSERT(sinkias<=0);
    if (sinkias>0) sinkias=0;
    return sinkias;
}
Esempio n. 12
0
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
/// Parses LXWP0 sentence.
///
/// @param d         device descriptor
/// @param sentence  received NMEA sentence
/// @param info      GPS info to be updated
///
/// @retval true if the sentence has been parsed
///
//static
bool DevLX16xx::LXWP0(PDeviceDescriptor_t d, const TCHAR* sentence, NMEA_INFO* info)
{
  // $LXWP0,logger_stored, airspeed, airaltitude,
  //   v1[0],v1[1],v1[2],v1[3],v1[4],v1[5], hdg, windspeed*CS<CR><LF>
  //
  // 0 loger_stored : [Y|N] (not used in LX1600)
  // 1 IAS [km/h] ----> Condor uses TAS!
  // 2 baroaltitude [m]
  // 3-8 vario values [m/s] (last 6 measurements in last second)
  // 9 heading of plane (not used in LX1600)
  // 10 windcourse [deg] (not used in LX1600)
  // 11 windspeed [km/h] (not used in LX1600)
  //
  // e.g.:
  // $LXWP0,Y,222.3,1665.5,1.71,,,,,,239,174,10.1

  double alt, airspeed;

  if (ParToDouble(sentence, 1, &airspeed))
  {
    airspeed /= TOKPH;
    info->TrueAirspeed = airspeed;
    info->AirspeedAvailable = TRUE;
  }
  if(LX166AltitudeUpdateTimeout >0)
	  LX166AltitudeUpdateTimeout--;
  else
    if (ParToDouble(sentence, 2, &alt))
    {
    	LX16xxAlt = (int) alt;
      info->IndicatedAirspeed = airspeed / AirDensityRatio(alt);
      UpdateBaroSource( info, 0,d, AltitudeToQNHAltitude(alt));
    }

  if (ParToDouble(sentence, 3, &info->Vario))
    info->VarioAvailable = TRUE;


 /*
  if (ParToDouble(sentence, 10, &info->ExternalWindDirection) &&
      ParToDouble(sentence, 11, &info->ExternalWindSpeed))
    info->ExternalWindAvailable = TRUE;
*/
  TriggerVarioUpdate();

  return(true);
} // LXWP0()
Esempio n. 13
0
// LK8000 IAS , in m/s*10  example: 346 for 34.6 m/s  which is = 124.56 km/h
BOOL NMEAParser::PLKAS(TCHAR *String, TCHAR **params, size_t nparams, NMEA_INFO *pGPS)
{
  (void)pGPS;

  double vias=StrToDouble(params[0],NULL)/10.0;
  if (vias >1) {
    pGPS->TrueAirspeed = vias*AirDensityRatio(CALCULATED_INFO.NavAltitude);
    pGPS->IndicatedAirspeed = vias;
  } else {
    pGPS->TrueAirspeed = 0;
    pGPS->IndicatedAirspeed = 0;
  }

  pGPS->AirspeedAvailable = TRUE;

  return FALSE;
}
Esempio n. 14
0
static BOOL cLXWP0(PDeviceDescriptor_t d, const TCHAR *String,
                   NMEA_INFO *GPS_INFO) {
  TCHAR ctemp[80];

  /*
  $LXWP0,Y,222.3,1665.5,1.71,,,,,,239,174,10.1

   0 loger_stored (Y/N)
   1 IAS (kph) ----> Condor uses TAS!
   2 baroaltitude (m)
   3 vario (m/s)
   4-8 unknown
   9 heading of plane
  10 windcourse (deg)
  11 windspeed (kph)

  */
  double alt, airspeed;

  NMEAParser::ExtractParameter(String,ctemp,1);
  airspeed = StrToDouble(ctemp,NULL)/TOKPH;

  NMEAParser::ExtractParameter(String,ctemp,2);
  alt = StrToDouble(ctemp,NULL);

  GPS_INFO->IndicatedAirspeed = airspeed/AirDensityRatio(alt);
  GPS_INFO->TrueAirspeed = airspeed;

  if (d == pDevPrimaryBaroSource){
    GPS_INFO->BaroAltitudeAvailable = TRUE;
    GPS_INFO->BaroAltitude = alt;    // ToDo check if QNH correction is needed!
  }

  NMEAParser::ExtractParameter(String,ctemp,3);
  GPS_INFO->Vario = StrToDouble(ctemp,NULL);

  GPS_INFO->AirspeedAvailable = TRUE;
  GPS_INFO->VarioAvailable = TRUE;

  TriggerVarioUpdate();

  return TRUE;
}
Esempio n. 15
0
// TASMAN instruments support for Tasman Flight Pack model Fp10
BOOL NMEAParser::PTAS1(TCHAR *String, TCHAR **params, size_t nparams, NMEA_INFO *pGPS)
{
  double wnet,baralt,vtas;

  wnet = (StrToDouble(params[0],NULL)-200)/(10*TOKNOTS);
  baralt = max(0.0, (StrToDouble(params[2],NULL)-2000)/TOFEET);
  vtas = StrToDouble(params[3],NULL)/TOKNOTS;
  
  pGPS->AirspeedAvailable = TRUE;
  pGPS->TrueAirspeed = vtas;
  pGPS->VarioAvailable = TRUE;
  pGPS->Vario = wnet;
  UpdateBaroSource(pGPS, BARO__TASMAN, NULL,  AltitudeToQNHAltitude(baralt));
  pGPS->IndicatedAirspeed = vtas/AirDensityRatio(baralt);
 
  TASAvailable = true; // 100411 
  TriggerVarioUpdate();

  return FALSE;
}
Esempio n. 16
0
bool PLXVF(PDeviceDescriptor_t d, const TCHAR* sentence, NMEA_INFO* info)
{

    double alt, airspeed;

    if (ParToDouble(sentence, 5, &airspeed)) {
        info->IndicatedAirspeed = airspeed;
        info->AirspeedAvailable = TRUE;
    }

    if (ParToDouble(sentence, 6, &alt)) {
	UpdateBaroSource( info, 0, d, AltitudeToQNHAltitude(alt));
        info->TrueAirspeed =  airspeed * AirDensityRatio(alt);
    }

    if (ParToDouble(sentence, 4, &info->Vario)) {
	info->VarioAvailable = TRUE;
	TriggerVarioUpdate();
    }

    return(true);
} 
Esempio n. 17
0
static BOOL FLYSEN(PDeviceDescriptor_t d, TCHAR *String, NMEA_INFO *GPS_INFO)
{

  TCHAR ctemp[80];
  double vtas;

  // VOID GPS SIGNAL
  NMEAParser::ExtractParameter(String,ctemp,8);
  if (_tcscmp(ctemp,_T("V"))==0) {
	GPS_INFO->NAVWarning=true;
	GPSCONNECT=false;
	goto label_nogps;
  }
  // ------------------------

  double tmplat;
  double tmplon;

  NMEAParser::ExtractParameter(String,ctemp,1);
  tmplat = MixedFormatToDegrees(StrToDouble(ctemp, NULL));
  NMEAParser::ExtractParameter(String,ctemp,2);
  tmplat = NorthOrSouth(tmplat, ctemp[0]);

  NMEAParser::ExtractParameter(String,ctemp,3);
  tmplon = MixedFormatToDegrees(StrToDouble(ctemp, NULL));
  NMEAParser::ExtractParameter(String,ctemp,4);
  tmplon = EastOrWest(tmplon,ctemp[0]);

  if (!((tmplat == 0.0) && (tmplon == 0.0))) {
        GPS_INFO->Latitude = tmplat;
        GPS_INFO->Longitude = tmplon;
	GPS_INFO->NAVWarning=false;
	GPSCONNECT=true;
  }

  // GPS SPEED
  NMEAParser::ExtractParameter(String,ctemp,6);
  GPS_INFO->Speed = StrToDouble(ctemp,NULL)/10;

  // TRACK BEARING
  if (GPS_INFO->Speed>1.0) {
	NMEAParser::ExtractParameter(String,ctemp,5);
	GPS_INFO->TrackBearing = AngleLimit360(StrToDouble(ctemp, NULL));
  }

  // HGPS
  NMEAParser::ExtractParameter(String,ctemp,7);
  GPS_INFO->Altitude = StrToDouble(ctemp,NULL);

  // ------------------------
  label_nogps: 

  // SATS
  NMEAParser::ExtractParameter(String,ctemp,9);
  GPS_INFO->SatellitesUsed = (int) StrToDouble(ctemp,NULL);
  GPS_INFO->SatellitesUsed = 4;

  // TIME
  // ignoring 00:00.00 , but that's minor problem. We don't have the date.
  // And no UTC, since this is local time already.
  NMEAParser::ExtractParameter(String,ctemp,0);
  double fixTime = StrToDouble(ctemp,NULL);
  if (fixTime>0 && GPS_INFO->SatellitesUsed>0) {
	double hours, mins,secs;
	hours = fixTime / 10000;
	GPS_INFO->Hour = (int)hours;
	mins = fixTime / 100;
	mins = mins - (GPS_INFO->Hour*100);
	GPS_INFO->Minute = (int)mins;
	secs = fixTime - (GPS_INFO->Hour*10000) - (GPS_INFO->Minute*100);
	GPS_INFO->Second = (int)secs;
  }


  // HPA from the pressure sensor
  //   NMEAParser::ExtractParameter(String,ctemp,10);
  //   double ps = StrToDouble(ctemp,NULL)/100;
  //   GPS_INFO->BaroAltitude = (1 - pow(fabs(ps / QNH),  0.190284)) * 44307.69;

  // HBAR 1013.25
  NMEAParser::ExtractParameter(String,ctemp,11);
  GPS_INFO->BaroAltitude = AltitudeToQNHAltitude(StrToDouble(ctemp,NULL));

  // VARIO
  NMEAParser::ExtractParameter(String,ctemp,12);
  GPS_INFO->Vario = StrToDouble(ctemp,NULL)/100;

  // TAS
  NMEAParser::ExtractParameter(String,ctemp,13);
  vtas=StrToDouble(ctemp,NULL)/10;
  GPS_INFO->IndicatedAirspeed = vtas/AirDensityRatio(GPS_INFO->BaroAltitude);
  GPS_INFO->TrueAirspeed = vtas;
  if (GPS_INFO->IndicatedAirspeed >0) 
	GPS_INFO->AirspeedAvailable = TRUE;
  else 
	GPS_INFO->AirspeedAvailable = FALSE;

  // ignore n.14 airspeed source

  // OAT
  NMEAParser::ExtractParameter(String,ctemp,15);
  GPS_INFO->OutsideAirTemperature = StrToDouble(ctemp,NULL);
  GPS_INFO->TemperatureAvailable=TRUE;

  // ignore n.16 baloon temperature  

  // BATTERY PERCENTAGES
  NMEAParser::ExtractParameter(String,ctemp,17);
  GPS_INFO->ExtBatt1_Voltage = StrToDouble(ctemp,NULL)+1000;
  NMEAParser::ExtractParameter(String,ctemp,18);
  GPS_INFO->ExtBatt2_Voltage = StrToDouble(ctemp,NULL)+1000;



  GPS_INFO->BaroAltitudeAvailable = TRUE;
  GPS_INFO->VarioAvailable = TRUE;

  // currently unused in LK, but ready for next future
  TriggerVarioUpdate();
  TriggerGPSUpdate();

  return TRUE;
}
Esempio n. 18
0
void Heading(NMEA_INFO *Basic, DERIVED_INFO *Calculated)
{
  double x0, y0, mag=0;
  static double LastTime = 0;
  static double lastHeading = 0;
  static double lastSpeed = 0;

  if (DoInit[MDI_HEADING]) {
	LastTime = 0;
	lastHeading = 0;
	DoInit[MDI_HEADING]=false;
  }

  if ((Basic->Speed>0)||(Calculated->WindSpeed>0)) {

    x0 = fastsine(Basic->TrackBearing)*Basic->Speed;
    y0 = fastcosine(Basic->TrackBearing)*Basic->Speed;
    x0 += fastsine(Calculated->WindBearing)*Calculated->WindSpeed;
    y0 += fastcosine(Calculated->WindBearing)*Calculated->WindSpeed;

    Calculated->Heading = AngleLimit360(atan2(x0,y0)*RAD_TO_DEG);

    if (!Calculated->Flying) {
      // don't take wind into account when on ground
      Calculated->Heading = Basic->TrackBearing;
    }

    // calculate turn rate in wind coordinates
    if(Basic->Time > LastTime) {
      double dT = Basic->Time - LastTime;

      LKASSERT(dT!=0);
      Calculated->TurnRateWind = AngleLimit180(Calculated->Heading
                                               - lastHeading)/dT;

      lastHeading = Calculated->Heading;
    }

    if (ISCAR) {
	// On ground, TAS is GS. Wind gradient irrilevant, normally.
	Calculated->TrueAirspeedEstimated = Basic->Speed;
	LKASSERT(AirDensityRatio(Calculated->NavAltitude)!=0);
	Calculated->IndicatedAirspeedEstimated = Basic->Speed/AirDensityRatio(Calculated->NavAltitude);
    } else {
	// calculate estimated true airspeed
	mag = isqrt4((unsigned long)(x0*x0*100+y0*y0*100))/10.0;
	Calculated->TrueAirspeedEstimated = mag;
	LKASSERT(AirDensityRatio(Calculated->NavAltitude)!=0);
	Calculated->IndicatedAirspeedEstimated = mag/AirDensityRatio(Calculated->NavAltitude);
    }
    // estimate bank angle (assuming balanced turn)
    double angle = atan(DEG_TO_RAD*Calculated->TurnRateWind*
			Calculated->TrueAirspeedEstimated/9.81);

    Calculated->BankAngle = RAD_TO_DEG*angle;

    if (ISCAR) {
	if(Basic->Time > LastTime) {
		Calculated->Gload = ((Basic->Speed - lastSpeed) / (Basic->Time-LastTime))/9.81;
		lastSpeed=Basic->Speed;
	} else {
		Calculated->Gload = 0;
	}
    } else {
	Calculated->Gload = 1.0/max(0.001,fabs(cos(angle)));
    }

    LastTime = Basic->Time;

    // estimate pitch angle (assuming balanced turn)
/*
    Calculated->PitchAngle = RAD_TO_DEG*
      atan2(Calculated->GPSVario-Calculated->Vario,
           Calculated->TrueAirspeedEstimated);
*/
	// should be used as here only when no real vario available
    Calculated->PitchAngle = RAD_TO_DEG*	
      atan2(Calculated->Vario,
           Calculated->TrueAirspeedEstimated);

    // update zigzag wind
    if (  ((AutoWindMode==D_AUTOWIND_ZIGZAG) || (AutoWindMode==D_AUTOWIND_BOTHCIRCZAG))
        && (!ReplayLogger::IsEnabled()) ) {
      double zz_wind_speed;
      double zz_wind_bearing;
      int quality=0;
      quality = WindKalmanUpdate(Basic, Calculated, &zz_wind_speed, &zz_wind_bearing);

      if (quality>0) {

        SetWindEstimate(zz_wind_speed, zz_wind_bearing);
	Calculated->WindSpeed   = zz_wind_speed;
	Calculated->WindBearing = zz_wind_bearing;

/* 100118 redundant!! removed. TOCHECK *
        Vector v_wind;
        v_wind.x = zz_wind_speed*cos(zz_wind_bearing*3.1415926/180.0);
        v_wind.y = zz_wind_speed*sin(zz_wind_bearing*3.1415926/180.0);
        LockFlightData();
        if (windanalyser) {
	  windanalyser->slot_newEstimate(Basic, Calculated, v_wind, quality);
        }
        UnlockFlightData();
*/
      }
    }
  // else basic speed is 0 and there is no wind.. 
  } else { 
    Calculated->Heading = Basic->TrackBearing;
    Calculated->TrueAirspeedEstimated = 0; // BUGFIX 100318
    Calculated->IndicatedAirspeedEstimated = 0; // BUGFIX 100318
  }

}
Esempio n. 19
0
static BOOL D(PDeviceDescriptor_t d, TCHAR *String, NMEA_INFO *pGPS) {
    /*
     * 00 : vario ist           in dm/sec
     * 01 : pressure            in cents of mB
     * 02 : nettovario          in dm/sec
     * 03 : anemometer          in km/h
     * 04 : temperature         in °C
     * 05 : trk compass (dis)   in °
     * 06 : speed (dis)         in km/h
     * 07 : mcr equ             in cm/sec
     * 08 : wind speed          in km/h
     * 09 : goto goal           in tenth of mt
     * 10 : effic to ground     in tenth
     * 11 : effic to goal       in tenth
     *
     *   $D,+0,100554,+25,18,+31,,0,-356,+25,+11,115,96*6A
     */
    TCHAR ctemp[80];

    // Vario
    NMEAParser::ExtractParameter(String,ctemp,0);
    if (ctemp[0] != '\0') {
        pGPS->Vario = StrToDouble(ctemp,NULL)/100;
        pGPS->VarioAvailable = TRUE;
    } else {
        pGPS->VarioAvailable = FALSE;
    }

    // Pressure
    NMEAParser::ExtractParameter(String,ctemp,1);
    if (ctemp[0] != '\0') {
        double abs_press = StrToDouble(ctemp,NULL);
        UpdateBaroSource(pGPS, 0, d, StaticPressureToAltitude(abs_press));
    }

    // Netto Vario
    NMEAParser::ExtractParameter(String,ctemp,2);
    if (ctemp[0] != '\0') {
        pGPS->NettoVario = StrToDouble(ctemp,NULL)/10;
        pGPS->NettoVarioAvailable = TRUE;
    } else {
        pGPS->NettoVarioAvailable = FALSE;
    }

    // airspeed
    NMEAParser::ExtractParameter(String,ctemp,3);
    if (ctemp[0] != '\0') {
        pGPS->TrueAirspeed = StrToDouble(ctemp,NULL) / 3600 * 1000;
        pGPS->IndicatedAirspeed = pGPS->TrueAirspeed / AirDensityRatio(pGPS->Altitude);
    } else {
        pGPS->TrueAirspeed = 0;
        pGPS->IndicatedAirspeed = 0;
    }

    // temperature
    NMEAParser::ExtractParameter(String,ctemp,4);
    if (ctemp[0] != '\0') {
        pGPS->OutsideAirTemperature = StrToDouble(ctemp,NULL);
        pGPS->TemperatureAvailable = TRUE;
    } else {
        pGPS->TemperatureAvailable = FALSE;
    }

    return TRUE;
}
Esempio n. 20
0
static BOOL PDGFTL1(PDeviceDescriptor_t d, TCHAR *String, NMEA_INFO *pGPS)
{
    /*
    	$PDGFTL1		     field     example
    	QNE 1013.25 altitude		0	2025  meters
    	QNH altitude			1	2000  meters
    	vario cm/s	 		2	 250  2.5m/s
    	netto vario			3	-14   dm/s
    	IAS				4	45    kmh
    	ground efficiency 		5	134   13:4 GR
    	Wind speed			6	28    kmh
    	Wind direction			7	65    degrees
    	Main lithium battery 3,82 v	8	382   0.01v
    	Backup AA battery 1,53 v 	9	153   0.01v

    	100209 Paolo Ventafridda
    	netto and ias optionals

    */

    TCHAR ctemp[80];
    double vtas, vias;
    double altqne, altqnh;
    static bool initqnh=true;



    NMEAParser::ExtractParameter(String,ctemp,0);
    altqne = StrToDouble(ctemp,NULL);
    NMEAParser::ExtractParameter(String,ctemp,1);
    altqnh = StrToDouble(ctemp,NULL);

    // AutoQNH will take care of setting an average QNH if nobody does it for a while
    if (initqnh) {
        // if digifly has qnh set by user qne and qnh are of course different
        if (altqne != altqnh) {
            UpdateQNH(FindQNH(altqne,altqnh));
            StartupStore(_T(". Using Digifly QNH %f%s"),QNH,NEWLINE);
            initqnh=false;
        } else {
            // if locally QNH was set, either by user of by AutoQNH, stop processing QNH from digifly
            if ( (QNH <= 1012) || (QNH>=1014)) initqnh=false;
            // else continue entering initqnh until somebody changes qnh in either digifly or lk8000
        }
    }
    UpdateBaroSource( pGPS,0, d,  AltitudeToQNHAltitude(altqne));


    NMEAParser::ExtractParameter(String,ctemp,2);
#if 1
    pGPS->Vario = StrToDouble(ctemp,NULL)/100;
#else
    double newVario = StrToDouble(ctemp,NULL)/100;
    pGPS->Vario = LowPassFilter(pGPS->Vario,newVario,0.1);
#endif
    pGPS->VarioAvailable = TRUE;


    NMEAParser::ExtractParameter(String,ctemp,3);
    if (ctemp[0] != '\0') {
        pGPS->NettoVario = StrToDouble(ctemp,NULL)/10; // dm/s
        pGPS->NettoVarioAvailable = TRUE;
    } else
        pGPS->NettoVarioAvailable = FALSE;


    NMEAParser::ExtractParameter(String,ctemp,4);
    if (ctemp[0] != '\0') {
        // we store m/s  , so we convert it from kmh
        vias = StrToDouble(ctemp,NULL)/3.6;

        if (vias >1) {
            vtas = vias*AirDensityRatio(altqne);
            pGPS->TrueAirspeed = vtas;
            pGPS->IndicatedAirspeed = vias;
            pGPS->AirspeedAvailable = TRUE;
        }
    } else {
        pGPS->AirspeedAvailable = FALSE;
    }


    NMEAParser::ExtractParameter(String,ctemp,8);
    pGPS->ExtBatt1_Voltage = StrToDouble(ctemp,NULL)/100;
    NMEAParser::ExtractParameter(String,ctemp,9);
    pGPS->ExtBatt2_Voltage = StrToDouble(ctemp,NULL)/100;

    TriggerVarioUpdate();

    return TRUE;
}
Esempio n. 21
0
//
// Support for new 2011 Digifly TOURTELL protocol 
// (Subset of TL1, pending upgrade in june 2011 for all devices)
//
static BOOL PDGFTTL(PDeviceDescriptor_t d, TCHAR *String, NMEA_INFO *pGPS)
{
/*
	$PDGFTTL		     field     example
	QNE 1013.25 altitude		0	2025  meters
	QNH altitude			1	2000  meters
	vario cm/s	 		2	 250  2.5m/s
	IAS				4	45    kmh
	netto vario			3	-14   dm/s
	
*/

  TCHAR ctemp[80];
  double vtas, vias;
  double altqne, altqnh;
  static bool initqnh=true;

  NMEAParser::ExtractParameter(String,ctemp,0);
  altqne = StrToDouble(ctemp,NULL);
  NMEAParser::ExtractParameter(String,ctemp,1);
  altqnh = StrToDouble(ctemp,NULL);

  // AutoQNH will take care of setting an average QNH if nobody does it for a while
  if (initqnh) {
	// if digifly has qnh set by user qne and qnh are of course different
	if (altqne != altqnh) {
		QNH=FindQNH(altqne,altqnh);
        CAirspaceManager::Instance().QnhChangeNotify(QNH);
		StartupStore(_T(". Using Digifly QNH %f%s"),QNH,NEWLINE);
		initqnh=false;
	} else {
		// if locally QNH was set, either by user of by AutoQNH, stop processing QNH from digifly
		if ( (QNH <= 1012) || (QNH>=1014)) initqnh=false;
		// else continue entering initqnh until somebody changes qnh in either digifly or lk8000
	}
  }
  UpdateBaroSource( pGPS, 0,d,  AltitudeToQNHAltitude(altqne));

  NMEAParser::ExtractParameter(String,ctemp,2);
#if 1
  pGPS->Vario = StrToDouble(ctemp,NULL)/100;
#else
  double newVario = StrToDouble(ctemp,NULL)/100;
  pGPS->Vario = LowPassFilter(pGPS->Vario,newVario,0.1);
#endif
  pGPS->VarioAvailable = TRUE;


  NMEAParser::ExtractParameter(String,ctemp,4);
  if (ctemp[0] != '\0') {
	pGPS->NettoVario = StrToDouble(ctemp,NULL)/10; // dm/s
	pGPS->NettoVarioAvailable = TRUE;
  } else
	pGPS->NettoVarioAvailable = FALSE;


  NMEAParser::ExtractParameter(String,ctemp,4);
  if (ctemp[0] != '\0') {
	// we store m/s  , so we convert it from kmh
	vias = StrToDouble(ctemp,NULL)/3.6;

	if (vias >1) {
		vtas = vias*AirDensityRatio(pGPS->BaroAltitude);
		pGPS->TrueAirspeed = vtas;
		pGPS->IndicatedAirspeed = vias;
		pGPS->AirspeedAvailable = TRUE;
  	}
  } else {
	pGPS->AirspeedAvailable = FALSE;
  }


  TriggerVarioUpdate();

  return TRUE;
}
Esempio n. 22
0
static BOOL cLXWP0(PDeviceDescriptor_t d, TCHAR *String, NMEA_INFO *pGPS) {
  TCHAR ctemp[80];

  /*
  $LXWP0,Y,222.3,1665.5,1.71,,,,,,239,174,10.1

   0 loger_stored (Y/N)
   1 IAS (kph) ----> Condor uses TAS!
   2 baroaltitude (m)
   3 vario (m/s)
   4-8 unknown
   9 heading of plane
  10 windcourse (deg)
  11 windspeed (kph)

  */
  double alt, airspeed, wspeed, wfrom;
 
  NMEAParser::ExtractParameter(String,ctemp,1);
  airspeed = StrToDouble(ctemp,NULL)/TOKPH;

  NMEAParser::ExtractParameter(String,ctemp,2);
  alt = StrToDouble(ctemp,NULL);

  pGPS->IndicatedAirspeed = airspeed/AirDensityRatio(alt);
  pGPS->TrueAirspeed = airspeed;

  UpdateBaroSource( pGPS, 0,d,  AltitudeToQNHAltitude(alt));

  NMEAParser::ExtractParameter(String,ctemp,3);
  pGPS->Vario = StrToDouble(ctemp,NULL);

  pGPS->AirspeedAvailable = TRUE;
  pGPS->VarioAvailable = TRUE;

  // we don't use heading for wind calculation since... wind is already calculated in condor!!
  NMEAParser::ExtractParameter(String,ctemp,11);
  wspeed=StrToDouble(ctemp,NULL);
  NMEAParser::ExtractParameter(String,ctemp,10);
  wfrom=StrToDouble(ctemp,NULL);

  #if 1 // 120424 fix correct wind setting

  wfrom+=180;
  if (wfrom==360) wfrom=0;
  if (wfrom>360) wfrom-=360;
  wspeed/=3.6;

  pGPS->ExternalWindAvailable = TRUE;
  pGPS->ExternalWindSpeed = wspeed;
  pGPS->ExternalWindDirection = wfrom;

  #else
  if (wspeed>0) {

	wfrom+=180;
	if (wfrom==360) wfrom=0;
	if (wfrom>360) wfrom-=360;
	wspeed/=3.6;

	// do not update if it has not changed
	if ( (wspeed!=CALCULATED_INFO.WindSpeed) || (wfrom != CALCULATED_INFO.WindBearing) ) {

		SetWindEstimate(wspeed, wfrom,9);
		CALCULATED_INFO.WindSpeed=wspeed;
		CALCULATED_INFO.WindBearing=wfrom;

	}
  }
  #endif


  return TRUE;
}
Esempio n. 23
0
static BOOL FLYSEN(PDeviceDescriptor_t d, TCHAR *String, NMEA_INFO *GPS_INFO)
{

  TCHAR ctemp[80];
  double vtas;
  static int offset=-1;


  // firmware 3.31h no offset
  // firmware 3.32  1 offset
  // Determine firmware version, assuming it will not change in the session!
  if (offset<0) {
	NMEAParser::ExtractParameter(String,ctemp,8);
	if ( (_tcscmp(ctemp,_T("A"))==0) || (_tcscmp(ctemp,_T("V"))==0))
		offset=0;
	else {
		NMEAParser::ExtractParameter(String,ctemp,9);
		if ( (_tcscmp(ctemp,_T("A"))==0) || (_tcscmp(ctemp,_T("V"))==0))
			offset=1;
		else 
			return TRUE;
	}
  }

  // VOID GPS SIGNAL
  NMEAParser::ExtractParameter(String,ctemp,8+offset);
  if (_tcscmp(ctemp,_T("V"))==0) {
	GPS_INFO->NAVWarning=true;
	GPSCONNECT=false;
	goto label_nogps;
  }
  // ------------------------

  double tmplat;
  double tmplon;

  NMEAParser::ExtractParameter(String,ctemp,1+offset);
  tmplat = MixedFormatToDegrees(StrToDouble(ctemp, NULL));
  NMEAParser::ExtractParameter(String,ctemp,2+offset);
  tmplat = NorthOrSouth(tmplat, ctemp[0]);

  NMEAParser::ExtractParameter(String,ctemp,3+offset);
  tmplon = MixedFormatToDegrees(StrToDouble(ctemp, NULL));
  NMEAParser::ExtractParameter(String,ctemp,4+offset);
  tmplon = EastOrWest(tmplon,ctemp[0]);

  if (!((tmplat == 0.0) && (tmplon == 0.0))) {
        GPS_INFO->Latitude = tmplat;
        GPS_INFO->Longitude = tmplon;
	GPS_INFO->NAVWarning=false;
	GPSCONNECT=true;
  }

  // GPS SPEED
  NMEAParser::ExtractParameter(String,ctemp,6+offset);
  GPS_INFO->Speed = StrToDouble(ctemp,NULL)/10;

  // TRACK BEARING
  if (GPS_INFO->Speed>1.0) {
	NMEAParser::ExtractParameter(String,ctemp,5+offset);
	GPS_INFO->TrackBearing = AngleLimit360(StrToDouble(ctemp, NULL));
  }

  // HGPS
  NMEAParser::ExtractParameter(String,ctemp,7+offset);
  GPS_INFO->Altitude = StrToDouble(ctemp,NULL);

  // ------------------------
  label_nogps: 

  // SATS
  NMEAParser::ExtractParameter(String,ctemp,9+offset);
  GPS_INFO->SatellitesUsed = (int) StrToDouble(ctemp,NULL);

  // DATE
  // Firmware 3.32 has got the date 
  if (offset>0) {
	NMEAParser::ExtractParameter(String,ctemp,0);
	long gy, gm, gd;
	TCHAR *Stop;
        gy = _tcstol(&ctemp[4], &Stop, 10) + 2000;
        ctemp[4] = '\0';
        gm = _tcstol(&ctemp[2], &Stop, 10);
        ctemp[2] = '\0';
        gd = _tcstol(&ctemp[0], &Stop, 10);

	if ( ((gy > 1980) && (gy <2100) ) && (gm != 0) && (gd != 0) ) {
		GPS_INFO->Year = gy;
		GPS_INFO->Month = gm;
		GPS_INFO->Day = gd;
	}
  }

  // TIME
  // ignoring 00:00.00
  // And no UTC, since this is local time already.
  NMEAParser::ExtractParameter(String,ctemp,0+offset);
  double fixTime = StrToDouble(ctemp,NULL);
  if (fixTime>0 && GPS_INFO->SatellitesUsed>0) {
	double hours, mins,secs;
	hours = fixTime / 10000;
	GPS_INFO->Hour = (int)hours;
	mins = fixTime / 100;
	mins = mins - (GPS_INFO->Hour*100);
	GPS_INFO->Minute = (int)mins;
	secs = fixTime - (GPS_INFO->Hour*10000) - (GPS_INFO->Minute*100);
	GPS_INFO->Second = (int)secs;
  }


  // HPA from the pressure sensor
  //   NMEAParser::ExtractParameter(String,ctemp,10+offset);
  //   double ps = StrToDouble(ctemp,NULL)/100;
  //   GPS_INFO->BaroAltitude = (1 - pow(fabs(ps / QNH),  0.190284)) * 44307.69;

  // HBAR 1013.25
  NMEAParser::ExtractParameter(String,ctemp,11+offset);
  if (d == pDevPrimaryBaroSource) {
	GPS_INFO->BaroAltitude = AltitudeToQNHAltitude(StrToDouble(ctemp,NULL));
	GPS_INFO->BaroAltitudeAvailable = TRUE;
  }

  // VARIO
  NMEAParser::ExtractParameter(String,ctemp,12+offset);
  GPS_INFO->Vario = StrToDouble(ctemp,NULL)/100;

  // TAS
  NMEAParser::ExtractParameter(String,ctemp,13+offset);
  vtas=StrToDouble(ctemp,NULL)/10;
  GPS_INFO->IndicatedAirspeed = vtas/AirDensityRatio(GPS_INFO->BaroAltitude);
  GPS_INFO->TrueAirspeed = vtas;
  if (GPS_INFO->IndicatedAirspeed >0) 
	GPS_INFO->AirspeedAvailable = TRUE;
  else 
	GPS_INFO->AirspeedAvailable = FALSE;

  // ignore n.14 airspeed source

  // OAT
  NMEAParser::ExtractParameter(String,ctemp,15+offset);
  GPS_INFO->OutsideAirTemperature = StrToDouble(ctemp,NULL);
  GPS_INFO->TemperatureAvailable=TRUE;

  // ignore n.16 baloon temperature  

  // BATTERY PERCENTAGES
  NMEAParser::ExtractParameter(String,ctemp,17+offset);
  GPS_INFO->ExtBatt1_Voltage = StrToDouble(ctemp,NULL)+1000;
  NMEAParser::ExtractParameter(String,ctemp,18+offset);
  GPS_INFO->ExtBatt2_Voltage = StrToDouble(ctemp,NULL)+1000;



  GPS_INFO->VarioAvailable = TRUE;

  // currently unused in LK, but ready for next future
  TriggerVarioUpdate();
  TriggerGPSUpdate();

  return TRUE;
}
Esempio n. 24
0
static BOOL FLYSEN(PDeviceDescriptor_t d, TCHAR *String, NMEA_INFO *pGPS)
{

  TCHAR ctemp[80];
  double vtas;
  static int offset=-1;

  d->nmeaParser.connected = true;

  // firmware 3.31h no offset
  // firmware 3.32  1 offset
  // Determine firmware version, assuming it will not change in the session!
  if (offset<0) {
	NMEAParser::ExtractParameter(String,ctemp,8);
	if ( (_tcscmp(ctemp,_T("A"))==0) || (_tcscmp(ctemp,_T("V"))==0))
		offset=0;
	else {
		NMEAParser::ExtractParameter(String,ctemp,9);
		if ( (_tcscmp(ctemp,_T("A"))==0) || (_tcscmp(ctemp,_T("V"))==0))
			offset=1;
		else
			return TRUE;
	}
  }

  // VOID GPS SIGNAL
  NMEAParser::ExtractParameter(String,ctemp,8+offset);
  if (_tcscmp(ctemp,_T("V"))==0) {
	pGPS->NAVWarning=true;
	// GPSCONNECT=false; // 121127 NO!!
	goto label_nogps;
  }
  // ------------------------

  double tmplat;
  double tmplon;

  NMEAParser::ExtractParameter(String,ctemp,1+offset);
  tmplat = MixedFormatToDegrees(StrToDouble(ctemp, NULL));
  NMEAParser::ExtractParameter(String,ctemp,2+offset);
  tmplat = NorthOrSouth(tmplat, ctemp[0]);

  NMEAParser::ExtractParameter(String,ctemp,3+offset);
  tmplon = MixedFormatToDegrees(StrToDouble(ctemp, NULL));
  NMEAParser::ExtractParameter(String,ctemp,4+offset);
  tmplon = EastOrWest(tmplon,ctemp[0]);

  if (!((tmplat == 0.0) && (tmplon == 0.0))) {
        pGPS->Latitude = tmplat;
        pGPS->Longitude = tmplon;
	pGPS->NAVWarning=false;
  }

  // GPS SPEED
  NMEAParser::ExtractParameter(String,ctemp,6+offset);
  pGPS->Speed = StrToDouble(ctemp,NULL)/10;

  // TRACK BEARING
  if (pGPS->Speed>1.0) {
	NMEAParser::ExtractParameter(String,ctemp,5+offset);
	pGPS->TrackBearing = AngleLimit360(StrToDouble(ctemp, NULL));
  }

  // HGPS
  NMEAParser::ExtractParameter(String,ctemp,7+offset);
  pGPS->Altitude = StrToDouble(ctemp,NULL);

  // ------------------------
  label_nogps:

  // SATS
  NMEAParser::ExtractParameter(String,ctemp,9+offset);
  pGPS->SatellitesUsed = (int) StrToDouble(ctemp,NULL);

  // DATE
  // Firmware 3.32 has got the date
  if (offset>0) {
	NMEAParser::ExtractParameter(String,ctemp,0);
	long gy, gm, gd;
	TCHAR *Stop;
        gy = _tcstol(&ctemp[4], &Stop, 10) + 2000;
        ctemp[4] = '\0';
        gm = _tcstol(&ctemp[2], &Stop, 10);
        ctemp[2] = '\0';
        gd = _tcstol(&ctemp[0], &Stop, 10);

	if ( ((gy > 1980) && (gy <2100) ) && (gm != 0) && (gd != 0) ) {
		pGPS->Year = gy;
		pGPS->Month = gm;
		pGPS->Day = gd;
	}
  }

  // TIME
  // ignoring 00:00.00
  // We need to manage UTC time
#ifndef OLD_TIME_MODIFY
  static int StartDay=-1;
  if (pGPS->SatellitesUsed>0) {
      NMEAParser::ExtractParameter(String,ctemp,0+offset);
      pGPS->Time = TimeModify(ctemp, pGPS, StartDay);
  }
  // TODO : check if TimeHasAdvanced check is needed (cf. Parser.cpp)
#else
  NMEAParser::ExtractParameter(String,ctemp,0+offset);
  double fixTime = StrToDouble(ctemp,NULL);

  static  int day_difference=0, previous_months_day_difference=0;
  static int startday=-1;

  if (fixTime>0 && pGPS->SatellitesUsed>0) {
	double hours, mins,secs;
	hours = fixTime / 10000;
	pGPS->Hour = (int)hours;
	mins = fixTime / 100;
	mins = mins - (pGPS->Hour*100);
	pGPS->Minute = (int)mins;
	secs = fixTime - (pGPS->Hour*10000) - (pGPS->Minute*100);
	pGPS->Second = (int)secs;

        fixTime = secs + (pGPS->Minute*60) + (pGPS->Hour*3600);

        if ((startday== -1) && (pGPS->Day != 0)) {
	   if (offset)
              StartupStore(_T(". FLYSEN First GPS DATE: %d-%d-%d%s"), pGPS->Year, pGPS->Month, pGPS->Day,NEWLINE);
	   else
              StartupStore(_T(". FLYSEN No Date, using PNA GPS DATE: %d-%d-%d%s"), pGPS->Year, pGPS->Month, pGPS->Day,NEWLINE);
           startday = pGPS->Day;
           day_difference=0;
           previous_months_day_difference=0;
        }
        if (startday != -1) {
           if (pGPS->Day < startday) {
              // detect change of month (e.g. day=1, startday=26)
              previous_months_day_difference=day_difference+1;
              day_difference=0;
              startday = pGPS->Day;
              StartupStore(_T(". FLYSEN Change GPS DATE to NEW MONTH: %d-%d-%d  (%d days running)%s"),
              pGPS->Year, pGPS->Month, pGPS->Day,previous_months_day_difference,NEWLINE);
           }
           if ( (pGPS->Day-startday)!=day_difference) {
              StartupStore(_T(". FLYSEN Change GPS DATE: %d-%d-%d%s"), pGPS->Year, pGPS->Month, pGPS->Day,NEWLINE);
           }

           day_difference = pGPS->Day-startday;
           if ((day_difference+previous_months_day_difference)>0) {
              fixTime += (day_difference+previous_months_day_difference) * 86400;
           }
        }
	pGPS->Time = fixTime;
  }
#endif

  // HPA from the pressure sensor
  //   NMEAParser::ExtractParameter(String,ctemp,10+offset);
  //   double ps = StrToDouble(ctemp,NULL)/100;
  //   pGPS->BaroAltitude = (1 - pow(fabs(ps / QNH),  0.190284)) * 44307.69;

  // HBAR 1013.25
  NMEAParser::ExtractParameter(String,ctemp,11+offset);
  double palt=StrToDouble(ctemp,NULL);
  UpdateBaroSource( pGPS, 0,d, QNEAltitudeToQNHAltitude(palt));

  // VARIO
  NMEAParser::ExtractParameter(String,ctemp,12+offset);
  pGPS->Vario = StrToDouble(ctemp,NULL)/100;

  // TAS
  NMEAParser::ExtractParameter(String,ctemp,13+offset);
  vtas=StrToDouble(ctemp,NULL)/10;
  pGPS->IndicatedAirspeed = vtas/AirDensityRatio(palt);
  pGPS->TrueAirspeed = vtas;
  if (pGPS->IndicatedAirspeed >0)
	pGPS->AirspeedAvailable = TRUE;
  else
	pGPS->AirspeedAvailable = FALSE;

  // ignore n.14 airspeed source

  // OAT
  NMEAParser::ExtractParameter(String,ctemp,15+offset);
  pGPS->OutsideAirTemperature = StrToDouble(ctemp,NULL);
  pGPS->TemperatureAvailable=TRUE;

  // ignore n.16 baloon temperature

  // BATTERY PERCENTAGES
  NMEAParser::ExtractParameter(String,ctemp,17+offset);
  pGPS->ExtBatt1_Voltage = StrToDouble(ctemp,NULL)+1000;
  NMEAParser::ExtractParameter(String,ctemp,18+offset);
  pGPS->ExtBatt2_Voltage = StrToDouble(ctemp,NULL)+1000;



  pGPS->VarioAvailable = TRUE;

  // currently unused in LK, but ready for next future
  TriggerVarioUpdate();
  TriggerGPSUpdate();

  return TRUE;
}
Esempio n. 25
0
/*
 * returns 0 if invalid, 999 if too high
 * EqMc is negative when no value is available, because recalculated and buffer still not usable
 */
double CalculateLDRotary(ldrotary_s *buf, DERIVED_INFO *Calculated ) {

	int altdiff;
	double eff;
	short bcold;
#ifdef DEBUG_ROTARY
	char ventabuffer[200];
	FILE *fp;
#endif
	double averias;
	double avertas;

	if ( Calculated->Circling || Calculated->OnGround || !Calculated->Flying ) {
#ifdef DEBUG_ROTARY
		sprintf(ventabuffer,"Not Calculating, on ground or circling, or not flying\r\n");
		if ((fp=fopen("DEBUG.TXT","a"))!= NULL)
			    {;fprintf(fp,"%s\n",ventabuffer);fclose(fp);}
#endif
		#if DEBUG_EQMC
		StartupStore(_T("... Circling, grounded or not flying, EqMc -1 (---)\n"));
		#endif
		Calculated->EqMc = -1;
		return(0);
	}

	if ( buf->start <0) {
#ifdef DEBUG_ROTARY
		sprintf(ventabuffer,"Calculate: invalid buf start<0\r\n");
		if ((fp=fopen("DEBUG.TXT","a"))!= NULL)
			    {;fprintf(fp,"%s\n",ventabuffer);fclose(fp);}
#endif
		#if DEBUG_EQMC
		StartupStore(_T("... Invalid buf start <0, EqMc -1 (---)\n"));
		#endif
		Calculated->EqMc = -1;
		return(0);
	}

	ldrotary_s bc;
  	memcpy(&bc, buf, sizeof(ldrotary_s));

	if (bc.valid == false ) {
		if (bc.start==0) {
			#if DEBUG_EQMC
			StartupStore(_T("... bc.valid is false, bc.start is 0, EqMc -1 (---)\n"));
			#endif
			Calculated->EqMc = -1;
			return(0); // unavailable
		}
		bcold=0;
	} else {

		if (bc.start < (bc.size-1))
			bcold=bc.start+1;
		else
			bcold=0;
	}

	altdiff= bc.altitude[bcold] - bc.altitude[bc.start];

	// if ( bc.valid == true ) {
	// bcsize<=0  should NOT happen, but we check it for safety
	if ( (bc.valid == true) && bc.size>0 ) {
		averias = bc.totalias/bc.size;
		averias/=100;

		if (ISCAR) {
			Rotary_Speed=averias;
		}
		// According to Welch & Irving, suggested by Dave..
		// MC = Vso*[ (V/Vo)^3 - (Vo/V)]
		// Vso: sink at best L/D
		// Vo : speed at best L/D
		// V  : TAS

		avertas=averias*AirDensityRatio(Calculated->NavAltitude);
		// This is just to be sure we are not using an impossible part of the polar
		if (avertas>(GlidePolar::Vminsink-8.3) && (avertas>0)) { // minsink - 30km/h 
			LKASSERT(GlidePolar::Vbestld>0);
			double dtmp= avertas/GlidePolar::Vbestld;
			Calculated->EqMc = -1*GlidePolar::sinkratecache[GlidePolar::Vbestld] * ( (dtmp*dtmp*dtmp) - ( GlidePolar::Vbestld/avertas));
			// Do not consider impossible MC values as Equivalent
			if (Calculated->EqMc>20) Calculated->EqMc=-1;
		} else  {
			Calculated->EqMc=-1;
			#if DEBUG_EQMC
			StartupStore(_T(".......too slow for eqmc\n"));
			#endif
		}
		#if DEBUG_EQMC
		StartupStore(_T(".. eMC=%.2f (=%.1f)  Averias=%f Avertas=%f kmh, sinktas=%.1f ms  sinkmc0=%.1f ms Vbestld=%.1f Vminsink=%.1f\n"),
		Calculated->EqMc, Calculated->EqMc, averias*TOKPH, avertas*TOKPH,-1*GlidePolar::sinkratecache[(int)avertas], 
		GlidePolar::sinkratecache[GlidePolar::Vbestld], GlidePolar::Vbestld*TOKPH, GlidePolar::Vminsink*TOKPH);
		#endif

	} else {
		Calculated->EqMc=-1;
		#if DEBUG_EQMC
		StartupStore(_T(".... bc.valid=%d bc.size=%d <=0, no eqMc\n"), bc.valid,bc.start);
		#endif
	}

	Rotary_Distance=bc.totaldistance;
	if (altdiff == 0 ) {
		return(INVALID_GR); // infinitum
	}
	eff= (double)bc.totaldistance / (double)altdiff;

#ifdef DEBUG_ROTARY
	sprintf(ventabuffer,"bcstart=%d bcold=%d altnew=%d altold=%d altdiff=%d totaldistance=%d eff=%f\r\n",
		bc.start, bcold,
		bc.altitude[bc.start], bc.altitude[bcold], altdiff, bc.totaldistance, eff);
	if ((fp=fopen("DEBUG.TXT","a"))!= NULL)
                    {;fprintf(fp,"%s\n",ventabuffer);fclose(fp);}
#endif

	if (eff>MAXEFFICIENCYSHOW) eff=INVALID_GR;

	return(eff);

}
Esempio n. 26
0
void TaskSpeed(NMEA_INFO *Basic, DERIVED_INFO *Calculated, const double this_maccready)
{
    int ifinal;
    static double LastTime = 0;
    static double LastTimeStats = 0;
    double TotalTime=0, TotalDistance=0, Vfinal=0;

    if (!ValidTaskPoint(ActiveWayPoint)) return;
    if (Calculated->ValidFinish) return;
    if (!Calculated->Flying) return;

    // in case we leave early due to error
    Calculated->TaskSpeedAchieved = 0;
    Calculated->TaskSpeed = 0;

    if (ActiveWayPoint<=0) { // no task speed before start
        Calculated->TaskSpeedInstantaneous = 0;
        return;
    }

    //  LockFlightData();
    LockTaskData();

    if (TaskAltitudeRequired(Basic, Calculated, this_maccready, &Vfinal,
                             &TotalTime, &TotalDistance, &ifinal)) {

        double t0 = TotalTime;
        // total time expected for task

        double t1 = Basic->Time-Calculated->TaskStartTime;
        // time elapsed since start

        double d0 = TotalDistance;
        // total task distance

        double d1 = Calculated->TaskDistanceCovered;
        // actual distance covered

        double dr = Calculated->TaskDistanceToGo;
        // distance remaining

        double hf = FAIFinishHeight(Basic, Calculated, -1);

        double h0 = Calculated->TaskAltitudeRequiredFromStart-hf;
        // total height required from start (takes safety arrival alt
        // and finish waypoint altitude into account)

        double h1 = max(0.0, Calculated->NavAltitude-hf);
        // height above target

        double dFinal;
        // final glide distance

        // equivalent speed
        double v2, v1;

        if ((t1<=0) || (d1<=0) || (d0<=0) || (t0<=0) || (h0<=0)) {
            // haven't started yet or not a real task
            Calculated->TaskSpeedInstantaneous = 0;
            //?      Calculated->TaskSpeed = 0;
            goto OnExit;
        }

        // JB's task speed...
        double hx = max(0.0, SpeedHeight(Basic, Calculated));
        double t1mod = t1-hx/MacCreadyOrAvClimbRate(Basic, Calculated, this_maccready);
        // only valid if flown for 5 minutes or more
        if (t1mod>300.0) {
            Calculated->TaskSpeedAchieved = d1/t1mod;
        } else {
            Calculated->TaskSpeedAchieved = d1/t1;
        }
        Calculated->TaskSpeed = Calculated->TaskSpeedAchieved;

        if (Vfinal<=0) {
            // can't reach target at current mc
            goto OnExit;
        }

        // distance that can be usefully final glided from here
        // (assumes average task glide angle of d0/h0)
        // JMW TODO accuracy: make this more accurate by working out final glide
        // through remaining turnpoints.  This will more correctly account
        // for wind.

#if BUGSTOP
        LKASSERT(h0!=0);
#endif
        if (h0==0) h0=1;
        dFinal = min(dr, d0*min(1.0,max(0.0,h1/h0)));

        if (Calculated->ValidFinish) {
            dFinal = 0;
        }

        double dc = max(0.0, dr-dFinal);
        // amount of extra distance to travel in cruise/climb before final glide

        // actual task speed achieved so far
        v1 = d1/t1;

#ifdef OLDTASKSPEED
        // time at end of final glide
        // equivalent time elapsed after final glide
        double t2 = t1+dFinal/Vfinal;

        // equivalent distance travelled after final glide
        // equivalent distance to end of final glide
        double d2 = d1+dFinal;

        // average speed to end of final glide from here
        v2 = d2/t2;
        Calculated->TaskSpeed = max(v1,v2);
#else
        // average speed to end of final glide from here, weighted
        // according to how much extra time would be spent in cruise/climb
        // the closer dc (the difference between remaining distance and
        // final glidable distance) gets to zero, the closer v2 approaches
        // the average speed to end of final glide from here
        // in other words, the more we consider the final glide part to have
        // been earned.

        // this will be bogus at fast starts though...

        LKASSERT((t1+dc/v1+dFinal/Vfinal)!=0);
        LKASSERT((t1+dFinal/Vfinal)!=0);
        if (v1>0) {
            v2 = (d1+dc+dFinal)/(t1+dc/v1+dFinal/Vfinal);
        } else {
            v2 = (d1+dFinal)/(t1+dFinal/Vfinal);
        }
        Calculated->TaskSpeed = v2;
#endif

        if(Basic->Time < LastTime) {
            LastTime = Basic->Time;
        } else if (Basic->Time-LastTime >=1.0) {

            double dt = Basic->Time-LastTime;
            LastTime = Basic->Time;

            // Calculate contribution to average task speed.
            // This is equal to the change in virtual distance
            // divided by the time step

            // This is a novel concept.
            // When climbing at the MC setting, this number should
            // be similar to the estimated task speed.
            // When climbing slowly or when flying off-course,
            // this number will drop.
            // In cruise at the optimum speed in zero lift, this
            // number will be similar to the estimated task speed.

            // A low pass filter is applied so it doesn't jump around
            // too much when circling.

            // If this number is higher than the overall task average speed,
            // it means that the task average speed is increasing.

            // When cruising in sink, this number will decrease.
            // When cruising in lift, this number will increase.

            // Therefore, it shows well whether at any time the glider
            // is wasting time.

            // VNT 090723 NOTICE: all of this is totally crazy. Did anyone ever cared to check
            // what happens with MC=0 ? Did anyone care to tell people how a simple "ETE" or TaskSpeed
            // has been complicated over any limit?
            // TODO: start back from scratch, not possible to trust any number here.

            static double dr_last = 0;

            double mc_safe = max(0.1,this_maccready);
            double Vstar = max(1.0,Calculated->VMacCready);

#if BUGSTOP
            LKASSERT(dt!=0);
#endif
            if (dt==0) dt=1;
            double vthis = (Calculated->LegDistanceCovered-dr_last)/dt;
            vthis /= AirDensityRatio(Calculated->NavAltitude);

            dr_last = Calculated->LegDistanceCovered;
            double ttg = max(1.0, Calculated->LegTimeToGo);
            //      double Vav = d0/max(1.0,t0);
            double Vrem = Calculated->LegDistanceToGo/ttg;
            double Vref = // Vav;
                Vrem;
            double sr = -GlidePolar::SinkRate(Vstar);
            double height_diff = max(0.0, -Calculated->TaskAltitudeDifference);

            if (Calculated->timeCircling>30) {
                mc_safe = max(mc_safe,
                              Calculated->TotalHeightClimb/Calculated->timeCircling);
            }
            // circling percentage during cruise/climb
            double rho_cruise = max(0.0,min(1.0,mc_safe/(sr+mc_safe)));
            double rho_climb = 1.0-rho_cruise;
#if BUGSTOP
            LKASSERT(mc_safe!=0);
#endif
            if (mc_safe==0) mc_safe=0.1;
            double time_climb = height_diff/mc_safe;

            // calculate amount of time in cruise/climb glide
            double rho_c = max(0.0, min(1.0, time_climb/ttg));

            if (Calculated->FinalGlide) {
                if (rho_climb>0) {
                    rho_c = max(0.0, min(1.0, rho_c/rho_climb));
                }
                if (!Calculated->Circling) {
                    if (Calculated->TaskAltitudeDifference>0) {
                        rho_climb *= rho_c;
                        rho_cruise *= rho_c;
                        // Vref = Vrem;
                    }
                }
            }

            LKASSERT(mc_safe!=0);
            double w_comp = min(10.0,max(-10.0,Calculated->Vario/mc_safe));
            double vdiff = vthis/Vstar + w_comp*rho_cruise + rho_climb;

            if (vthis > SAFTEYSPEED*2) {
                vdiff = 1.0;
                // prevent funny numbers when starting mid-track
            }
            //      Calculated->Experimental = vdiff*100.0;

            vdiff *= Vref;

            if (t1<5) {
                Calculated->TaskSpeedInstantaneous = vdiff;
                // initialise
            } else {
                static int lastActiveWayPoint = 0;
                static double tsi_av = 0;
                static int n_av = 0;
                if ((ActiveWayPoint==lastActiveWayPoint)
                        && (Calculated->LegDistanceToGo>1000.0)
                        && (Calculated->LegDistanceCovered>1000.0)) {

                    Calculated->TaskSpeedInstantaneous =
                        LowPassFilter(Calculated->TaskSpeedInstantaneous, vdiff, 0.1);

                    // update stats
                    if(Basic->Time < LastTimeStats) {
                        LastTimeStats = Basic->Time;
                        tsi_av = 0;
                        n_av = 0;
                    } else if (n_av>=60) {
                        tsi_av/= n_av;
                        flightstats.Task_Speed.
                        least_squares_update(
                            max(0.0,
                                Basic->Time-Calculated->TaskStartTime)/3600.0,
                            max(0.0, min(100.0,tsi_av)));
                        LastTimeStats = Basic->Time;
                        tsi_av = 0;
                        n_av = 0;
                    }
                    tsi_av += Calculated->TaskSpeedInstantaneous;
                    n_av ++;

                } else {

                    Calculated->TaskSpeedInstantaneous =
                        LowPassFilter(Calculated->TaskSpeedInstantaneous, vdiff, 0.5);

                    //	  Calculated->TaskSpeedInstantaneous = vdiff;
                    tsi_av = 0;
                    n_av = 0;
                }
                lastActiveWayPoint = ActiveWayPoint;
            }
        }
    }
OnExit:
    UnlockTaskData();

}
Esempio n. 27
0
bool DoTarget(NMEA_INFO *Basic, DERIVED_INFO *Calculated)
{
   double x0, y0, etas, ttgo=0;
   double bearing, distance;

   if (LKTargetIndex<0 || LKTargetIndex>=MAXTRAFFIC) return false;

   // DoTarget is called from MapWindow, in real time. We have enough CPU power there now

   #if 0
   if (  LastDoTarget > Basic->Time ) LastDoTarget=Basic->Time;
   // We calculate in real time, because PFLAA sentences are calculated too in real time
   if ( Basic->Time < (LastDoTarget+3.0) ) { 
	return false;
   }
   LastDoTarget=Basic->Time;
   #endif

   #ifdef DEBUG_LKT
   StartupStore(_T("... DoTarget Copy LKTraffic\n"));
   #endif
   //LockFlightData();
   memcpy(LKTraffic, Basic->FLARM_Traffic, sizeof(LKTraffic));
   //UnlockFlightData();

   if (LKTARG.ID <=0) return false;

   DistanceBearing(Basic->Latitude, Basic->Longitude, 
		LKTARG.Latitude, LKTARG.Longitude,
		&distance, &bearing);
   LKTARG.Distance=distance;
   LKTARG.Bearing=bearing;

   if (LKTARG.Speed>1) {
	x0 = fastsine(LKTARG.TrackBearing)*LKTARG.Speed;
	y0 = fastcosine(LKTARG.TrackBearing)*LKTARG.Speed;
	x0 += fastsine(Calculated->WindBearing)*Calculated->WindSpeed;
	y0 += fastcosine(Calculated->WindBearing)*Calculated->WindSpeed;
	// LKTARG.Heading = AngleLimit360(atan2(x0,y0)*RAD_TLK_DEG); // 101210 check
	etas = isqrt4((unsigned long)(x0*x0*100+y0*y0*100))/10.0;
	LKASSERT(AirDensityRatio(LKTARG.Altitude)!=0);
	LKTARG.EIAS = etas/AirDensityRatio(LKTARG.Altitude);
   } else {
	LKTARG.EIAS=0;
   }


   //double height_above_target = Calculated->NavAltitude - LKTARG.Altitude;

   // We DONT use EnergyHeight here because we are not considering the Target's TE either
   LKTARG.AltArriv = Calculated->NavAltitude - GlidePolar::MacCreadyAltitude(MACCREADY,
	distance,
	bearing,
	Calculated->WindSpeed, Calculated->WindBearing,
	0, 0,
	// final glide, use wind
	true, 
	&ttgo) - LKTARG.Altitude;
	
   // We CANNOT use RelativeAltitude because when ghost or zombie, it wont be updated in real time in respect
   // to OUR real position!! Lets use the last target altitude known.
   double relalt=Calculated->NavAltitude - LKTARG.Altitude;
   if (relalt==0)
	LKTARG.GR=999;
   else {
	// we need thus to invert the relative altitude
	LKTARG.GR=distance/(relalt);
   }


   return true;
}
Esempio n. 28
0
/*
 * returns 0 if invalid, 999 if too high
 * EqMc is negative when no value is available, because recalculated and buffer still not usable
 */
double CalculateLDRotary(ldrotary_s *buf, NMEA_INFO *Basic, DERIVED_INFO *Calculated ) {

	double eff;
#ifdef DEBUG_ROTARY
	short bcold;
	char ventabuffer[200];
	FILE *fp;
#endif
	double averias;
	double avertas;

	if ( Calculated->Circling || Calculated->OnGround || !Calculated->Flying ) {
#ifdef DEBUG_ROTARY
		sprintf(ventabuffer,"Not Calculating, on ground or circling, or not flying\r\n");
		if ((fp=fopen("DEBUG.TXT","a"))!= NULL)
			    {;fprintf(fp,"%s\n",ventabuffer);fclose(fp);}
#endif
		#if DEBUG_EQMC
		StartupStore(_T("... Circling, grounded or not flying, EqMc -1 (---)\n"));
		#endif
		Calculated->EqMc = -1;
		return(0);
	}

	if ( buf->start <0) {
#ifdef DEBUG_ROTARY
		sprintf(ventabuffer,"Calculate: invalid buf start<0\r\n");
		if ((fp=fopen("DEBUG.TXT","a"))!= NULL)
			    {;fprintf(fp,"%s\n",ventabuffer);fclose(fp);}
#endif
		#if DEBUG_EQMC
		StartupStore(_T("... Invalid buf start <0, EqMc -1 (---)\n"));
		#endif
		Calculated->EqMc = -1;
		return(0);
	}

	ldrotary_s bc;
	memcpy(&bc, buf, sizeof(ldrotary_s));

	if (bc.valid == false ) {
		if (bc.start==0) {
			#if DEBUG_EQMC
			StartupStore(_T("... bc.valid is false, bc.start is 0, EqMc -1 (---)\n"));
			#endif
			Calculated->EqMc = -1;
			return(0); // unavailable
		}
    }
	// if ( bc.valid == true ) {
	// bcsize<=0  should NOT happen, but we check it for safety
	if ( (bc.valid == true) && bc.size>0 ) {
		averias = bc.totalias/bc.size;
		averias/=100;

		if (ISCAR) {
			Rotary_Speed=averias;
		}

		// We use GPS altitude to be sure that the tas is correct, we dont know in fact
		// if qnh is correct, while gps is generally accurate for the purpose.
		avertas=averias*AirDensityRatio(QNHAltitudeToQNEAltitude(Basic->Altitude));
		// This is just to be sure we are not using an impossible part of the polar
		if (avertas>(GlidePolar::Vminsink()-8.3) && (avertas>0)) { // minsink - 30km/h

            Calculated->EqMc = GlidePolar::EquMC(averias);

			// Do not consider impossible MC values as Equivalent
			if (Calculated->EqMc>20) Calculated->EqMc=-1;
		} else  {
			Calculated->EqMc=-1;
			#if DEBUG_EQMC
			StartupStore(_T(".......too slow for eqmc\n"));
			#endif
		}
		#if DEBUG_EQMC
		StartupStore(_T(".. eMC=%.2f (=%.1f)  Averias=%f Avertas=%f kmh, sinktas=%.1f ms  sinkmc0=%.1f ms Vbestld=%.1f Vminsink=%.1f\n"),
		Calculated->EqMc, Calculated->EqMc, averias*TOKPH, avertas*TOKPH,-1*GlidePolar::SinkRateFast(0,avertas),
		GlidePolar::SinkRateBestLd(), GlidePolar::Vbestld()*TOKPH, GlidePolar::Vminsink()*TOKPH);
		#endif

	} else {
		Calculated->EqMc=-1;
		#if DEBUG_EQMC
		StartupStore(_T(".... bc.valid=%d bc.size=%d <=0, no eqMc\n"), bc.valid,bc.start);
		#endif
	}

	Rotary_Distance=bc.totaldistance;
	if (bc.totalaltitude == 0 ) {
		return(INVALID_GR); // infinitum
	}
	eff= ((double)bc.totaldistance) / ((double)bc.totalaltitude);

#ifdef DEBUG_ROTARY
	if (bc.valid && bc.start < (bc.size-1))
		bcold=bc.start+1;
	else
		bcold=0;

	sprintf(ventabuffer,"bcstart=%d bcold=%d altnew=%d altold=%d altdiff=%d totaldistance=%d eff=%f\r\n",
		bc.start, bcold,
		bc.altitude[bc.start], bc.altitude[bcold], bc.totalaltitude, bc.totaldistance, eff);
	if ((fp=fopen("DEBUG.TXT","a"))!= NULL)
                    {;fprintf(fp,"%s\n",ventabuffer);fclose(fp);}
#endif

	if (eff>MAXEFFICIENCYSHOW) eff=INVALID_GR;

	return(eff);

}