//
// Called by LD in Calc thread
//
void InsertLDRotary(ldrotary_s *buf, int distance, NMEA_INFO *Basic, DERIVED_INFO *Calculated) {
static short errs=0;
#ifdef DEBUG_ROTARY
char ventabuffer[200];
FILE *fp;
#endif
if (Calculated->OnGround) {
#ifdef DEBUG_ROTARY
sprintf(ventabuffer,"OnGround, ignore LDrotary\r\n");
if ((fp=fopen("DEBUG.TXT","a"))!= NULL)
{;fprintf(fp,"%s\n",ventabuffer);fclose(fp);}
#endif
return;
}
if (ISCAR) {
if (LKSW_ResetLDRotary) {
#if TESTBENCH
StartupStore(_T("... LD ROTARY SWITCH RESET\n"));
#endif
LKSW_ResetLDRotary=false;
InitLDRotary(&rotaryLD);
}
goto _noautoreset;
}
if (Calculated->Circling) {
#ifdef DEBUG_ROTARY
sprintf(ventabuffer,"Circling, ignore LDrotary\r\n");
if ((fp=fopen("DEBUG.TXT","a"))!= NULL)
{;fprintf(fp,"%s\n",ventabuffer);fclose(fp);}
#endif
return;
}
if (distance<3 || distance>150) { // just ignore, no need to reset rotary
if (errs>2) {
#ifdef DEBUG_ROTARY
sprintf(ventabuffer,"Rotary reset after exceeding errors\r\n");
if ((fp=fopen("DEBUG.TXT","a"))!= NULL)
{;fprintf(fp,"%s\n",ventabuffer);fclose(fp);}
#endif
InitLDRotary(&rotaryLD);
errs=0;
return;
}
errs++;
#ifdef DEBUG_ROTARY
sprintf(ventabuffer,"(errs=%d) IGNORE INVALID distance=%d altitude=%d\r\n",errs,distance,(int)Calculated->NavAltitude);
if ((fp=fopen("DEBUG.TXT","a"))!= NULL)
{;fprintf(fp,"%s\n",ventabuffer);fclose(fp);}
#endif
return;
}
errs=0;
_noautoreset:
if (++buf->start >=buf->size) {
#ifdef DEBUG_ROTARY
sprintf(ventabuffer,"*** rotary reset and VALID=TRUE ++bufstart=%d >=bufsize=%d\r\n",buf->start, buf->size);
if ((fp=fopen("DEBUG.TXT","a"))!= NULL)
{;fprintf(fp,"%s\n",ventabuffer);fclose(fp);}
#endif
buf->start=0;
buf->valid=true; // flag for a full usable buffer
}
// need to fill up buffer before starting to empty it
if ( buf->valid == true) {
buf->totaldistance-=buf->distance[buf->start];
buf->totalias-=buf->ias[buf->start];
}
buf->totaldistance+=distance;
buf->distance[buf->start]=distance;
// insert IAS in the rotary buffer, either real or estimated
if (Basic->AirspeedAvailable) {
buf->totalias += (int)(Basic->IndicatedAirspeed*100);
buf->ias[buf->start] = (int)(Basic->IndicatedAirspeed*100);
} else {
if (ISCAR) {
buf->totalias += (int)(Basic->Speed*100);
buf->ias[buf->start] = (int)(Basic->Speed*100);
} else {
buf->totalias += (int)(Calculated->IndicatedAirspeedEstimated*100);
buf->ias[buf->start] = (int)(Calculated->IndicatedAirspeedEstimated*100);
}
}
buf->altitude[buf->start]=(int)Calculated->NavAltitude;
#ifdef DEBUG_ROTARY
sprintf(ventabuffer,"insert buf[%d/%d], distance=%d totdist=%d\r\n",buf->start, buf->size-1, distance,buf->totaldistance);
if ((fp=fopen("DEBUG.TXT","a"))!= NULL)
{;fprintf(fp,"%s\n",ventabuffer);fclose(fp);}
#endif
}
void Turning(NMEA_INFO *Basic, DERIVED_INFO *Calculated)
{
static double LastTrack = 0;
static double StartTime = 0;
static double StartLong = 0;
static double StartLat = 0;
static double StartAlt = 0;
static double StartEnergyHeight = 0;
static double LastTime = 0;
static int MODE = CRUISE;
static bool LEFT = FALSE;
double Rate;
static double LastRate=0;
double dRate;
double dT;
if (!Calculated->Flying) {
if (MODE!=CRUISE) {
#if TESTBENCH
StartupStore(_T(".... Not flying, still circling -> Cruise forced!\n"));
#endif
goto _forcereset;
}
return;
}
// Back in time in IGC replay mode?
if(Basic->Time <= LastTime) {
#if TESTBENCH
StartupStore(_T("...... Turning check is back in time. Now=%f Last=%f: reset %s\n"),Basic->Time, LastTime,WhatTimeIsIt());
#endif
_forcereset:
LastTime = Basic->Time; // 101216 PV not sure of this..
LastTrack = 0;
StartTime = 0;
StartLong = 0;
StartLat = 0;
StartAlt = 0;
StartEnergyHeight = 0;
LastTime = 0;
LEFT = FALSE;
if (MODE!=CRUISE) {
MODE = CRUISE;
// Finally do the transition to cruise
Calculated->Circling = false;
SwitchZoomClimb(Basic, Calculated, false, LEFT);
InputEvents::processGlideComputer(GCE_FLIGHTMODE_CRUISE);
}
return;
}
dT = Basic->Time - LastTime;
LastTime = Basic->Time;
#if BUGSTOP
LKASSERT(dT!=0);
#else
if (dT==0) dT=1;
#endif
Rate = AngleLimit180(Basic->TrackBearing-LastTrack)/dT;
#if DEBUGTURN
StartupStore(_T("... Rate=%f in time=%f\n"),Rate,dT);
#endif
if (dT<2.0 && dT!=0) {
// time step ok
// calculate acceleration
dRate = (Rate-LastRate)/dT;
double dtlead=0.3;
// integrate assuming constant acceleration, for one second
Calculated->NextTrackBearing = Basic->TrackBearing
+ dtlead*(Rate+0.5*dtlead*dRate);
// s = u.t+ 0.5*a*t*t
Calculated->NextTrackBearing =
AngleLimit360(Calculated->NextTrackBearing);
} else {
// time step too big, so just take it at last measurement
Calculated->NextTrackBearing = Basic->TrackBearing;
}
Calculated->TurnRate = Rate;
// JMW limit rate to 50 deg per second otherwise a big spike
// will cause spurious lock on circling for a long time
if (Rate>50) {
Rate = 50;
}
if (Rate<-50) {
Rate = -50;
}
// average rate, to detect essing
static double rate_history[60];
double rate_ave=0;
for (int i=59; i>0; i--) {
rate_history[i] = rate_history[i-1];
rate_ave += rate_history[i];
}
rate_history[0] = Rate;
rate_ave /= 60;
// THIS IS UNUSED in 4.0
Calculated->Essing = fabs(rate_ave)*100/MinTurnRate;
if (MODE==CLIMB||MODE==WAITCRUISE)
Rate = LowPassFilter(LastRate,Rate,0.9);
else
Rate = LowPassFilter(LastRate,Rate,0.3);
LastRate = Rate;
if(Rate <0)
{
if (LEFT) {
// OK, already going left
} else {
LEFT = true;
}
Rate *= -1;
} else {
if (!LEFT) {
// OK, already going right
} else {
LEFT = false;
}
}
PercentCircling(Basic, Calculated, Rate);
LastTrack = Basic->TrackBearing;
bool forcecruise = false;
bool forcecircling = false;
#if 1 // UNUSED, EnableExternalTriggerCruise not configurable, set to false since ever
if (EnableExternalTriggerCruise ) {
if (ExternalTriggerCruise && ExternalTriggerCircling) {
// this should never happen
ExternalTriggerCircling = false;
}
forcecruise = ExternalTriggerCruise;
forcecircling = ExternalTriggerCircling;
}
#endif
switch(MODE) {
case CRUISE:
double cruise_turnthreshold;
if (ISPARAGLIDER)
cruise_turnthreshold=5;
else
cruise_turnthreshold=4;
if((Rate >= cruise_turnthreshold)||(forcecircling)) {
// This is initialising the potential thermalling start
// We still dont know if we are really circling for thermal
StartTime = Basic->Time;
StartLong = Basic->Longitude;
StartLat = Basic->Latitude;
StartAlt = Calculated->NavAltitude;
StartEnergyHeight = Calculated->EnergyHeight;
#if DEBUGTURN
StartupStore(_T("... CRUISE -> WAITCLIMB\n"));
#endif
MODE = WAITCLIMB;
}
if (forcecircling) {
MODE = WAITCLIMB;
} else {
break;
}
case WAITCLIMB:
if (forcecruise) {
MODE = CRUISE;
break;
}
double waitclimb_turnthreshold;
double cruiseclimbswitch;
if (ISPARAGLIDER) {
waitclimb_turnthreshold=5;
cruiseclimbswitch=15; // this should be finetuned for PGs
} else {
waitclimb_turnthreshold=4;
cruiseclimbswitch=15; // this is ok for gliders
}
if((Rate >= waitclimb_turnthreshold)||(forcecircling)) {
// WE CANNOT do this, because we also may need Circling mode to detect FF!!
// if( (Calculated->FreeFlying && ((Basic->Time - StartTime) > cruiseclimbswitch))|| forcecircling) {
if( (!ISCAR && !ISGAAIRCRAFT && ((Basic->Time - StartTime) > cruiseclimbswitch))|| forcecircling) {
#ifdef TOW_CRUISE
// If free flight (FF) hasn�t yet been detected, then we may
// still be on tow. The following prevents climb mode from
// engaging due to normal on-aerotow turns.
if (!Calculated->FreeFlying && (fabs(Calculated->TurnRate) < 12))
break;
#endif
#if DEBUGTURN
StartupStore(_T("... WAITCLIMB -> CLIMB\n"));
#endif
Calculated->Circling = true;
// JMW Transition to climb
MODE = CLIMB;
// Probably a replay flight, with fast forward with no cruise init
if (StartTime==0) {
StartTime = Basic->Time;
StartLong = Basic->Longitude;
StartLat = Basic->Latitude;
StartAlt = Calculated->NavAltitude;
StartEnergyHeight = Calculated->EnergyHeight;
}
Calculated->ClimbStartLat = StartLat;
Calculated->ClimbStartLong = StartLong;
Calculated->ClimbStartAlt = StartAlt+StartEnergyHeight;
Calculated->ClimbStartTime = StartTime;
if (flightstats.Altitude_Ceiling.sum_n>0) {
// only update base if have already climbed, otherwise
// we will catch the takeoff height as the base.
flightstats.Altitude_Base.
least_squares_update(max(0.0, Calculated->ClimbStartTime
- Calculated->TakeOffTime)/3600.0,
StartAlt);
}
SwitchZoomClimb(Basic, Calculated, true, LEFT);
InputEvents::processGlideComputer(GCE_FLIGHTMODE_CLIMB);
}
} else {
// nope, not turning, so go back to cruise
#if DEBUGTURN
StartupStore(_T("... WAITCLIMB -> CRUISE\n"));
#endif
MODE = CRUISE;
}
break;
case CLIMB:
if ( (AutoWindMode == D_AUTOWIND_CIRCLING) || (AutoWindMode==D_AUTOWIND_BOTHCIRCZAG) ) {
LockFlightData();
windanalyser->slot_newSample(Basic, Calculated);
UnlockFlightData();
}
double climb_turnthreshold;
if (ISPARAGLIDER)
climb_turnthreshold=10;
else
climb_turnthreshold=4;
if((Rate < climb_turnthreshold)||(forcecruise)) {
StartTime = Basic->Time;
StartLong = Basic->Longitude;
StartLat = Basic->Latitude;
StartAlt = Calculated->NavAltitude;
StartEnergyHeight = Calculated->EnergyHeight;
// JMW Transition to cruise, due to not properly turning
MODE = WAITCRUISE;
#if DEBUGTURN
StartupStore(_T("... CLIMB -> WAITCRUISE\n"));
#endif
}
if (forcecruise) {
MODE = WAITCRUISE;
} else {
break;
}
case WAITCRUISE:
if (forcecircling) {
MODE = CLIMB;
break;
}
double waitcruise_turnthreshold;
double climbcruiseswitch;
if (ISPARAGLIDER) {
waitcruise_turnthreshold=10;
climbcruiseswitch=15;
} else {
waitcruise_turnthreshold=4;
climbcruiseswitch=9; // ok for gliders
}
//
// Exiting climb mode?
//
if((Rate < waitcruise_turnthreshold) || forcecruise) {
if( ((Basic->Time - StartTime) > climbcruiseswitch) || forcecruise) {
// We are no more in climb mode
if (StartTime==0) {
StartTime = Basic->Time;
StartLong = Basic->Longitude;
StartLat = Basic->Latitude;
StartAlt = Calculated->NavAltitude;
StartEnergyHeight = Calculated->EnergyHeight;
}
Calculated->CruiseStartLat = StartLat;
Calculated->CruiseStartLong = StartLong;
Calculated->CruiseStartAlt = StartAlt;
Calculated->CruiseStartTime = StartTime;
// Here we assign automatically this last thermal to the L> multitarget
if (Calculated->ThermalGain >100) {
// Force immediate calculation of average thermal, it would be made
// during next cycle, but we need it here immediately
AverageThermal(Basic,Calculated);
if (EnableThermalLocator) {
InsertThermalHistory(Calculated->ClimbStartTime,
Calculated->ThermalEstimate_Latitude, Calculated->ThermalEstimate_Longitude,
Calculated->ClimbStartAlt, Calculated->NavAltitude, Calculated->AverageThermal);
} else {
InsertThermalHistory(Calculated->ClimbStartTime,
Calculated->ClimbStartLat, Calculated->ClimbStartLong,
Calculated->ClimbStartAlt, Calculated->NavAltitude, Calculated->AverageThermal);
}
}
InitLDRotary(&rotaryLD);
InitWindRotary(&rotaryWind);
flightstats.Altitude_Ceiling.
least_squares_update(max(0.0, Calculated->CruiseStartTime
- Calculated->TakeOffTime)/3600.0,
Calculated->CruiseStartAlt);
// Finally do the transition to cruise
Calculated->Circling = false;
MODE = CRUISE;
#if DEBUGTURN
StartupStore(_T("... WAITCRUISE -> CRUISE\n"));
#endif
SwitchZoomClimb(Basic, Calculated, false, LEFT);
InputEvents::processGlideComputer(GCE_FLIGHTMODE_CRUISE);
} // climbcruiseswitch time in range
} else { // Rate>Minturnrate, back to climb, turning again
#if DEBUGTURN
StartupStore(_T("... WAITCRUISE -> CLIMB\n"));
#endif
MODE = CLIMB;
}
break;
default:
// error, go to cruise
MODE = CRUISE;
}
// generate new wind vector if altitude changes or a new
// estimate is available
if (AutoWindMode>D_AUTOWIND_MANUAL && AutoWindMode <D_AUTOWIND_EXTERNAL) {
LockFlightData();
windanalyser->slot_Altitude(Basic, Calculated);
UnlockFlightData();
}
if (EnableThermalLocator) {
if (Calculated->Circling) {
thermallocator.AddPoint(Basic->Time, Basic->Longitude, Basic->Latitude,
Calculated->NettoVario);
thermallocator.Update(Basic->Time, Basic->Longitude, Basic->Latitude,
Calculated->WindSpeed, Calculated->WindBearing,
Basic->TrackBearing,
&Calculated->ThermalEstimate_Longitude,
&Calculated->ThermalEstimate_Latitude,
&Calculated->ThermalEstimate_W,
&Calculated->ThermalEstimate_R);
} else {
Calculated->ThermalEstimate_W = 0;
Calculated->ThermalEstimate_R = -1;
thermallocator.Reset();
}
}
// update atmospheric model
CuSonde::updateMeasurements(Basic, Calculated);
}
//
// Called by LD in Calc thread
//
void InsertLDRotary(ldrotary_s *buf, double distance, NMEA_INFO *Basic, DERIVED_INFO *Calculated) {
static unsigned short errs=0;
#ifdef TESTBENCH
static bool zeroerrs=true;
#endif
#ifdef DEBUG_ROTARY
char ventabuffer[200];
FILE *fp;
#endif
if (LKSW_ResetLDRotary) {
#if TESTBENCH
StartupStore(_T("... LD ROTARY SWITCH RESET @%s%s"),WhatTimeIsIt(),NEWLINE);
#endif
LKSW_ResetLDRotary=false;
InitLDRotary(&rotaryLD);
}
// For IGC Replay, we are never OnGround, see Calc/TakeoffLanding.cpp
if (Calculated->OnGround) {
#ifdef DEBUG_ROTARY
sprintf(ventabuffer,"OnGround, ignore LDrotary\r\n");
if ((fp=fopen("DEBUG.TXT","a"))!= NULL)
{;fprintf(fp,"%s\n",ventabuffer);fclose(fp);}
#endif
return;
}
if (ISCAR) {
goto _noautoreset;
}
if (Calculated->Circling) {
#ifdef DEBUG_ROTARY
sprintf(ventabuffer,"Circling, ignore LDrotary\r\n");
if ((fp=fopen("DEBUG.TXT","a"))!= NULL)
{;fprintf(fp,"%s\n",ventabuffer);fclose(fp);}
#endif
return;
}
if (distance<3 || distance>150) { // just ignore, no need to reset rotary
#ifdef TESTBENCH
if (distance==0 && zeroerrs) {
StartupStore(_T("... InsertLDRotary distance error=%f @%s%s"),distance,WhatTimeIsIt(),NEWLINE);
zeroerrs=false;
}
#endif
if (errs==9) {
#ifdef DEBUG_ROTARY
sprintf(ventabuffer,"Rotary reset after exceeding errors\r\n");
if ((fp=fopen("DEBUG.TXT","a"))!= NULL)
{;fprintf(fp,"%s\n",ventabuffer);fclose(fp);}
#endif
#if TESTBENCH
StartupStore(_T("... LDROTARY RESET, distance errors%s"),NEWLINE);
#endif
InitLDRotary(&rotaryLD);
errs=10; // an no more here until errs reset with valid data
return;
}
if (errs<9) errs++; // make it up to 9
#ifdef DEBUG_ROTARY
sprintf(ventabuffer,"(errs=%d) IGNORE INVALID distance=%d altitude=%d\r\n",errs,(int)(distance),(int)(Calculated->NavAltitude));
if ((fp=fopen("DEBUG.TXT","a"))!= NULL)
{;fprintf(fp,"%s\n",ventabuffer);fclose(fp);}
#endif
return;
}
errs=0;
#ifdef TESTBENCH
zeroerrs=true;
#endif
_noautoreset:
if((buf->start) < -1) {
#if BUGSTOP
LKASSERT(buf->start==-2);
#endif
// this is the first run after reset,
// save NavAltitude for calculate in AltDiff next run
// and return;
buf->start=-1;
buf->prevaltitude = iround(Calculated->NavAltitude*100);
return;
}
int diffAlt = buf->prevaltitude - iround(Calculated->NavAltitude*100);
buf->prevaltitude = iround(Calculated->NavAltitude*100);
if (++buf->start >=buf->size) {
#ifdef DEBUG_ROTARY
sprintf(ventabuffer,"*** rotary reset and VALID=TRUE ++bufstart=%d >=bufsize=%d\r\n",buf->start, buf->size);
if ((fp=fopen("DEBUG.TXT","a"))!= NULL)
{;fprintf(fp,"%s\n",ventabuffer);fclose(fp);}
#endif
buf->start=0;
buf->valid=true; // flag for a full usable buffer
}
LKASSERT(buf->start>=0 && buf->start<MAXLDROTARYSIZE);
if (buf->start<0 ||buf->start>=MAXLDROTARYSIZE) buf->start=0; // UNMANAGED RECOVERY!
// need to fill up buffer before starting to empty it
if ( buf->valid == true) {
buf->totaldistance-=buf->distance[buf->start];
buf->totalaltitude-=buf->altitude[buf->start];
buf->totalias-=buf->ias[buf->start];
}
buf->totaldistance+=iround(distance*100);
buf->distance[buf->start]=iround(distance*100);
buf->totalaltitude+=diffAlt;
buf->altitude[buf->start]=diffAlt;
// insert IAS in the rotary buffer, either real or estimated
if (Basic->AirspeedAvailable) {
buf->totalias += (int)(Basic->IndicatedAirspeed*100);
buf->ias[buf->start] = (int)(Basic->IndicatedAirspeed*100);
} else {
if (ISCAR) {
buf->totalias += (int)(Basic->Speed*100);
buf->ias[buf->start] = (int)(Basic->Speed*100);
} else {
buf->totalias += (int)(Calculated->IndicatedAirspeedEstimated*100);
buf->ias[buf->start] = (int)(Calculated->IndicatedAirspeedEstimated*100);
}
}
#ifdef DEBUG_ROTARY
sprintf(ventabuffer,"insert buf[%d/%d], distance=%d totdist=%d\r\n",buf->start, buf->size-1, buf->distance[buf->start], buf->totaldistance);
if ((fp=fopen("DEBUG.TXT","a"))!= NULL)
{;fprintf(fp,"%s\n",ventabuffer);fclose(fp);}
#endif
}
