]> git.mxchange.org Git - flightgear.git/blobdiff - src/Navaids/navlist.cxx
Prepare and implement reinit methods for instruments
[flightgear.git] / src / Navaids / navlist.cxx
index f490521253300af170d352f8635a13845d590753..a2bdcb082bd7fb088d205684d573e23ea105142c 100644 (file)
 #  include <config.h>
 #endif
 
+#include <boost/foreach.hpp>
+#include <algorithm>
+
 #include <simgear/debug/logstream.hxx>
 #include <simgear/math/sg_geodesy.hxx>
 #include <simgear/sg_inlines.h>
 
 #include "navlist.hxx"
 
+#include <Airports/runways.hxx>
+
 using std::string;
 
+namespace { // anonymous
+
+class NavRecordDistanceSortPredicate
+{
+public:
+  NavRecordDistanceSortPredicate( const SGGeod & position ) :
+  _position(SGVec3d::fromGeod(position)) {}
+  
+  bool operator()( const nav_rec_ptr & n1, const nav_rec_ptr & n2 )
+  {
+    if( n1 == NULL || n2 == NULL ) return false;
+    return distSqr(n1->cart(), _position) < distSqr(n2->cart(), _position);
+  }
+private:
+  SGVec3d _position;
+  
+};
+
+} // of anonymous namespace
+
 // FGNavList ------------------------------------------------------------------
 
+
+FGNavList::TypeFilter::TypeFilter(const FGPositioned::Type type)
+{
+  if (type == FGPositioned::INVALID) {
+    _mintype = FGPositioned::VOR;
+    _maxtype = FGPositioned::GS;
+  } else {
+    _mintype = _maxtype = type;
+  }
+}
+
+
 FGNavList::FGNavList( void )
 {
 }
@@ -71,86 +108,90 @@ FGNavRecord *FGNavList::findByFreq( double freq, const SGGeod& position)
     return findNavFromList( position, stations );
 }
 
-class VORNDBFilter : public FGPositioned::Filter
+nav_list_type FGNavList::findAllByFreq( double freq, const SGGeod& position, const FGPositioned::Type type)
 {
-public:
-  virtual FGPositioned::Type minType() const {
-    return FGPositioned::VOR;
+  nav_list_type stations;
+  TypeFilter filter(type);
+  
+  BOOST_FOREACH(nav_rec_ptr nav, navaids[(int)(freq*100.0 + 0.5)]) {
+    if (filter.pass(nav.ptr())) {
+      stations.push_back(nav);
+    }
   }
+  
+  NavRecordDistanceSortPredicate sortPredicate( position );
+  std::sort( stations.begin(), stations.end(), sortPredicate );
+  return stations;
+}
 
-  virtual FGPositioned::Type maxType()  const {
-    return FGPositioned::NDB;
-  }
-};
 
-// Given an Ident and optional freqency, return the first matching
+// Given an Ident and optional frequency, return the first matching
 // station.
