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