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/misc/sgstream.hxx>
  30 #include <simgear/math/sg_geodesy.hxx>
  31
  32 #include "navlist.hxx"
  33
  34
  35 // Return true if the nav record matches the type
  36 static bool isTypeMatch(const FGNavRecord* n, fg_nav_types type)
  37 {
  38   if (type == FG_NAV_ANY) return true;
  39   return type == n->type();
  40 }
  41
  42
  43
  44 // FGNavList ------------------------------------------------------------------
  45
  46 FGNavList::FGNavList( void )
  47 {
  48 }
  49
  50
  51 FGNavList::~FGNavList( void )
  52 {
  53     navaids_by_tile.erase( navaids_by_tile.begin(), navaids_by_tile.end() );
  54     nav_list_type navlist = navaids.begin()->second;
  55     navaids.erase( navaids.begin(), navaids.end() );
  56 }
  57
  58
  59 // load the navaids and build the map
  60 bool FGNavList::init()
  61 {
  62     // No need to delete the original navaid structures
  63     // since we're using an SGSharedPointer
  64     nav_list_type navlist = navaids.begin()->second;
  65     navaids.erase( navaids.begin(), navaids.end() );
  66     navaids_by_tile.erase( navaids_by_tile.begin(), navaids_by_tile.end() );
  67     ident_navaids.erase( ident_navaids.begin(), ident_navaids.end() );
  68
  69     return true;
  70 }
  71
  72
  73 // real add a marker beacon
  74 static void real_add( nav_map_type &navmap, const int master_index,
  75                       FGNavRecord *n )
  76 {
  77     navmap[master_index].push_back( n );
  78 }
  79
  80
  81 // front end for add a marker beacon
  82 static void tile_add( nav_map_type &navmap, FGNavRecord *n )
  83 {
  84     double diff = 0;
  85
  86     double lon = n->get_lon();
  87     double lat = n->get_lat();
  88
  89     int lonidx = (int)lon;
  90     diff = lon - (double)lonidx;
  91     if ( (lon < 0.0) && (fabs(diff) > SG_EPSILON) ) {
  92         lonidx -= 1;
  93     }
  94     double lonfrac = lon - (double)lonidx;
  95     lonidx += 180;
  96
  97     int latidx = (int)lat;
  98     diff = lat - (double)latidx;
  99     if ( (lat < 0.0) && (fabs(diff) > SG_EPSILON) ) {
 100         latidx -= 1;
 101     }
 102     double latfrac = lat - (double)latidx;
 103     latidx += 90;
 104
 105     int master_index = lonidx * 1000 + latidx;
 106     // cout << "lonidx = " << lonidx << " latidx = " << latidx << "  ";
 107     // cout << "Master index = " << master_index << endl;
 108
 109     // add to the actual bucket
 110     real_add( navmap, master_index, n );
 111
 112     // if we are close to the edge, add to adjacent buckets so we only
 113     // have to search one bucket at run time
 114
 115     // there are 8 cases since there are 8 adjacent tiles
 116
 117     if ( lonfrac < 0.2 ) {
 118         real_add( navmap, master_index - 1000, n );
 119         if ( latfrac < 0.2 ) {
 120             real_add( navmap, master_index - 1000 - 1, n );
 121         } else if ( latfrac > 0.8 ) {
 122             real_add( navmap, master_index - 1000 + 1, n );
 123         }
 124     } else if ( lonfrac > 0.8 ) {
 125         real_add( navmap, master_index + 1000, n );
 126         if ( latfrac < 0.2 ) {
 127             real_add( navmap, master_index + 1000 - 1, n );
 128         } else if ( latfrac > 0.8 ) {
 129             real_add( navmap, master_index + 1000 + 1, n );
 130         }
 131     } else if ( latfrac < 0.2 ) {
 132         real_add( navmap, master_index - 1, n );
 133     } else if ( latfrac > 0.8 ) {
 134         real_add( navmap, master_index + 1, n );
 135     }
 136 }
 137
 138
 139 // add an entry to the lists
 140 bool FGNavList::add( FGNavRecord *n )
 141 {
 142     navaids[n->get_freq()].push_back(n);
 143     ident_navaids[n->get_ident()].push_back(n);
 144     tile_add( navaids_by_tile, n );
 145     return true;
 146 }
 147
 148 FGNavRecord *FGNavList::findByFreq( double freq, double lon, double lat, double elev )
 149 {
 150     const nav_list_type& stations = navaids[(int)(freq*100.0 + 0.5)];
 151
 152     SGGeod geod = SGGeod::fromRadM(lon, lat, elev);
 153     SGVec3d aircraft = SGVec3d::fromGeod(geod);
 154     SG_LOG( SG_INSTR, SG_DEBUG, "findbyFreq " << freq << " size " << stations.size()  );
 155
 156     return findNavFromList( aircraft, stations );
 157 }
 158
 159
 160 FGNavRecord *FGNavList::findByIdent( const char* ident,
 161                                const double lon, const double lat )
 162 {
 163     const nav_list_type& stations = ident_navaids[ident];
 164     SGGeod geod = SGGeod::fromRad(lon, lat);
 165     SGVec3d aircraft = SGVec3d::fromGeod(geod);
 166     return findNavFromList( aircraft, stations );
 167 }
 168
 169
 170
 171
 172 // Given an Ident and optional freqency, return the first matching
 173 // station.
 174 FGNavRecord *FGNavList::findByIdentAndFreq( const char* ident, const double freq )
 175 {
 176     nav_list_type stations = ident_navaids[ident];
 177     SG_LOG( SG_INSTR, SG_DEBUG, "findByIdent " << ident<< " size " << stations.size()  );
 178     if ( freq > 0.0 ) {
 179         // sometimes there can be duplicated idents.  If a freq is
 180         // specified, use it to refine the search.
 181         int f = (int)(freq*100.0 + 0.5);
 182         nav_list_const_iterator it, end = stations.end();
 183         for ( it = stations.begin(); it != end; ++it ) {
 184             if ( f == (*it)->get_freq() ) {
 185                 return (*it);
 186             }
 187         }
 188     } else if (!stations.empty()) {
 189         return stations[0];
 190     }
 191
 192     return NULL;
 193 }
 194
 195 // LOC, ILS, GS, and DME antenna's could potentially be
 196 // installed at the opposite end of the runway.  So it's not
 197 // enough to simply find the closest antenna with the right
 198 // frequency.  We need the closest antenna with the right
 199 // frequency that is most oriented towards us.  (We penalize
 200 // stations that are facing away from us by adding 5000 meters
 201 // which is further than matching stations would ever be
 202 // placed from each other.  (Do the expensive check only for
 203 // directional atennas and only when there is a chance it is
 204 // the closest station.)
 205
 206 static bool penaltyForNav(FGNavRecord* aNav, const SGVec3d &aPos)
 207 {
 208   switch (aNav->type()) {
 209   case FGPositioned::ILS:
 210   case FGPositioned::LOC:
 211   case FGPositioned::GS:
 212 // FIXME
 213 //  case FGPositioned::DME: we can't get the heading for a DME transmitter, oops
 214     break;
 215   default:
 216     return false;
 217   }
 218
 219   double hdg_deg = 0.0;
 220   if (aNav->type() == FGPositioned::GS) {
 221     int tmp = (int)(aNav->get_multiuse() / 1000.0);
 222     hdg_deg = aNav->get_multiuse() - (tmp * 1000);
 223   } else {
 224     hdg_deg = aNav->get_multiuse();
 225   }
 226
 227   double az1 = 0.0, az2 = 0.0, s = 0.0;
 228   SGGeod geod = SGGeod::fromCart(aPos);
 229   geo_inverse_wgs_84( geod, aNav->geod(), &az1, &az2, &s);
 230   az1 = az1 - hdg_deg;
 231
 232   if ( az1 >  180.0) az1 -= 360.0;
 233   if ( az1 < -180.0) az1 += 360.0;
 234
 235   return fabs(az1) > 90.0;
 236 }
 237
 238 // Given a point and a list of stations, return the closest one to the
 239 // specified point.
 240 FGNavRecord *FGNavList::findNavFromList( const SGVec3d &aircraft,
 241                                          const nav_list_type &stations )
 242 {
 243     FGNavRecord *nav = NULL;
 244     double d2;                  // in meters squared
 245     double min_dist
 246         = FG_NAV_MAX_RANGE*SG_NM_TO_METER*FG_NAV_MAX_RANGE*SG_NM_TO_METER;
 247
 248     nav_list_const_iterator it;
 249     nav_list_const_iterator end = stations.end();
 250     // find the closest station within a sensible range (FG_NAV_MAX_RANGE)
 251     for ( it = stations.begin(); it != end; ++it ) {
 252         FGNavRecord *station = *it;
 253         // cout << "testing " << current->get_ident() << endl;
 254         d2 = distSqr(station->get_cart(), aircraft);
 255         if ( d2 < min_dist && penaltyForNav(station, aircraft))
 256         {
 257           double dist = sqrt(d2);
 258           d2 = (dist + 5000) * (dist + 5000);
 259         }
 260
 261         if ( d2 < min_dist ) {
 262             min_dist = d2;
 263             nav = station;
 264         }
 265     }
 266
 267     return nav;
 268 }
 269
 270
 271 // returns the closest entry to the give lon/lat/elev
 272 FGNavRecord *FGNavList::findClosest( double lon_rad, double lat_rad,
 273                                      double elev_m, fg_nav_types type)
 274 {
 275     FGNavRecord *result = NULL;
 276     double diff;
 277
 278     double lon_deg = lon_rad * SG_RADIANS_TO_DEGREES;
 279     double lat_deg = lat_rad * SG_RADIANS_TO_DEGREES;
 280     int lonidx = (int)lon_deg;
 281     diff = lon_deg - (double)lonidx;
 282     if ( (lon_deg < 0.0) && (fabs(diff) > SG_EPSILON) ) {
 283         lonidx -= 1;
 284     }
 285     lonidx += 180;
 286
 287     int latidx = (int)lat_deg;
 288     diff = lat_deg - (double)latidx;
 289     if ( (lat_deg < 0.0) && (fabs(diff) > SG_EPSILON) ) {
 290         latidx -= 1;
 291     }
 292     latidx += 90;
 293
 294     int master_index = lonidx * 1000 + latidx;
 295
 296     const nav_list_type& navs = navaids_by_tile[ master_index ];
 297     // cout << "Master index = " << master_index << endl;
 298     // cout << "beacon search length = " << beacons.size() << endl;
 299
 300     nav_list_const_iterator current = navs.begin();
 301     nav_list_const_iterator last = navs.end();
 302
 303     SGGeod geod = SGGeod::fromRadM(lon_rad, lat_rad, elev_m);
 304     SGVec3d aircraft = SGVec3d::fromGeod(geod);
 305
 306     double min_dist = 999999999.0;
 307
 308     for ( ; current != last ; ++current ) {
 309         if(isTypeMatch(*current, type)) {
 310             // cout << "  testing " << (*current)->get_ident() << endl;
 311
 312             double d = distSqr((*current)->get_cart(), aircraft);
 313             // cout << "  distance = " << d << " ("
 314             //      << FG_ILS_DEFAULT_RANGE * SG_NM_TO_METER
 315             //         * FG_ILS_DEFAULT_RANGE * SG_NM_TO_METER
 316             //      << ")" << endl;
 317
 318             // cout << "  range = " << sqrt(d) << endl;
 319
 320             if ( d < min_dist ) {
 321                 min_dist = d;
 322                 result = (*current);
 323             }
 324         }
 325     }
 326
 327     // cout << "lon = " << lon << " lat = " << lat
 328     //      << "  closest beacon = " << sqrt( min_dist ) << endl;
 329
 330     return result;
 331 }
 332
 333 // Given a frequency, return the first matching station.
 334 FGNavRecord *FGNavList::findStationByFreq( double freq )
 335 {
 336     const nav_list_type& stations = navaids[(int)(freq*100.0 + 0.5)];
 337
 338     SG_LOG( SG_INSTR, SG_DEBUG, "findStationByFreq " << freq << " size " << stations.size()  );
 339
 340     if (!stations.empty()) {
 341         return stations[0];
 342     }
 343     return NULL;
 344 }
 345
 346
 347
 348 // FGTACANList ----------------------------------------------------------------
 349
 350 FGTACANList::FGTACANList( void )
 351 {
 352 }
 353
 354
 355 FGTACANList::~FGTACANList( void )
 356 {
 357 }
 358
 359
 360 bool FGTACANList::init()
 361 {
 362     return true;
 363 }
 364
 365
 366 // add an entry to the lists
 367 bool FGTACANList::add( FGTACANRecord *c )
 368 {
 369     ident_channels[c->get_channel()].push_back(c);
 370     return true;
 371 }
 372
 373
 374 // Given a TACAN Channel return the first matching frequency
 375 FGTACANRecord *FGTACANList::findByChannel( const string& channel )
 376 {
 377     const tacan_list_type& stations = ident_channels[channel];
 378     SG_LOG( SG_INSTR, SG_DEBUG, "findByChannel " << channel<< " size " << stations.size() );
 379
 380     if (!stations.empty()) {
 381         return stations[0];
 382     }
 383     return NULL;
 384 }
 385
 386