-FGNavRecord *FGNavList::findByIdentAndFreq(const string& ident, const double freq )
+const nav_list_type FGNavList::findByIdentAndFreq(const string& ident, const double freq, const FGPositioned::Type type )
 {
   FGPositionedRef cur;
-  VORNDBFilter filter;
+  TypeFilter filter(type);
+  nav_list_type reply;
+
   cur = FGPositioned::findNextWithPartialId(cur, ident, &filter);
   
-  if (freq <= 0.0) {
-    return static_cast<FGNavRecord*>(cur.ptr()); // might be null
-  }
-  
   int f = (int)(freq*100.0 + 0.5);
   while (cur) {
     FGNavRecord* nav = static_cast<FGNavRecord*>(cur.ptr());
-    if (nav->get_freq() == f) {
-      return nav;
+    if ( f <= 0.0 || nav->get_freq() == f) {
+        reply.push_back( nav );
     }
     
     cur = FGPositioned::findNextWithPartialId(cur, ident, &filter);
   }
 
-  return NULL;
-}
-
-// LOC, ILS, GS, and DME antenna's could potentially be
-// installed at the opposite end of the runway.  So it's not
-// enough to simply find the closest antenna with the right
-// frequency.  We need the closest antenna with the right
-// frequency that is most oriented towards us.  (We penalize
-// stations that are facing away from us by adding 5000 meters
-// which is further than matching stations would ever be
-// placed from each other.  (Do the expensive check only for
-// directional atennas and only when there is a chance it is
-// the closest station.)
-
-static bool penaltyForNav(FGNavRecord* aNav, const SGGeod &aGeod)
-{
-  switch (aNav->type()) {
-  case FGPositioned::ILS:
-  case FGPositioned::LOC:
-  case FGPositioned::GS:
-// FIXME
-//  case FGPositioned::DME: we can't get the heading for a DME transmitter, oops
-    break;
-  default:
-    return false;
+  return reply;
+}
+
+// Given an Ident and optional frequency and type ,
+// return a list of matching stations sorted by distance to the given position
+const nav_list_type FGNavList::findByIdentAndFreq( const SGGeod & position,
+        const std::string& ident, const double freq, const FGPositioned::Type type )
+{
+    nav_list_type reply = findByIdentAndFreq( ident, freq, type );
+    NavRecordDistanceSortPredicate sortPredicate( position );
+    std::sort( reply.begin(), reply.end(), sortPredicate );
+
+    return reply;
+}
+
+// discount navids if they conflict with another on the same frequency
+// this only applies to navids associated with opposite ends of a runway,
+// with matching frequencies.
+static bool navidUsable(FGNavRecord* aNav, const SGGeod &aircraft)
+{
+  FGRunway* r(aNav->runway());
+  if (!r || !r->reciprocalRunway()) {
+    return true;
   }
   
-  double hdg_deg = 0.0;
-  if (aNav->type() == FGPositioned::GS) {
-    int tmp = (int)(aNav->get_multiuse() / 1000.0);
-    hdg_deg = aNav->get_multiuse() - (tmp * 1000);
-  } else {    
-    hdg_deg = aNav->get_multiuse();
+// check if the runway frequency is paired
+  FGNavRecord* locA = r->ILS();
+  FGNavRecord* locB = r->reciprocalRunway()->ILS();
+  
+  if (!locA || !locB || (locA->get_freq() != locB->get_freq())) {
+    return true; // not paired, ok
   }
   
-  double az1, az2, s;
-  SGGeodesy::inverse(aGeod, aNav->geod(), az1, az2, s);
-  az1 = az1 - hdg_deg;
-  SG_NORMALIZE_RANGE(az1, -180.0, 180.0);
-  return fabs(az1) > 90.0;
+// okay, both ends have an ILS, and they're paired. We need to select based on
+// aircraft position. What we're going to use is *runway* (not navid) position,
+// ie whichever runway end we are closer too. This makes back-course / missed
+// approach behaviour incorrect, but that's the price we accept.
+  double crs = SGGeodesy::courseDeg(aircraft, r->geod());
+  double hdgDiff = crs - r->headingDeg();
+  SG_NORMALIZE_RANGE(hdgDiff, -180.0, 180.0);  
+  return (fabs(hdgDiff) < 90.0);
 }
 
-// Given a point and a list of stations, return the closest one to the
-// specified point.
-FGNavRecord *FGNavList::findNavFromList( const SGGeod &aircraft,
-                                         const nav_list_type &stations )
+// Given a point and a list of stations, return the closest one to
+// the specified point.
+FGNavRecordFGNavList::findNavFromList( const SGGeod &aircraft,
+                                  const nav_list_type &stations )
 {
     FGNavRecord *nav = NULL;
     double d2;                  // in meters squared
@@ -163,18 +204,13 @@ FGNavRecord *FGNavList::findNavFromList( const SGGeod &aircraft,
     // find the closest station within a sensible range (FG_NAV_MAX_RANGE)
     for ( it = stations.begin(); it != end; ++it ) {
         FGNavRecord *station = *it;
-        // cout << "testing " << current->get_ident() << endl;
         d2 = distSqr(station->cart(), aircraftCart);
-        if ( d2 < min_dist && penaltyForNav(station, aircraft))
-        {
-          double dist = sqrt(d2);
-          d2 = (dist + 5000) * (dist + 5000);
+        if ( d2 > min_dist || !navidUsable(station, aircraft)) {
+          continue;
         }
         
-        if ( d2 < min_dist ) {
-            min_dist = d2;
-            nav = station;
-        }
+        min_dist = d2;
+        nav = station;
     }
 
     return nav;