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., 675 Mass Ave, Cambridge, MA 02139, 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 // Constructor
  36 FGNavList::FGNavList( void ) {
  37 }
  38
  39 FGTACANList::FGTACANList( void ){
  40 }
  41
  42
  43 // Destructor
  44 FGNavList::~FGNavList( void ) {
  45 }
  46
  47 FGTACANList::~FGTACANList( void ){
  48 }
  49
  50 // load the navaids and build the map
  51 bool FGNavList::init() {
  52
  53     // FIXME: leaves all the individual navaid entries leaked
  54     navaids.erase( navaids.begin(), navaids.end() );
  55     navaids_by_tile.erase( navaids_by_tile.begin(), navaids_by_tile.end() );
  56     ident_navaids.erase( ident_navaids.begin(), ident_navaids.end() );
  57
  58     return true;
  59 }
  60
  61 bool FGTACANList::init() {
  62
  63     return true;
  64 }
  65
  66 // real add a marker beacon
  67 static void real_add( nav_map_type &navmap, const int master_index,
  68                       FGNavRecord *n )
  69 {
  70     navmap[master_index].push_back( n );
  71 }
  72
  73
  74 // front end for add a marker beacon
  75 static void tile_add( nav_map_type &navmap, FGNavRecord *n ) {
  76     double diff;
  77
  78     double lon = n->get_lon();
  79     double lat = n->get_lat();
  80
  81     int lonidx = (int)lon;
  82     diff = lon - (double)lonidx;
  83     if ( (lon < 0.0) && (fabs(diff) > SG_EPSILON) ) {
  84         lonidx -= 1;
  85     }
  86     double lonfrac = lon - (double)lonidx;
  87     lonidx += 180;
  88
  89     int latidx = (int)lat;
  90     diff = lat - (double)latidx;
  91     if ( (lat < 0.0) && (fabs(diff) > SG_EPSILON) ) {
  92         latidx -= 1;
  93     }
  94     double latfrac = lat - (double)latidx;
  95     latidx += 90;
  96
  97     int master_index = lonidx * 1000 + latidx;
  98     // cout << "lonidx = " << lonidx << " latidx = " << latidx << "  ";
  99     // cout << "Master index = " << master_index << endl;
 100
 101     // add to the actual bucket
 102     real_add( navmap, master_index, n );
 103
 104     // if we are close to the edge, add to adjacent buckets so we only
 105     // have to search one bucket at run time
 106
 107     // there are 8 cases since there are 8 adjacent tiles
 108
 109     if ( lonfrac < 0.2 ) {
 110         real_add( navmap, master_index - 1000, n );
 111         if ( latfrac < 0.2 ) {
 112             real_add( navmap, master_index - 1000 - 1, n );
 113         } else if ( latfrac > 0.8 ) {
 114             real_add( navmap, master_index - 1000 + 1, n );
 115         }
 116     } else if ( lonfrac > 0.8 ) {
 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 ( latfrac < 0.2 ) {
 124         real_add( navmap, master_index - 1, n );
 125     } else if ( latfrac > 0.8 ) {
 126         real_add( navmap, master_index + 1, n );
 127     }
 128 }
 129
 130
 131
 132 // add an entry to the lists
 133 bool FGNavList::add( FGNavRecord *n ) {
 134     navaids[n->get_freq()].push_back(n);
 135     ident_navaids[n->get_ident()].push_back(n);
 136     tile_add( navaids_by_tile, n );
 137     return true;
 138 }
 139
 140 // add an entry to the lists
 141 bool FGTACANList::add( FGTACANRecord *c ) {
 142     ident_channels[c->get_channel()].push_back(c);
 143     return true;
 144 }
 145
 146 // Query the database for the specified frequency.  It is assumed that
 147 // there will be multiple stations with matching frequencies so a
 148 // position must be specified.  Lon and lat are in degrees, elev is in
 149 // meters.
 150 FGNavRecord *FGNavList::findByFreq( double freq, double lon, double lat, double elev )
 151 {
 152     nav_list_type stations = navaids[(int)(freq*100.0 + 0.5)];
 153     Point3D aircraft = sgGeodToCart( Point3D(lon, lat, elev) );
 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     nav_list_type stations = ident_navaids[ident];
 164     Point3D aircraft = sgGeodToCart( Point3D(lon, lat, 0.0) );
 165
 166     return findNavFromList( aircraft, stations );
 167 }
 168
 169
 170 // Given an Ident and optional freqency, return the first matching
 171 // station.
 172 FGNavRecord *FGNavList::findByIdentAndFreq( const char* ident, const double freq )
 173 {
 174     nav_list_type stations = ident_navaids[ident];
 175     SG_LOG( SG_INSTR, SG_DEBUG, "findByIdent " << ident<< " size " << stations.size()  );
 176     if ( freq > 0.0 ) {
 177         // sometimes there can be duplicated idents.  If a freq is
 178         // specified, use it to refine the search.
 179         int f = (int)(freq*100.0 + 0.5);
 180         for ( unsigned int i = 0; i < stations.size(); ++i ) {
 181             if ( f == stations[i]->get_freq() ) {
 182                 return stations[i];
 183             }
 184         }
 185     } else if (stations.size()) {
 186         return stations[0];
 187     }
 188
 189     return NULL;
 190 }
 191
 192
 193 // Given a point and a list of stations, return the closest one to the
 194 // specified point.
 195 FGNavRecord *FGNavList::findNavFromList( const Point3D &aircraft,
 196                                          const nav_list_type &stations )
 197 {
 198     FGNavRecord *nav = NULL;
 199     Point3D station;
 200     double d2;                  // in meters squared
 201     double min_dist
 202         = FG_NAV_MAX_RANGE*SG_NM_TO_METER*FG_NAV_MAX_RANGE*SG_NM_TO_METER;
 203
 204     // find the closest station within a sensible range (FG_NAV_MAX_RANGE)
 205     for ( unsigned int i = 0; i < stations.size(); ++i ) {
 206         // cout << "testing " << current->get_ident() << endl;
 207         station = Point3D( stations[i]->get_x(),
 208                            stations[i]->get_y(),
 209                            stations[i]->get_z() );
 210
 211         d2 = aircraft.distance3Dsquared( station );
 212
 213         // cout << "  dist = " << sqrt(d)
 214         //      << "  range = " << current->get_range() * SG_NM_TO_METER
 215         //      << endl;
 216
 217         // LOC, ILS, GS, and DME antenna's could potentially be
 218         // installed at the opposite end of the runway.  So it's not
 219         // enough to simply find the closest antenna with the right
 220         // frequency.  We need the closest antenna with the right
 221         // frequency that is most oriented towards us.  (We penalize
 222         // stations that are facing away from us by adding 5000 meters
 223         // which is further than matching stations would ever be
 224         // placed from each other.  (Do the expensive check only for
 225         // directional atennas and only when there is a chance it is
 226         // the closest station.)
 227         if ( d2 < min_dist &&
 228              (stations[i]->get_type() == 4 || stations[i]->get_type() == 5 ||
 229               stations[i]->get_type() == 6 || stations[i]->get_type() == 12) )
 230         {
 231             double hdg_deg = 0.0;
 232             if ( stations[i]->get_type() == 4 || stations[i]->get_type() == 5 ){
 233                 hdg_deg = stations[i]->get_multiuse();
 234             } else if ( stations[i]->get_type() == 6 ) {
 235                 int tmp = (int)(stations[i]->get_multiuse() / 1000.0);
 236                 hdg_deg = stations[i]->get_multiuse() - (tmp * 1000);
 237             } else if ( stations[i]->get_type() == 12 ) {
 238                 // oops, Robin's data format doesn't give us the
 239                 // needed information to compute a heading for a DME
 240                 // transmitter.  FIXME Robin!
 241             }
 242
 243             double az1 = 0.0, az2 = 0.0, s = 0.0;
 244             double elev_m = 0.0, lat_rad = 0.0, lon_rad = 0.0;
 245             double xyz[3] = { aircraft.x(), aircraft.y(), aircraft.z() };
 246             sgCartToGeod( xyz, &lat_rad, &lon_rad, &elev_m );
 247             geo_inverse_wgs_84( elev_m,
 248                                 lat_rad * SG_RADIANS_TO_DEGREES,
 249                                 lon_rad * SG_RADIANS_TO_DEGREES,
 250                                 stations[i]->get_lat(), stations[i]->get_lon(),
 251                                 &az1, &az2, &s);
 252             az1 = az1 - stations[i]->get_multiuse();
 253             if ( az1 >  180.0) az1 -= 360.0;
 254             if ( az1 < -180.0) az1 += 360.0;
 255             // penalize opposite facing stations by adding 5000 meters
 256             // (squared) which is further than matching stations would
 257             // ever be placed from each other.
 258             if ( fabs(az1) > 90.0 ) {
 259                 double dist = sqrt(d2);
 260                 d2 = (dist + 5000) * (dist + 5000);
 261             }
 262         }
 263
 264         if ( d2 < min_dist ) {
 265             min_dist = d2;
 266             nav = stations[i];
 267         }
 268     }
 269
 270     return nav;
 271 }
 272
 273
 274 // returns the closest entry to the give lon/lat/elev
 275 FGNavRecord *FGNavList::findClosest( double lon_rad, double lat_rad,
 276                                      double elev_m )
 277 {
 278     FGNavRecord *result = NULL;
 279     double diff;
 280
 281     double lon_deg = lon_rad * SG_RADIANS_TO_DEGREES;
 282     double lat_deg = lat_rad * SG_RADIANS_TO_DEGREES;
 283     int lonidx = (int)lon_deg;
 284     diff = lon_deg - (double)lonidx;
 285     if ( (lon_deg < 0.0) && (fabs(diff) > SG_EPSILON) ) {
 286         lonidx -= 1;
 287     }
 288     lonidx += 180;
 289
 290     int latidx = (int)lat_deg;
 291     diff = lat_deg - (double)latidx;
 292     if ( (lat_deg < 0.0) && (fabs(diff) > SG_EPSILON) ) {
 293         latidx -= 1;
 294     }
 295     latidx += 90;
 296
 297     int master_index = lonidx * 1000 + latidx;
 298
 299     nav_list_type navs = navaids_by_tile[ master_index ];
 300     // cout << "Master index = " << master_index << endl;
 301     // cout << "beacon search length = " << beacons.size() << endl;
 302
 303     nav_list_iterator current = navs.begin();
 304     nav_list_iterator last = navs.end();
 305
 306     Point3D aircraft = sgGeodToCart( Point3D(lon_rad,
 307                                              lat_rad,
 308                                              elev_m) );
 309
 310     double min_dist = 999999999.0;
 311
 312     for ( ; current != last ; ++current ) {
 313         // cout << "  testing " << (*current)->get_ident() << endl;
 314         Point3D station = Point3D( (*current)->get_x(),
 315                                    (*current)->get_y(),
 316                                    (*current)->get_z() );
 317         // cout << "    aircraft = " << aircraft << " station = " << station
 318         //      << endl;
 319
 320         double d = aircraft.distance3Dsquared( station ); // meters^2
 321         // cout << "  distance = " << d << " ("
 322         //      << FG_ILS_DEFAULT_RANGE * SG_NM_TO_METER
 323         //         * FG_ILS_DEFAULT_RANGE * SG_NM_TO_METER
 324         //      << ")" << endl;
 325
 326         // cout << "  range = " << sqrt(d) << endl;
 327
 328         if ( d < min_dist ) {
 329             min_dist = d;
 330             result = (*current);
 331         }
 332     }
 333
 334     // cout << "lon = " << lon << " lat = " << lat
 335     //      << "  closest beacon = " << sqrt( min_dist ) << endl;
 336
 337     return result;
 338 }
 339
 340 // Given a TACAN Channel return the first matching frequency
 341 FGTACANRecord *FGTACANList::findByChannel( string channel )
 342 {
 343     tacan_list_type stations = ident_channels[channel];
 344     SG_LOG( SG_INSTR, SG_DEBUG, "findByChannel " << channel<< " size " << stations.size()  );
 345
 346     if (stations.size()) {
 347         return stations[0];
 348     }
 349     return NULL;
 350 }
 351
 352 // Given a frequency, return the first matching station.
 353 FGNavRecord *FGNavList::findStationByFreq( double freq )
 354 {
 355     nav_list_type stations = navaids[(int)(freq*100.0 + 0.5)];
 356
 357     SG_LOG( SG_INSTR, SG_DEBUG, "findStationByFreq " << freq << " size " << stations.size()  );
 358
 359     if (stations.size()) {
 360         return stations[0];
 361     }
 362     return NULL;
 363 }