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