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 using std::string;
  35
  36 // FGNavList ------------------------------------------------------------------
  37
  38 FGNavList::FGNavList( void )
  39 {
  40 }
  41
  42
  43 FGNavList::~FGNavList( void )
  44 {
  45     nav_list_type navlist = navaids.begin()->second;
  46     navaids.erase( navaids.begin(), navaids.end() );
  47 }
  48
  49
  50 // load the navaids and build the map
  51 bool FGNavList::init()
  52 {
  53     // No need to delete the original navaid structures
  54     // since we're using an SGSharedPointer
  55     nav_list_type navlist = navaids.begin()->second;
  56     navaids.erase( navaids.begin(), navaids.end() );
  57     return true;
  58 }
  59
  60 // add an entry to the lists
  61 bool FGNavList::add( FGNavRecord *n )
  62 {
  63     navaids[n->get_freq()].push_back(n);
  64     return true;
  65 }
  66
  67 FGNavRecord *FGNavList::findByFreq( double freq, const SGGeod& position)
  68 {
  69     const nav_list_type& stations = navaids[(int)(freq*100.0 + 0.5)];
  70     SG_LOG( SG_INSTR, SG_DEBUG, "findbyFreq " << freq << " size " << stations.size()  );
  71     return findNavFromList( position, stations );
  72 }
  73
  74 class VORNDBFilter : public FGPositioned::Filter
  75 {
  76 public:
  77   virtual bool pass(FGPositioned* aPos) const
  78   {
  79     return (aPos->type() == FGPositioned::VOR) || (aPos->type() == FGPositioned::NDB);
  80   }
  81 };
  82
  83 // Given an Ident and optional freqency, return the first matching
  84 // station.
  85 FGNavRecord *FGNavList::findByIdentAndFreq(const string& ident, const double freq )
  86 {
  87   FGPositionedRef cur;
  88   VORNDBFilter filter;
  89   cur = FGPositioned::findNextWithPartialId(cur, ident, &filter);
  90
  91   if (freq <= 0.0) {
  92     return static_cast<FGNavRecord*>(cur.ptr()); // might be null
  93   }
  94
  95   int f = (int)(freq*100.0 + 0.5);
  96   while (cur) {
  97     FGNavRecord* nav = static_cast<FGNavRecord*>(cur.ptr());
  98     if (nav->get_freq() == f) {
  99       return nav;
 100     }
 101
 102     cur = FGPositioned::findNextWithPartialId(cur, ident, &filter);
 103   }
 104
 105   return NULL;
 106 }
 107
 108 // LOC, ILS, GS, and DME antenna's could potentially be
 109 // installed at the opposite end of the runway.  So it's not
 110 // enough to simply find the closest antenna with the right
 111 // frequency.  We need the closest antenna with the right
 112 // frequency that is most oriented towards us.  (We penalize
 113 // stations that are facing away from us by adding 5000 meters
 114 // which is further than matching stations would ever be
 115 // placed from each other.  (Do the expensive check only for
 116 // directional atennas and only when there is a chance it is
 117 // the closest station.)
 118
 119 static bool penaltyForNav(FGNavRecord* aNav, const SGGeod &aGeod)
 120 {
 121   switch (aNav->type()) {
 122   case FGPositioned::ILS:
 123   case FGPositioned::LOC:
 124   case FGPositioned::GS:
 125 // FIXME
 126 //  case FGPositioned::DME: we can't get the heading for a DME transmitter, oops
 127     break;
 128   default:
 129     return false;
 130   }
 131
 132   double hdg_deg = 0.0;
 133   if (aNav->type() == FGPositioned::GS) {
 134     int tmp = (int)(aNav->get_multiuse() / 1000.0);
 135     hdg_deg = aNav->get_multiuse() - (tmp * 1000);
 136   } else {
 137     hdg_deg = aNav->get_multiuse();
 138   }
 139
 140   double az1, az2, s;
 141   SGGeodesy::inverse(aGeod, aNav->geod(), az1, az2, s);
 142   az1 = az1 - hdg_deg;
 143   SG_NORMALIZE_RANGE(az1, -180.0, 180.0);
 144   return fabs(az1) > 90.0;
 145 }
 146
 147 // Given a point and a list of stations, return the closest one to the
 148 // specified point.
 149 FGNavRecord *FGNavList::findNavFromList( const SGGeod &aircraft,
 150                                          const nav_list_type &stations )
 151 {
 152     FGNavRecord *nav = NULL;
 153     double d2;                  // in meters squared
 154     double min_dist
 155         = FG_NAV_MAX_RANGE*SG_NM_TO_METER*FG_NAV_MAX_RANGE*SG_NM_TO_METER;
 156     SGVec3d aircraftCart = SGVec3d::fromGeod(aircraft);
 157
 158     nav_list_const_iterator it;
 159     nav_list_const_iterator end = stations.end();
 160     // find the closest station within a sensible range (FG_NAV_MAX_RANGE)
 161     for ( it = stations.begin(); it != end; ++it ) {
 162         FGNavRecord *station = *it;
 163         // cout << "testing " << current->get_ident() << endl;
 164         d2 = distSqr(station->cart(), aircraftCart);
 165         if ( d2 < min_dist && penaltyForNav(station, aircraft))
 166         {
 167           double dist = sqrt(d2);
 168           d2 = (dist + 5000) * (dist + 5000);
 169         }
 170
 171         if ( d2 < min_dist ) {
 172             min_dist = d2;
 173             nav = station;
 174         }
 175     }
 176
 177     return nav;
 178 }
 179
 180 // Given a frequency, return the first matching station.
 181 FGNavRecord *FGNavList::findStationByFreq( double freq )
 182 {
 183     const nav_list_type& stations = navaids[(int)(freq*100.0 + 0.5)];
 184
 185     SG_LOG( SG_INSTR, SG_DEBUG, "findStationByFreq " << freq << " size " << stations.size()  );
 186
 187     if (!stations.empty()) {
 188         return stations[0];
 189     }
 190     return NULL;
 191 }
 192
 193
 194
 195 // FGTACANList ----------------------------------------------------------------
 196
 197 FGTACANList::FGTACANList( void )
 198 {
 199 }
 200
 201
 202 FGTACANList::~FGTACANList( void )
 203 {
 204 }
 205
 206
 207 bool FGTACANList::init()
 208 {
 209     return true;
 210 }
 211
 212
 213 // add an entry to the lists
 214 bool FGTACANList::add( FGTACANRecord *c )
 215 {
 216     ident_channels[c->get_channel()].push_back(c);
 217     return true;
 218 }
 219
 220
 221 // Given a TACAN Channel return the first matching frequency
 222 FGTACANRecord *FGTACANList::findByChannel( const string& channel )
 223 {
 224     const tacan_list_type& stations = ident_channels[channel];
 225     SG_LOG( SG_INSTR, SG_DEBUG, "findByChannel " << channel<< " size " << stations.size() );
 226
 227     if (!stations.empty()) {
 228         return stations[0];
 229     }
 230     return NULL;
 231 }
 232
 233