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 <simgear/debug/logstream.hxx>
  29 #include <simgear/math/sg_geodesy.hxx>
  30 #include <simgear/sg_inlines.h>
  31
  32 #include "navlist.hxx"
  33
  34 #include <Airports/runways.hxx>
  35
  36 #include <algorithm>
  37
  38 using std::string;
  39
  40 // FGNavList ------------------------------------------------------------------
  41
  42 FGNavList::FGNavList( void )
  43 {
  44 }
  45
  46
  47 FGNavList::~FGNavList( void )
  48 {
  49     nav_list_type navlist = navaids.begin()->second;
  50     navaids.erase( navaids.begin(), navaids.end() );
  51 }
  52
  53
  54 // load the navaids and build the map
  55 bool FGNavList::init()
  56 {
  57     // No need to delete the original navaid structures
  58     // since we're using an SGSharedPointer
  59     nav_list_type navlist = navaids.begin()->second;
  60     navaids.erase( navaids.begin(), navaids.end() );
  61     return true;
  62 }
  63
  64 // add an entry to the lists
  65 bool FGNavList::add( FGNavRecord *n )
  66 {
  67     navaids[n->get_freq()].push_back(n);
  68     return true;
  69 }
  70
  71 FGNavRecord *FGNavList::findByFreq( double freq, const SGGeod& position)
  72 {
  73     const nav_list_type& stations = navaids[(int)(freq*100.0 + 0.5)];
  74     SG_LOG( SG_INSTR, SG_DEBUG, "findbyFreq " << freq << " size " << stations.size()  );
  75     return findNavFromList( position, stations );
  76 }
  77
  78 class TypeFilter : public FGPositioned::Filter
  79 {
  80 public:
  81     TypeFilter( const FGPositioned::Type mintype, const FGPositioned::Type maxtype ) : _mintype(mintype), _maxtype(maxtype) {}
  82
  83   virtual FGPositioned::Type minType() const {
  84     return _mintype;
  85   }
  86
  87   virtual FGPositioned::Type maxType()  const {
  88     return _maxtype;
  89   }
  90 private:
  91     FGPositioned::Type _mintype;
  92     FGPositioned::Type _maxtype;
  93 };
  94
  95 // Given an Ident and optional freqency, return the first matching
  96 // station.
  97 const nav_list_type FGNavList::findByIdentAndFreq(const string& ident, const double freq, const FGPositioned::Type type )
  98 {
  99   FGPositionedRef cur;
 100   TypeFilter filter(
 101       type == FGPositioned::INVALID ? FGPositioned::VOR : type,
 102       type == FGPositioned::INVALID ? FGPositioned::NDB : type );
 103   nav_list_type reply;
 104
 105   cur = FGPositioned::findNextWithPartialId(cur, ident, &filter);
 106
 107   int f = (int)(freq*100.0 + 0.5);
 108   while (cur) {
 109     FGNavRecord* nav = static_cast<FGNavRecord*>(cur.ptr());
 110     if ( f <= 0.0 || nav->get_freq() == f) {
 111         reply.push_back( nav );
 112     }
 113
 114     cur = FGPositioned::findNextWithPartialId(cur, ident, &filter);
 115   }
 116
 117   return reply;
 118 }
 119
 120 class NavRecordDistanceSortPredicate
 121 {
 122 public:
 123     NavRecordDistanceSortPredicate( const SGGeod & position ) :
 124     _position(SGVec3d::fromGeod(position)) {}
 125
 126     bool operator()( const nav_rec_ptr & n1, const nav_rec_ptr & n2 )
 127     {
 128         if( n1 == NULL || n2 == NULL ) return false;
 129         return distSqr(n1->cart(), _position) < distSqr(n2->cart(), _position);
 130     }
 131 private:
 132     SGVec3d _position;
 133
 134 };
 135
 136 // Given an Ident and optional freqency and type ,
 137 // return a list of matching stations sorted by distance to the given position
 138 const nav_list_type FGNavList::findByIdentAndFreq( const SGGeod & position,
 139         const std::string& ident, const double freq, const FGPositioned::Type type )
 140 {
 141     nav_list_type reply = findByIdentAndFreq( ident, freq, type );
 142     NavRecordDistanceSortPredicate sortPredicate( position );
 143     std::sort( reply.begin(), reply.end(), sortPredicate );
 144
 145     return reply;
 146 }
 147
 148 // discount navids if they conflict with another on the same frequency
 149 // this only applies to navids associated with opposite ends of a runway,
 150 // with matching frequencies.
 151 static bool navidUsable(FGNavRecord* aNav, const SGGeod &aircraft)
 152 {
 153   FGRunway* r(aNav->runway());
 154   if (!r || !r->reciprocalRunway()) {
 155     return true;
 156   }
 157
 158 // check if the runway frequency is paired
 159   FGNavRecord* locA = r->ILS();
 160   FGNavRecord* locB = r->reciprocalRunway()->ILS();
 161
 162   if (!locA || !locB || (locA->get_freq() != locB->get_freq())) {
 163     return true; // not paired, ok
 164   }
 165
 166 // okay, both ends have an ILS, and they're paired. We need to select based on
 167 // aircraft position. What we're going to use is *runway* (not navid) position,
 168 // ie whichever runway end we are closer too. This makes back-course / missed
 169 // approach behaviour incorrect, but that's the price we accept.
 170   double crs = SGGeodesy::courseDeg(aircraft, r->geod());
 171   double hdgDiff = crs - r->headingDeg();
 172   SG_NORMALIZE_RANGE(hdgDiff, -180.0, 180.0);
 173   return (fabs(hdgDiff) < 90.0);
 174 }
 175
 176 // Given a point and a list of stations, return the closest one to
 177 // the specified point.
 178 FGNavRecord* FGNavList::findNavFromList( const SGGeod &aircraft,
 179                                   const nav_list_type &stations )
 180 {
 181     FGNavRecord *nav = NULL;
 182     double d2;                  // in meters squared
 183     double min_dist
 184         = FG_NAV_MAX_RANGE*SG_NM_TO_METER*FG_NAV_MAX_RANGE*SG_NM_TO_METER;
 185     SGVec3d aircraftCart = SGVec3d::fromGeod(aircraft);
 186
 187     nav_list_const_iterator it;
 188     nav_list_const_iterator end = stations.end();
 189     // find the closest station within a sensible range (FG_NAV_MAX_RANGE)
 190     for ( it = stations.begin(); it != end; ++it ) {
 191         FGNavRecord *station = *it;
 192         d2 = distSqr(station->cart(), aircraftCart);
 193         if ( d2 > min_dist || !navidUsable(station, aircraft)) {
 194           continue;
 195         }
 196
 197         min_dist = d2;
 198         nav = station;
 199     }
 200
 201     return nav;
 202 }
 203
 204 // Given a frequency, return the first matching station.
 205 FGNavRecord *FGNavList::findStationByFreq( double freq )
 206 {
 207     const nav_list_type& stations = navaids[(int)(freq*100.0 + 0.5)];
 208
 209     SG_LOG( SG_INSTR, SG_DEBUG, "findStationByFreq " << freq << " size " << stations.size()  );
 210
 211     if (!stations.empty()) {
 212         return stations[0];
 213     }
 214     return NULL;
 215 }
 216
 217
 218
 219 // FGTACANList ----------------------------------------------------------------
 220
 221 FGTACANList::FGTACANList( void )
 222 {
 223 }
 224
 225
 226 FGTACANList::~FGTACANList( void )
 227 {
 228 }
 229
 230
 231 bool FGTACANList::init()
 232 {
 233     return true;
 234 }
 235
 236
 237 // add an entry to the lists
 238 bool FGTACANList::add( FGTACANRecord *c )
 239 {
 240     ident_channels[c->get_channel()].push_back(c);
 241     return true;
 242 }
 243
 244
 245 // Given a TACAN Channel return the first matching frequency
 246 FGTACANRecord *FGTACANList::findByChannel( const string& channel )
 247 {
 248     const tacan_list_type& stations = ident_channels[channel];
 249     SG_LOG( SG_INSTR, SG_DEBUG, "findByChannel " << channel<< " size " << stations.size() );
 250
 251     if (!stations.empty()) {
 252         return stations[0];
 253     }
 254     return NULL;
 255 }
 256
 257