コード例 #1
0
ファイル: DoNearest.cpp プロジェクト: PhilColbert/LK8000
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);
}
コード例 #2
0
ファイル: BestAlternate.cpp プロジェクト: bugburner/xcsoar
/*
 * 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
  }
}
コード例 #3
0
ファイル: DoAlternates.cpp プロジェクト: IvanSantanna/LK8000
/*
 * 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);
} 
コード例 #4
0
ファイル: BestAlternate.cpp プロジェクト: bugburner/xcsoar
/**
 * 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);
  }
}
コード例 #5
0
ファイル: BestAlternate.cpp プロジェクト: PhilColbert/LK8000
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++) {