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     switch(type) {
  38         case FG_NAV_ANY: return(true);
  39         case FG_NAV_VOR: return(n->get_type() == 3);
  40         case FG_NAV_NDB: return(n->get_type() == 2);
  41         case FG_NAV_ILS: return(n->get_type() == 4);    // Note - very simplified, only matches loc as part of full ILS.
  42         default: return false;
  43     }
  44 }
  45
  46
  47
  48 // FGNavList ------------------------------------------------------------------
  49
  50 FGNavList::FGNavList( void )
  51 {
  52 }
  53
  54
  55 FGNavList::~FGNavList( void )
  56 {
  57 }
  58
  59
  60 // load the navaids and build the map
  61 bool FGNavList::init()
  62 {
  63     // FIXME: leaves all the individual navaid entries leaked
  64     navaids.erase( navaids.begin(), navaids.end() );
  65     navaids_by_tile.erase( navaids_by_tile.begin(), navaids_by_tile.end() );
  66     ident_navaids.erase( ident_navaids.begin(), ident_navaids.end() );
  67
  68     return true;
  69 }
  70
  71
  72 // real add a marker beacon
  73 static void real_add( nav_map_type &navmap, const int master_index,
  74                       FGNavRecord *n )
  75 {
  76     navmap[master_index].push_back( n );
  77 }
  78
  79
  80 // front end for add a marker beacon
  81 static void tile_add( nav_map_type &navmap, FGNavRecord *n )
  82 {
  83     double diff;
  84
  85     double lon = n->get_lon();
  86     double lat = n->get_lat();
  87
  88     int lonidx = (int)lon;
  89     diff = lon - (double)lonidx;
  90     if ( (lon < 0.0) && (fabs(diff) > SG_EPSILON) ) {
  91         lonidx -= 1;
  92     }
  93     double lonfrac = lon - (double)lonidx;
  94     lonidx += 180;
  95
  96     int latidx = (int)lat;
  97     diff = lat - (double)latidx;
  98     if ( (lat < 0.0) && (fabs(diff) > SG_EPSILON) ) {
  99         latidx -= 1;
 100     }
 101     double latfrac = lat - (double)latidx;
 102     latidx += 90;
 103
 104     int master_index = lonidx * 1000 + latidx;
 105     // cout << "lonidx = " << lonidx << " latidx = " << latidx << "  ";
 106     // cout << "Master index = " << master_index << endl;
 107
 108     // add to the actual bucket
 109     real_add( navmap, master_index, n );
 110
 111     // if we are close to the edge, add to adjacent buckets so we only
 112     // have to search one bucket at run time
 113
 114     // there are 8 cases since there are 8 adjacent tiles
 115
 116     if ( lonfrac < 0.2 ) {
 117         real_add( navmap, master_index - 1000, n );
 118         if ( latfrac < 0.2 ) {
 119             real_add( navmap, master_index - 1000 - 1, n );
 120         } else if ( latfrac > 0.8 ) {
 121             real_add( navmap, master_index - 1000 + 1, n );
 122         }
 123     } else if ( lonfrac > 0.8 ) {
 124         real_add( navmap, master_index + 1000, n );
 125         if ( latfrac < 0.2 ) {
 126             real_add( navmap, master_index + 1000 - 1, n );
 127         } else if ( latfrac > 0.8 ) {
 128             real_add( navmap, master_index + 1000 + 1, n );
 129         }
 130     } else if ( latfrac < 0.2 ) {
 131         real_add( navmap, master_index - 1, n );
 132     } else if ( latfrac > 0.8 ) {
 133         real_add( navmap, master_index + 1, n );
 134     }
 135 }
 136
 137
 138 // add an entry to the lists
 139 bool FGNavList::add( FGNavRecord *n )
 140 {
 141     navaids[n->get_freq()].push_back(n);
 142     ident_navaids[n->get_ident()].push_back(n);
 143     tile_add( navaids_by_tile, n );
 144     return true;
 145 }
 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 nav_list_type FGNavList::findFirstByIdent( const string& ident, fg_nav_types type, bool exact)
 171 {
 172     nav_list_type n2;
 173     n2.clear();
 174
 175     int iType;
 176     if(type == FG_NAV_VOR) iType = 3;
 177     else if(type == FG_NAV_NDB) iType = 2;
 178     else return(n2);
 179
 180     nav_ident_map_iterator it;
 181     if(exact) {
 182         it = ident_navaids.find(ident);
 183     } else {
 184         bool typeMatch = false;
 185         int safety_count = 0;
 186         it = ident_navaids.lower_bound(ident);
 187         while(!typeMatch) {
 188             nav_list_type n0 = it->second;
 189             // local copy, so we should be able to do anything with n0.
 190             // Remove the types that don't match request.
 191             for(nav_list_iterator it0 = n0.begin(); it0 != n0.end();) {
 192                 FGNavRecord* nv = *it0;
 193                 if(nv->get_type() == iType) {
 194                     typeMatch = true;
 195                     ++it0;
 196                 } else {
 197                     it0 = n0.erase(it0);
 198                 }
 199             }
 200             if(typeMatch) {
 201                 return(n0);
 202             }
 203             if(it == ident_navaids.begin()) {
 204                 // We didn't find a match before reaching the beginning of the map
 205                 n0.clear();
 206                 return(n0);
 207             }
 208             safety_count++;
 209             if(safety_count == 1000000) {
 210                 SG_LOG(SG_INSTR, SG_ALERT,
 211                        "safety_count triggered exit from while loop in findFirstByIdent!");
 212                 break;
 213             }
 214             ++it;
 215             if(it == ident_navaids.end()) {
 216                 n0.clear();
 217                 return(n0);
 218             }
 219         }
 220     }
 221     if(it == ident_navaids.end()) {
 222         n2.clear();
 223         return(n2);
 224     } else {
 225         nav_list_type n1 = it->second;
 226         n2.clear();
 227         for(nav_list_iterator it2 = n1.begin(); it2 != n1.end(); ++it2) {
 228             FGNavRecord* nv = *it2;
 229             if(nv->get_type() == iType) n2.push_back(nv);
 230         }
 231         return(n2);
 232     }
 233 }
 234
 235
 236 // Given an Ident and optional freqency, return the first matching
 237 // station.
 238 FGNavRecord *FGNavList::findByIdentAndFreq( const char* ident, const double freq )
 239 {
 240     nav_list_type stations = ident_navaids[ident];
 241     SG_LOG( SG_INSTR, SG_DEBUG, "findByIdent " << ident<< " size " << stations.size()  );
 242     if ( freq > 0.0 ) {
 243         // sometimes there can be duplicated idents.  If a freq is
 244         // specified, use it to refine the search.
 245         int f = (int)(freq*100.0 + 0.5);
 246         nav_list_const_iterator it, end = stations.end();
 247         for ( it = stations.begin(); it != end; ++it ) {
 248             if ( f == (*it)->get_freq() ) {
 249                 return (*it);
 250             }
 251         }
 252     } else if (!stations.empty()) {
 253         return stations[0];
 254     }
 255
 256     return NULL;
 257 }
 258
 259
 260 // Given a point and a list of stations, return the closest one to the
 261 // specified point.
 262 FGNavRecord *FGNavList::findNavFromList( const SGVec3d &aircraft,
 263                                          const nav_list_type &stations )
 264 {
 265     FGNavRecord *nav = NULL;
 266     double d2;                  // in meters squared
 267     double min_dist
 268         = FG_NAV_MAX_RANGE*SG_NM_TO_METER*FG_NAV_MAX_RANGE*SG_NM_TO_METER;
 269
 270     nav_list_const_iterator it;
 271     nav_list_const_iterator end = stations.end();
 272     // find the closest station within a sensible range (FG_NAV_MAX_RANGE)
 273     for ( it = stations.begin(); it != end; ++it ) {
 274         FGNavRecord *station = *it;
 275         // cout << "testing " << current->get_ident() << endl;
 276         d2 = distSqr(station->get_cart(), aircraft);
 277
 278         // cout << "  dist = " << sqrt(d)
 279         //      << "  range = " << current->get_range() * SG_NM_TO_METER
 280         //      << endl;
 281
 282         // LOC, ILS, GS, and DME antenna's could potentially be
 283         // installed at the opposite end of the runway.  So it's not
 284         // enough to simply find the closest antenna with the right
 285         // frequency.  We need the closest antenna with the right
 286         // frequency that is most oriented towards us.  (We penalize
 287         // stations that are facing away from us by adding 5000 meters
 288         // which is further than matching stations would ever be
 289         // placed from each other.  (Do the expensive check only for
 290         // directional atennas and only when there is a chance it is
 291         // the closest station.)
 292         int type = station->get_type();
 293         if ( d2 < min_dist &&
 294              (type == 4 || type == 5 || type == 6 || type == 12 || type == 13) )
 295         {
 296             double hdg_deg = 0.0;
 297             if ( type == 4 || type == 5 ){
 298                 hdg_deg = station->get_multiuse();
 299
 300             } else if ( type == 6 ) {
 301                 int tmp = (int)(station->get_multiuse() / 1000.0);
 302                 hdg_deg = station->get_multiuse() - (tmp * 1000);
 303
 304             } else if ( type == 12 || type == 13 ) {
 305                 // oops, Robin's data format doesn't give us the
 306                 // needed information to compute a heading for a DME
 307                 // transmitter.  FIXME Robin!
 308             }
 309
 310             double az1 = 0.0, az2 = 0.0, s = 0.0;
 311             SGGeod geod = SGGeod::fromCart(aircraft);
 312             geo_inverse_wgs_84( geod, station->get_pos(), &az1, &az2, &s);
 313             az1 = az1 - station->get_multiuse();
 314             if ( az1 >  180.0) az1 -= 360.0;
 315             if ( az1 < -180.0) az1 += 360.0;
 316             // penalize opposite facing stations by adding 5000 meters
 317             // (squared) which is further than matching stations would
 318             // ever be placed from each other.
 319             if ( fabs(az1) > 90.0 ) {
 320                 double dist = sqrt(d2);
 321                 d2 = (dist + 5000) * (dist + 5000);
 322             }
 323         }
 324
 325         if ( d2 < min_dist ) {
 326             min_dist = d2;
 327             nav = station;
 328         }
 329     }
 330
 331     return nav;
 332 }
 333
 334
 335 // returns the closest entry to the give lon/lat/elev
 336 FGNavRecord *FGNavList::findClosest( double lon_rad, double lat_rad,
 337                                      double elev_m, fg_nav_types type)
 338 {
 339     FGNavRecord *result = NULL;
 340     double diff;
 341
 342     double lon_deg = lon_rad * SG_RADIANS_TO_DEGREES;
 343     double lat_deg = lat_rad * SG_RADIANS_TO_DEGREES;
 344     int lonidx = (int)lon_deg;
 345     diff = lon_deg - (double)lonidx;
 346     if ( (lon_deg < 0.0) && (fabs(diff) > SG_EPSILON) ) {
 347         lonidx -= 1;
 348     }
 349     lonidx += 180;
 350
 351     int latidx = (int)lat_deg;
 352     diff = lat_deg - (double)latidx;
 353     if ( (lat_deg < 0.0) && (fabs(diff) > SG_EPSILON) ) {
 354         latidx -= 1;
 355     }
 356     latidx += 90;
 357
 358     int master_index = lonidx * 1000 + latidx;
 359
 360     const nav_list_type& navs = navaids_by_tile[ master_index ];
 361     // cout << "Master index = " << master_index << endl;
 362     // cout << "beacon search length = " << beacons.size() << endl;
 363
 364     nav_list_const_iterator current = navs.begin();
 365     nav_list_const_iterator last = navs.end();
 366
 367     SGGeod geod = SGGeod::fromRadM(lon_rad, lat_rad, elev_m);
 368     SGVec3d aircraft = SGVec3d::fromGeod(geod);
 369
 370     double min_dist = 999999999.0;
 371
 372     for ( ; current != last ; ++current ) {
 373         if(isTypeMatch(*current, type)) {
 374             // cout << "  testing " << (*current)->get_ident() << endl;
 375
 376             double d = distSqr((*current)->get_cart(), aircraft);
 377             // cout << "  distance = " << d << " ("
 378             //      << FG_ILS_DEFAULT_RANGE * SG_NM_TO_METER
 379             //         * FG_ILS_DEFAULT_RANGE * SG_NM_TO_METER
 380             //      << ")" << endl;
 381
 382             // cout << "  range = " << sqrt(d) << endl;
 383
 384             if ( d < min_dist ) {
 385                 min_dist = d;
 386                 result = (*current);
 387             }
 388         }
 389     }
 390
 391     // cout << "lon = " << lon << " lat = " << lat
 392     //      << "  closest beacon = " << sqrt( min_dist ) << endl;
 393
 394     return result;
 395 }
 396
 397 // Given a frequency, return the first matching station.
 398 FGNavRecord *FGNavList::findStationByFreq( double freq )
 399 {
 400     const nav_list_type& stations = navaids[(int)(freq*100.0 + 0.5)];
 401
 402     SG_LOG( SG_INSTR, SG_DEBUG, "findStationByFreq " << freq << " size " << stations.size()  );
 403
 404     if (!stations.empty()) {
 405         return stations[0];
 406     }
 407     return NULL;
 408 }
 409
 410
 411
 412 // FGTACANList ----------------------------------------------------------------
 413
 414 FGTACANList::FGTACANList( void )
 415 {
 416 }
 417
 418
 419 FGTACANList::~FGTACANList( void )
 420 {
 421 }
 422
 423
 424 bool FGTACANList::init()
 425 {
 426     return true;
 427 }
 428
 429
 430 // add an entry to the lists
 431 bool FGTACANList::add( FGTACANRecord *c )
 432 {
 433     ident_channels[c->get_channel()].push_back(c);
 434     return true;
 435 }
 436
 437
 438 // Given a TACAN Channel return the first matching frequency
 439 FGTACANRecord *FGTACANList::findByChannel( const string& channel )
 440 {
 441     const tacan_list_type& stations = ident_channels[channel];
 442     SG_LOG( SG_INSTR, SG_DEBUG, "findByChannel " << channel<< " size " << stations.size() );
 443
 444     if (!stations.empty()) {
 445         return stations[0];
 446     }
 447     return NULL;
 448 }
 449
 450