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