src/Navaids/navdb.cxx

   1 // navdb.cxx -- top level navaids management routines
   2 //
   3 // Written by Curtis Olson, started May 2004.
   4 //
   5 // Copyright (C) 2004  Curtis L. Olson - curt@flightgear.org
   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 #include <simgear/compiler.h>
  25
  26 #include STL_STRING
  27
  28 #include <simgear/debug/logstream.hxx>
  29
  30 #include <Airports/runways.hxx>
  31 #include <Main/globals.hxx>
  32
  33 #include "navrecord.hxx"
  34 #include "navdb.hxx"
  35
  36 SG_USING_STD( string );
  37
  38
  39 // load and initialize the navigational databases
  40 bool fgNavDBInit( FGNavList *navlist, FGNavList *loclist, FGNavList *gslist,
  41                   FGNavList *dmelist, FGNavList *mkrlist )
  42 {
  43     SG_LOG(SG_GENERAL, SG_INFO, "Loading Navaid Databases");
  44     // SG_LOG(SG_GENERAL, SG_INFO, "  VOR/NDB");
  45     // SGPath p_nav( globals->get_fg_root() );
  46     // p_nav.append( "Navaids/default.nav" );
  47     // navlist->init( p_nav );
  48
  49     // SG_LOG(SG_GENERAL, SG_INFO, "  ILS and Marker Beacons");
  50     // beacons->init();
  51     // SGPath p_ils( globals->get_fg_root() );
  52     // p_ils.append( "Navaids/default.ils" );
  53     // ilslist->init( p_ils );
  54
  55
  56     SGPath path( globals->get_fg_root() );
  57     path.append( "Navaids/nav.dat" );
  58
  59     sg_gzifstream in( path.str() );
  60     if ( !in.is_open() ) {
  61         SG_LOG( SG_GENERAL, SG_ALERT, "Cannot open file: " << path.str() );
  62         exit(-1);
  63     }
  64
  65     // skip first two lines
  66     in >> skipeol;
  67     in >> skipeol;
  68
  69
  70 #ifdef __MWERKS__
  71     char c = 0;
  72     while ( in.get(c) && c != '\0'  ) {
  73         in.putback(c);
  74 #else
  75     while ( ! in.eof() ) {
  76 #endif
  77
  78         FGNavRecord *r = new FGNavRecord;
  79         in >> (*r);
  80         if ( r->get_type() > 95 ) {
  81             break;
  82         }
  83
  84         /* cout << "id = " << n.get_ident() << endl;
  85         cout << " type = " << n.get_type() << endl;
  86         cout << " lon = " << n.get_lon() << endl;
  87         cout << " lat = " << n.get_lat() << endl;
  88         cout << " elev = " << n.get_elev() << endl;
  89         cout << " freq = " << n.get_freq() << endl;
  90         cout << " range = " << n.get_range() << endl << endl; */
  91
  92         if ( r->get_type() == 2 || r->get_type() == 3 ) {
  93             // NDB=2, VOR=3
  94             navlist->add( r );
  95         } else if ( r->get_type() == 4 || r->get_type() == 5 ) {
  96             // ILS=4, LOC(only)=5
  97             loclist->add( r );
  98         } else if ( r->get_type() == 6 ) {
  99             // GS=6
 100             gslist->add( r );
 101         } else if ( r->get_type() == 7 || r->get_type() == 8
 102                     || r->get_type() == 9 )
 103         {
 104             // Marker Beacon = 7,8,9
 105             mkrlist->add( r );
 106         } else if ( r->get_type() == 12 ) {
 107             // DME=12
 108             dmelist->add( r );
 109         }
 110
 111         in >> skipcomment;
 112     }
 113
 114     // cout << "min freq = " << min << endl;
 115     // cout << "max freq = " << max << endl;
 116
 117     return true;
 118 }
 119
 120
 121 // Given a localizer record and it's corresponding runway record,
 122 // adjust the localizer position so it is in perfect alignment with
 123 // the runway.
 124 static void update_loc_position( FGNavRecord *loc, FGRunway *rwy,
 125                                  double threshold )
 126 {
 127     double hdg = rwy->heading;
 128     hdg += 180.0;
 129     if ( hdg > 360.0 ) {
 130         hdg -= 360.0;
 131     }
 132
 133     // calculate runway threshold point
 134     double thresh_lat, thresh_lon, return_az;
 135     geo_direct_wgs_84 ( 0.0, rwy->lat, rwy->lon, hdg,
 136                         rwy->length/2.0 * SG_FEET_TO_METER,
 137                         &thresh_lat, &thresh_lon, &return_az );
 138     // cout << "Threshold = " << thresh_lat << "," << thresh_lon << endl;
 139
 140     // calculate distance from threshold to localizer
 141     double az1, az2, dist_m;
 142     geo_inverse_wgs_84( 0.0, loc->get_lat(), loc->get_lon(),
 143                         thresh_lat, thresh_lon,
 144                         &az1, &az2, &dist_m );
 145     // cout << "Distance = " << dist_m << endl;
 146
 147     // back project that distance along the runway center line
 148     double nloc_lat, nloc_lon;
 149     geo_direct_wgs_84 ( 0.0, thresh_lat, thresh_lon, hdg + 180.0,
 150                         dist_m, &nloc_lat, &nloc_lon, &return_az );
 151     // printf("New localizer = %.6f %.6f\n", nloc_lat, nloc_lon );
 152
 153     // sanity check, how far have we moved the localizer?
 154     geo_inverse_wgs_84( 0.0, loc->get_lat(), loc->get_lon(),
 155                         nloc_lat, nloc_lon,
 156                         &az1, &az2, &dist_m );
 157     // cout << "Distance moved = " << dist_m << endl;
 158
 159     // cout << "orig heading = " << loc->get_multiuse() << endl;
 160     // cout << "new heading = " << rwy->heading << endl;
 161
 162     double hdg_diff = loc->get_multiuse() - rwy->heading;
 163
 164     // clamp to [-180.0 ... 180.0]
 165     if ( hdg_diff < -180.0 ) {
 166         hdg_diff += 360.0;
 167     } else if ( hdg_diff > 180.0 ) {
 168         hdg_diff -= 360.0;
 169     }
 170
 171     if ( fabs(hdg_diff) <= threshold ) {
 172         loc->set_lat( nloc_lat );
 173         loc->set_lon( nloc_lon );
 174         loc->set_multiuse( rwy->heading );
 175     }
 176 }
 177
 178
 179 // This routines traverses the localizer list and attempts to match
 180 // each entry with it's corresponding runway.  When it is successful,
 181 // it then "moves" the localizer and updates it's heading so it
 182 // *perfectly* aligns with the runway, but is still the same distance
 183 // from the runway threshold.
 184 void fgNavDBAlignLOCwithRunway( FGRunwayList *runways, FGNavList *loclist,
 185                                 double threshold ) {
 186     nav_map_type navmap = loclist->get_navaids();
 187
 188     nav_map_iterator freq = navmap.begin();
 189     while ( freq != navmap.end() ) {
 190         nav_list_type locs = freq->second;
 191         nav_list_iterator loc = locs.begin();
 192         while ( loc != locs.end() ) {
 193             string name = (*loc)->get_name();
 194             string::size_type pos1 = name.find(" ");
 195             string id = name.substr(0, pos1);
 196             name = name.substr(pos1+1);
 197             string::size_type pos2 = name.find(" ");
 198             string rwy = name.substr(0, pos2);
 199
 200             FGRunway r;
 201             if ( runways->search(id, rwy, &r) ) {
 202                 update_loc_position( (*loc), &r, threshold );
 203             }
 204             ++loc;
 205         }
 206         ++freq;
 207     }
 208 }