/**
* SearchBestAlternate() beta
* based on SortLandableWaypoints and extended
* @author Paolo Ventafridda
*/
void
GlideComputerTask::SearchBestAlternate()
{
int SortedLandableIndex[MAXBEST];
double SortedArrivalAltitude[MAXBEST];
int SortedApproxDistance[MAXBEST*2];
int SortedApproxIndex[MAXBEST*2];
int i, k, l;
double arrival_altitude;
int active_bestalternate_on_entry=-1;
int bestalternate=-1;
/*
* VENTA3 search in range of optimum gliding capability
* and in any case within an acceptable distance, say 100km.
* Anything else is not considered, since we want a safe landing not a long glide.
* Preferred waypoints and home are nevertheless checked in any case later.
* Notice that if you are over 100km away from the nearest non-preferred landing point you can't
* expect a computer to be helpful in case of troubles.
*
* ApproxDistance is in km, very approximate
*/
double searchrange=(Basic().Altitude-
SettingsComputer().SafetyAltitudeArrival)
*GlidePolar::bestld /1000;
if (searchrange <= 0)
searchrange=2; // lock to home airport at once
if (searchrange > ALTERNATE_MAXRANGE)
searchrange=ALTERNATE_MAXRANGE;
active_bestalternate_on_entry = Calculated().BestAlternate;
// Do preliminary fast search
POINT sc_aircraft;
LatLon2Flat(Basic().Location, sc_aircraft);
// Clear search lists
for (i=0; i<MAXBEST*2; i++) {
SortedApproxIndex[i]= -1;
SortedApproxDistance[i] = 0;
}
for (i = 0; way_points.verify_index(i); i++) {
const WAYPOINT &way_point = way_points.get(i);
if (!(((way_point.Flags & AIRPORT) == AIRPORT) ||
((way_point.Flags & LANDPOINT) == LANDPOINT))) {
continue; // ignore non-landable fields
}
int approx_distance =
CalculateWaypointApproxDistance(sc_aircraft, way_point);
// Size a reasonable distance, wide enough VENTA3
if ( approx_distance > searchrange ) continue;
// see if this fits into slot
for (k=0; k< MAXBEST*2; k++) {
if (((approx_distance < SortedApproxDistance[k])
// wp is closer than this one
|| (SortedApproxIndex[k]== -1)) // or this one isn't filled
&& (SortedApproxIndex[k]!= i)) // and not replacing with same
{
// ok, got new biggest, put it into the slot.
for (l=MAXBEST*2-1; l>k; l--) {
if (l>0) {
SortedApproxDistance[l] = SortedApproxDistance[l-1];
SortedApproxIndex[l] = SortedApproxIndex[l-1];
}
}
SortedApproxDistance[k] = approx_distance;
SortedApproxIndex[k] = i;
k=MAXBEST*2;
}
} // for k
} // for i
// Now do detailed search
for (i=0; i<MAXBEST; i++) {
SortedLandableIndex[i]= -1;
SortedArrivalAltitude[i] = 0;
}
bool found_reachable_airport = false;
for (int scan_airports_slot=0;
scan_airports_slot<2;
scan_airports_slot++) {
if (found_reachable_airport ) {
continue; // don't bother filling the rest of the list
}
for (i=0; i<MAXBEST*2; i++) {
if (SortedApproxIndex[i]<0) { // ignore invalid points
continue;
}
const WAYPOINT &way_point = way_points.get(SortedApproxIndex[i]);
WPCALC &wpcalc = way_points.set_calc(SortedApproxIndex[i]);
if ((scan_airports_slot==0) &&
((way_point.Flags & AIRPORT) != AIRPORT)) {
// we are in the first scan, looking for airports only
continue;
}
arrival_altitude = CalculateWaypointArrivalAltitude(way_point, wpcalc);
wpcalc.AltArrival = arrival_altitude;
// This is holding the real arrival value
/*
* We can't use degraded polar here, but we can't accept an
* arrival 1m over safety. That is 2m away from being
* unreachable! So we higher this value to 100m.
*/
arrival_altitude -= ALTERNATE_OVERSAFETY;
if (scan_airports_slot==0) {
if (arrival_altitude<0) {
// in first scan, this airport is unreachable, so ignore it.
continue;
} else
// this airport is reachable
found_reachable_airport = true;
}
// see if this fits into slot
for (k=0; k< MAXBEST; k++) {
if (((arrival_altitude > SortedArrivalAltitude[k])
// closer than this one
||(SortedLandableIndex[k]== -1))
// or this one isn't filled
&&(SortedLandableIndex[k]!= i)) // and not replacing
// with same
{
double wp_distance, wp_bearing;
DistanceBearing(Basic().Location, way_point.Location,
&wp_distance, &wp_bearing);
wpcalc.Distance = wp_distance;
wpcalc.Bearing = wp_bearing;
bool out_of_range;
terrain.Lock();
double distance_soarable =
FinalGlideThroughTerrain(wp_bearing,
Basic(), Calculated(),
SettingsComputer(),
terrain,
NULL,
wp_distance,
&out_of_range, NULL);
terrain.Unlock();
if ((distance_soarable>= wp_distance)||(arrival_altitude<0)) {
// only put this in the index if it is reachable
// and doesn't go through terrain, OR, if it is unreachable
// it doesn't matter if it goes through terrain because
// pilot has to climb first anyway
// ok, got new biggest, put it into the slot.
for (l=MAXBEST-1; l>k; l--) {
if (l>0) {
SortedArrivalAltitude[l] = SortedArrivalAltitude[l-1];
SortedLandableIndex[l] = SortedLandableIndex[l-1];
}
}
SortedArrivalAltitude[k] = arrival_altitude;
SortedLandableIndex[k] = SortedApproxIndex[i];
k=MAXBEST;
}
} // if (((arrival_altitude > SortedArrivalAltitude[k]) ...
} // for (k=0; k< MAXBEST; k++) {
}
}
// extended part by Paolo
bestalternate=-1; // reset the good choice
double safecalc = Calculated().NavAltitude -
SettingsComputer().SafetyAltitudeArrival;
static double grpolar = GlidePolar::bestld *SAFELD_FACTOR;
int curwp, curbestairport=-1, curbestoutlanding=-1;
double curgr=0, curbestgr=INVALID_GR;
if ( safecalc <= 0 ) {
/*
* We're under the absolute safety altitude at MSL, can't be any better elsewhere!
* Use the closer, hopefully you are landing on your airport
*/
} else
for (k=0; k< MAXBEST; k++) {
curwp = SortedLandableIndex[k];
if ( !way_points.verify_index(curwp) ) {
continue;
// break; // that list is unsorted !
}
const WAYPOINT &way_point = way_points.get(curwp);
WPCALC &wpcalc = way_points.set_calc(SortedApproxIndex[i]);
// At the first unsafe landing, stop searching down the list and
// use the best found or the first
double grsafe = safecalc - way_point.Altitude;
if ( grsafe <= 0 ) {
/*
* We're under the safety altitude for this waypoint.
*/
break;
//continue;
}
wpcalc.GR = wpcalc.Distance / grsafe;
grsafe = wpcalc.GR;
curgr = wpcalc.GR;
if ( grsafe > grpolar ) {
/*
* Over degraded polar GR for this waypoint
*/
continue;
// break;
}
// Anything now is within degraded glide ratio, so if our homewaypoint is safely reachable then
// attempt to lock it even if we already have a valid best, even if it is preferred and even
// if it has a better GR
if ( (SettingsComputer().HomeWaypoint >= 0)
&& (curwp == SettingsComputer().HomeWaypoint) ) {
bestalternate = curwp;
break;
}
// If we already found a preferred, stop searching for anything but home
if (bestalternate >= 0 && way_points.get_calc(bestalternate).Preferred) {
continue;
}
// VENTA5 TODO: extend search on other preferred, choosing the closer one
// Preferred list has priority, first found is taken (could be smarted)
if (wpcalc.Preferred) {
bestalternate=curwp;
continue;
}
// else we remember the best landable GR found so far. We shall use this in case
// at the end of the search no home and no preferred were found.
if ( curgr < curbestgr ) {
if ((way_point.Flags & AIRPORT) == AIRPORT) {
curbestairport=curwp;
curbestgr=curgr; // ONLY FOR AIRPORT! NOT FOR OUTLANDINGS!!
}
else {
curbestoutlanding=curwp;
}
}
continue;
}
if ( bestalternate <0 ) {
if ( curbestairport >= 0 ) {
bestalternate=curbestairport;
} else {
if ( curbestoutlanding >= 0 ) {
bestalternate=curbestoutlanding;
} else {
/*
* Here we are in troubles, nothing really reachable, but we
* might still be lucky to be within the "yellow" glide
* path. In any case we select the best arrival altitude place
* available, even if it is "red".
*/
if ( way_points.verify_index(SortedLandableIndex[0]) ) {
bestalternate=SortedLandableIndex[0];
} else {
/*
* Else the Landable list is EMPTY, although we might be
* near to a landable point but the terrain obstacles look
* too high (or the DEM resolution is not high enough to
* show a passage).
*
* Still the old BestAlternate could simply be out of range,
* but reachable... These values have certainly just been
* calculated by DoAlternates() , so they are usable.
*/
// Attempt to use the old best, but check there's one.. it
// might be empty for the first run
if ( way_points.verify_index(active_bestalternate_on_entry) )
{
bestalternate=active_bestalternate_on_entry;
if (way_points.get_calc(bestalternate).AltArrival < 0) {
// Pick up the closest!
if ( way_points.verify_index( SortedApproxIndex[0]) ) {
bestalternate=SortedApproxIndex[0];
} else {
// CRITIC POINT
// Otherwise .. nothing, either keep the old best or
// set it empty
// Put here "PULL-UP! PULL-UP! Boeing
// cockpit voice sound and possibly shake the stick.
}
} else
{
// MapWindow2 is checking for reachables separately,
// se let's see if this closest is reachable
if ( way_points.verify_index( SortedApproxIndex[0] )) {
if (way_points.get_calc(SortedApproxIndex[0]).Reachable) {
bestalternate = SortedApproxIndex[0];
} else
{
}
} else
{
}
}
} else
{
// CRITIC POINT
}
}
/*
* Don't make any sound at low altitudes, pilot is either taking off
* or landing, or searching for an immediate outlanding. Do not disturb.
* If safetyaltitude is 300m, then below 500m be quiet.
* If there was no active alternate on entry, and nothing was found, then we
* better be quiet since probably the user had already been alerted previously
* and now he is low..
*/
if ( bestalternate >0 &&
((safecalc - way_points.get(bestalternate).Altitude) > ALTERNATE_QUIETMARGIN)) {
if (way_points.get_calc(bestalternate).AltArrivalAGL <100 )
AlertBestAlternate(2);
// if (EnableSoundModes) PlayResource(TEXT("IDR_WAV_RED"));
}
}
}
}
/*
* If still invalid, it should mean we are just taking off
* in this case no problems, we set the very first bestalternate of the day as the home
* trusting the user to be home really!
*/
if ( bestalternate < 0 ) {
if ( SettingsComputer().HomeWaypoint >= 0 ) {
bestalternate=SettingsComputer().HomeWaypoint;
}
} else {
// If still invalid, i.e. not -1, then there's a big problem
if ( !way_points.verify_index(bestalternate) ) {
AlertBestAlternate(2);
Message::AddMessage(_T("Error, invalid best alternate!"));
// todo: immediate disable function
}
}
if (active_bestalternate_on_entry != bestalternate) {
SetCalculated().BestAlternate = bestalternate;
if ( bestalternate >0 &&
((safecalc - way_points.get(bestalternate).Altitude) > ALTERNATE_QUIETMARGIN))
AlertBestAlternate(1);
}
}
bool DoRangeWaypointList(NMEA_INFO *Basic, DERIVED_INFO *Calculated) {
size_t i;
int kl, kt, ka;
//double arrival_altitude;
#if DEBUG_DORANGE
StartupStore(_T(".... >> DoRangeWaypointList is running! <<\n"));
#endif
if (WayPointList.empty()) {
return false;
}
// Initialise turnpoint and landable range distance to default
// These are reset to defaults at DoInit time, also.
static int dstrangeturnpoint=DSTRANGETURNPOINT;
static int dstrangelandable=DSTRANGELANDABLE;
// Number of attempts to recalculate distances to reduce range, for huge cup files
// This will reduce number of waypoints in range, and fit them.
#define MAXRETUNEDST 6
// Number of attempts to retry the list to grow up again.
// This will only happen after a minimum granted interval, so this is really only
// safety limit to avoid unusual overloads (100x10m = 1000m = several hours)
#define MAXRETRYRETUNEDST 100
// The minimum interval after which is worth considering retuning distances.
// At 120kmh in 10m we do 20km. Taken.
#define MAXRETRYRETUNEINT 600 // 10m, minimum interval in seconds
static short retunecount=0;
static short retryretunecount=0;
static double lastRetryRetuneTime=0;
// Do init and RETURN, caution!
// We need a locked GPS position to proceed!
// TODO FIX LOCK DATA IN DRAWNEAREST when updating this list!
if (DoInit[MDI_DORANGEWAYPOINTLIST]) {
for (i=0; i<MAXRANGELANDABLE; i++) {
RangeLandableIndex[i]= -1;
RangeAirportIndex[i]= -1;
}
for (i=0; i<MAXRANGETURNPOINT; i++) {
RangeTurnpointIndex[i]= -1;
}
RangeLandableNumber=0;
RangeAirportNumber=0;
RangeTurnpointNumber=0;
// Reset all statics for Master DoInits actions
dstrangeturnpoint=DSTRANGETURNPOINT;
dstrangelandable=DSTRANGELANDABLE;
retunecount=0;
retryretunecount=0;
lastRetryRetuneTime=0;
DoInit[MDI_DORANGEWAYPOINTLIST]=false;
#if DEBUG_DORANGE
StartupStore(_T(".... >> DoRangeWaypointList INIT done, return <<\n"));
#endif
return false;
}
#if 0
DoStatusMessage(_T("WAIT RECALCULATING WAYPOINTS"));
#endif
LockTaskData();
int scx_aircraft, scy_aircraft;
LatLon2Flat(Basic->Longitude, Basic->Latitude, &scx_aircraft, &scy_aircraft);
bool retunedst_tps;
bool retunedst_lnd;
_retunedst:
retunedst_tps=false;
retunedst_lnd=false;
for (i=0; i<MAXRANGELANDABLE; i++) {
RangeLandableNumber=0;
RangeLandableIndex[i]= -1;
RangeAirportNumber=0;
RangeAirportIndex[i]= -1;
}
for (i=0; i<MAXRANGETURNPOINT; i++) {
RangeTurnpointNumber=0;
RangeTurnpointIndex[i]= -1;
}
#if DEBUG_DORANGE
StartupStore(_T(".... dstrangeturnpoint=%d dstrangelandable=%d\n"),dstrangeturnpoint,dstrangelandable);
#endif
for (i=0, kt=0, kl=0, ka=0; i<WayPointList.size(); i++) {
int approx_distance = CalculateWaypointApproxDistance(scx_aircraft, scy_aircraft, i);
// Size a reasonable distance, wide enough
if ( approx_distance > dstrangeturnpoint ) goto LabelLandables;
// Get only non landables
if (
( (TpFilter==(TpFilter_t)TfNoLandables) && (!WayPointCalc[i].IsLandable ) ) ||
( (TpFilter==(TpFilter_t)TfAll) ) ||
( (TpFilter==(TpFilter_t)TfTps) && ((WayPointList[i].Flags & TURNPOINT) == TURNPOINT) )
) {
if (kt+1<MAXRANGETURNPOINT) { // never mind if we use maxrange-2
RangeTurnpointIndex[kt++]=i;
RangeTurnpointNumber++;
#if DEBUG_DORANGE
StartupStore(_T(".. insert turnpoint <%s>\n"),WayPointList[i].Name);
#endif
} else {
#if DEBUG_DORANGE
StartupStore(_T("... OVERFLOW RangeTurnpoint cannot insert <%s> in list\n"),WayPointList[i].Name);
#endif
// Attempt to reduce range since we have a huge concentration of waypoints
// This can only happen "count" times, and it is not reversible: it wont grow up anymore.
if (retunecount <MAXRETUNEDST) {
#if DEBUG_DORANGE
StartupStore(_T("... Retunedst attempt %d of %d\n"),retunecount+1,MAXRETUNEDST);
#endif
retunedst_tps=true;
retunecount++;
break;
}
}
}
LabelLandables:
if ( approx_distance > dstrangelandable ) continue;
// Skip non landable waypoints that are between DSTRANGETURNPOINT and DSTRANGELANDABLE
if (!WayPointCalc[i].IsLandable )
continue;
if (kl+1<MAXRANGELANDABLE) { // never mind if we use maxrange-2
RangeLandableIndex[kl++]=i;
RangeLandableNumber++;
#if DEBUG_DORANGE
StartupStore(_T(".. insert landable <%s>\n"),WayPointList[i].Name);
#endif
}
else {
#if DEBUG_DORANGE
StartupStore(_T("... OVERFLOW RangeLandable cannot insert <%s> in list\n"),WayPointList[i].Name);
#endif
if (retunecount <MAXRETUNEDST) {
#if DEBUG_DORANGE
StartupStore(_T("... Retunedst attempt %d of %d\n"),retunecount+1,MAXRETUNEDST);
#endif
retunedst_lnd=true;
retunecount++;
break;
}
}
// If it's an Airport then we also take it into account separately
if ( WayPointCalc[i].IsAirport )
{
if (ka+1<MAXRANGELANDABLE) { // never mind if we use maxrange-2
RangeAirportIndex[ka++]=i;
RangeAirportNumber++;
}
else {
#if DEBUG_DORANGE
StartupStore(_T("... OVERFLOW RangeAirport cannot insert <%s> in list\n"),WayPointList[i].Name);
#endif
if (retunecount <MAXRETUNEDST) {
#if DEBUG_DORANGE
StartupStore(_T("... Retunedst attempt %d of %d\n"),retunecount+1,MAXRETUNEDST);
#endif
retunedst_lnd=true;
retunecount++;
break;
}
}
}
} // for i
// We lower full value, we higher by halved values
#define TPTRANGEDELTA 12
#define LNDRANGEDELTA 15
if (retunedst_tps) {
dstrangeturnpoint-=TPTRANGEDELTA; // 100km 90 80 70 60 50
if (dstrangeturnpoint<30) dstrangeturnpoint=30; // min 30km radius
#if (DEBUG_DORANGE || TESTBENCH)
StartupStore(_T("... Retuning dstrangeturnpoint to %d km \n"),dstrangeturnpoint);
#endif
goto _retunedst;
}
if (retunedst_lnd) {
dstrangelandable-=LNDRANGEDELTA; // 150km - 135 - 120 - 105 - 90 - 75
if (dstrangelandable<50) dstrangelandable=50; // min 50km radius
#if (DEBUG_DORANGE || TESTBENCH)
StartupStore(_T("... Retuning dstrangelandable to %d km \n"),dstrangelandable);
#endif
goto _retunedst;
}
// This should only happen in simulation, logger replay mode
// But no problems to do a spare more check during flight, caring about
// faulty gps firmware.
if (Basic->Time < (lastRetryRetuneTime-10) ) {
#if (DEBUG_DORANGE || TESTBENCH)
StartupStore(_T("... lastRetryRetuneTime back in time, reset.\n"));
#endif
lastRetryRetuneTime=Basic->Time;
}
// Set it for the first time only
if (lastRetryRetuneTime==0) lastRetryRetuneTime=Basic->Time;
#if TESTBENCH
StartupStore(_T(".... RangeTurnepoint=%d/%d (TPrange=%d) RangeLandable=%d/%d (LNrange=%d) retunecount=%d retrycount=%d sinceretry=%0.0fs \n"),
RangeTurnpointNumber, MAXRANGETURNPOINT, dstrangeturnpoint,
RangeLandableNumber, MAXRANGELANDABLE, dstrangelandable,
retunecount, retryretunecount,
Basic->Time-lastRetryRetuneTime);
#endif
// If we are not over the counter limit, and enough time has passed, check for recalibration
if (retryretunecount<MAXRETRYRETUNEDST && ((Basic->Time - lastRetryRetuneTime)>MAXRETRYRETUNEINT) ) {
bool retryretune=false;
// If we have space for 20% more points, lets retry to use them in the next run
if ( (dstrangeturnpoint<DSTRANGETURNPOINT) && ((MAXRANGETURNPOINT - RangeTurnpointNumber) > (MAXRANGETURNPOINT/5)) ) {
#if (DEBUG_DORANGE || TESTBENCH)
StartupStore(_T(".... (TPT) Retryretune=%d, RangeTurnepoint=%d too low, range grow from %d to %d\n"),
retryretunecount,RangeTurnpointNumber, dstrangeturnpoint, dstrangeturnpoint+(TPTRANGEDELTA/2));
#endif
dstrangeturnpoint+=(TPTRANGEDELTA/2);
retryretune=true;
}
if ( (dstrangelandable<DSTRANGELANDABLE) && ((MAXRANGELANDABLE - RangeLandableNumber) > (MAXRANGELANDABLE/5)) ) {
#if (DEBUG_DORANGE || TESTBENCH)
StartupStore(_T(".... (LND) Retryretune=%d, RangeLandable=%d too low, range grow from %d to %d\n"),
retryretunecount,RangeLandableNumber, dstrangelandable, dstrangelandable+(LNDRANGEDELTA/2));
#endif
dstrangelandable+=(LNDRANGEDELTA/2);
retryretune=true;
}
if (retryretune) {
if (dstrangeturnpoint>DSTRANGETURNPOINT) dstrangeturnpoint=DSTRANGETURNPOINT;
if (dstrangelandable>DSTRANGELANDABLE) dstrangelandable=DSTRANGELANDABLE;
// Notice that we are storing the time we flagged the reequest, which will happen later on!
lastRetryRetuneTime=Basic->Time;
retryretunecount++;
retunecount=0;
#if DEBUG_DORANGE
StartupStore(_T(".... (Retry retune flagged for next time dorange)\n"));
#endif
}
}
// We have filled the list... which was too small
// this cannot happen with UNSORTED RANGE
if (RangeTurnpointNumber>MAXRANGETURNPOINT) RangeTurnpointNumber=MAXRANGETURNPOINT;
if (RangeAirportNumber>MAXRANGELANDABLE) RangeAirportNumber=MAXRANGELANDABLE;
if (RangeLandableNumber>MAXRANGELANDABLE) RangeLandableNumber=MAXRANGELANDABLE;
UnlockTaskData();
return true;
}
void SearchBestAlternate(NMEA_INFO *Basic,
DERIVED_INFO *Calculated)
{
int sortedLandableIndex[MAXBEST];
double sortedArrivalAltitude[MAXBEST];
int sortApproxDistance[MAXBEST*2];
int sortApproxIndex[MAXBEST*2];
int i, k, l;
int j;
double arrival_altitude;
int active_bestalternate_on_entry=-1;
int bestalternate=-1;
#ifdef DEBUG_BESTALTERNATE
TCHAR ventabuffer[200];
#endif
if (WayPointList.empty()) return;
CScopeLock Lock(LockTaskData, UnlockTaskData);
if( DisableBestAlternate ) {
if ( HomeWaypoint >= 0 )
BestAlternate=HomeWaypoint;
else
BestAlternate = 0; // RESWP_TAKEOFF
return;
}
// We are not considering total energy here, forbidden for safety reasons
// V5: double searchrange=(Calculated->NavAltitude-(SAFETYALTITUDEARRIVAL/10))* GlidePolar::bestld /1000;
// V6: we enlarge preliminar search range because of possible tail wind for some destinations
double searchrange=(Calculated->NavAltitude)* 1.2 * GlidePolar::bestld /1000;
if (searchrange <= 0)
searchrange=2; // lock to home airport at once
if (searchrange > ALTERNATE_MAXRANGE)
searchrange=ALTERNATE_MAXRANGE;
active_bestalternate_on_entry = BestAlternate;
// Do preliminary fast search
int scx_aircraft, scy_aircraft;
LatLon2Flat(Basic->Longitude, Basic->Latitude, &scx_aircraft, &scy_aircraft);
// Clear search lists
for (i=0; i<MAXBEST*2; i++) {
sortApproxIndex[i]= -1;
sortApproxDistance[i] = 0;
}
#ifdef LOGBEST
STS("\n\nNEW SEARCH\n\n"));
#endif
for (j=0; j<RangeLandableNumber; j++) {
i=RangeLandableIndex[j];
int approx_distance = CalculateWaypointApproxDistance(scx_aircraft, scy_aircraft, i);
// Size a reasonable distance, wide enough
if ( approx_distance > searchrange ) continue;
// see if this fits into slot
for (k=0; k< MAXBEST*2; k++) {
if (((approx_distance < sortApproxDistance[k])
// wp is closer than this one
|| (sortApproxIndex[k]== -1)) // or this one isn't filled
&& (sortApproxIndex[k]!= i)) // and not replacing with same
{
// ok, got new biggest, put it into the slot.
for (l=MAXBEST*2-1; l>k; l--) {
if (l>0) {
sortApproxDistance[l] = sortApproxDistance[l-1];
sortApproxIndex[l] = sortApproxIndex[l-1];
}
}
sortApproxDistance[k] = approx_distance;
sortApproxIndex[k] = i;
#ifdef LOGBEST
STS("INSERT FROM RANGELANDABLE: [%d] %d %s\n"),k,sortApproxIndex[k],WayPointList[sortApproxIndex[k]].Name);
#endif
k=MAXBEST*2;
}
} // for k
#ifdef LOGBEST
STS("------------\n"));
#endif
} // for all waypoints, or the reduced list by Range
#ifdef DEBUG_BESTALTERNATE
FILE *fp;
if ( (fp=_tfopen(_T("DEBUG.TXT"),_T("a"))) != NULL ) {
_stprintf(ventabuffer,TEXT("==================\n"));
fprintf(fp,"%S",ventabuffer);
_stprintf(ventabuffer,TEXT("[GPSTIME=%02d:%02d:%02d] Altitude=%dm searchrange=%dKm Curr.Best=%d\n\n"),
GPS_INFO.Hour, GPS_INFO.Minute, GPS_INFO.Second,
(int)Calculated->NavAltitude, (int)searchrange, BestAlternate);
fprintf(fp,"%S",ventabuffer);
for ( int dbug=0; dbug<MAXBEST*2; dbug++) {
if ( sortApproxIndex[dbug] <0 ) _stprintf(ventabuffer,_T("%d=empty\n"), dbug);
else
_stprintf(ventabuffer,TEXT("%d=%s(%d)\n"), dbug,
WayPointList[sortApproxIndex[dbug]].Name, sortApproxDistance[dbug] );
fprintf(fp,"%S",ventabuffer);
}
fclose(fp);
} else
DoStatusMessage(_T("CANNOT OPEN DEBUG FILE"));
#endif
// Now do detailed search
for (i=0; i<MAXBEST; i++) {
sortedLandableIndex[i]= -1;
sortedArrivalAltitude[i] = 0;
}
for (int scan_airports_slot=0; scan_airports_slot<2; scan_airports_slot++) {
#ifdef LOGBEST
STS("SCAN SLOT= %d\n"),scan_airports_slot);
#endif
for (i=0; i<MAXBEST*2; i++) {
if (sortApproxIndex[i]<0) { // ignore invalid points
continue;
}
#ifdef LOGBEST
STS("Examine: [%d] %d %s\n"),i,sortApproxIndex[i],WayPointList[sortApproxIndex[i]].Name);
#endif
if ((scan_airports_slot==0) &&
(!WayPointCalc[sortApproxIndex[i]].IsAirport))
{
// we are in the first scan, looking for airports only
// In second scan we accept again airports and also outlandings
#ifdef LOGBEST
STS("... scanning only for airports, this one is not an airport\n"));
#endif
continue;
}
arrival_altitude = CalculateWaypointArrivalAltitude(Basic, Calculated, sortApproxIndex[i]);
#ifdef LOGBEST
STS("...... arrival altitude is %f\n"),arrival_altitude);
#endif
WayPointCalc[sortApproxIndex[i]].AltArriv[AltArrivMode] = arrival_altitude;
// This is holding the real arrival value
if (scan_airports_slot==0) {
if (arrival_altitude<0) {
#ifdef LOGBEST
STS("... scanning only for airports, and this is unreachable (%f), continue\n"),arrival_altitude);
#endif
// in first scan, this airport is unreachable, so ignore it.
continue;
}
}
// see if this fits into slot
for (k=0; k< MAXBEST; k++) {
if (((arrival_altitude > sortedArrivalAltitude[k])
// closer than this one
||(sortedLandableIndex[k]== -1))
// or this one isn't filled
&&(sortedLandableIndex[k]!= sortApproxIndex[i])) // and not replacing with same
{
double wp_distance, wp_bearing;
DistanceBearing(Basic->Latitude , Basic->Longitude ,
WayPointList[sortApproxIndex[i]].Latitude,
WayPointList[sortApproxIndex[i]].Longitude,
&wp_distance, &wp_bearing);
WayPointCalc[sortApproxIndex[i]].Distance = wp_distance;
WayPointCalc[sortApproxIndex[i]].Bearing = wp_bearing;
bool out_of_range;
#ifdef GTL2
double distance_soarable = FinalGlideThroughTerrain(wp_bearing, Basic->Latitude,
Basic->Longitude, Calculated->NavAltitude, Calculated,
#else
double distance_soarable = FinalGlideThroughTerrain(wp_bearing, Basic, Calculated,
#endif
NULL, NULL, wp_distance, &out_of_range, NULL);
// V5 bug: if ((distance_soarable>= wp_distance)||(arrival_altitude<0)) {
if ((distance_soarable>= wp_distance)) {
// only put this in the index if it is reachable
// and doesn't go through terrain, OR, if it is unreachable
// it doesn't matter if it goes through terrain because
// pilot has to climb first anyway
// 160407: THE ^^ ABOVE WAS A BUG. Because we were inserting a waypoint
// if it had no obstacles, OR if it had an obstacle but with a NEGATIVE arrival
// altitude. We were thus including negative arrival altitudes wp.
// ok, got new biggest, put it into the slot.
for (l=MAXBEST-1; l>k; l--) {
if (l>0) {
sortedArrivalAltitude[l] = sortedArrivalAltitude[l-1];
sortedLandableIndex[l] = sortedLandableIndex[l-1];
}
}
sortedArrivalAltitude[k] = arrival_altitude;
sortedLandableIndex[k] = sortApproxIndex[i];
#ifdef LOGBEST
STS("INSERT INTO LANDABLES: [%d] %d %s\n"),k,sortedLandableIndex[k],WayPointList[sortedLandableIndex[k]].Name);
#endif
k=MAXBEST;
}
} // if (((arrival_altitude > sortedArrivalAltitude[k]) ...
} // for (k=0; k< MAXBEST; k++) {
