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
}
// This is also called by DoNearest and it is overwriting AltitudeRequired
double CalculateWaypointArrivalAltitude(NMEA_INFO *Basic, DERIVED_INFO *Calculated, int i) {
CScopeLock Lock(LockTaskData, UnlockTaskData);
double altReqd;
double wDistance, wBearing;
double wStartDistance=0, wStartBearing=0;
double safetyaltitudearrival;
safetyaltitudearrival=GetSafetyAltitude(i);
DistanceBearing(Basic->Latitude,
Basic->Longitude,
WayPointList[i].Latitude,
WayPointList[i].Longitude,
&wDistance, &wBearing);
WayPointCalc[i].Distance = wDistance;
WayPointCalc[i].Bearing = wBearing;
if (ISCAR) {
simpleETE(Basic,Calculated,i);
return (Basic->Altitude-WayPointList[i].Altitude);
}
altReqd = GlidePolar::MacCreadyAltitude ( GetMacCready(i,GMC_DEFAULT),
wDistance, wBearing,
Calculated->WindSpeed, Calculated->WindBearing,
0, 0, true, &WayPointCalc[i].NextETE);
// if gates are in use with a real task, and we are at start
// then calculate ETE for reaching the cylinder. Also working when we are
// in the wrong side of cylinder
if (UseGates() && !DoOptimizeRoute()) {
if (ActiveTaskPoint==0 && i==Task[0].Index ) {
if (PGStartOut) {
if (CorrectSide()) {
// start out, from outside cylinder
wStartDistance=wDistance-StartRadius;
wStartBearing=wBearing;
} else {
// start out, but inside cylinder
wStartDistance=StartRadius-wDistance;
wStartBearing=wBearing+180;
if (wStartBearing>360) wStartBearing-=360;
}
} else {
if (CorrectSide()) {
// start in, correct side is inside cylinder
wStartDistance=StartRadius-wDistance;
wStartBearing=wBearing+180;
if (wStartBearing>360) wStartBearing-=360;
} else {
// start in, and we are still outside
wStartDistance=wDistance-StartRadius;
wStartBearing=wBearing;
}
}
// we don't use GetMacCready(i,GMC_DEFAULT)
GlidePolar::MacCreadyAltitude ( MACCREADY,
wStartDistance, wStartBearing,
Calculated->WindSpeed, Calculated->WindBearing,
0, 0, true, &WayPointCalc[i].NextETE);
#ifdef DEBUGTGATES
StartupStore(_T("wStartDistance=%f wStartBearing=%f\n"),wStartDistance,wStartBearing);
#endif
}
}
// we should build a function for this since it is used also in lkcalc
WayPointCalc[i].AltReqd[AltArrivMode] = altReqd+safetyaltitudearrival+WayPointList[i].Altitude -Calculated->EnergyHeight;
WayPointCalc[i].AltArriv[AltArrivMode] = Calculated->NavAltitude + Calculated->EnergyHeight
- altReqd
- WayPointList[i].Altitude
- safetyaltitudearrival;
/*
WayPointCalc[i].AltArriv[ALTA_AVEFF] = Calculated->NavAltitude
- (wDistance / GetCurrentEfficiency(Calculated, 0))
- WayPointList[i].Altitude
-safetyaltitudearrival;
WayPointCalc[i].AltReqd[ALTA_AVEFF] = Calculated->NavAltitude - WayPointCalc[i].AltArriv[ALTA_AVEFF];
WayPointCalc[i].NextETE=600.0;
*/
// for GA recalculate simple ETE
if (ISGAAIRCRAFT) {
simpleETE(Basic,Calculated,i);
}
return(WayPointCalc[i].AltArriv[AltArrivMode]);
}
