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