/**
* 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;
}
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
/// 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()
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;
}
void
NMEAInfo::ProvideIndicatedAirspeedWithAltitude(double ias, double altitude)
{
indicated_airspeed = ias;
true_airspeed = indicated_airspeed * AirDensityRatio(altitude);
airspeed_available.Update(clock);
airspeed_real = true;
}
void
NMEAInfo::ProvideTrueAirspeedWithAltitude(double tas, double altitude)
{
true_airspeed = tas;
indicated_airspeed = true_airspeed / AirDensityRatio(altitude);
airspeed_available.Update(clock);
airspeed_real = true;
}
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()
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;
}
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
/// 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()
//
// 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;
}
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);
}
//
// 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;
}
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
/// 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()
// 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;
}
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;
}
// 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;
}
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);
}
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;
}
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
}
}
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;
}
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;
}
//
// 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;
}
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;
}
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;
}
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;
}
/*
* 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);
}
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();
}
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;
}
/*
* 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);
}