bool CheckCondition(NMEA_INFO *Basic, DERIVED_INFO *Calculated) {
wind_mag = Calculated->WindSpeed;
wind_bearing = Calculated->WindBearing;
if (!Calculated->Flying) {
last_wind_mag = wind_mag;
last_wind_bearing = wind_bearing;
return false;
}
double mag_change = fabs(wind_mag - last_wind_mag);
double dir_change = fabs(AngleLimit180(wind_bearing-last_wind_bearing));
if (mag_change > 5/TOKNOTS) {
return true;
}
if ((wind_mag>10/TOKNOTS) && (dir_change > 45)) {
return true;
}
return false;
};
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);
}
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
}
}
void AATDistance::ShiftTargetFromBehind(double longitude, double latitude,
int taskwaypoint) {
// JMWAAT if being externally updated e.g. from task dialog, don't move it
if (TargetDialogOpen) return;
if (taskwaypoint==0) return;
// best is decreasing or first entry in sector, so project
// target in direction of improvement or first entry into sector
double course_bearing;
double course_bearing_orig;
double d_total_orig;
double d_total_this;
d_total_this = DoubleLegDistance(taskwaypoint,
longitude,
latitude);
d_total_orig = DoubleLegDistance(taskwaypoint,
Task[taskwaypoint].AATTargetLon,
Task[taskwaypoint].AATTargetLat);
if (d_total_this>d_total_orig-2.0*AATCloseDistance()) {
// this is better than the previous best! (or very close)
ShiftTargetFromInFront(longitude, latitude, taskwaypoint);
return;
}
// JMWAAT if locked, don't move it
if (Task[taskwaypoint].AATTargetLocked) {
// 20080615 JMW don't do this; locked stays locked
// Task[taskwaypoint].AATTargetLocked = false; // JMWAAT JB
return;
}
/*
// check to see if deviation is big enough to adjust target along track
DistanceBearing(Task[taskwaypoint-1].AATTargetLat,
Task[taskwaypoint-1].AATTargetLon,
latitude,
longitude,
NULL, &course_bearing);
DistanceBearing(Task[taskwaypoint-1].AATTargetLat,
Task[taskwaypoint-1].AATTargetLon,
Task[taskwaypoint].AATTargetLat,
Task[taskwaypoint].AATTargetLon,
NULL, &course_bearing_orig);
if (fabs(AngleLimit180(course_bearing-course_bearing_orig))<5.0) {
// don't update it if course deviation is less than 5 degrees,
// otherwise we end up wasting a lot of CPU in recalculating, and also
// the target ends up drifting.
return;
}
course_bearing = AngleLimit360(course_bearing+
Task[taskwaypoint].AATTargetOffsetRadial);
//JMWAAT Task[taskwaypoint].AATTargetOffsetRadial = course_bearing;
*/
DistanceBearing(Task[taskwaypoint-1].AATTargetLat,
Task[taskwaypoint-1].AATTargetLon,
latitude,
longitude,
NULL, &course_bearing);
course_bearing = AngleLimit360(course_bearing+
Task[taskwaypoint].AATTargetOffsetRadial);
DistanceBearing(latitude,
longitude,
Task[taskwaypoint].AATTargetLat,
Task[taskwaypoint].AATTargetLon,
NULL, &course_bearing_orig);
if (fabs(AngleLimit180(course_bearing-course_bearing_orig))<5.0) {
// don't update it if course deviation is less than 5 degrees,
// otherwise we end up wasting a lot of CPU in recalculating, and also
// the target ends up drifting.
return;
}
double max_distance =
FindInsideAATSectorDistance(latitude,
longitude,
taskwaypoint,
course_bearing,
0);
// total distance of legs from previous through this to next target
double delta = max_distance/2;
// move target in line with previous target along track
// at an offset to improve on max distance
double t_distance_lower = 0;
double t_distance = delta*2;
int steps = 0;
do {
// find target position along projected line but
// make sure it is in sector, and set at a distance
// to preserve total task distance
// we are aiming to make d_total_this = d_total_orig
double t_lat, t_lon;
FindLatitudeLongitude(latitude, longitude,
course_bearing, t_distance,
&t_lat,
&t_lon);
if (InAATTurnSector(t_lon, t_lat, taskwaypoint, 0)) {
d_total_this = DoubleLegDistance(taskwaypoint,
t_lon,
t_lat);
if (d_total_orig - d_total_this>0.0) {
t_distance_lower = t_distance;
// ok, can go further
t_distance += delta;
} else {
t_distance -= delta;
}
} else {
t_distance -= delta;
}
delta /= 2.0;
} while ((delta>5.0) && (steps++<20));
// now scan to edge of sector to find approximate range %
if (t_distance_lower>5.0) {
FindLatitudeLongitude(latitude, longitude,
course_bearing, t_distance_lower,
&Task[taskwaypoint].AATTargetLat,
&Task[taskwaypoint].AATTargetLon);
UpdateTargetAltitude(Task[taskwaypoint]);
Task[taskwaypoint].AATTargetOffsetRadius =
FindInsideAATSectorRange(latitude,
longitude,
taskwaypoint, course_bearing,
t_distance_lower);
TargetModified = true;
CalculateAATIsoLines();
}
// if ((!t_in_sector) && (d_diff_total>1.0)) {
// JMW TODO enhancement: this is too short now so need to lengthen the
// next waypoint if possible
// (re discussion with paul mander)
// }
}
void CalculateAATTaskSectors()
{
int i;
int awp = ActiveTaskPoint;
if(AATEnabled == FALSE || DoOptimizeRoute())
return;
double latitude = GPS_INFO.Latitude;
double longitude = GPS_INFO.Longitude;
double altitude = GPS_INFO.Altitude;
LockTaskData();
Task[0].AATTargetOffsetRadius = 0.0;
Task[0].AATTargetOffsetRadial = 0.0;
if (Task[0].Index>=0) {
Task[0].AATTargetLat = WayPointList[Task[0].Index].Latitude;
Task[0].AATTargetLon = WayPointList[Task[0].Index].Longitude;
}
for(i=1;i<MAXTASKPOINTS;i++) {
if(ValidTaskPoint(i)) {
if (!ValidTaskPoint(i+1)) {
// This must be the final waypoint, so it's not an AAT OZ
Task[i].AATTargetLat = WayPointList[Task[i].Index].Latitude;
Task[i].AATTargetLon = WayPointList[Task[i].Index].Longitude;
continue;
}
if(Task[i].AATType == SECTOR) {
FindLatitudeLongitude (WayPointList[Task[i].Index].Latitude,
WayPointList[Task[i].Index].Longitude,
Task[i].AATStartRadial ,
Task[i].AATSectorRadius ,
&Task[i].AATStartLat,
&Task[i].AATStartLon);
FindLatitudeLongitude (WayPointList[Task[i].Index].Latitude,
WayPointList[Task[i].Index].Longitude,
Task[i].AATFinishRadial ,
Task[i].AATSectorRadius,
&Task[i].AATFinishLat,
&Task[i].AATFinishLon);
}
// JMWAAT: if locked, don't move it
if (i<awp) {
// only update targets for current/later waypoints
continue;
}
Task[i].AATTargetOffsetRadius =
min(1.0, max(Task[i].AATTargetOffsetRadius,-1.0));
Task[i].AATTargetOffsetRadial =
min(90.0, max(-90.0, Task[i].AATTargetOffsetRadial));
double targetbearing;
double targetrange;
targetbearing = AngleLimit360(Task[i].Bisector+Task[i].AATTargetOffsetRadial);
if(Task[i].AATType == SECTOR) {
//AATStartRadial
//AATFinishRadial
targetrange = ((Task[i].AATTargetOffsetRadius+1.0)/2.0);
double aatbisector = HalfAngle(Task[i].AATStartRadial,
Task[i].AATFinishRadial);
if (fabs(AngleLimit180(aatbisector-targetbearing))>90) {
// bisector is going away from sector
targetbearing = Reciprocal(targetbearing);
targetrange = 1.0-targetrange;
}
if (!AngleInRange(Task[i].AATStartRadial,
Task[i].AATFinishRadial,
targetbearing,true)) {
// Bisector is not within AAT sector, so
// choose the closest radial as the target line
if (fabs(AngleLimit180(Task[i].AATStartRadial-targetbearing))
<fabs(AngleLimit180(Task[i].AATFinishRadial-targetbearing))) {
targetbearing = Task[i].AATStartRadial;
} else {
targetbearing = Task[i].AATFinishRadial;
}
}
targetrange*= Task[i].AATSectorRadius;
} else {
targetrange = Task[i].AATTargetOffsetRadius
*Task[i].AATCircleRadius;
}
// TODO accuracy: if i=awp and in sector, range parameter needs to
// go from current aircraft position to projection of target
// out to the edge of the sector
if (InAATTurnSector(longitude, latitude, i, altitude) && (awp==i) &&
!Task[i].AATTargetLocked) {
// special case, currently in AAT sector/cylinder
double dist;
double qdist;
double bearing;
// find bearing from last target through current aircraft position with offset
DistanceBearing(Task[i-1].AATTargetLat,
Task[i-1].AATTargetLon,
latitude,
longitude,
&qdist, &bearing);
bearing = AngleLimit360(bearing+Task[i].AATTargetOffsetRadial);
dist = ((Task[i].AATTargetOffsetRadius+1)/2.0)*
FindInsideAATSectorDistance(latitude, longitude, i, bearing);
// if (dist+qdist>aatdistance.LegDistanceAchieved(awp)) {
// JMW: don't prevent target from being closer to the aircraft
// than the best achieved, so can properly plan arrival time
FindLatitudeLongitude (latitude,
longitude,
bearing,
dist,
&Task[i].AATTargetLat,
&Task[i].AATTargetLon);
UpdateTargetAltitude(Task[i]);
TargetModified = true;
// }
} else {
FindLatitudeLongitude (WayPointList[Task[i].Index].Latitude,
WayPointList[Task[i].Index].Longitude,
targetbearing,
targetrange,
&Task[i].AATTargetLat,
&Task[i].AATTargetLon);
UpdateTargetAltitude(Task[i]);
TargetModified = true;
}
}
}
CalculateAATIsoLines();
if (!TargetDialogOpen) {
TargetModified = false;
// allow target dialog to detect externally changed targets
}
UnlockTaskData();
}
bool InFinishSector(NMEA_INFO *Basic, DERIVED_INFO *Calculated, const int i)
{
static int LastInSector = FALSE;
double AircraftBearing;
double FirstPointDistance;
bool retval = false;
if (WayPointList.empty()) return FALSE;
if (!ValidFinish(Basic, Calculated)) return FALSE;
// Finish invalid
if (!ValidTaskPoint(i)) return FALSE;
LockTaskData();
// distance from aircraft to start point
DistanceBearing(Basic->Latitude,
Basic->Longitude,
WayPointList[Task[i].Index].Latitude,
WayPointList[Task[i].Index].Longitude,
&FirstPointDistance,
&AircraftBearing);
bool inrange = false;
inrange = (FirstPointDistance<FinishRadius);
if (!inrange) {
LastInSector = false;
}
if(!FinishLine) // Start Circle
{
retval = inrange;
goto OnExit;
}
// Finish line
AircraftBearing = AngleLimit180(AircraftBearing - Task[i].InBound);
// JMW bugfix, was Bisector, which is invalid
bool approaching;
if(FinishLine==1) { // Finish line
approaching = ((AircraftBearing >= -90) && (AircraftBearing <= 90));
} else {
// FAI 90 degree
approaching = !((AircraftBearing >= 135) || (AircraftBearing <= -135));
}
if (inrange) {
if (LastInSector) {
// previously approaching the finish line
if (!approaching) {
// now moving away from finish line
LastInSector = false;
retval = TRUE;
goto OnExit;
}
} else {
if (approaching) {
// now approaching the finish line
LastInSector = true;
}
}
} else {
LastInSector = false;
}
OnExit:
UnlockTaskData();
return retval;
}