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); }
/* * VENTA3 Alternates destinations * * Used by Alternates and BestAlternate * * Colors VGR are disabled, but available */ void GlideComputerTask::DoAlternates(int AltWaypoint) { if (!way_points.verify_index(AltWaypoint)) { return; } const WAYPOINT &way_point = way_points.get(AltWaypoint); WPCALC &way_point_calc = way_points.set_calc(AltWaypoint); GEOPOINT w1 = way_point.Location; GEOPOINT w0 = Basic().Location; double *altwp_dist = &way_point_calc.Distance; double *altwp_gr = &way_point_calc.GR; double *altwp_arrival = &way_point_calc.AltArrival; short *altwp_vgr = &way_point_calc.VGR; *altwp_dist = Distance(w1, w0); double GRsafecalc = Calculated().NavAltitude - (way_point.Altitude + SettingsComputer().SafetyAltitudeArrival); if (GRsafecalc <= 0) { *altwp_gr = INVALID_GR; } else { *altwp_gr = *altwp_dist / GRsafecalc; if (*altwp_gr > ALTERNATE_MAXVALIDGR || *altwp_gr < 0) { *altwp_gr = INVALID_GR; } else if (*altwp_gr < 1) { *altwp_gr = 1; } } // 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(way_point, way_point_calc); if ((*altwp_arrival - ALTERNATE_OVERSAFETY) > 0) { if (*altwp_gr <= (GlidePolar::bestld * SAFELD_FACTOR)) { *altwp_vgr = 1; // full green vgr } else if (*altwp_gr <= GlidePolar::bestld) { *altwp_vgr = 2; // yellow vgr } else *altwp_vgr = 3; // RED vgr } else { *altwp_vgr = 3; // full red } }
/* * 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); }
/** * 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); } }
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++) {