int CalculateWaypointApproxDistance(int scx_aircraft, int scy_aircraft, int i) { // Do preliminary fast search, by converting to screen coordinates int sc_x, sc_y; LatLon2Flat(WayPointList[i].Longitude, WayPointList[i].Latitude, &sc_x, &sc_y); int dx, dy; dx = scx_aircraft-sc_x; dy = scy_aircraft-sc_y; return isqrt4(dx*dx+dy*dy); }
/** * 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++) {