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