src/Instrumentation/dclgps.cxx

   1 // dclgps.cxx - a class to extend the operation of FG's current GPS
   2 // code, and provide support for a KLN89-specific instrument.  It
   3 // is envisioned that eventually this file and class will be split
   4 // up between current FG code and new KLN89-specific code and removed.
   5 //
   6 // Written by David Luff, started 2005.
   7 //
   8 // Copyright (C) 2005 - David C Luff - david.luff@nottingham.ac.uk
   9 //
  10 // This program is free software; you can redistribute it and/or
  11 // modify it under the terms of the GNU General Public License as
  12 // published by the Free Software Foundation; either version 2 of the
  13 // License, or (at your option) any later version.
  14 //
  15 // This program is distributed in the hope that it will be useful, but
  16 // WITHOUT ANY WARRANTY; without even the implied warranty of
  17 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  18 // General Public License for more details.
  19 //
  20 // You should have received a copy of the GNU General Public License
  21 // along with this program; if not, write to the Free Software
  22 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  23 //
  24 // $Id$
  25
  26 #include "dclgps.hxx"
  27
  28 #include <simgear/sg_inlines.h>
  29 #include <simgear/structure/commands.hxx>
  30 #include <Main/fg_props.hxx>
  31 #include <iostream>
  32 SG_USING_STD(cout);
  33
  34 //using namespace std;
  35
  36 // Command callbacks for FlightGear
  37
  38 static bool do_kln89_msg_pressed(const SGPropertyNode* arg) {
  39         //cout << "do_kln89_msg_pressed called!\n";
  40         DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89");
  41         gps->MsgPressed();
  42         return(true);
  43 }
  44
  45 static bool do_kln89_obs_pressed(const SGPropertyNode* arg) {
  46         //cout << "do_kln89_obs_pressed called!\n";
  47         DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89");
  48         gps->OBSPressed();
  49         return(true);
  50 }
  51
  52 static bool do_kln89_alt_pressed(const SGPropertyNode* arg) {
  53         //cout << "do_kln89_alt_pressed called!\n";
  54         DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89");
  55         gps->AltPressed();
  56         return(true);
  57 }
  58
  59 static bool do_kln89_nrst_pressed(const SGPropertyNode* arg) {
  60         DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89");
  61         gps->NrstPressed();
  62         return(true);
  63 }
  64
  65 static bool do_kln89_dto_pressed(const SGPropertyNode* arg) {
  66         DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89");
  67         gps->DtoPressed();
  68         return(true);
  69 }
  70
  71 static bool do_kln89_clr_pressed(const SGPropertyNode* arg) {
  72         DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89");
  73         gps->ClrPressed();
  74         return(true);
  75 }
  76
  77 static bool do_kln89_ent_pressed(const SGPropertyNode* arg) {
  78         DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89");
  79         gps->EntPressed();
  80         return(true);
  81 }
  82
  83 static bool do_kln89_crsr_pressed(const SGPropertyNode* arg) {
  84         DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89");
  85         gps->CrsrPressed();
  86         return(true);
  87 }
  88
  89 static bool do_kln89_knob1left1(const SGPropertyNode* arg) {
  90         DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89");
  91         gps->Knob1Left1();
  92         return(true);
  93 }
  94
  95 static bool do_kln89_knob1right1(const SGPropertyNode* arg) {
  96         DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89");
  97         gps->Knob1Right1();
  98         return(true);
  99 }
 100
 101 static bool do_kln89_knob2left1(const SGPropertyNode* arg) {
 102         DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89");
 103         gps->Knob2Left1();
 104         return(true);
 105 }
 106
 107 static bool do_kln89_knob2right1(const SGPropertyNode* arg) {
 108         DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89");
 109         gps->Knob2Right1();
 110         return(true);
 111 }
 112
 113 // End command callbacks
 114
 115 GPSWaypoint::GPSWaypoint() {
 116     appType = GPS_APP_NONE;
 117 }
 118
 119 GPSWaypoint::~GPSWaypoint() {}
 120
 121 string GPSWaypoint::GetAprId() {
 122         if(appType == GPS_IAF) return(id + 'i');
 123         else if(appType == GPS_FAF) return(id + 'f');
 124         else if(appType == GPS_MAP) return(id + 'm');
 125         else if(appType == GPS_MAHP) return(id + 'h');
 126         else return(id);
 127 }
 128
 129 ostream& operator << (ostream& os, GPSAppWpType type) {
 130         switch(type) {
 131                 case(GPS_IAF):       return(os << "IAF");
 132                 case(GPS_IAP):       return(os << "IAP");
 133                 case(GPS_FAF):       return(os << "FAF");
 134                 case(GPS_MAP):       return(os << "MAP");
 135                 case(GPS_MAHP):      return(os << "MAHP");
 136                 case(GPS_HDR):       return(os << "HEADER");
 137                 case(GPS_FENCE):     return(os << "FENCE");
 138                 case(GPS_APP_NONE):  return(os << "NONE");
 139         }
 140         return(os << "ERROR - Unknown switch in GPSAppWpType operator << ");
 141 }
 142
 143 FGIAP::FGIAP() {
 144 }
 145
 146 FGIAP::~FGIAP() {
 147 }
 148
 149 FGNPIAP::FGNPIAP() {
 150 }
 151
 152 FGNPIAP::~FGNPIAP() {
 153 }
 154
 155 GPSPage::GPSPage(DCLGPS* parent) {
 156         _parent = parent;
 157         _subPage = 0;
 158 }
 159
 160 GPSPage::~GPSPage() {
 161 }
 162
 163 void GPSPage::Update(double dt) {}
 164
 165 void GPSPage::Knob1Left1() {}
 166 void GPSPage::Knob1Right1() {}
 167
 168 void GPSPage::Knob2Left1() {
 169         _parent->_activePage->LooseFocus();
 170         _subPage--;
 171         if(_subPage < 0) _subPage = _nSubPages - 1;
 172 }
 173
 174 void GPSPage::Knob2Right1() {
 175         _parent->_activePage->LooseFocus();
 176         _subPage++;
 177         if(_subPage >= _nSubPages) _subPage = 0;
 178 }
 179
 180 void GPSPage::CrsrPressed() {}
 181 void GPSPage::EntPressed() {}
 182 void GPSPage::ClrPressed() {}
 183 void GPSPage::DtoPressed() {}
 184 void GPSPage::NrstPressed() {}
 185 void GPSPage::AltPressed() {}
 186 void GPSPage::OBSPressed() {}
 187 void GPSPage::MsgPressed() {}
 188
 189 string GPSPage::GPSitoa(int n) {
 190         char buf[4];
 191         // TODO - sanity check n!
 192         sprintf(buf, "%i", n);
 193         string s = buf;
 194         return(s);
 195 }
 196
 197 void GPSPage::CleanUp() {}
 198 void GPSPage::LooseFocus() {}
 199 void GPSPage::SetId(const string& s) {}
 200
 201 // ------------------------------------------------------------------------------------- //
 202
 203 DCLGPS::DCLGPS(RenderArea2D* instrument) {
 204         _instrument = instrument;
 205         _nFields = 1;
 206         _maxFields = 2;
 207         _pages.clear();
 208
 209         // Units - lets default to US units - FG can set them to other units from config during startup if desired.
 210         _altUnits = GPS_ALT_UNITS_FT;
 211         _baroUnits = GPS_PRES_UNITS_IN;
 212         _velUnits = GPS_VEL_UNITS_KT;
 213         _distUnits = GPS_DIST_UNITS_NM;
 214
 215         _lon_node = fgGetNode("/instrumentation/gps/indicated-longitude-deg", true);
 216         _lat_node = fgGetNode("/instrumentation/gps/indicated-latitude-deg", true);
 217         _alt_node = fgGetNode("/instrumentation/gps/indicated-altitude-ft", true);
 218         _grnd_speed_node = fgGetNode("/instrumentation/gps/indicated-ground-speed-kt", true);
 219         _true_track_node = fgGetNode("/instrumentation/gps/indicated-track-true-deg", true);
 220         _mag_track_node = fgGetNode("/instrumentation/gps/indicated-track-magnetic-deg", true);
 221
 222         // Use FG's position values at construction in case FG's gps has not run first update yet.
 223         _lon = fgGetDouble("/position/longitude-deg") * SG_DEGREES_TO_RADIANS;
 224         _lat = fgGetDouble("/position/latitude-deg") * SG_DEGREES_TO_RADIANS;
 225         _alt = fgGetDouble("/position/altitude-ft");
 226         // Note - we can depriciate _gpsLat and _gpsLon if we implement error handling in FG
 227         // gps code and not our own.
 228         _gpsLon = _lon;
 229         _gpsLat = _lat;
 230         _checkLon = _gpsLon;
 231         _checkLat = _gpsLat;
 232         _groundSpeed_ms = 0.0;
 233         _groundSpeed_kts = 0.0;
 234         _track = 0.0;
 235         _magTrackDeg = 0.0;
 236
 237         // Sensible defaults.  These can be overriden by derived classes if desired.
 238         _cdiScales.clear();
 239         _cdiScales.push_back(5.0);
 240         _cdiScales.push_back(1.0);
 241         _cdiScales.push_back(0.3);
 242         _currentCdiScaleIndex = 0;
 243         _targetCdiScaleIndex = 0;
 244         _sourceCdiScaleIndex = 0;
 245         _cdiScaleTransition = false;
 246         _currentCdiScale = 5.0;
 247
 248         _cleanUpPage = -1;
 249
 250         _activeWaypoint.id.clear();
 251         _dist2Act = 0.0;
 252         _crosstrackDist = 0.0;
 253         _headingBugTo = true;
 254         _navFlagged = true;
 255         _waypointAlert = false;
 256         _departed = false;
 257         _departureTimeString = "----";
 258         _elapsedTime = 0.0;
 259
 260         // Configuration Initialisation
 261         // Should this be in kln89.cxx ?
 262         _turnAnticipationEnabled = false;
 263         _suaAlertEnabled = false;
 264         _altAlertEnabled = false;
 265
 266         _time = new SGTime;
 267
 268         _messageStack.clear();
 269
 270         _dto = false;
 271
 272         _approachLoaded = false;
 273         _approachArm = false;
 274         _approachReallyArmed = false;
 275         _approachActive = false;
 276         _approachFP = new GPSFlightPlan;
 277 }
 278
 279 DCLGPS::~DCLGPS() {
 280         delete _time;
 281         for(gps_waypoint_map_iterator itr = _waypoints.begin(); itr != _waypoints.end(); ++itr) {
 282                 for(unsigned int i = 0; i < (*itr).second.size(); ++i) {
 283                         delete(((*itr).second)[i]);
 284                 }
 285         }
 286         delete _approachFP;             // Don't need to delete the waypoints inside since they point to
 287                                                         // the waypoints in the approach database.
 288         // TODO - may need to delete the approach database!!
 289 }
 290
 291 void DCLGPS::draw() {
 292         //cout << "draw called!\n";
 293         _instrument->draw();
 294 }
 295
 296 void DCLGPS::init() {
 297         globals->get_commands()->addCommand("kln89_msg_pressed", do_kln89_msg_pressed);
 298         globals->get_commands()->addCommand("kln89_obs_pressed", do_kln89_obs_pressed);
 299         globals->get_commands()->addCommand("kln89_alt_pressed", do_kln89_alt_pressed);
 300         globals->get_commands()->addCommand("kln89_nrst_pressed", do_kln89_nrst_pressed);
 301         globals->get_commands()->addCommand("kln89_dto_pressed", do_kln89_dto_pressed);
 302         globals->get_commands()->addCommand("kln89_clr_pressed", do_kln89_clr_pressed);
 303         globals->get_commands()->addCommand("kln89_ent_pressed", do_kln89_ent_pressed);
 304         globals->get_commands()->addCommand("kln89_crsr_pressed", do_kln89_crsr_pressed);
 305         globals->get_commands()->addCommand("kln89_knob1left1", do_kln89_knob1left1);
 306         globals->get_commands()->addCommand("kln89_knob1right1", do_kln89_knob1right1);
 307         globals->get_commands()->addCommand("kln89_knob2left1", do_kln89_knob2left1);
 308         globals->get_commands()->addCommand("kln89_knob2right1", do_kln89_knob2right1);
 309
 310         // Build the GPS-specific databases.
 311         // TODO - consider splitting into real life GPS database regions - eg Americas, Europe etc.
 312         // Note that this needs to run after FG's airport and nav databases are up and running
 313         _waypoints.clear();
 314         const airport_list* apts = globals->get_airports()->getAirportList();
 315         for(unsigned int i = 0; i < apts->size(); ++i) {
 316                 FGAirport* a = (*apts)[i];
 317                 GPSWaypoint* w = new GPSWaypoint;
 318                 w->id = a->getId();
 319                 w->lat = a->getLatitude() * SG_DEGREES_TO_RADIANS;
 320                 w->lon = a->getLongitude() * SG_DEGREES_TO_RADIANS;
 321                 w->type = GPS_WP_APT;
 322                 gps_waypoint_map_iterator wtr = _waypoints.find(a->getId());
 323                 if(wtr == _waypoints.end()) {
 324                         gps_waypoint_array arr;
 325                         arr.push_back(w);
 326                         _waypoints[w->id] = arr;
 327                 } else {
 328                         wtr->second.push_back(w);
 329                 }
 330         }
 331         nav_map_type navs = globals->get_navlist()->get_navaids();
 332         for(nav_map_iterator itr = navs.begin(); itr != navs.end(); ++itr) {
 333                 nav_list_type nlst = itr->second;
 334                 for(unsigned int i = 0; i < nlst.size(); ++i) {
 335                         FGNavRecord* n = nlst[i];
 336                         if(n->get_fg_type() == FG_NAV_VOR || n->get_fg_type() == FG_NAV_NDB) {  // We don't bother with ILS etc.
 337                                 GPSWaypoint* w = new GPSWaypoint;
 338                                 w->id = n->get_ident();
 339                                 w->lat = n->get_lat() * SG_DEGREES_TO_RADIANS;
 340                                 w->lon = n->get_lon() * SG_DEGREES_TO_RADIANS;
 341                                 w->type = (n->get_fg_type() == FG_NAV_VOR ? GPS_WP_VOR : GPS_WP_NDB);
 342                                 gps_waypoint_map_iterator wtr = _waypoints.find(n->get_ident());
 343                                 if(wtr == _waypoints.end()) {
 344                                         gps_waypoint_array arr;
 345                                         arr.push_back(w);
 346                                         _waypoints[w->id] = arr;
 347                                 } else {
 348                                         wtr->second.push_back(w);
 349                                 }
 350                         }
 351                 }
 352         }
 353         const fix_map_type* fixes = globals->get_fixlist()->getFixList();
 354         for(fix_map_const_iterator itr = fixes->begin(); itr != fixes->end(); ++itr) {
 355                 FGFix f = itr->second;
 356                 GPSWaypoint* w = new GPSWaypoint;
 357                 w->id = f.get_ident();
 358                 w->lat = f.get_lat() * SG_DEGREES_TO_RADIANS;
 359                 w->lon = f.get_lon() * SG_DEGREES_TO_RADIANS;
 360                 w->type = GPS_WP_INT;
 361                 gps_waypoint_map_iterator wtr = _waypoints.find(f.get_ident());
 362                 if(wtr == _waypoints.end()) {
 363                         gps_waypoint_array arr;
 364                         arr.push_back(w);
 365                         _waypoints[w->id] = arr;
 366                 } else {
 367                         wtr->second.push_back(w);
 368                 }
 369         }
 370         // TODO - add USR waypoints as well.
 371
 372         // Not sure if this should be here, but OK for now.
 373         CreateDefaultFlightPlans();
 374
 375         // Hack - hardwire some instrument approaches for testing.
 376         // TODO - read these from file - either all at startup or as needed.
 377         FGNPIAP* iap = new FGNPIAP;
 378         iap->_id = "KHWD";
 379         iap->_name = "VOR/DME OR GPS-B";
 380         iap->_abbrev = "VOR/D";
 381         iap->_rwyStr = "B";
 382         iap->_IAF.clear();
 383         iap->_IAP.clear();
 384         iap->_MAP.clear();
 385         // -------
 386         GPSWaypoint* wp = new GPSWaypoint;
 387         wp->id = "SUNOL";
 388         bool multi;
 389         // Nasty using the find any function here, but it saves converting data from FGFix etc.
 390         const GPSWaypoint* fp = FindFirstById(wp->id, multi, true);
 391         *wp = *fp;
 392         wp->appType = GPS_IAF;
 393         iap->_IAF.push_back(wp);
 394         // -------
 395         wp = new GPSWaypoint;
 396         wp->id = "MABRY";
 397         fp = FindFirstById(wp->id, multi, true);
 398         *wp = *fp;
 399         wp->appType = GPS_IAF;
 400         iap->_IAF.push_back(wp);
 401         // -------
 402         wp = new GPSWaypoint;
 403         wp->id = "IMPLY";
 404         fp = FindFirstById(wp->id, multi, true);
 405         *wp = *fp;
 406         wp->appType = GPS_IAP;
 407         iap->_IAP.push_back(wp);
 408         // -------
 409         wp = new GPSWaypoint;
 410         wp->id = "DECOT";
 411         fp = FindFirstById(wp->id, multi, true);
 412         *wp = *fp;
 413         wp->appType = GPS_FAF;
 414         iap->_IAP.push_back(wp);
 415         // -------
 416         wp = new GPSWaypoint;
 417         wp->id = "MAPVV";
 418         fp = FindFirstById(wp->id, multi, true);
 419         *wp = *fp;
 420         wp->appType = GPS_MAP;
 421         iap->_IAP.push_back(wp);
 422         // -------
 423         wp = new GPSWaypoint;
 424         wp->id = "OAK";
 425         fp = FindFirstById(wp->id, multi, true);
 426         *wp = *fp;
 427         wp->appType = GPS_MAHP;
 428         iap->_MAP.push_back(wp);
 429         // -------
 430         _np_iap[iap->_id].push_back(iap);
 431         // -----------------------
 432         // -----------------------
 433         iap = new FGNPIAP;
 434         iap->_id = "KHWD";
 435         iap->_name = "VOR OR GPS-A";
 436         iap->_abbrev = "VOR-";
 437         iap->_rwyStr = "A";
 438         iap->_IAF.clear();
 439         iap->_IAP.clear();
 440         iap->_MAP.clear();
 441         // -------
 442         wp = new GPSWaypoint;
 443         wp->id = "SUNOL";
 444         // Nasty using the find any function here, but it saves converting data from FGFix etc.
 445         fp = FindFirstById(wp->id, multi, true);
 446         *wp = *fp;
 447         wp->appType = GPS_IAF;
 448         iap->_IAF.push_back(wp);
 449         // -------
 450         wp = new GPSWaypoint;
 451         wp->id = "MABRY";
 452         fp = FindFirstById(wp->id, multi, true);
 453         *wp = *fp;
 454         wp->appType = GPS_IAF;
 455         iap->_IAF.push_back(wp);
 456         // -------
 457         wp = new GPSWaypoint;
 458         wp->id = "IMPLY";
 459         fp = FindFirstById(wp->id, multi, true);
 460         *wp = *fp;
 461         wp->appType = GPS_IAP;
 462         iap->_IAP.push_back(wp);
 463         // -------
 464         wp = new GPSWaypoint;
 465         wp->id = "DECOT";
 466         fp = FindFirstById(wp->id, multi, true);
 467         *wp = *fp;
 468         wp->appType = GPS_FAF;
 469         iap->_IAP.push_back(wp);
 470         // -------
 471         wp = new GPSWaypoint;
 472         wp->id = "MAPVV";
 473         fp = FindFirstById(wp->id, multi, true);
 474         *wp = *fp;
 475         wp->appType = GPS_MAP;
 476         iap->_IAP.push_back(wp);
 477         // -------
 478         wp = new GPSWaypoint;
 479         wp->id = "OAK";
 480         fp = FindFirstById(wp->id, multi, true);
 481         *wp = *fp;
 482         wp->appType = GPS_MAHP;
 483         iap->_MAP.push_back(wp);
 484         // -------
 485         _np_iap[iap->_id].push_back(iap);
 486         // ------------------
 487         // ------------------
 488         /*
 489         // Ugh - don't load this one - the waypoints required aren't in fix.dat.gz - result: program crash!
 490         // TODO - make the IAP loader robust to absent waypoints.
 491         iap = new FGNPIAP;
 492         iap->_id = "KHWD";
 493         iap->_name = "GPS RWY 28L";
 494         iap->_abbrev = "GPS";
 495         iap->_rwyStr = "28L";
 496         iap->_IAF.clear();
 497         iap->_IAP.clear();
 498         iap->_MAP.clear();
 499         // -------
 500         wp = new GPSWaypoint;
 501         wp->id = "SUNOL";
 502         // Nasty using the find any function here, but it saves converting data from FGFix etc.
 503         fp = FindFirstById(wp->id, multi, true);
 504         *wp = *fp;
 505         wp->appType = GPS_IAF;
 506         iap->_IAF.push_back(wp);
 507         // -------
 508         wp = new GPSWaypoint;
 509         wp->id = "SJC";
 510         fp = FindFirstById(wp->id, multi, true);
 511         *wp = *fp;
 512         wp->appType = GPS_IAF;
 513         iap->_IAF.push_back(wp);
 514         // -------
 515         wp = new GPSWaypoint;
 516         wp->id = "JOCPI";
 517         fp = FindFirstById(wp->id, multi, true);
 518         *wp = *fp;
 519         wp->appType = GPS_IAP;
 520         iap->_IAP.push_back(wp);
 521         // -------
 522         wp = new GPSWaypoint;
 523         wp->id = "SUDGE";
 524         fp = FindFirstById(wp->id, multi, true);
 525         *wp = *fp;
 526         wp->appType = GPS_FAF;
 527         iap->_IAP.push_back(wp);
 528         // -------
 529         wp = new GPSWaypoint;
 530         wp->id = "RW28L";
 531         wp->appType = GPS_MAP;
 532         if(wp->id.substr(0, 2) == "RW" && wp->appType == GPS_MAP) {
 533                 // Assume that this is a missed-approach point based on the runway number
 534                 // Get the runway threshold location etc
 535         } else {
 536                 fp = FindFirstById(wp->id, multi, true);
 537                 if(fp == NULL) {
 538                         cout << "Failed to find waypoint " << wp->id << " in database...\n";
 539                 } else {
 540                         *wp = *fp;
 541                 }
 542         }
 543         iap->_IAP.push_back(wp);
 544         // -------
 545         wp = new GPSWaypoint;
 546         wp->id = "OAK";
 547         fp = FindFirstById(wp->id, multi, true);
 548         *wp = *fp;
 549         wp->appType = GPS_MAHP;
 550         iap->_MAP.push_back(wp);
 551         // -------
 552         _np_iap[iap->_id].push_back(iap);
 553         */
 554         iap = new FGNPIAP;
 555         iap->_id = "C83";
 556         iap->_name = "GPS RWY 30";
 557         iap->_abbrev = "GPS";
 558         iap->_rwyStr = "30";
 559         iap->_IAF.clear();
 560         iap->_IAP.clear();
 561         iap->_MAP.clear();
 562         // -------
 563         wp = new GPSWaypoint;
 564         wp->id = "MAXNI";
 565         // Nasty using the find any function here, but it saves converting data from FGFix etc.
 566         fp = FindFirstById(wp->id, multi, true);
 567         if(fp) {
 568                 *wp = *fp;
 569                 wp->appType = GPS_IAF;
 570                 iap->_IAF.push_back(wp);
 571         }
 572         // -------
 573         wp = new GPSWaypoint;
 574         wp->id = "PATYY";
 575         fp = FindFirstById(wp->id, multi, true);
 576         if(fp) {
 577                 *wp = *fp;
 578                 wp->appType = GPS_IAF;
 579                 iap->_IAF.push_back(wp);
 580         }
 581         // -------
 582         wp = new GPSWaypoint;
 583         wp->id = "TRACY";
 584         fp = FindFirstById(wp->id, multi, true);
 585         if(fp) {
 586                 *wp = *fp;
 587                 wp->appType = GPS_IAF;
 588                 iap->_IAF.push_back(wp);
 589         }
 590         // -------
 591         wp = new GPSWaypoint;
 592         wp->id = "TRACY";
 593         fp = FindFirstById(wp->id, multi, true);
 594         if(fp) {
 595                 *wp = *fp;
 596                 wp->appType = GPS_IAP;
 597                 iap->_IAP.push_back(wp);
 598         }
 599         // -------
 600         wp = new GPSWaypoint;
 601         wp->id = "BABPI";
 602         fp = FindFirstById(wp->id, multi, true);
 603         if(fp) {
 604                 *wp = *fp;
 605                 wp->appType = GPS_FAF;
 606                 iap->_IAP.push_back(wp);
 607         }
 608         // -------
 609         wp = new GPSWaypoint;
 610         wp->id = "AMOSY";
 611         wp->appType = GPS_MAP;
 612         if(wp->id.substr(0, 2) == "RW" && wp->appType == GPS_MAP) {
 613                 // Assume that this is a missed-approach point based on the runway number
 614                 // TODO: Get the runway threshold location etc
 615                 cout << "TODO - implement missed-approach point based on rwy no.\n";
 616         } else {
 617                 fp = FindFirstById(wp->id, multi, true);
 618                 if(fp == NULL) {
 619                         cout << "Failed to find waypoint " << wp->id << " in database...\n";
 620                 } else {
 621                         *wp = *fp;
 622                         wp->appType = GPS_MAP;
 623                 }
 624         }
 625         iap->_IAP.push_back(wp);
 626         // -------
 627         wp = new GPSWaypoint;
 628         wp->id = "HAIRE";
 629         fp = FindFirstById(wp->id, multi, true);
 630         *wp = *fp;
 631         wp->appType = GPS_MAHP;
 632         iap->_MAP.push_back(wp);
 633         // -------
 634         _np_iap[iap->_id].push_back(iap);
 635 }
 636
 637 void DCLGPS::bind() {
 638         fgTie("/instrumentation/gps/waypoint-alert", this, &DCLGPS::GetWaypointAlert);
 639         fgTie("/instrumentation/gps/leg-mode", this, &DCLGPS::GetLegMode);
 640         fgTie("/instrumentation/gps/obs-mode", this, &DCLGPS::GetOBSMode);
 641         fgTie("/instrumentation/gps/approach-arm", this, &DCLGPS::GetApproachArm);
 642         fgTie("/instrumentation/gps/approach-active", this, &DCLGPS::GetApproachActive);
 643         fgTie("/instrumentation/gps/cdi-deflection", this, &DCLGPS::GetCDIDeflection);
 644         fgTie("/instrumentation/gps/to-flag", this, &DCLGPS::GetToFlag);
 645 }
 646
 647 void DCLGPS::unbind() {
 648         fgUntie("/instrumentation/gps/waypoint-alert");
 649         fgUntie("/instrumentation/gps/leg-mode");
 650         fgUntie("/instrumentation/gps/obs-mode");
 651         fgUntie("/instrumentation/gps/approach-arm");
 652         fgUntie("/instrumentation/gps/approach-active");
 653         fgUntie("/instrumentation/gps/cdi-deflection");
 654 }
 655
 656 void DCLGPS::update(double dt) {
 657         //cout << "update called!\n";
 658
 659         _lon = _lon_node->getDoubleValue() * SG_DEGREES_TO_RADIANS;
 660         _lat = _lat_node->getDoubleValue() * SG_DEGREES_TO_RADIANS;
 661         _alt = _alt_node->getDoubleValue();
 662         _groundSpeed_kts = _grnd_speed_node->getDoubleValue();
 663         _groundSpeed_ms = _groundSpeed_kts * 0.5144444444;
 664         _track = _true_track_node->getDoubleValue();
 665         _magTrackDeg = _mag_track_node->getDoubleValue();
 666         // Note - we can depriciate _gpsLat and _gpsLon if we implement error handling in FG
 667         // gps code and not our own.
 668         _gpsLon = _lon;
 669         _gpsLat = _lat;
 670         // Check for abnormal position slew
 671         if(GetGreatCircleDistance(_gpsLat, _gpsLon, _checkLat, _checkLon) > 1.0) {
 672                 OrientateToActiveFlightPlan();
 673         }
 674         _checkLon = _gpsLon;
 675         _checkLat = _gpsLat;
 676
 677         if(!_departed) {
 678                 if(_groundSpeed_kts > 30.0) {
 679                         _departed = true;
 680                         string th = fgGetString("/instrumentation/clock/indicated-hour");
 681                         string tm = fgGetString("/instrumentation/clock/indicated-min");
 682                         if(th.size() == 1) th = "0" + th;
 683                         if(tm.size() == 1) tm = "0" + tm;
 684                         _departureTimeString = th + tm;
 685                 }
 686         } else {
 687                 // TODO - check - is this prone to drift error over time?
 688                 // Should we difference the departure and current times?
 689                 // What about when the user resets the time of day from the menu?
 690                 _elapsedTime += dt;
 691         }
 692
 693         _time->update(_gpsLon * SG_DEGREES_TO_RADIANS, _gpsLat * SG_DEGREES_TO_RADIANS, 0, 0);
 694         // FIXME - currently all the below assumes leg mode and no DTO or OBS cancelled.
 695         if(_activeFP->IsEmpty()) {
 696                 // Not sure if we need to reset these each update or only when fp altered
 697                 _activeWaypoint.id.clear();
 698                 _navFlagged = true;
 699         } else if(_activeFP->waypoints.size() == 1) {
 700                 _activeWaypoint.id.clear();
 701         } else {
 702                 _navFlagged = false;
 703                 if(_activeWaypoint.id.empty() || _fromWaypoint.id.empty()) {
 704                         //cout << "Error, in leg mode with flightplan of 2 or more waypoints, but either active or from wp is NULL!\n";
 705                         OrientateToActiveFlightPlan();
 706                 }
 707
 708                 // Approach stuff
 709                 if(_approachLoaded) {
 710                         if(!_approachReallyArmed && !_approachActive) {
 711                                 // arm if within 30nm of airport.
 712                                 // TODO - let user cancel approach arm using external GPS-APR switch
 713                                 bool multi;
 714                                 const FGAirport* ap = FindFirstAptById(_approachID, multi, true);
 715                                 if(ap != NULL) {
 716                                         double d = GetGreatCircleDistance(_gpsLat, _gpsLon, ap->getLatitude() * SG_DEGREES_TO_RADIANS, ap->getLongitude() * SG_DEGREES_TO_RADIANS);
 717                                         if(d <= 30) {
 718                                                 _approachArm = true;
 719                                                 _approachReallyArmed = true;
 720                                                 _messageStack.push_back("*Press ALT To Set Baro");
 721                                                 // Not sure what we do if the user has already set CDI to 0.3 nm?
 722                                                 _targetCdiScaleIndex = 1;
 723                                                 if(_currentCdiScaleIndex == 1) {
 724                                                         // No problem!
 725                                                 } else if(_currentCdiScaleIndex == 0) {
 726                                                         _sourceCdiScaleIndex = 0;
 727                                                         _cdiScaleTransition = true;
 728                                                         _cdiTransitionTime = 30.0;
 729                                                         _currentCdiScale = _cdiScales[_currentCdiScaleIndex];
 730                                                 }
 731                                         }
 732                                 }
 733                         } else {
 734                                 // Check for approach active - we can only activate approach if it is really armed.
 735                                 if(_activeWaypoint.appType == GPS_FAF) {
 736                                         //cout << "Active waypoint is FAF, id is " << _activeWaypoint.id << '\n';
 737                                         if(GetGreatCircleDistance(_gpsLat, _gpsLon, _activeWaypoint.lat, _activeWaypoint.lon) <= 2.0 && !_obsMode) {
 738                                                 // Assume heading is OK for now
 739                                                 _approachArm = false;   // TODO - check - maybe arm is left on when actv comes on?
 740                                                 _approachReallyArmed = false;
 741                                                 _approachActive = true;
 742                                                 _targetCdiScaleIndex = 2;
 743                                                 if(_currentCdiScaleIndex == 2) {
 744                                                         // No problem!
 745                                                 } else if(_currentCdiScaleIndex == 1) {
 746                                                         _sourceCdiScaleIndex = 1;
 747                                                         _cdiScaleTransition = true;
 748                                                         _cdiTransitionTime = 30.0;      // TODO - compress it if time to FAF < 30sec
 749                                                         _currentCdiScale = _cdiScales[_currentCdiScaleIndex];
 750                                                 } else {
 751                                                         // Abort going active?
 752                                                         _approachActive = false;
 753                                                 }
 754                                         }
 755                                 }
 756                         }
 757                 }
 758
 759                 // CDI scale transition stuff
 760                 if(_cdiScaleTransition) {
 761                         if(fabs(_currentCdiScale - _cdiScales[_targetCdiScaleIndex]) < 0.001) {
 762                                 _currentCdiScale = _cdiScales[_targetCdiScaleIndex];
 763                                 _currentCdiScaleIndex = _targetCdiScaleIndex;
 764                                 _cdiScaleTransition = false;
 765                         } else {
 766                                 double scaleDiff = (_targetCdiScaleIndex > _sourceCdiScaleIndex
 767                                                     ? _cdiScales[_sourceCdiScaleIndex] - _cdiScales[_targetCdiScaleIndex]
 768                                                                         : _cdiScales[_targetCdiScaleIndex] - _cdiScales[_sourceCdiScaleIndex]);
 769                                 //cout << "ScaleDiff = " << scaleDiff << '\n';
 770                                 if(_targetCdiScaleIndex > _sourceCdiScaleIndex) {
 771                                         // Scaling down eg. 5nm -> 1nm
 772                                         _currentCdiScale -= (scaleDiff * dt / _cdiTransitionTime);
 773                                         if(_currentCdiScale < _cdiScales[_targetCdiScaleIndex]) {
 774                                                 _currentCdiScale = _cdiScales[_targetCdiScaleIndex];
 775                                                 _currentCdiScaleIndex = _targetCdiScaleIndex;
 776                                                 _cdiScaleTransition = false;
 777                                         }
 778                                 } else {
 779                                         _currentCdiScale += (scaleDiff * dt / _cdiTransitionTime);
 780                                         if(_currentCdiScale > _cdiScales[_targetCdiScaleIndex]) {
 781                                                 _currentCdiScale = _cdiScales[_targetCdiScaleIndex];
 782                                                 _currentCdiScaleIndex = _targetCdiScaleIndex;
 783                                                 _cdiScaleTransition = false;
 784                                         }
 785                                 }
 786                                 //cout << "_currentCdiScale = " << _currentCdiScale << '\n';
 787                         }
 788                 } else {
 789                         _currentCdiScale = _cdiScales[_currentCdiScaleIndex];
 790                 }
 791
 792
 793                 // Urgh - I've been setting the heading bug based on DTK,
 794                 // bug I think it should be based on heading re. active waypoint
 795                 // based on what the sim does after the final waypoint is passed.
 796                 // (DTK remains the same, but if track is held == DTK heading bug
 797                 // reverses to from once wp is passed).
 798                 /*
 799                 if(_fromWaypoint != NULL) {
 800                         // TODO - how do we handle the change of track with distance over long legs?
 801                         _dtkTrue = GetGreatCircleCourse(_fromWaypoint->lat, _fromWaypoint->lon, _activeWaypoint->lat, _activeWaypoint->lon) * SG_RADIANS_TO_DEGREES;
 802                         _dtkMag = GetMagHeadingFromTo(_fromWaypoint->lat, _fromWaypoint->lon, _activeWaypoint->lat, _activeWaypoint->lon);
 803                         // Don't change the heading bug if speed is too low otherwise it flickers to/from at rest
 804                         if(_groundSpeed_ms > 5) {
 805                                 //cout << "track = " << _track << ", dtk = " << _dtkTrue << '\n';
 806                                 double courseDev = _track - _dtkTrue;
 807                                 //cout << "courseDev = " << courseDev << ", normalized = ";
 808                                 SG_NORMALIZE_RANGE(courseDev, -180.0, 180.0);
 809                                 //cout << courseDev << '\n';
 810                                 _headingBugTo = (fabs(courseDev) > 90.0 ? false : true);
 811                         }
 812                 } else {
 813                         _dtkTrue = 0.0;
 814                         _dtkMag = 0.0;
 815                         // TODO - in DTO operation the position of initiation of DTO defines the "from waypoint".
 816                 }
 817                 */
 818                 if(!_activeWaypoint.id.empty()) {
 819                         double hdgTrue = GetGreatCircleCourse(_gpsLat, _gpsLon, _activeWaypoint.lat, _activeWaypoint.lon) * SG_RADIANS_TO_DEGREES;
 820                         if(_groundSpeed_ms > 5) {
 821                                 //cout << "track = " << _track << ", hdgTrue = " << hdgTrue << '\n';
 822                                 double courseDev = _track - hdgTrue;
 823                                 //cout << "courseDev = " << courseDev << ", normalized = ";
 824                                 SG_NORMALIZE_RANGE(courseDev, -180.0, 180.0);
 825                                 //cout << courseDev << '\n';
 826                                 _headingBugTo = (fabs(courseDev) > 90.0 ? false : true);
 827                         }
 828                         if(!_fromWaypoint.id.empty()) {
 829                                 _dtkTrue = GetGreatCircleCourse(_fromWaypoint.lat, _fromWaypoint.lon, _activeWaypoint.lat, _activeWaypoint.lon) * SG_RADIANS_TO_DEGREES;
 830                                 _dtkMag = GetMagHeadingFromTo(_fromWaypoint.lat, _fromWaypoint.lon, _activeWaypoint.lat, _activeWaypoint.lon);
 831                         } else {
 832                                 _dtkTrue = 0.0;
 833                                 _dtkMag = 0.0;
 834                         }
 835                 }
 836
 837                 _dist2Act = GetGreatCircleDistance(_gpsLat, _gpsLon, _activeWaypoint.lat, _activeWaypoint.lon) * SG_NM_TO_METER;
 838                 if(_groundSpeed_ms > 10.0) {
 839                         _eta = _dist2Act / _groundSpeed_ms;
 840                         if(_eta <= 36) {        // TODO - this is slightly different if turn anticipation is enabled.
 841                                 if(_headingBugTo) {
 842                                         _waypointAlert = true;  // TODO - not if the from flag is set.
 843                                 }
 844                         }
 845                         if(_eta < 60) {
 846                                 // Check if we should sequence to next leg.
 847                                 // Perhaps this should be done on distance instead, but 60s time (about 1 - 2 nm) seems reasonable for now.
 848                                 //double reverseHeading = GetGreatCircleCourse(_activeWaypoint->lat, _activeWaypoint->lon, _fromWaypoint->lat, _fromWaypoint->lon);
 849                                 // Hack - let's cheat and do it on heading bug for now.  TODO - that stops us 'cutting the corner'
 850                                 // when we happen to approach the inside turn of a waypoint - we should probably sequence at the midpoint
 851                                 // of the heading difference between legs in this instance.
 852                                 int idx = GetActiveWaypointIndex();
 853                                 bool finalLeg = (idx == (int)(_activeFP->waypoints.size()) - 1 ? true : false);
 854                                 bool finalDto = (_dto && idx == -1);    // Dto operation to a waypoint not in the flightplan - we don't sequence in this instance
 855                                 if(!_headingBugTo) {
 856                                         if(finalLeg) {
 857                                                 // Do nothing - not sure if Dto should switch off when arriving at the final waypoint of a flightplan
 858                                         } else if(finalDto) {
 859                                                 // Do nothing
 860                                         } else if(_activeWaypoint.appType == GPS_MAP) {
 861                                                 // Don't sequence beyond the missed approach point
 862                                                 cout << "ACTIVE WAYPOINT is MAP - not sequencing!!!!!\n";
 863                                         } else {
 864                                                 cout << "Sequencing...\n";
 865                                                 _fromWaypoint = _activeWaypoint;
 866                                                 _activeWaypoint = *_activeFP->waypoints[idx + 1];
 867                                                 _dto = false;
 868                                                 // TODO - course alteration message format is dependent on whether we are slaved HSI/CDI indicator or not.
 869                                                 // For now assume we are not.
 870                                                 string s;
 871                                                 if(fgGetBool("/instrumentation/nav[0]/slaved-to-gps")) {
 872                                                         // TODO - avoid the hardwiring on nav[0]
 873                                                         s = "Adj Nav Crs to ";
 874                                                 } else {
 875                                                         string s = "GPS Course is ";
 876                                                 }
 877                                                 double d = GetMagHeadingFromTo(_fromWaypoint.lat, _fromWaypoint.lon, _activeWaypoint.lat, _activeWaypoint.lon);
 878                                                 while(d < 0.0) d += 360.0;
 879                                                 while(d >= 360.0) d -= 360.0;
 880                                                 char buf[4];
 881                                                 snprintf(buf, 4, "%03i", (int)(d + 0.5));
 882                                                 s += buf;
 883                                                 _messageStack.push_back(s);
 884                                         }
 885                                         _waypointAlert = false;
 886                                 }
 887                         }
 888                 } else {
 889                         _eta = 0.0;
 890                 }
 891
 892                 /*
 893                 // First attempt at a sensible cross-track correction calculation
 894                 // Uh? - I think this is implemented further down the file!
 895                 if(_fromWaypoint != NULL) {
 896
 897                 } else {
 898                         _crosstrackDist = 0.0;
 899                 }
 900                 */
 901         }
 902 }
 903
 904 double DCLGPS::GetCDIDeflection() const {
 905         double xtd = CalcCrossTrackDeviation(); //nm
 906         return((xtd / _currentCdiScale) * 5.0 * 2.5 * -1.0);
 907 }
 908
 909 void DCLGPS::DtoInitiate(const string& s) {
 910         //cout << "DtoInitiate, s = " << s << '\n';
 911         bool multi;
 912         const GPSWaypoint* wp = FindFirstById(s, multi, true);
 913         if(wp) {
 914                 //cout << "Waypoint found, starting dto operation!\n";
 915                 _dto = true;
 916                 _activeWaypoint = *wp;
 917                 _fromWaypoint.lat = _gpsLat;
 918                 _fromWaypoint.lon = _gpsLon;
 919                 _fromWaypoint.type = GPS_WP_VIRT;
 920                 _fromWaypoint.id = "DTOWP";
 921         } else {
 922                 //cout << "Waypoint not found, ignoring dto request\n";
 923                 // Should bring up the user waypoint page, but we're not implementing that yet.
 924                 _dto = false;   // TODO - implement this some day.
 925         }
 926 }
 927
 928 void DCLGPS::DtoCancel() {
 929         if(_dto) {
 930                 // i.e. don't bother reorientating if we're just cancelling a DTO button press
 931                 // without having previously initiated DTO.
 932                 OrientateToActiveFlightPlan();
 933         }
 934         _dto = false;
 935 }
 936
 937 void DCLGPS::Knob1Left1() {}
 938 void DCLGPS::Knob1Right1() {}
 939 void DCLGPS::Knob2Left1() {}
 940 void DCLGPS::Knob2Right1() {}
 941 void DCLGPS::CrsrPressed() { _activePage->CrsrPressed(); }
 942 void DCLGPS::EntPressed() { _activePage->EntPressed(); }
 943 void DCLGPS::ClrPressed() { _activePage->ClrPressed(); }
 944 void DCLGPS::DtoPressed() {}
 945 void DCLGPS::NrstPressed() {}
 946 void DCLGPS::AltPressed() {}
 947
 948 void DCLGPS::OBSPressed() {
 949         _obsMode = !_obsMode;
 950         if(_obsMode) {
 951                 if(!_activeWaypoint.id.empty()) {
 952                         _obsHeading = static_cast<int>(_dtkMag);
 953                 }
 954                 // TODO - the _fromWaypoint location will change as the OBS heading changes.
 955                 // Might need to store the OBS initiation position somewhere in case it is needed again.
 956                 SetOBSFromWaypoint();
 957         }
 958 }
 959
 960 // Set the _fromWaypoint position based on the active waypoint and OBS radial.
 961 void DCLGPS::SetOBSFromWaypoint() {
 962         if(!_obsMode) return;
 963         if(_activeWaypoint.id.empty()) return;
 964
 965         // TODO - base the 180 deg correction on the to/from flag.
 966         _fromWaypoint = GetPositionOnMagRadial(_activeWaypoint, 10, _obsHeading + 180.0);
 967         _fromWaypoint.id = "OBSWP";
 968 }
 969
 970 void DCLGPS::MsgPressed() {}
 971
 972 void DCLGPS::CDIFSDIncrease() {
 973         if(_currentCdiScaleIndex == 0) {
 974                 _currentCdiScaleIndex = _cdiScales.size() - 1;
 975         } else {
 976                 _currentCdiScaleIndex--;
 977         }
 978 }
 979
 980 void DCLGPS::CDIFSDDecrease() {
 981         _currentCdiScaleIndex++;
 982         if(_currentCdiScaleIndex == _cdiScales.size()) {
 983                 _currentCdiScaleIndex = 0;
 984         }
 985 }
 986
 987 void DCLGPS::DrawChar(char c, int field, int px, int py, bool bold) {
 988 }
 989
 990 void DCLGPS::DrawText(const string& s, int field, int px, int py, bool bold) {
 991 }
 992
 993 void DCLGPS::SetBaroUnits(int n, bool wrap) {
 994         if(n < 1) {
 995                 _baroUnits = (GPSPressureUnits)(wrap ? 3 : 1);
 996         } else if(n > 3) {
 997                 _baroUnits = (GPSPressureUnits)(wrap ? 1 : 3);
 998         } else {
 999                 _baroUnits = (GPSPressureUnits)n;
1000         }
1001 }
1002
1003 void DCLGPS::CreateDefaultFlightPlans() {}
1004
1005 // Get the time to the active waypoint in seconds.
1006 // Returns -1 if groundspeed < 30 kts
1007 double DCLGPS::GetTimeToActiveWaypoint() {
1008         if(_groundSpeed_kts < 30.0) {
1009                 return(-1.0);
1010         } else {
1011                 return(_eta);
1012         }
1013 }
1014
1015 // Get the time to the final waypoint in seconds.
1016 // Returns -1 if groundspeed < 30 kts
1017 double DCLGPS::GetETE() {
1018         if(_groundSpeed_kts < 30.0) {
1019                 return(-1.0);
1020         } else {
1021                 // TODO - handle OBS / DTO operation appropriately
1022                 if(_activeFP->waypoints.empty()) {
1023                         return(-1.0);
1024                 } else {
1025                         return(GetTimeToWaypoint(_activeFP->waypoints[_activeFP->waypoints.size() - 1]->id));
1026                 }
1027         }
1028 }
1029
1030 // Get the time to a given waypoint (spec'd by ID) in seconds.
1031 // returns -1 if groundspeed is less than 30kts.
1032 // If the waypoint is an unreached part of the active flight plan the time will be via each leg.
1033 // otherwise it will be a direct-to time.
1034 double DCLGPS::GetTimeToWaypoint(const string& id) {
1035         if(_groundSpeed_kts < 30.0) {
1036                 return(-1.0);
1037         }
1038
1039         double eta = 0.0;
1040         int n1 = GetActiveWaypointIndex();
1041         int n2 = GetWaypointIndex(id);
1042         if(n2 > n1) {
1043                 eta = _eta;
1044                 for(unsigned int i=n1+1; i<_activeFP->waypoints.size(); ++i) {
1045                         GPSWaypoint* wp1 = _activeFP->waypoints[i-1];
1046                         GPSWaypoint* wp2 = _activeFP->waypoints[i];
1047                         double distm = GetGreatCircleDistance(wp1->lat, wp1->lon, wp2->lat, wp2->lon) * SG_NM_TO_METER;
1048                         eta += (distm / _groundSpeed_ms);
1049                 }
1050                 return(eta);
1051         } else if(id == _activeWaypoint.id) {
1052                 return(_eta);
1053         } else {
1054                 bool multi;
1055                 const GPSWaypoint* wp = FindFirstById(id, multi, true);
1056                 if(wp == NULL) return(-1.0);
1057                 double distm = GetGreatCircleDistance(_gpsLat, _gpsLon, wp->lat, wp->lon);
1058                 return(distm / _groundSpeed_ms);
1059         }
1060         return(-1.0);   // Hopefully we never get here!
1061 }
1062
1063 // Returns magnetic great-circle heading
1064 // TODO - document units.
1065 float DCLGPS::GetHeadingToActiveWaypoint() {
1066         if(_activeWaypoint.id.empty()) {
1067                 return(-888);
1068         } else {
1069                 double h = GetMagHeadingFromTo(_gpsLat, _gpsLon, _activeWaypoint.lat, _activeWaypoint.lon);
1070                 while(h <= 0.0) h += 360.0;
1071                 while(h > 360.0) h -= 360.0;
1072                 return((float)h);
1073         }
1074 }
1075
1076 // Returns magnetic great-circle heading
1077 // TODO - what units?
1078 float DCLGPS::GetHeadingFromActiveWaypoint() {
1079         if(_activeWaypoint.id.empty()) {
1080                 return(-888);
1081         } else {
1082                 double h = GetMagHeadingFromTo(_activeWaypoint.lat, _activeWaypoint.lon, _gpsLat, _gpsLon);
1083                 while(h <= 0.0) h += 360.0;
1084                 while(h > 360.0) h -= 360.0;
1085                 return(h);
1086         }
1087 }
1088
1089 void DCLGPS::ClearFlightPlan(int n) {
1090         for(unsigned int i=0; i<_flightPlans[n]->waypoints.size(); ++i) {
1091                 delete _flightPlans[n]->waypoints[i];
1092         }
1093         _flightPlans[n]->waypoints.clear();
1094 }
1095
1096 void DCLGPS::ClearFlightPlan(GPSFlightPlan* fp) {
1097         for(unsigned int i=0; i<fp->waypoints.size(); ++i) {
1098                 delete fp->waypoints[i];
1099         }
1100         fp->waypoints.clear();
1101 }
1102
1103 int DCLGPS::GetActiveWaypointIndex() {
1104         for(unsigned int i=0; i<_flightPlans[0]->waypoints.size(); ++i) {
1105                 if(_flightPlans[0]->waypoints[i]->id == _activeWaypoint.id) return((int)i);
1106         }
1107         return(-1);
1108 }
1109
1110 int DCLGPS::GetWaypointIndex(const string& id) {
1111         for(unsigned int i=0; i<_flightPlans[0]->waypoints.size(); ++i) {
1112                 if(_flightPlans[0]->waypoints[i]->id == id) return((int)i);
1113         }
1114         return(-1);
1115 }
1116
1117 void DCLGPS::OrientateToFlightPlan(GPSFlightPlan* fp) {
1118         //cout << "Orientating...\n";
1119         //cout << "_lat = " << _lat << ", _lon = " << _lon << ", _gpsLat = " << _gpsLat << ", gpsLon = " << _gpsLon << '\n';
1120         if(fp->IsEmpty()) {
1121                 _activeWaypoint.id.clear();
1122                 _navFlagged = true;
1123         } else {
1124                 _navFlagged = false;
1125                 if(fp->waypoints.size() == 1) {
1126                         // TODO - may need to flag nav here if not dto or obs, or possibly handle it somewhere else.
1127                         _activeWaypoint = *fp->waypoints[0];
1128                         _fromWaypoint.id.clear();
1129                 } else {
1130                         // FIXME FIXME FIXME
1131                         _fromWaypoint = *fp->waypoints[0];
1132                         _activeWaypoint = *fp->waypoints[1];
1133                         double dmin = 1000000;  // nm!!
1134                         // For now we will simply start on the leg closest to our current position.
1135                         // It's possible that more fancy algorithms may take either heading or track
1136                         // into account when setting inital leg - I'm not sure.
1137                         // This method should handle most cases perfectly OK though.
1138                         for(unsigned int i = 1; i < fp->waypoints.size(); ++i) {
1139                                 //cout << "Pass " << i << ", dmin = " << dmin << ", leg is " << fp->waypoints[i-1]->id << " to " << fp->waypoints[i]->id << '\n';
1140                                 // First get the cross track correction.
1141                                 double d0 = fabs(CalcCrossTrackDeviation(*fp->waypoints[i-1], *fp->waypoints[i]));
1142                                 // That is the shortest distance away we could be though - check for
1143                                 // longer distances if we are 'off the end' of the leg.
1144                                 double ht1 = GetGreatCircleCourse(fp->waypoints[i-1]->lat, fp->waypoints[i-1]->lon,
1145                                                                   fp->waypoints[i]->lat, fp->waypoints[i]->lon)
1146                                                                                                   * SG_RADIANS_TO_DEGREES;
1147                                 // not simply the reverse of the above due to great circle navigation.
1148                                 double ht2 = GetGreatCircleCourse(fp->waypoints[i]->lat, fp->waypoints[i]->lon,
1149                                                                   fp->waypoints[i-1]->lat, fp->waypoints[i-1]->lon)
1150                                                                                                   * SG_RADIANS_TO_DEGREES;
1151                                 double hw1 = GetGreatCircleCourse(_gpsLat, _gpsLon,
1152                                                                   fp->waypoints[i]->lat, fp->waypoints[i]->lon)
1153                                                                                                   * SG_RADIANS_TO_DEGREES;
1154                                 double hw2 = GetGreatCircleCourse(_gpsLat, _gpsLon,
1155                                                                   fp->waypoints[i-1]->lat, fp->waypoints[i-1]->lon)
1156                                                                                                   * SG_RADIANS_TO_DEGREES;
1157                                 double h1 = ht1 - hw1;
1158                                 double h2 = ht2 - hw2;
1159                                 //cout << "d0, h1, h2 = " << d0 << ", " << h1 << ", " << h2 << '\n';
1160                                 //cout << "Normalizing...\n";
1161                                 SG_NORMALIZE_RANGE(h1, -180.0, 180.0);
1162                                 SG_NORMALIZE_RANGE(h2, -180.0, 180.0);
1163                                 //cout << "d0, h1, h2 = " << d0 << ", " << h1 << ", " << h2 << '\n';
1164                                 if(fabs(h1) > 90.0) {
1165                                         // We are past the end of the to waypoint
1166                                         double d = GetGreatCircleDistance(_gpsLat, _gpsLon, fp->waypoints[i]->lat, fp->waypoints[i]->lon);
1167                                         if(d > d0) d0 = d;
1168                                         //cout << "h1 triggered, d0 now = " << d0 << '\n';
1169                                 } else if(fabs(h2) > 90.0) {
1170                                         // We are past the end (not yet at!) the from waypoint
1171                                         double d = GetGreatCircleDistance(_gpsLat, _gpsLon, fp->waypoints[i-1]->lat, fp->waypoints[i-1]->lon);
1172                                         if(d > d0) d0 = d;
1173                                         //cout << "h2 triggered, d0 now = " << d0 << '\n';
1174                                 }
1175                                 if(d0 < dmin) {
1176                                         //cout << "THIS LEG NOW ACTIVE!\n";
1177                                         dmin = d0;
1178                                         _fromWaypoint = *fp->waypoints[i-1];
1179                                         _activeWaypoint = *fp->waypoints[i];
1180                                 }
1181                         }
1182                 }
1183         }
1184 }
1185
1186 void DCLGPS::OrientateToActiveFlightPlan() {
1187         OrientateToFlightPlan(_activeFP);
1188 }
1189
1190 /***************************************/
1191
1192 // Utility function - create a flightplan from a list of waypoint ids and types
1193 void DCLGPS::CreateFlightPlan(GPSFlightPlan* fp, vector<string> ids, vector<GPSWpType> wps) {
1194         if(fp == NULL) fp = new GPSFlightPlan;
1195         unsigned int i;
1196         if(!fp->waypoints.empty()) {
1197                 for(i=0; i<fp->waypoints.size(); ++i) {
1198                         delete fp->waypoints[i];
1199                 }
1200                 fp->waypoints.clear();
1201         }
1202         if(ids.size() != wps.size()) {
1203                 cout << "ID and Waypoint types list size mismatch in GPS::CreateFlightPlan - no flightplan created!\n";
1204                 return;
1205         }
1206         for(i=0; i<ids.size(); ++i) {
1207                 bool multi;
1208                 const FGAirport* ap;
1209                 FGNavRecord* np;
1210                 GPSWaypoint* wp = new GPSWaypoint;
1211                 wp->type = wps[i];
1212                 switch(wp->type) {
1213                 case GPS_WP_APT:
1214                         ap = FindFirstAptById(ids[i], multi, true);
1215                         if(ap == NULL) {
1216                                 // error
1217                                 delete wp;
1218                         } else {
1219                                 wp->lat = ap->getLatitude() * SG_DEGREES_TO_RADIANS;
1220                                 wp->lon = ap->getLongitude() * SG_DEGREES_TO_RADIANS;
1221                                 wp->id = ids[i];
1222                                 fp->waypoints.push_back(wp);
1223                         }
1224                         break;
1225                 case GPS_WP_VOR:
1226                         np = FindFirstVorById(ids[i], multi, true);
1227                         if(np == NULL) {
1228                                 // error
1229                                 delete wp;
1230                         } else {
1231                                 wp->lat = np->get_lat() * SG_DEGREES_TO_RADIANS;
1232                                 wp->lon = np->get_lon() * SG_DEGREES_TO_RADIANS;
1233                                 wp->id = ids[i];
1234                                 fp->waypoints.push_back(wp);
1235                         }
1236                         break;
1237                 case GPS_WP_NDB:
1238                         np = FindFirstNDBById(ids[i], multi, true);
1239                         if(np == NULL) {
1240                                 // error
1241                                 delete wp;
1242                         } else {
1243                                 wp->lat = np->get_lat() * SG_DEGREES_TO_RADIANS;
1244                                 wp->lon = np->get_lon() * SG_DEGREES_TO_RADIANS;
1245                                 wp->id = ids[i];
1246                                 fp->waypoints.push_back(wp);
1247                         }
1248                         break;
1249                 case GPS_WP_INT:
1250                         // TODO TODO
1251                         break;
1252                 case GPS_WP_USR:
1253                         // TODO
1254                         break;
1255                 }
1256         }
1257 }
1258
1259 /***************************************/
1260
1261 const GPSWaypoint* DCLGPS::ActualFindFirstById(const string& id, bool exact) {
1262     gps_waypoint_map_const_iterator itr;
1263     if(exact) {
1264         itr = _waypoints.find(id);
1265     } else {
1266         itr = _waypoints.lower_bound(id);
1267     }
1268     if(itr == _waypoints.end()) {
1269         return(NULL);
1270     } else {
1271                 // TODO - don't just return the first one - either return all or the nearest one.
1272         return((itr->second)[0]);
1273     }
1274 }
1275
1276 const GPSWaypoint* DCLGPS::FindFirstById(const string& id, bool &multi, bool exact) {
1277         multi = false;
1278         if(exact) return(ActualFindFirstById(id, exact));
1279
1280         // OK, that was the easy case, now the fuzzy case
1281         const GPSWaypoint* w1 = ActualFindFirstById(id);
1282         if(w1 == NULL) return(w1);
1283
1284         // The non-trivial code from here to the end of the function is all to deal with the fact that
1285         // the KLN89 alphabetical order (numbers AFTER letters) differs from ASCII order (numbers BEFORE letters).
1286         string id2 = id;
1287         //string id3 = id+'0';
1288         string id4 = id+'A';
1289         // Increment the last char to provide the boundary.  Note that 'Z' -> '[' but we also need to check '0' for all since GPS has numbers after letters
1290         //bool alfa = isalpha(id2[id2.size() - 1]);
1291         id2[id2.size() - 1] = id2[id2.size() - 1] + 1;
1292         const GPSWaypoint* w2 = ActualFindFirstById(id2);
1293         //FGAirport* a3 = globals->get_airports()->findFirstById(id3);
1294         const GPSWaypoint* w4 = ActualFindFirstById(id4);
1295         //cout << "Strings sent were " << id << ", " << id2 << " and " << id4 << '\n';
1296         //cout << "Airports returned were (a1, a2, a4): " << a1->getId() << ", " << a2->getId() << ", " << a4->getId() << '\n';
1297         //cout << "Pointers were " << a1 << ", " << a2 << ", " << a4 << '\n';
1298
1299         // TODO - the below handles the imediately following char OK
1300         // eg id = "KD" returns "KDAA" instead of "KD5"
1301         // but it doesn't handle numbers / letters further down the string,
1302         // eg - id = "I" returns "IA01" instead of "IAN"
1303         // We either need to provide a custom comparison operator, or recurse this function if !isalpha further down the string.
1304         // (Currenly fixed with recursion).
1305
1306         if(w4 != w2) { // A-Z match - preferred
1307                 //cout << "A-Z match!\n";
1308                 if(w4->id.size() - id.size() > 2) {
1309                         // Check for numbers further on
1310                         for(unsigned int i=id.size(); i<w4->id.size(); ++i) {
1311                                 if(!isalpha(w4->id[i])) {
1312                                         //cout << "SUBSTR is " << (a4->getId()).substr(0, i) << '\n';
1313                                         return(FindFirstById(w4->id.substr(0, i), multi, exact));
1314                                 }
1315                         }
1316                 }
1317                 return(w4);
1318         } else if(w1 != w2) {  // 0-9 match
1319                 //cout << "0-9 match!\n";
1320                 if(w1->id.size() - id.size() > 2) {
1321                         // Check for numbers further on
1322                         for(unsigned int i=id.size(); i<w1->id.size(); ++i) {
1323                                 if(!isalpha(w1->id[i])) {
1324                                         //cout << "SUBSTR2 is " << (a4->getId()).substr(0, i) << '\n';
1325                                         return(FindFirstById(w1->id.substr(0, i), multi, exact));
1326                                 }
1327                         }
1328                 }
1329                 return(w1);
1330         } else {  // No match
1331                 return(NULL);
1332         }
1333         return NULL;
1334 }
1335
1336 // Host specific lookup functions
1337 // TODO - add the ASCII / alphabetical stuff from the Atlas version
1338 FGNavRecord* DCLGPS::FindFirstVorById(const string& id, bool &multi, bool exact) {
1339         // NOTE - at the moment multi is never set.
1340         multi = false;
1341         //if(exact) return(_overlays->FindFirstVorById(id, exact));
1342
1343         nav_list_type nav = globals->get_navlist()->findFirstByIdent(id, FG_NAV_VOR, exact);
1344
1345         if(nav.size() > 1) multi = true;
1346         //return(nav.empty() ? NULL : *(nav.begin()));
1347
1348         // The above is sort of what we want - unfortunately we can't guarantee no NDB/ILS at the moment
1349         if(nav.empty()) return(NULL);
1350
1351         for(nav_list_iterator it = nav.begin(); it != nav.end(); ++it) {
1352                 if((*it)->get_type() == 3) return(*it);
1353         }
1354         return(NULL);   // Shouldn't get here!
1355 }
1356 #if 0
1357 Overlays::NAV* DCLGPS::FindFirstVorById(const string& id, bool &multi, bool exact) {
1358         // NOTE - at the moment multi is never set.
1359         multi = false;
1360         if(exact) return(_overlays->FindFirstVorById(id, exact));
1361
1362         // OK, that was the easy case, now the fuzzy case
1363         Overlays::NAV* n1 = _overlays->FindFirstVorById(id);
1364         if(n1 == NULL) return(n1);
1365
1366         string id2 = id;
1367         string id3 = id+'0';
1368         string id4 = id+'A';
1369         // Increment the last char to provide the boundary.  Note that 'Z' -> '[' but we also need to check '0' for all since GPS has numbers after letters
1370         bool alfa = isalpha(id2[id2.size() - 1]);
1371         id2[id2.size() - 1] = id2[id2.size() - 1] + 1;
1372         Overlays::NAV* n2 = _overlays->FindFirstVorById(id2);
1373         //Overlays::NAV* n3 = _overlays->FindFirstVorById(id3);
1374         //Overlays::NAV* n4 = _overlays->FindFirstVorById(id4);
1375         //cout << "Strings sent were " << id << ", " << id2 << ", " << id3 << ", " << id4 << '\n';
1376
1377
1378         if(alfa) {
1379                 if(n1 != n2) { // match
1380                         return(n1);
1381                 } else {
1382                         return(NULL);
1383                 }
1384         }
1385
1386         /*
1387         if(n1 != n2) {
1388                 // Something matches - the problem is the number/letter preference order is reversed between the GPS and the STL
1389                 if(n4 != n2) {
1390                         // There's a letter match - return that
1391                         return(n4);
1392                 } else {
1393                         // By definition we must have a number match
1394                         if(n3 == n2) cout << "HELP - LOGIC FLAW in find VOR!\n";
1395                         return(n3);
1396                 }
1397         } else {
1398                 // No match
1399                 return(NULL);
1400         }
1401         */
1402         return NULL;
1403 }
1404 #endif //0
1405
1406 // TODO - add the ASCII / alphabetical stuff from the Atlas version
1407 FGNavRecord* DCLGPS::FindFirstNDBById(const string& id, bool &multi, bool exact) {
1408         // NOTE - at the moment multi is never set.
1409         multi = false;
1410         //if(exact) return(_overlays->FindFirstVorById(id, exact));
1411
1412         nav_list_type nav = globals->get_navlist()->findFirstByIdent(id, FG_NAV_NDB, exact);
1413
1414         if(nav.size() > 1) multi = true;
1415         //return(nav.empty() ? NULL : *(nav.begin()));
1416
1417         // The above is sort of what we want - unfortunately we can't guarantee no NDB/ILS at the moment
1418         if(nav.empty()) return(NULL);
1419
1420         for(nav_list_iterator it = nav.begin(); it != nav.end(); ++it) {
1421                 if((*it)->get_type() == 2) return(*it);
1422         }
1423         return(NULL);   // Shouldn't get here!
1424 }
1425 #if 0
1426 Overlays::NAV* DCLGPS::FindFirstNDBById(const string& id, bool &multi, bool exact) {
1427         // NOTE - at the moment multi is never set.
1428         multi = false;
1429         if(exact) return(_overlays->FindFirstNDBById(id, exact));
1430
1431         // OK, that was the easy case, now the fuzzy case
1432         Overlays::NAV* n1 = _overlays->FindFirstNDBById(id);
1433         if(n1 == NULL) return(n1);
1434
1435         string id2 = id;
1436         string id3 = id+'0';
1437         string id4 = id+'A';
1438         // Increment the last char to provide the boundary.  Note that 'Z' -> '[' but we also need to check '0' for all since GPS has numbers after letters
1439         bool alfa = isalpha(id2[id2.size() - 1]);
1440         id2[id2.size() - 1] = id2[id2.size() - 1] + 1;
1441         Overlays::NAV* n2 = _overlays->FindFirstNDBById(id2);
1442         //Overlays::NAV* n3 = _overlays->FindFirstNDBById(id3);
1443         //Overlays::NAV* n4 = _overlays->FindFirstNDBById(id4);
1444         //cout << "Strings sent were " << id << ", " << id2 << ", " << id3 << ", " << id4 << '\n';
1445
1446
1447         if(alfa) {
1448                 if(n1 != n2) { // match
1449                         return(n1);
1450                 } else {
1451                         return(NULL);
1452                 }
1453         }
1454
1455         /*
1456         if(n1 != n2) {
1457                 // Something matches - the problem is the number/letter preference order is reversed between the GPS and the STL
1458                 if(n4 != n2) {
1459                         // There's a letter match - return that
1460                         return(n4);
1461                 } else {
1462                         // By definition we must have a number match
1463                         if(n3 == n2) cout << "HELP - LOGIC FLAW in find VOR!\n";
1464                         return(n3);
1465                 }
1466         } else {
1467                 // No match
1468                 return(NULL);
1469         }
1470         */
1471         return NULL;
1472 }
1473 #endif //0
1474
1475 // TODO - add the ASCII / alphabetical stuff from the Atlas version
1476 const FGFix* DCLGPS::FindFirstIntById(const string& id, bool &multi, bool exact) {
1477         // NOTE - at the moment multi is never set, and indeed can't be
1478         // since FG can only map one Fix per ID at the moment.
1479         multi = false;
1480         if(exact) return(globals->get_fixlist()->findFirstByIdent(id, exact));
1481
1482         const FGFix* f1 = globals->get_fixlist()->findFirstByIdent(id, exact);
1483         if(f1 == NULL) return(f1);
1484
1485         // The non-trivial code from here to the end of the function is all to deal with the fact that
1486         // the KLN89 alphabetical order (numbers AFTER letters) differs from ASCII order (numbers BEFORE letters).
1487         // It is copied from the airport version which is definately needed, but at present I'm not actually
1488         // sure if any fixes in FG or real-life have numbers in them!
1489         string id2 = id;
1490         //string id3 = id+'0';
1491         string id4 = id+'A';
1492         // Increment the last char to provide the boundary.  Note that 'Z' -> '[' but we also need to check '0' for all since GPS has numbers after letters
1493         //bool alfa = isalpha(id2[id2.size() - 1]);
1494         id2[id2.size() - 1] = id2[id2.size() - 1] + 1;
1495         const FGFix* f2 = globals->get_fixlist()->findFirstByIdent(id2);
1496         //const FGFix* a3 = globals->get_fixlist()->findFirstByIdent(id3);
1497         const FGFix* f4 = globals->get_fixlist()->findFirstByIdent(id4);
1498
1499         // TODO - the below handles the imediately following char OK
1500         // eg id = "KD" returns "KDAA" instead of "KD5"
1501         // but it doesn't handle numbers / letters further down the string,
1502         // eg - id = "I" returns "IA01" instead of "IAN"
1503         // We either need to provide a custom comparison operator, or recurse this function if !isalpha further down the string.
1504         // (Currenly fixed with recursion).
1505
1506         if(f4 != f2) { // A-Z match - preferred
1507                 //cout << "A-Z match!\n";
1508                 if(f4->get_ident().size() - id.size() > 2) {
1509                         // Check for numbers further on
1510                         for(unsigned int i=id.size(); i<f4->get_ident().size(); ++i) {
1511                                 if(!isalpha(f4->get_ident()[i])) {
1512                                         //cout << "SUBSTR is " << (a4->getId()).substr(0, i) << '\n';
1513                                         return(FindFirstIntById(f4->get_ident().substr(0, i), multi, exact));
1514                                 }
1515                         }
1516                 }
1517                 return(f4);
1518         } else if(f1 != f2) {  // 0-9 match
1519                 //cout << "0-9 match!\n";
1520                 if(f1->get_ident().size() - id.size() > 2) {
1521                         // Check for numbers further on
1522                         for(unsigned int i=id.size(); i<f1->get_ident().size(); ++i) {
1523                                 if(!isalpha(f1->get_ident()[i])) {
1524                                         //cout << "SUBSTR2 is " << (a4->getId()).substr(0, i) << '\n';
1525                                         return(FindFirstIntById(f1->get_ident().substr(0, i), multi, exact));
1526                                 }
1527                         }
1528                 }
1529                 return(f1);
1530         } else {  // No match
1531                 return(NULL);
1532         }
1533
1534         return NULL;    // Don't think we can ever get here.
1535 }
1536
1537 const FGAirport* DCLGPS::FindFirstAptById(const string& id, bool &multi, bool exact) {
1538         // NOTE - at the moment multi is never set.
1539         //cout << "FindFirstAptById, id = " << id << '\n';
1540         multi = false;
1541         if(exact) return(globals->get_airports()->findFirstById(id, exact));
1542
1543         // OK, that was the easy case, now the fuzzy case
1544         const FGAirport* a1 = globals->get_airports()->findFirstById(id);
1545         if(a1 == NULL) return(a1);
1546
1547         // The non-trivial code from here to the end of the function is all to deal with the fact that
1548         // the KLN89 alphabetical order (numbers AFTER letters) differs from ASCII order (numbers BEFORE letters).
1549         string id2 = id;
1550         //string id3 = id+'0';
1551         string id4 = id+'A';
1552         // Increment the last char to provide the boundary.  Note that 'Z' -> '[' but we also need to check '0' for all since GPS has numbers after letters
1553         //bool alfa = isalpha(id2[id2.size() - 1]);
1554         id2[id2.size() - 1] = id2[id2.size() - 1] + 1;
1555         const FGAirport* a2 = globals->get_airports()->findFirstById(id2);
1556         //FGAirport* a3 = globals->get_airports()->findFirstById(id3);
1557         const FGAirport* a4 = globals->get_airports()->findFirstById(id4);
1558         //cout << "Strings sent were " << id << ", " << id2 << " and " << id4 << '\n';
1559         //cout << "Airports returned were (a1, a2, a4): " << a1->getId() << ", " << a2->getId() << ", " << a4->getId() << '\n';
1560         //cout << "Pointers were " << a1 << ", " << a2 << ", " << a4 << '\n';
1561
1562         // TODO - the below handles the imediately following char OK
1563         // eg id = "KD" returns "KDAA" instead of "KD5"
1564         // but it doesn't handle numbers / letters further down the string,
1565         // eg - id = "I" returns "IA01" instead of "IAN"
1566         // We either need to provide a custom comparison operator, or recurse this function if !isalpha further down the string.
1567         // (Currenly fixed with recursion).
1568
1569         if(a4 != a2) { // A-Z match - preferred
1570                 //cout << "A-Z match!\n";
1571                 if(a4->getId().size() - id.size() > 2) {
1572                         // Check for numbers further on
1573                         for(unsigned int i=id.size(); i<a4->getId().size(); ++i) {
1574                                 if(!isalpha(a4->getId()[i])) {
1575                                         //cout << "SUBSTR is " << (a4->getId()).substr(0, i) << '\n';
1576                                         return(FindFirstAptById(a4->getId().substr(0, i), multi, exact));
1577                                 }
1578                         }
1579                 }
1580                 return(a4);
1581         } else if(a1 != a2) {  // 0-9 match
1582                 //cout << "0-9 match!\n";
1583                 if(a1->getId().size() - id.size() > 2) {
1584                         // Check for numbers further on
1585                         for(unsigned int i=id.size(); i<a1->getId().size(); ++i) {
1586                                 if(!isalpha(a1->getId()[i])) {
1587                                         //cout << "SUBSTR2 is " << (a4->getId()).substr(0, i) << '\n';
1588                                         return(FindFirstAptById(a1->getId().substr(0, i), multi, exact));
1589                                 }
1590                         }
1591                 }
1592                 return(a1);
1593         } else {  // No match
1594                 return(NULL);
1595         }
1596
1597         return NULL;
1598 }
1599
1600 FGNavRecord* DCLGPS::FindClosestVor(double lat_rad, double lon_rad) {
1601         return(globals->get_navlist()->findClosest(lon_rad, lat_rad, 0.0, FG_NAV_VOR));
1602 }
1603
1604 //----------------------------------------------------------------------------------------------------------
1605
1606 // Takes lat and lon in RADIANS!!!!!!!
1607 double DCLGPS::GetMagHeadingFromTo(double latA, double lonA, double latB, double lonB) {
1608         double h = GetGreatCircleCourse(latA, lonA, latB, lonB);
1609         h *= SG_RADIANS_TO_DEGREES;
1610         // TODO - use the real altitude below instead of 0.0!
1611         //cout << "MagVar = " << sgGetMagVar(_gpsLon, _gpsLat, 0.0, _time->getJD()) * SG_RADIANS_TO_DEGREES << '\n';
1612         h -= sgGetMagVar(_gpsLon, _gpsLat, 0.0, _time->getJD()) * SG_RADIANS_TO_DEGREES;
1613         while(h >= 360.0) h -= 360.0;
1614         while(h < 0.0) h += 360.0;
1615         return(h);
1616 }
1617
1618 // ---------------- Great Circle formulae from "The Aviation Formulary" -------------
1619 // Note that all of these assume that the world is spherical.
1620
1621 double Rad2Nm(double theta) {
1622         return(((180.0*60.0)/SG_PI)*theta);
1623 }
1624
1625 double Nm2Rad(double d) {
1626         return((SG_PI/(180.0*60.0))*d);
1627 }
1628
1629 /* QUOTE:
1630
1631 The great circle distance d between two points with coordinates {lat1,lon1} and {lat2,lon2} is given by:
1632
1633 d=acos(sin(lat1)*sin(lat2)+cos(lat1)*cos(lat2)*cos(lon1-lon2))
1634
1635 A mathematically equivalent formula, which is less subject to rounding error for short distances is:
1636
1637 d=2*asin(sqrt((sin((lat1-lat2)/2))^2 +
1638                  cos(lat1)*cos(lat2)*(sin((lon1-lon2)/2))^2))
1639
1640 */
1641
1642 // Returns distance in nm, takes lat & lon in RADIANS
1643 double DCLGPS::GetGreatCircleDistance(double lat1, double lon1, double lat2, double lon2) const {
1644         double d = 2.0 * asin(sqrt(((sin((lat1-lat2)/2.0))*(sin((lat1-lat2)/2.0))) +
1645                    cos(lat1)*cos(lat2)*(sin((lon1-lon2)/2.0))*(sin((lon1-lon2)/2.0))));
1646         return(Rad2Nm(d));
1647 }
1648
1649 // fmod dosen't do what we want :-(
1650 static double mod(double d1, double d2) {
1651         return(d1 - d2*floor(d1/d2));
1652 }
1653
1654 // Returns great circle course from point 1 to point 2
1655 // Input and output in RADIANS.
1656 double DCLGPS::GetGreatCircleCourse (double lat1, double lon1, double lat2, double lon2) const {
1657         //double h = 0.0;
1658         /*
1659         // Special case the poles
1660         if(cos(lat1) < SG_EPSILON) {
1661                 if(lat1 > 0) {
1662                         // Starting from North Pole
1663                         h = SG_PI;
1664                 } else {
1665                         // Starting from South Pole
1666                         h = 2.0 * SG_PI;
1667                 }
1668         } else {
1669                 // Urgh - the formula below is for negative lon +ve !!!???
1670                 double d = GetGreatCircleDistance(lat1, lon1, lat2, lon2);
1671                 cout << "d = " << d;
1672                 d = Nm2Rad(d);
1673                 //cout << ", d_theta = " << d;
1674                 //cout << ", and d = " << Rad2Nm(d) << ' ';
1675                 if(sin(lon2 - lon1) < 0) {
1676                         cout << " A ";
1677                         h = acos((sin(lat2)-sin(lat1)*cos(d))/(sin(d)*cos(lat1)));
1678                 } else {
1679                         cout << " B ";
1680                         h = 2.0 * SG_PI - acos((sin(lat2)-sin(lat1)*cos(d))/(sin(d)*cos(lat1)));
1681                 }
1682         }
1683         cout << h * SG_RADIANS_TO_DEGREES << '\n';
1684         */
1685
1686         return( mod(atan2(sin(lon2-lon1)*cos(lat2),
1687             cos(lat1)*sin(lat2)-sin(lat1)*cos(lat2)*cos(lon2-lon1)),
1688             2.0*SG_PI) );
1689 }
1690
1691 // Return a position on a radial from wp1 given distance d (nm) and magnetic heading h (degrees)
1692 // Note that d should be less that 1/4 Earth diameter!
1693 GPSWaypoint DCLGPS::GetPositionOnMagRadial(const GPSWaypoint& wp1, double d, double h) {
1694         h += sgGetMagVar(wp1.lon, wp1.lat, 0.0, _time->getJD()) * SG_RADIANS_TO_DEGREES;
1695         return(GetPositionOnRadial(wp1, d, h));
1696 }
1697
1698 // Return a position on a radial from wp1 given distance d (nm) and TRUE heading h (degrees)
1699 // Note that d should be less that 1/4 Earth diameter!
1700 GPSWaypoint DCLGPS::GetPositionOnRadial(const GPSWaypoint& wp1, double d, double h) {
1701         while(h < 0.0) h += 360.0;
1702         while(h > 360.0) h -= 360.0;
1703
1704         h *= SG_DEGREES_TO_RADIANS;
1705         d *= (SG_PI / (180.0 * 60.0));
1706
1707         double lat=asin(sin(wp1.lat)*cos(d)+cos(wp1.lat)*sin(d)*cos(h));
1708         double lon;
1709         if(cos(lat)==0) {
1710                 lon=wp1.lon;      // endpoint a pole
1711         } else {
1712                 lon=mod(wp1.lon+asin(sin(h)*sin(d)/cos(lat))+SG_PI,2*SG_PI)-SG_PI;
1713         }
1714
1715         GPSWaypoint wp;
1716         wp.lat = lat;
1717         wp.lon = lon;
1718         wp.type = GPS_WP_VIRT;
1719         return(wp);
1720 }
1721
1722 // Returns cross-track deviation in Nm.
1723 double DCLGPS::CalcCrossTrackDeviation() const {
1724         return(CalcCrossTrackDeviation(_fromWaypoint, _activeWaypoint));
1725 }
1726
1727 // Returns cross-track deviation of the current position between two arbitary waypoints in nm.
1728 double DCLGPS::CalcCrossTrackDeviation(const GPSWaypoint& wp1, const GPSWaypoint& wp2) const {
1729         //if(wp1 == NULL || wp2 == NULL) return(0.0);
1730         if(wp1.id.empty() || wp2.id.empty()) return(0.0);
1731         double xtd = asin(sin(Nm2Rad(GetGreatCircleDistance(wp1.lat, wp1.lon, _gpsLat, _gpsLon)))
1732                           * sin(GetGreatCircleCourse(wp1.lat, wp1.lon, _gpsLat, _gpsLon) - GetGreatCircleCourse(wp1.lat, wp1.lon, wp2.lat, wp2.lon)));
1733         return(Rad2Nm(xtd));
1734 }