src/Navaids/navlist.cxx

   1 // navlist.cxx -- navaids management class
   2 //
   3 // Written by Curtis Olson, started April 2000.
   4 //
   5 // Copyright (C) 2000  Curtis L. Olson - http://www.flightgear.org/~curt
   6 //
   7 // This program is free software; you can redistribute it and/or
   8 // modify it under the terms of the GNU General Public License as
   9 // published by the Free Software Foundation; either version 2 of the
  10 // License, or (at your option) any later version.
  11 //
  12 // This program is distributed in the hope that it will be useful, but
  13 // WITHOUT ANY WARRANTY; without even the implied warranty of
  14 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  15 // General Public License for more details.
  16 //
  17 // You should have received a copy of the GNU General Public License
  18 // along with this program; if not, write to the Free Software
  19 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  20 //
  21 // $Id$
  22
  23
  24 #ifdef HAVE_CONFIG_H
  25 #  include <config.h>
  26 #endif
  27
  28 #include <boost/foreach.hpp>
  29 #include <algorithm>
  30
  31 #include <simgear/debug/logstream.hxx>
  32 #include <simgear/math/sg_geodesy.hxx>
  33 #include <simgear/sg_inlines.h>
  34
  35 #include "navlist.hxx"
  36
  37 #include <Airports/runways.hxx>
  38
  39 using std::string;
  40
  41 namespace { // anonymous
  42
  43 class NavRecordDistanceSortPredicate
  44 {
  45 public:
  46   NavRecordDistanceSortPredicate( const SGGeod & position ) :
  47   _position(SGVec3d::fromGeod(position)) {}
  48
  49   bool operator()( const nav_rec_ptr & n1, const nav_rec_ptr & n2 )
  50   {
  51     if( n1 == NULL || n2 == NULL ) return false;
  52     return distSqr(n1->cart(), _position) < distSqr(n2->cart(), _position);
  53   }
  54 private:
  55   SGVec3d _position;
  56
  57 };
  58
  59 } // of anonymous namespace
  60
  61 // FGNavList ------------------------------------------------------------------
  62
  63
  64 FGNavList::TypeFilter::TypeFilter(const FGPositioned::Type type)
  65 {
  66   if (type == FGPositioned::INVALID) {
  67     _mintype = FGPositioned::VOR;
  68     _maxtype = FGPositioned::GS;
  69   } else {
  70     _mintype = _maxtype = type;
  71   }
  72 }
  73
  74
  75 FGNavList::FGNavList( void )
  76 {
  77 }
  78
  79
  80 FGNavList::~FGNavList( void )
  81 {
  82     nav_list_type navlist = navaids.begin()->second;
  83     navaids.erase( navaids.begin(), navaids.end() );
  84 }
  85
  86
  87 // load the navaids and build the map
  88 bool FGNavList::init()
  89 {
  90     // No need to delete the original navaid structures
  91     // since we're using an SGSharedPointer
  92     nav_list_type navlist = navaids.begin()->second;
  93     navaids.erase( navaids.begin(), navaids.end() );
  94     return true;
  95 }
  96
  97 // add an entry to the lists
  98 bool FGNavList::add( FGNavRecord *n )
  99 {
 100     navaids[n->get_freq()].push_back(n);
 101     return true;
 102 }
 103
 104 FGNavRecord *FGNavList::findByFreq( double freq, const SGGeod& position)
 105 {
 106     const nav_list_type& stations = navaids[(int)(freq*100.0 + 0.5)];
 107     SG_LOG( SG_INSTR, SG_DEBUG, "findbyFreq " << freq << " size " << stations.size()  );
 108     return findNavFromList( position, stations );
 109 }
 110
 111 nav_list_type FGNavList::findAllByFreq( double freq, const SGGeod& position, const FGPositioned::Type type)
 112 {
 113   nav_list_type stations;
 114   TypeFilter filter(type);
 115
 116   BOOST_FOREACH(nav_rec_ptr nav, navaids[(int)(freq*100.0 + 0.5)]) {
 117     if (filter.pass(nav.ptr())) {
 118       stations.push_back(nav);
 119     }
 120   }
 121
 122   NavRecordDistanceSortPredicate sortPredicate( position );
 123   std::sort( stations.begin(), stations.end(), sortPredicate );
 124   return stations;
 125 }
 126
 127
 128 // Given an Ident and optional freqency, return the first matching
 129 // station.
 130 const nav_list_type FGNavList::findByIdentAndFreq(const string& ident, const double freq, const FGPositioned::Type type )
 131 {
 132   FGPositionedRef cur;
 133   TypeFilter filter(type);
 134   nav_list_type reply;
 135
 136   cur = FGPositioned::findNextWithPartialId(cur, ident, &filter);
 137
 138   int f = (int)(freq*100.0 + 0.5);
 139   while (cur) {
 140     FGNavRecord* nav = static_cast<FGNavRecord*>(cur.ptr());
 141     if ( f <= 0.0 || nav->get_freq() == f) {
 142         reply.push_back( nav );
 143     }
 144
 145     cur = FGPositioned::findNextWithPartialId(cur, ident, &filter);
 146   }
 147
 148   return reply;
 149 }
 150
 151 // Given an Ident and optional freqency and type ,
 152 // return a list of matching stations sorted by distance to the given position
 153 const nav_list_type FGNavList::findByIdentAndFreq( const SGGeod & position,
 154         const std::string& ident, const double freq, const FGPositioned::Type type )
 155 {
 156     nav_list_type reply = findByIdentAndFreq( ident, freq, type );
 157     NavRecordDistanceSortPredicate sortPredicate( position );
 158     std::sort( reply.begin(), reply.end(), sortPredicate );
 159
 160     return reply;
 161 }
 162
 163 // discount navids if they conflict with another on the same frequency
 164 // this only applies to navids associated with opposite ends of a runway,
 165 // with matching frequencies.
 166 static bool navidUsable(FGNavRecord* aNav, const SGGeod &aircraft)
 167 {
 168   FGRunway* r(aNav->runway());
 169   if (!r || !r->reciprocalRunway()) {
 170     return true;
 171   }
 172
 173 // check if the runway frequency is paired
 174   FGNavRecord* locA = r->ILS();
 175   FGNavRecord* locB = r->reciprocalRunway()->ILS();
 176
 177   if (!locA || !locB || (locA->get_freq() != locB->get_freq())) {
 178     return true; // not paired, ok
 179   }
 180
 181 // okay, both ends have an ILS, and they're paired. We need to select based on
 182 // aircraft position. What we're going to use is *runway* (not navid) position,
 183 // ie whichever runway end we are closer too. This makes back-course / missed
 184 // approach behaviour incorrect, but that's the price we accept.
 185   double crs = SGGeodesy::courseDeg(aircraft, r->geod());
 186   double hdgDiff = crs - r->headingDeg();
 187   SG_NORMALIZE_RANGE(hdgDiff, -180.0, 180.0);
 188   return (fabs(hdgDiff) < 90.0);
 189 }
 190
 191 // Given a point and a list of stations, return the closest one to
 192 // the specified point.
 193 FGNavRecord* FGNavList::findNavFromList( const SGGeod &aircraft,
 194                                   const nav_list_type &stations )
 195 {
 196     FGNavRecord *nav = NULL;
 197     double d2;                  // in meters squared
 198     double min_dist
 199         = FG_NAV_MAX_RANGE*SG_NM_TO_METER*FG_NAV_MAX_RANGE*SG_NM_TO_METER;
 200     SGVec3d aircraftCart = SGVec3d::fromGeod(aircraft);
 201
 202     nav_list_const_iterator it;
 203     nav_list_const_iterator end = stations.end();
 204     // find the closest station within a sensible range (FG_NAV_MAX_RANGE)
 205     for ( it = stations.begin(); it != end; ++it ) {
 206         FGNavRecord *station = *it;
 207         d2 = distSqr(station->cart(), aircraftCart);
 208         if ( d2 > min_dist || !navidUsable(station, aircraft)) {
 209           continue;
 210         }
 211
 212         min_dist = d2;
 213         nav = station;
 214     }
 215
 216     return nav;
 217 }
 218
 219 // Given a frequency, return the first matching station.
 220 FGNavRecord *FGNavList::findStationByFreq( double freq )
 221 {
 222     const nav_list_type& stations = navaids[(int)(freq*100.0 + 0.5)];
 223
 224     SG_LOG( SG_INSTR, SG_DEBUG, "findStationByFreq " << freq << " size " << stations.size()  );
 225
 226     if (!stations.empty()) {
 227         return stations[0];
 228     }
 229     return NULL;
 230 }
 231
 232
 233
 234 // FGTACANList ----------------------------------------------------------------
 235
 236 FGTACANList::FGTACANList( void )
 237 {
 238 }
 239
 240
 241 FGTACANList::~FGTACANList( void )
 242 {
 243 }
 244
 245
 246 bool FGTACANList::init()
 247 {
 248     return true;
 249 }
 250
 251
 252 // add an entry to the lists
 253 bool FGTACANList::add( FGTACANRecord *c )
 254 {
 255     ident_channels[c->get_channel()].push_back(c);
 256     return true;
 257 }
 258
 259
 260 // Given a TACAN Channel return the first matching frequency
 261 FGTACANRecord *FGTACANList::findByChannel( const string& channel )
 262 {
 263     const tacan_list_type& stations = ident_channels[channel];
 264     SG_LOG( SG_INSTR, SG_DEBUG, "findByChannel " << channel<< " size " << stations.size() );
 265
 266     if (!stations.empty()) {
 267         return stations[0];
 268     }
 269     return NULL;
 270 }
 271
 272