void WindDirectionProcessing(int UpDown)
{
if(UpDown==1)
{
CALCULATED_INFO.WindBearing += 5;
while (CALCULATED_INFO.WindBearing >= 360)
{
CALCULATED_INFO.WindBearing -= 360;
}
}
else if (UpDown==-1)
{
CALCULATED_INFO.WindBearing -= 5;
while (CALCULATED_INFO.WindBearing < 0)
{
CALCULATED_INFO.WindBearing += 360;
}
} else if (UpDown == 0) {
SetWindEstimate(CALCULATED_INFO.WindSpeed,
CALCULATED_INFO.WindBearing);
#ifndef NOWINDREGISTRY
SaveWindToRegistry();
#endif
}
return;
}
static BOOL PILC(PDeviceDescriptor_t d, TCHAR *String, NMEA_INFO *GPS_INFO)
{
TCHAR ctemp[80];
NMEAParser::ExtractParameter(String,ctemp,0);
if (_tcscmp(ctemp,_T("PDA1"))==0) {
NMEAParser::ExtractParameter(String,ctemp,1);
if (d == pDevPrimaryBaroSource)
{
UpdateBaroSource( GPS_INFO, ILEC, AltitudeToQNHAltitude(StrToDouble(ctemp, NULL)));
}
NMEAParser::ExtractParameter(String,ctemp,2);
GPS_INFO->Vario = StrToDouble(ctemp,NULL);
NMEAParser::ExtractParameter(String,ctemp,3); // wind direction, integer
double wfrom;
wfrom = StrToDouble(ctemp,NULL); //@ could also be the NMEA checksum!
NMEAParser::ExtractParameter(String,ctemp,4); // wind speed kph integer
if (_tcslen(ctemp)!=0) {
double wspeed, wconfidence;
wspeed = StrToDouble(ctemp,NULL);
NMEAParser::ExtractParameter(String,ctemp,5); // confidence 0-100 percentage
wconfidence = StrToDouble(ctemp,NULL);
// StartupStore(_T(".... WIND from %.0f speed=%.0f kph confidence=%.0f\n"),wfrom,wspeed,wconfidence);
SetWindEstimate(wspeed/TOKPH, wfrom,(int)(wconfidence/100)*10);
CALCULATED_INFO.WindSpeed=wspeed/TOKPH;
CALCULATED_INFO.WindBearing=wfrom;
}
// else StartupStore(_T(".. NO WIND\n"));
GPS_INFO->VarioAvailable = TRUE;
TriggerVarioUpdate();
return TRUE;
}
if (_tcscmp(ctemp,_T("SET"))==0) {
NMEAParser::ExtractParameter(String,ctemp,1);
QNH = StrToDouble(ctemp,NULL);
// StartupStore(_T("... SET QNH= %.1f\n"),QNH);
CAirspaceManager::Instance().QnhChangeNotify(QNH);
return TRUE;
}
// Using the polar requires recalculating polar coefficients internally each time.. no thanks.
// if (_tcscmp(ctemp,_T("POLAR"))==0) { }
return TRUE;
}
static BOOL PZAN3(PDeviceDescriptor_t d, TCHAR *String, NMEA_INFO *apGPS)
{
//$PZAN3,+,026,A,321,035,V*cc
//Windkomponente (+=Rückenwind, -=Gegenwind)
//A=active (Messung Windkomponente ok) / V=void (Messung nicht verwendbar)
//Windrichtung (true, Wind aus dieser Richtung))
//Windstärke (km/h)
//A=active (Windmessung ok) / V=void (Windmessung nicht verwendbar)
//Windmessung im Geradeausflug: mit ZS1-Kompass A,A, ohne Kompass A,V
//Windmessung im Kreisflug: V,A
TCHAR ctemp[80];
double wspeed, wfrom;
char wind_usable;
NMEAParser::ExtractParameter(String,ctemp,3);
wfrom=StrToDouble(ctemp,NULL);
NMEAParser::ExtractParameter(String,ctemp,4);
wspeed=StrToDouble(ctemp,NULL);
NMEAParser::ExtractParameter(String,ctemp,5);
wind_usable=ctemp[0];
if (wind_usable == 'A') {
wspeed/=3.6;
#if 1 // 120424 fix correct wind setting
apGPS->ExternalWindAvailable = TRUE;
apGPS->ExternalWindSpeed = wspeed;
apGPS->ExternalWindDirection = wfrom;
#else
// 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 void UpdateWind(bool set) {
WndProperty *wp;
double ws = 0.0, wb = 0.0;
wp = (WndProperty*)wf->FindByName(TEXT("prpSpeed"));
if (wp) {
ws = wp->GetDataField()->GetAsFloat()/SPEEDMODIFY;
}
wp = (WndProperty*)wf->FindByName(TEXT("prpDirection"));
if (wp) {
wb = wp->GetDataField()->GetAsFloat();
}
if ((ws != CALCULATED_INFO.WindSpeed)
||(wb != CALCULATED_INFO.WindBearing)) {
if (set) {
SetWindEstimate(ws, wb);
}
CALCULATED_INFO.WindSpeed = ws;
CALCULATED_INFO.WindBearing = wb;
}
}
void WindSpeedProcessing(int UpDown)
{
if(UpDown==1)
CALCULATED_INFO.WindSpeed += (1/SPEEDMODIFY);
else if (UpDown== -1)
{
CALCULATED_INFO.WindSpeed -= (1/SPEEDMODIFY);
if(CALCULATED_INFO.WindSpeed < 0)
CALCULATED_INFO.WindSpeed = 0;
}
// JMW added faster way of changing wind direction
else if (UpDown== -2) {
WindDirectionProcessing(-1);
} else if (UpDown== 2) {
WindDirectionProcessing(1);
} else if (UpDown == 0) {
SetWindEstimate(CALCULATED_INFO.WindSpeed,
CALCULATED_INFO.WindBearing);
#ifndef NOWINDREGISTRY
SaveWindToRegistry();
#endif
}
return;
}
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;
}
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
}
}
