void DoNearestAlternate(NMEA_INFO *Basic, DERIVED_INFO *Calculated, int AltWaypoint) {
#if BUGSTOP
LKASSERT(ValidWayPoint(AltWaypoint));
#endif
if (!ValidWayPoint(AltWaypoint)) return;
double *altwp_gr = &WayPointCalc[AltWaypoint].GR;
double *altwp_arrival = &WayPointCalc[AltWaypoint].AltArriv[AltArrivMode];
*altwp_gr = CalculateGlideRatio( WayPointCalc[AltWaypoint].Distance,
Calculated->NavAltitude - WayPointList[AltWaypoint].Altitude - GetSafetyAltitude(AltWaypoint));
*altwp_arrival = CalculateWaypointArrivalAltitude(Basic, Calculated, AltWaypoint);
WayPointCalc[AltWaypoint].VGR = GetVisualGlideRatio(*altwp_arrival, *altwp_gr);
}
/*
* Used by Alternates and BestAlternate
* Colors VGR are used by DrawNearest &c.
*/
void DoAlternates(NMEA_INFO *Basic, DERIVED_INFO *Calculated, int AltWaypoint) {
CScopeLock(LockTaskData, UnlockTaskData);
#ifdef GTL2
// If flying an AAT and working on the RESWP_OPTIMIZED waypoint, then use
// this "optimized" waypoint to store data for the AAT virtual waypoint.
if ((AltWaypoint == RESWP_OPTIMIZED) && (!ISPARAGLIDER || (AATEnabled && !DoOptimizeRoute()))) {
WayPointList[RESWP_OPTIMIZED].Latitude = Task[ActiveWayPoint].AATTargetLat;
WayPointList[RESWP_OPTIMIZED].Longitude = Task[ActiveWayPoint].AATTargetLon;
WayPointList[RESWP_OPTIMIZED].Altitude = WayPointList[Task[ActiveWayPoint].Index].Altitude;
WaypointAltitudeFromTerrain(&WayPointList[RESWP_OPTIMIZED]);
_stprintf(WayPointList[RESWP_OPTIMIZED].Name, _T("!%s"),WayPointList[Task[ActiveWayPoint].Index].Name);
}
#endif
// handle virtual wps as alternates
if (AltWaypoint<=RESWP_END) {
if (!ValidResWayPoint(AltWaypoint)) return;
} else {
if (!ValidWayPoint(AltWaypoint)) return;
}
double *altwp_dist = &WayPointCalc[AltWaypoint].Distance;
double *altwp_gr = &WayPointCalc[AltWaypoint].GR;
double *altwp_arrival = &WayPointCalc[AltWaypoint].AltArriv[AltArrivMode];
DistanceBearing(WayPointList[AltWaypoint].Latitude, WayPointList[AltWaypoint].Longitude,
Basic->Latitude, Basic->Longitude,
altwp_dist, NULL);
*altwp_gr = CalculateGlideRatio( *altwp_dist,
Calculated->NavAltitude - WayPointList[AltWaypoint].Altitude - GetSafetyAltitude(AltWaypoint));
// We need to calculate arrival also for BestAlternate, since the last "reachable" could be
// even 60 seconds old and things may have changed drastically
*altwp_arrival = CalculateWaypointArrivalAltitude(Basic, Calculated, AltWaypoint);
WayPointCalc[AltWaypoint].VGR = GetVisualGlideRatio(*altwp_arrival, *altwp_gr);
}
void TaskStatistics(NMEA_INFO *Basic, DERIVED_INFO *Calculated,
const double this_maccready)
{
if (!ValidTaskPoint(ActiveWayPoint) ||
((ActiveWayPoint>0) && !ValidTaskPoint(ActiveWayPoint-1))) {
Calculated->LegSpeed = 0;
Calculated->LegDistanceToGo = 0;
Calculated->LegDistanceCovered = 0;
Calculated->LegTimeToGo = 0;
if (!AATEnabled) {
Calculated->AATTimeToGo = 0;
}
// Calculated->TaskSpeed = 0;
Calculated->TaskDistanceToGo = 0;
Calculated->TaskDistanceCovered = 0;
Calculated->TaskTimeToGo = 0;
Calculated->LKTaskETE = 0;
Calculated->TaskTimeToGoTurningNow = -1;
Calculated->TaskAltitudeRequired = 0;
Calculated->TaskAltitudeDifference = 0;
Calculated->TaskAltitudeDifference0 = 0;
Calculated->TaskAltitudeArrival = 0;
Calculated->TerrainWarningLatitude = 0.0;
Calculated->TerrainWarningLongitude = 0.0;
Calculated->GRFinish = INVALID_GR;
Calculated->FinalGlide = false;
CheckGlideThroughTerrain(Basic, Calculated); // BUGFIX 091123
// no task selected, so work things out at current heading
GlidePolar::MacCreadyAltitude(this_maccready, 100.0,
Basic->TrackBearing,
Calculated->WindSpeed,
Calculated->WindBearing,
&(Calculated->BestCruiseTrack),
&(Calculated->VMacCready),
(Calculated->FinalGlide==true),
NULL, 1.0e6, CRUISE_EFFICIENCY);
return;
}
// LockFlightData();
LockTaskData();
// Calculate Task Distances
// First calculate distances for this waypoint
double LegCovered, LegToGo=0, LegXTD=0, LegCurrentCourse;
double LegDistance, LegBearing=0;
bool calc_turning_now;
double w1lat;
double w1lon;
double w0lat;
double w0lon;
if (AATEnabled && (ActiveWayPoint>0) && (ValidTaskPoint(ActiveWayPoint))) {
w1lat = Task[ActiveWayPoint].AATTargetLat;
w1lon = Task[ActiveWayPoint].AATTargetLon;
} else {
w1lat = WayPointList[TASKINDEX].Latitude;
w1lon = WayPointList[TASKINDEX].Longitude;
}
DistanceBearing(Basic->Latitude,
Basic->Longitude,
w1lat,
w1lon,
&LegToGo, &LegBearing);
if (AATEnabled && (ActiveWayPoint>0) && ValidTaskPoint(ActiveWayPoint+1)
&& Calculated->IsInSector && (this_maccready>0.1) ) {
calc_turning_now = true;
} else {
calc_turning_now = false;
}
if (ActiveWayPoint<1) {
LegCovered = 0;
LegCurrentCourse=LegBearing;
if (ValidTaskPoint(ActiveWayPoint+1)) { // BUGFIX 091221
LegToGo=0;
}
} else {
if (AATEnabled) {
LKASSERT((ActiveWayPoint-1)>=0);
// TODO accuracy: Get best range point to here...
w0lat = Task[ActiveWayPoint-1].AATTargetLat;
w0lon = Task[ActiveWayPoint-1].AATTargetLon;
} else {
LKASSERT((ActiveWayPoint-1)>=0);
LKASSERT(ValidTaskPoint(ActiveWayPoint-1));
w0lat = WayPointList[Task[ActiveWayPoint-1].Index].Latitude;
w0lon = WayPointList[Task[ActiveWayPoint-1].Index].Longitude;
}
DistanceBearing(w1lat,
w1lon,
w0lat,
w0lon,
&LegDistance, NULL);
LegCovered = ProjectedDistance(w0lon, w0lat,
w1lon, w1lat,
Basic->Longitude,
Basic->Latitude,
&LegXTD, &LegCurrentCourse);
if ((StartLine==0) && (ActiveWayPoint==1)) {
// Correct speed calculations for radius
// JMW TODO accuracy: legcovered replace this with more accurate version
// LegDistance -= StartRadius;
LegCovered = max(0.0, LegCovered-StartRadius);
}
}
Calculated->LegDistanceToGo = LegToGo;
Calculated->LegDistanceCovered = LegCovered;
Calculated->LegCrossTrackError = LegXTD;
Calculated->LegActualTrueCourse = LegCurrentCourse;
Calculated->TaskDistanceCovered = LegCovered;
if (Basic->Time > Calculated->LegStartTime) {
if (flightstats.LegStartTime[ActiveWayPoint]<0) {
flightstats.LegStartTime[ActiveWayPoint] = Basic->Time;
}
Calculated->LegSpeed = Calculated->LegDistanceCovered
/ (Basic->Time - Calculated->LegStartTime);
}
// Now add distances for start to previous waypoint
if (!AATEnabled) {
for(int i=0;i< ActiveWayPoint-1; i++)
{
if (!ValidTaskPoint(i) || !ValidTaskPoint(i+1)) continue;
w1lat = WayPointList[Task[i].Index].Latitude;
w1lon = WayPointList[Task[i].Index].Longitude;
w0lat = WayPointList[Task[i+1].Index].Latitude;
w0lon = WayPointList[Task[i+1].Index].Longitude;
DistanceBearing(w1lat,
w1lon,
w0lat,
w0lon,
&LegDistance, NULL);
Calculated->TaskDistanceCovered += LegDistance;
}
} else if (ActiveWayPoint>0) {
// JMW added correction for distance covered
Calculated->TaskDistanceCovered =
aatdistance.DistanceCovered(Basic->Longitude,
Basic->Latitude,
ActiveWayPoint);
}
CheckTransitionFinalGlide(Basic, Calculated);
// accumulators
double TaskAltitudeRequired = 0;
double TaskAltitudeRequired0 = 0;
Calculated->TaskDistanceToGo = 0;
Calculated->TaskTimeToGo = 0;
Calculated->LKTaskETE = 0;
Calculated->TaskTimeToGoTurningNow = 0;
Calculated->TaskAltitudeArrival = 0;
double LegTime0;
// Calculate Final Glide To Finish
int FinalWayPoint = getFinalWaypoint();
double final_height = FAIFinishHeight(Basic, Calculated, -1);
double total_energy_height = Calculated->NavAltitude + Calculated->EnergyHeight;
double height_above_finish = total_energy_height - final_height;
if (ISPARAGLIDER) {
TaskAltitudeRequired = final_height;
TaskAltitudeRequired0 = final_height;
}
// Now add it for remaining waypoints
int task_index= FinalWayPoint;
double StartBestCruiseTrack = -1;
while ((task_index>ActiveWayPoint) && (ValidTaskPoint(task_index))) {
double this_LegTimeToGo;
bool this_is_final = (task_index==FinalWayPoint)
|| ForceFinalGlide;
this_is_final = true; // JMW CHECK FGAMT
if (AATEnabled) {
w1lat = Task[task_index].AATTargetLat;
w1lon = Task[task_index].AATTargetLon;
w0lat = Task[task_index-1].AATTargetLat;
w0lon = Task[task_index-1].AATTargetLon;
} else {
w1lat = WayPointList[Task[task_index].Index].Latitude;
w1lon = WayPointList[Task[task_index].Index].Longitude;
w0lat = WayPointList[Task[task_index-1].Index].Latitude;
w0lon = WayPointList[Task[task_index-1].Index].Longitude;
}
double NextLegDistance, NextLegBearing;
DistanceBearing(w0lat,
w0lon,
w1lat,
w1lon,
&NextLegDistance, &NextLegBearing);
double LegAltitude = GlidePolar::
MacCreadyAltitude(this_maccready,
NextLegDistance, NextLegBearing,
Calculated->WindSpeed,
Calculated->WindBearing,
0, 0,
this_is_final,
&this_LegTimeToGo,
height_above_finish, CRUISE_EFFICIENCY);
double LegAltitude0 = GlidePolar::
MacCreadyAltitude(0,
NextLegDistance, NextLegBearing,
Calculated->WindSpeed,
Calculated->WindBearing,
0, 0,
true,
&LegTime0, 1.0e6, CRUISE_EFFICIENCY
);
if (LegTime0>=0.9*ERROR_TIME) {
// can't make it, so assume flying at current mc
LegAltitude0 = LegAltitude;
}
TaskAltitudeRequired += LegAltitude;
TaskAltitudeRequired0 += LegAltitude0;
if(ISPARAGLIDER) {
// if required altitude is less than previous turpoint altitude,
// use previous turn point altitude
double w0Alt = FAIFinishHeight(Basic, Calculated, task_index-1);
if(TaskAltitudeRequired < w0Alt) {
Calculated->TaskAltitudeArrival += w0Alt - TaskAltitudeRequired;
TaskAltitudeRequired = w0Alt;
}
if(TaskAltitudeRequired0 < w0Alt) {
TaskAltitudeRequired0 = w0Alt;
}
}
Calculated->TaskDistanceToGo += NextLegDistance;
Calculated->TaskTimeToGo += this_LegTimeToGo;
if (task_index==1) {
StartBestCruiseTrack = NextLegBearing;
}
if (calc_turning_now) {
if (task_index == ActiveWayPoint+1) {
double NextLegDistanceTurningNow, NextLegBearingTurningNow;
double this_LegTimeToGo_turningnow=0;
DistanceBearing(Basic->Latitude,
Basic->Longitude,
w1lat,
w1lon,
&NextLegDistanceTurningNow,
&NextLegBearingTurningNow);
GlidePolar::
MacCreadyAltitude(this_maccready,
NextLegDistanceTurningNow,
NextLegBearingTurningNow,
Calculated->WindSpeed,
Calculated->WindBearing,
0, 0,
this_is_final,
&this_LegTimeToGo_turningnow,
height_above_finish, CRUISE_EFFICIENCY);
Calculated->TaskTimeToGoTurningNow += this_LegTimeToGo_turningnow;
} else {
Calculated->TaskTimeToGoTurningNow += this_LegTimeToGo;
}
}
height_above_finish-= LegAltitude;
task_index--;
}
// current waypoint, do this last!
if (AATEnabled && (ActiveWayPoint>0) && ValidTaskPoint(ActiveWayPoint+1) && Calculated->IsInSector) {
if (Calculated->WaypointDistance<AATCloseDistance()*3.0) {
LegBearing = AATCloseBearing(Basic, Calculated);
}
}
#ifdef BCT_ALT_FIX
// Don't calculate BCT yet. LegAltitude will be used to calculate
// task altitude difference, which will then be used to calculate BCT.
#endif
double LegAltitude =
GlidePolar::MacCreadyAltitude(this_maccready,
LegToGo,
LegBearing,
Calculated->WindSpeed,
Calculated->WindBearing,
#ifdef BCT_ALT_FIX
0,
#else
&(Calculated->BestCruiseTrack),
#endif
&(Calculated->VMacCready),
// (Calculated->FinalGlide==1),
true, // JMW CHECK FGAMT
&(Calculated->LegTimeToGo),
height_above_finish, CRUISE_EFFICIENCY);
double LegAltitude0 =
GlidePolar::MacCreadyAltitude(0,
LegToGo,
LegBearing,
Calculated->WindSpeed,
Calculated->WindBearing,
0,
0,
true,
&LegTime0, 1.0e6, CRUISE_EFFICIENCY
);
#ifndef BCT_ALT_FIX
// fix problem of blue arrow wrong in task sector
if (StartBestCruiseTrack>=0) // use it only if assigned, workaround
if (Calculated->IsInSector && (ActiveWayPoint==0)) {
// set best cruise track to first leg bearing when in start sector
Calculated->BestCruiseTrack = StartBestCruiseTrack;
}
#endif
// JMW TODO accuracy: Use safetymc where appropriate
if (LegTime0>= 0.9*ERROR_TIME) {
// can't make it, so assume flying at current mc
LegAltitude0 = LegAltitude;
}
TaskAltitudeRequired += LegAltitude;
TaskAltitudeRequired0 += LegAltitude0;
Calculated->TaskDistanceToGo += LegToGo;
Calculated->TaskTimeToGo += Calculated->LegTimeToGo;
#ifndef BCT_ALT_FIX
height_above_finish-= LegAltitude;
#endif
if (calc_turning_now) {
Calculated->TaskTimeToGoTurningNow +=
Basic->Time-Calculated->TaskStartTime;
} else {
Calculated->TaskTimeToGoTurningNow = -1;
}
if (ISPARAGLIDER) {
Calculated->TaskAltitudeRequired = TaskAltitudeRequired;
} else {
Calculated->TaskAltitudeRequired = TaskAltitudeRequired + final_height;
TaskAltitudeRequired0 += final_height;
}
Calculated->TaskAltitudeDifference = total_energy_height - Calculated->TaskAltitudeRequired;
Calculated->TaskAltitudeDifference0 = total_energy_height - TaskAltitudeRequired0;
Calculated->NextAltitudeDifference0 = total_energy_height - Calculated->NextAltitudeRequired0;
Calculated->TaskAltitudeArrival += Calculated->TaskAltitudeDifference;
Calculated->GRFinish= CalculateGlideRatio(Calculated->TaskDistanceToGo, Calculated->NavAltitude - final_height);
if (Calculated->TaskSpeedAchieved >0)
Calculated->LKTaskETE = Calculated->TaskDistanceToGo/Calculated->TaskSpeedAchieved;
else
Calculated->LKTaskETE=0;
#ifdef BCT_ALT_FIX
// This MCA call's only purpose is to update BestCruiseTrack (BCT).
// It must occur after TaskAltitudeDifference (TAD) is updated,
// since BCT depends on TAD.
GlidePolar::MacCreadyAltitude(this_maccready,
LegToGo,
LegBearing,
Calculated->WindSpeed,
Calculated->WindBearing,
&(Calculated->BestCruiseTrack),
0,
true,
0,
height_above_finish,
CRUISE_EFFICIENCY,
Calculated->TaskAltitudeDifference);
// fix problem of blue arrow wrong in task sector
if (StartBestCruiseTrack>=0) // use it only if assigned, workaround
if (Calculated->IsInSector && (ActiveWayPoint==0)) {
// set best cruise track to first leg bearing when in start sector
Calculated->BestCruiseTrack = StartBestCruiseTrack;
}
height_above_finish-= LegAltitude;
#endif
CheckGlideThroughTerrain(Basic, Calculated);
CheckForceFinalGlide(Basic, Calculated);
UnlockTaskData();
}
void MapWindow::LKCalculateWaypointReachable(const bool forced)
{
#if USEONEHZLIMITER
if (!forced) ONEHZLIMITER;
#endif
static short multicalc_slot=0; // -1 (which becomes immediately 0) will force full loading on startup, but this is not good
// because currently we are not waiting for ProgramStarted=3
// and the first scan is made while still initializing other things
short numslots=1; // TODO assign numslots with a function, based also on available CPU time
if (NumberOfWayPoints>200) {
numslots=NumberOfWayPoints/400;
// keep numslots optimal
if (numslots<MULTICALC_MINROBIN) numslots=MULTICALC_MINROBIN; // seconds for full scan, as this is executed at 1Hz
if (numslots>MULTICALC_MAXROBIN) numslots=MULTICALC_MAXROBIN;
// When waypointnumber has changed, we wont be using an exceeded multicalc_slot, which would crash the sw
// In this case, we shall probably continue for the first round to calculate without going from the beginning
// but this is not a problem, we are round-robin all the time here.
if (++multicalc_slot>numslots) multicalc_slot=1;
} else {
multicalc_slot=0; // forcing full scan
}
unsigned int i;
double waypointDistance, waypointBearing,altitudeRequired,altitudeDifference;
// LandableReachable is used only by the thermal bar indicator in MapWindow2, after here
// apparently, is used to tell you if you are below final glide but in range for a landable wp
// Since nov 2011 we dont user LandableReachable in FinalGlide anymore.
// However it is still to be understood what drawbacks we might have by changing calculations here.
LandableReachable = false;
if (!WayPointList) return;
unsigned int scanstart;
unsigned int scanend;
#if DEBUGCW
unsigned int numwpscanned=0;
#endif
LockTaskData();
if (multicalc_slot==0) {
scanstart=0; // including this
scanend=NumberOfWayPoints; // will be used -1, so up to this excluded value
#if DEBUGCW
StartupStore(_T("... wps=%d multicalc_slot=0 ignored numslot=%d, full scan %d < %d%s"),NumberOfWayPoints,
numslots,scanstart,scanend,NEWLINE);
#endif
} else {
scanstart=(NumberOfWayPoints/numslots)*(multicalc_slot-1);
if (multicalc_slot==numslots)
scanend=NumberOfWayPoints;
else
scanend=scanstart+(NumberOfWayPoints/numslots);
#if DEBUGCW
StartupStore(_T("... wps=%d multicalc_slot=%d of %d, scan %d < %d%s"),NumberOfWayPoints,
multicalc_slot, numslots,scanstart,scanend,NEWLINE);
#endif
}
int overtarg=GetOvertargetIndex();
if (overtarg<0) overtarg=999999;
for(i=scanstart;i<scanend;i++) {
// signed Overtgarget -1 becomes a very high number, casted unsigned
if ( ( ((WayPointCalc[i].AltArriv[AltArrivMode] >=0)||(WayPointList[i].Visible)) && (WayPointCalc[i].IsLandable || (WayPointList[i].Style==STYLE_THERMAL)))
|| WaypointInTask(i) || (i==(unsigned int)overtarg) ) {
DistanceBearing(DrawInfo.Latitude, DrawInfo.Longitude, WayPointList[i].Latitude, WayPointList[i].Longitude,
&waypointDistance, &waypointBearing);
WayPointCalc[i].Distance=waypointDistance;
WayPointCalc[i].Bearing=waypointBearing;
CalculateGlideRatio(waypointDistance,
DerivedDrawInfo.NavAltitude - WayPointList[i].Altitude - GetSafetyAltitude(i));
altitudeRequired = GlidePolar::MacCreadyAltitude (GetMacCready(i,0), waypointDistance, waypointBearing,
DerivedDrawInfo.WindSpeed, DerivedDrawInfo.WindBearing, 0,0,true,0)
+ WayPointList[i].Altitude + GetSafetyAltitude(i) - DerivedDrawInfo.EnergyHeight;
WayPointCalc[i].AltReqd[AltArrivMode] = altitudeRequired;
WayPointList[i].AltArivalAGL = DerivedDrawInfo.NavAltitude - altitudeRequired;
if(WayPointList[i].AltArivalAGL >=0){
WayPointList[i].Reachable = TRUE;
if (CheckLandableReachableTerrainNew(&DrawInfo, &DerivedDrawInfo, waypointDistance, waypointBearing)) {
if ((signed)i!=TASKINDEX) {
LandableReachable = true;
}
} else {
WayPointList[i].Reachable = FALSE;
}
} else {
WayPointList[i].Reachable = FALSE;
}
#if DEBUGCW
numwpscanned++;
#endif
} // if landable or in task
} // for all waypoints
// This is wrong, because multicalc will not necessarily find the LandableReachable at each pass
// As of nov 2011 it is better not to change it, and let further investigation after 3.0
if (!LandableReachable) // indentation wrong here
for(i=scanstart;i<scanend;i++) {
if(!WayPointList[i].Visible && WayPointList[i].FarVisible) {
// visible but only at a distance (limit this to 100km radius)
if( WayPointCalc[i].IsLandable ) {
#if DEBUGCW
numwpscanned++;
#endif
DistanceBearing(DrawInfo.Latitude,
DrawInfo.Longitude,
WayPointList[i].Latitude,
WayPointList[i].Longitude,
&waypointDistance,
&waypointBearing);
WayPointCalc[i].Distance=waypointDistance; // VENTA6
WayPointCalc[i].Bearing=waypointBearing;
if (waypointDistance<100000.0) {
altitudeRequired = GlidePolar::MacCreadyAltitude (GetMacCready(i,0), waypointDistance, waypointBearing, // 091221
DerivedDrawInfo.WindSpeed, DerivedDrawInfo.WindBearing, 0,0,true,0)
+ WayPointList[i].Altitude + GetSafetyAltitude(i);
altitudeDifference = DerivedDrawInfo.NavAltitude + DerivedDrawInfo.EnergyHeight - altitudeRequired;
WayPointList[i].AltArivalAGL = altitudeDifference;
WayPointCalc[i].AltReqd[AltArrivMode] = altitudeRequired;
if(altitudeDifference >=0){
WayPointList[i].Reachable = TRUE;
if (CheckLandableReachableTerrainNew(&DrawInfo, &DerivedDrawInfo, waypointDistance, waypointBearing)) {
LandableReachable = true;
} else
WayPointList[i].Reachable = FALSE;
}
else {
WayPointList[i].Reachable = FALSE;
}
} else {
WayPointList[i].Reachable = FALSE;
} // <100000
} // landable wp
} // visible or far visible
} // for all waypoint
UnlockTaskData();
#if DEBUGCW
StartupStore(_T("...... processed wps: %d\n"),numwpscanned);
#endif
}
