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 <Navaids/fix.hxx>
  35
  36 #include <iostream>
  37 using std::cout;
  38
  39 //using namespace std;
  40
  41 // Command callbacks for FlightGear
  42
  43 static bool do_kln89_msg_pressed(const SGPropertyNode* arg) {
  44         //cout << "do_kln89_msg_pressed called!\n";
  45         DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89");
  46         gps->MsgPressed();
  47         return(true);
  48 }
  49
  50 static bool do_kln89_obs_pressed(const SGPropertyNode* arg) {
  51         //cout << "do_kln89_obs_pressed called!\n";
  52         DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89");
  53         gps->OBSPressed();
  54         return(true);
  55 }
  56
  57 static bool do_kln89_alt_pressed(const SGPropertyNode* arg) {
  58         //cout << "do_kln89_alt_pressed called!\n";
  59         DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89");
  60         gps->AltPressed();
  61         return(true);
  62 }
  63
  64 static bool do_kln89_nrst_pressed(const SGPropertyNode* arg) {
  65         DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89");
  66         gps->NrstPressed();
  67         return(true);
  68 }
  69
  70 static bool do_kln89_dto_pressed(const SGPropertyNode* arg) {
  71         DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89");
  72         gps->DtoPressed();
  73         return(true);
  74 }
  75
  76 static bool do_kln89_clr_pressed(const SGPropertyNode* arg) {
  77         DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89");
  78         gps->ClrPressed();
  79         return(true);
  80 }
  81
  82 static bool do_kln89_ent_pressed(const SGPropertyNode* arg) {
  83         DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89");
  84         gps->EntPressed();
  85         return(true);
  86 }
  87
  88 static bool do_kln89_crsr_pressed(const SGPropertyNode* arg) {
  89         DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89");
  90         gps->CrsrPressed();
  91         return(true);
  92 }
  93
  94 static bool do_kln89_knob1left1(const SGPropertyNode* arg) {
  95         DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89");
  96         gps->Knob1Left1();
  97         return(true);
  98 }
  99
 100 static bool do_kln89_knob1right1(const SGPropertyNode* arg) {
 101         DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89");
 102         gps->Knob1Right1();
 103         return(true);
 104 }
 105
 106 static bool do_kln89_knob2left1(const SGPropertyNode* arg) {
 107         DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89");
 108         gps->Knob2Left1();
 109         return(true);
 110 }
 111
 112 static bool do_kln89_knob2right1(const SGPropertyNode* arg) {
 113         DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89");
 114         gps->Knob2Right1();
 115         return(true);
 116 }
 117
 118 // End command callbacks
 119
 120 GPSWaypoint::GPSWaypoint() {
 121     appType = GPS_APP_NONE;
 122 }
 123
 124 GPSWaypoint::GPSWaypoint(const std::string& aIdent, float aLat, float aLon, GPSWpType aType) :
 125   id(aIdent),
 126   lat(aLat),
 127   lon(aLon),
 128   type(aType),
 129   appType(GPS_APP_NONE)
 130 {
 131 }
 132
 133 GPSWaypoint::~GPSWaypoint() {}
 134
 135 string GPSWaypoint::GetAprId() {
 136         if(appType == GPS_IAF) return(id + 'i');
 137         else if(appType == GPS_FAF) return(id + 'f');
 138         else if(appType == GPS_MAP) return(id + 'm');
 139         else if(appType == GPS_MAHP) return(id + 'h');
 140         else return(id);
 141 }
 142
 143 GPSWaypoint* GPSWaypoint::createFromFix(const FGFix* aFix)
 144 {
 145   assert(aFix);
 146   return new GPSWaypoint(aFix->get_ident(),
 147     aFix->get_lat() * SG_DEGREES_TO_RADIANS,
 148     aFix->get_lon() * SG_DEGREES_TO_RADIANS,
 149     GPS_WP_INT);
 150 }
 151
 152 GPSWaypoint* GPSWaypoint::createFromNav(const FGNavRecord* aNav)
 153 {
 154   assert(aNav);
 155   return new GPSWaypoint(aNav->get_ident(),
 156     aNav->get_lat() * SG_DEGREES_TO_RADIANS,
 157     aNav->get_lon() * SG_DEGREES_TO_RADIANS,
 158     (aNav->get_fg_type() == FG_NAV_VOR ? GPS_WP_VOR : GPS_WP_NDB));
 159 }
 160
 161 GPSWaypoint* GPSWaypoint::createFromAirport(const FGAirport* aApt)
 162 {
 163   assert(aApt);
 164   return new GPSWaypoint(aApt->getId(),
 165     aApt->getLatitude() * SG_DEGREES_TO_RADIANS,
 166     aApt->getLongitude() * SG_DEGREES_TO_RADIANS,
 167     GPS_WP_APT);
 168 }
 169
 170 ostream& operator << (ostream& os, GPSAppWpType type) {
 171         switch(type) {
 172                 case(GPS_IAF):       return(os << "IAF");
 173                 case(GPS_IAP):       return(os << "IAP");
 174                 case(GPS_FAF):       return(os << "FAF");
 175                 case(GPS_MAP):       return(os << "MAP");
 176                 case(GPS_MAHP):      return(os << "MAHP");
 177                 case(GPS_HDR):       return(os << "HEADER");
 178                 case(GPS_FENCE):     return(os << "FENCE");
 179                 case(GPS_APP_NONE):  return(os << "NONE");
 180         }
 181         return(os << "ERROR - Unknown switch in GPSAppWpType operator << ");
 182 }
 183
 184 FGIAP::FGIAP() {
 185 }
 186
 187 FGIAP::~FGIAP() {
 188 }
 189
 190 FGNPIAP::FGNPIAP() {
 191 }
 192
 193 FGNPIAP::~FGNPIAP() {
 194 }
 195
 196 ClockTime::ClockTime() {
 197     _hr = 0;
 198     _min = 0;
 199 }
 200
 201 ClockTime::ClockTime(int hr, int min) {
 202     while(hr < 0) { hr += 24; }
 203     _hr = hr % 24;
 204     while(min < 0) { min += 60; }
 205     while(min > 60) { min -= 60; }
 206         _min = min;
 207 }
 208
 209 ClockTime::~ClockTime() {
 210 }
 211
 212 GPSPage::GPSPage(DCLGPS* parent) {
 213         _parent = parent;
 214         _subPage = 0;
 215 }
 216
 217 GPSPage::~GPSPage() {
 218 }
 219
 220 void GPSPage::Update(double dt) {}
 221
 222 void GPSPage::Knob1Left1() {}
 223 void GPSPage::Knob1Right1() {}
 224
 225 void GPSPage::Knob2Left1() {
 226         _parent->_activePage->LooseFocus();
 227         _subPage--;
 228         if(_subPage < 0) _subPage = _nSubPages - 1;
 229 }
 230
 231 void GPSPage::Knob2Right1() {
 232         _parent->_activePage->LooseFocus();
 233         _subPage++;
 234         if(_subPage >= _nSubPages) _subPage = 0;
 235 }
 236
 237 void GPSPage::CrsrPressed() {}
 238 void GPSPage::EntPressed() {}
 239 void GPSPage::ClrPressed() {}
 240 void GPSPage::DtoPressed() {}
 241 void GPSPage::NrstPressed() {}
 242 void GPSPage::AltPressed() {}
 243 void GPSPage::OBSPressed() {}
 244 void GPSPage::MsgPressed() {}
 245
 246 string GPSPage::GPSitoa(int n) {
 247         char buf[4];
 248         // TODO - sanity check n!
 249         sprintf(buf, "%i", n);
 250         string s = buf;
 251         return(s);
 252 }
 253
 254 void GPSPage::CleanUp() {}
 255 void GPSPage::LooseFocus() {}
 256 void GPSPage::SetId(const string& s) {}
 257
 258 // ------------------------------------------------------------------------------------- //
 259
 260 DCLGPS::DCLGPS(RenderArea2D* instrument) {
 261         _instrument = instrument;
 262         _nFields = 1;
 263         _maxFields = 2;
 264         _pages.clear();
 265
 266         // Units - lets default to US units - FG can set them to other units from config during startup if desired.
 267         _altUnits = GPS_ALT_UNITS_FT;
 268         _baroUnits = GPS_PRES_UNITS_IN;
 269         _velUnits = GPS_VEL_UNITS_KT;
 270         _distUnits = GPS_DIST_UNITS_NM;
 271
 272         _lon_node = fgGetNode("/instrumentation/gps/indicated-longitude-deg", true);
 273         _lat_node = fgGetNode("/instrumentation/gps/indicated-latitude-deg", true);
 274         _alt_node = fgGetNode("/instrumentation/gps/indicated-altitude-ft", true);
 275         _grnd_speed_node = fgGetNode("/instrumentation/gps/indicated-ground-speed-kt", true);
 276         _true_track_node = fgGetNode("/instrumentation/gps/indicated-track-true-deg", true);
 277         _mag_track_node = fgGetNode("/instrumentation/gps/indicated-track-magnetic-deg", true);
 278
 279         // Use FG's position values at construction in case FG's gps has not run first update yet.
 280         _lon = fgGetDouble("/position/longitude-deg") * SG_DEGREES_TO_RADIANS;
 281         _lat = fgGetDouble("/position/latitude-deg") * SG_DEGREES_TO_RADIANS;
 282         _alt = fgGetDouble("/position/altitude-ft");
 283         // Note - we can depriciate _gpsLat and _gpsLon if we implement error handling in FG
 284         // gps code and not our own.
 285         _gpsLon = _lon;
 286         _gpsLat = _lat;
 287         _checkLon = _gpsLon;
 288         _checkLat = _gpsLat;
 289         _groundSpeed_ms = 0.0;
 290         _groundSpeed_kts = 0.0;
 291         _track = 0.0;
 292         _magTrackDeg = 0.0;
 293
 294         // Sensible defaults.  These can be overriden by derived classes if desired.
 295         _cdiScales.clear();
 296         _cdiScales.push_back(5.0);
 297         _cdiScales.push_back(1.0);
 298         _cdiScales.push_back(0.3);
 299         _currentCdiScaleIndex = 0;
 300         _targetCdiScaleIndex = 0;
 301         _sourceCdiScaleIndex = 0;
 302         _cdiScaleTransition = false;
 303         _currentCdiScale = 5.0;
 304
 305         _cleanUpPage = -1;
 306
 307         _activeWaypoint.id.clear();
 308         _dist2Act = 0.0;
 309         _crosstrackDist = 0.0;
 310         _headingBugTo = true;
 311         _navFlagged = true;
 312         _waypointAlert = false;
 313         _departed = false;
 314         _departureTimeString = "----";
 315         _elapsedTime = 0.0;
 316         _powerOnTime.set_hr(0);
 317         _powerOnTime.set_min(0);
 318         _powerOnTimerSet = false;
 319         _alarmSet = false;
 320
 321         // Configuration Initialisation
 322         // Should this be in kln89.cxx ?
 323         _turnAnticipationEnabled = false;
 324         _suaAlertEnabled = false;
 325         _altAlertEnabled = false;
 326
 327         _time = new SGTime;
 328
 329         _messageStack.clear();
 330
 331         _dto = false;
 332
 333         _approachLoaded = false;
 334         _approachArm = false;
 335         _approachReallyArmed = false;
 336         _approachActive = false;
 337         _approachFP = new GPSFlightPlan;
 338 }
 339
 340 DCLGPS::~DCLGPS() {
 341         delete _time;
 342   delete _approachFP;           // Don't need to delete the waypoints inside since they point to
 343                                                         // the waypoints in the approach database.
 344         // TODO - may need to delete the approach database!!
 345 }
 346
 347 void DCLGPS::draw() {
 348         //cout << "draw called!\n";
 349         _instrument->draw();
 350 }
 351
 352 void DCLGPS::init() {
 353         globals->get_commands()->addCommand("kln89_msg_pressed", do_kln89_msg_pressed);
 354         globals->get_commands()->addCommand("kln89_obs_pressed", do_kln89_obs_pressed);
 355         globals->get_commands()->addCommand("kln89_alt_pressed", do_kln89_alt_pressed);
 356         globals->get_commands()->addCommand("kln89_nrst_pressed", do_kln89_nrst_pressed);
 357         globals->get_commands()->addCommand("kln89_dto_pressed", do_kln89_dto_pressed);
 358         globals->get_commands()->addCommand("kln89_clr_pressed", do_kln89_clr_pressed);
 359         globals->get_commands()->addCommand("kln89_ent_pressed", do_kln89_ent_pressed);
 360         globals->get_commands()->addCommand("kln89_crsr_pressed", do_kln89_crsr_pressed);
 361         globals->get_commands()->addCommand("kln89_knob1left1", do_kln89_knob1left1);
 362         globals->get_commands()->addCommand("kln89_knob1right1", do_kln89_knob1right1);
 363         globals->get_commands()->addCommand("kln89_knob2left1", do_kln89_knob2left1);
 364         globals->get_commands()->addCommand("kln89_knob2right1", do_kln89_knob2right1);
 365
 366         // Not sure if this should be here, but OK for now.
 367         CreateDefaultFlightPlans();
 368 }
 369
 370 void DCLGPS::bind() {
 371         fgTie("/instrumentation/gps/waypoint-alert", this, &DCLGPS::GetWaypointAlert);
 372         fgTie("/instrumentation/gps/leg-mode", this, &DCLGPS::GetLegMode);
 373         fgTie("/instrumentation/gps/obs-mode", this, &DCLGPS::GetOBSMode);
 374         fgTie("/instrumentation/gps/approach-arm", this, &DCLGPS::GetApproachArm);
 375         fgTie("/instrumentation/gps/approach-active", this, &DCLGPS::GetApproachActive);
 376         fgTie("/instrumentation/gps/cdi-deflection", this, &DCLGPS::GetCDIDeflection);
 377         fgTie("/instrumentation/gps/to-flag", this, &DCLGPS::GetToFlag);
 378 }
 379
 380 void DCLGPS::unbind() {
 381         fgUntie("/instrumentation/gps/waypoint-alert");
 382         fgUntie("/instrumentation/gps/leg-mode");
 383         fgUntie("/instrumentation/gps/obs-mode");
 384         fgUntie("/instrumentation/gps/approach-arm");
 385         fgUntie("/instrumentation/gps/approach-active");
 386         fgUntie("/instrumentation/gps/cdi-deflection");
 387 }
 388
 389 void DCLGPS::update(double dt) {
 390         //cout << "update called!\n";
 391
 392         _lon = _lon_node->getDoubleValue() * SG_DEGREES_TO_RADIANS;
 393         _lat = _lat_node->getDoubleValue() * SG_DEGREES_TO_RADIANS;
 394         _alt = _alt_node->getDoubleValue();
 395         _groundSpeed_kts = _grnd_speed_node->getDoubleValue();
 396         _groundSpeed_ms = _groundSpeed_kts * 0.5144444444;
 397         _track = _true_track_node->getDoubleValue();
 398         _magTrackDeg = _mag_track_node->getDoubleValue();
 399         // Note - we can depriciate _gpsLat and _gpsLon if we implement error handling in FG
 400         // gps code and not our own.
 401         _gpsLon = _lon;
 402         _gpsLat = _lat;
 403         // Check for abnormal position slew
 404         if(GetGreatCircleDistance(_gpsLat, _gpsLon, _checkLat, _checkLon) > 1.0) {
 405                 OrientateToActiveFlightPlan();
 406         }
 407         _checkLon = _gpsLon;
 408         _checkLat = _gpsLat;
 409
 410         // TODO - check for unit power before running this.
 411         if(!_powerOnTimerSet) {
 412                 SetPowerOnTimer();
 413         }
 414
 415         // Check if an alarm timer has expired
 416         if(_alarmSet) {
 417                 if(_alarmTime.hr() == atoi(fgGetString("/instrumentation/clock/indicated-hour"))
 418                 && _alarmTime.min() == atoi(fgGetString("/instrumentation/clock/indicated-min"))) {
 419                         _messageStack.push_back("*Timer Expired");
 420                         _alarmSet = false;
 421                 }
 422         }
 423
 424         if(!_departed) {
 425                 if(_groundSpeed_kts > 30.0) {
 426                         _departed = true;
 427                         string th = fgGetString("/instrumentation/clock/indicated-hour");
 428                         string tm = fgGetString("/instrumentation/clock/indicated-min");
 429                         if(th.size() == 1) th = "0" + th;
 430                         if(tm.size() == 1) tm = "0" + tm;
 431                         _departureTimeString = th + tm;
 432                 }
 433         } else {
 434                 // TODO - check - is this prone to drift error over time?
 435                 // Should we difference the departure and current times?
 436                 // What about when the user resets the time of day from the menu?
 437                 _elapsedTime += dt;
 438         }
 439
 440         _time->update(_gpsLon * SG_DEGREES_TO_RADIANS, _gpsLat * SG_DEGREES_TO_RADIANS, 0, 0);
 441         // FIXME - currently all the below assumes leg mode and no DTO or OBS cancelled.
 442         if(_activeFP->IsEmpty()) {
 443                 // Not sure if we need to reset these each update or only when fp altered
 444                 _activeWaypoint.id.clear();
 445                 _navFlagged = true;
 446         } else if(_activeFP->waypoints.size() == 1) {
 447                 _activeWaypoint.id.clear();
 448         } else {
 449                 _navFlagged = false;
 450                 if(_activeWaypoint.id.empty() || _fromWaypoint.id.empty()) {
 451                         //cout << "Error, in leg mode with flightplan of 2 or more waypoints, but either active or from wp is NULL!\n";
 452                         OrientateToActiveFlightPlan();
 453                 }
 454
 455                 // Approach stuff
 456                 if(_approachLoaded) {
 457                         if(!_approachReallyArmed && !_approachActive) {
 458                                 // arm if within 30nm of airport.
 459                                 // TODO - let user cancel approach arm using external GPS-APR switch
 460                                 bool multi;
 461                                 const FGAirport* ap = FindFirstAptById(_approachID, multi, true);
 462                                 if(ap != NULL) {
 463                                         double d = GetGreatCircleDistance(_gpsLat, _gpsLon, ap->getLatitude() * SG_DEGREES_TO_RADIANS, ap->getLongitude() * SG_DEGREES_TO_RADIANS);
 464                                         if(d <= 30) {
 465                                                 _approachArm = true;
 466                                                 _approachReallyArmed = true;
 467                                                 _messageStack.push_back("*Press ALT To Set Baro");
 468                                                 // Not sure what we do if the user has already set CDI to 0.3 nm?
 469                                                 _targetCdiScaleIndex = 1;
 470                                                 if(_currentCdiScaleIndex == 1) {
 471                                                         // No problem!
 472                                                 } else if(_currentCdiScaleIndex == 0) {
 473                                                         _sourceCdiScaleIndex = 0;
 474                                                         _cdiScaleTransition = true;
 475                                                         _cdiTransitionTime = 30.0;
 476                                                         _currentCdiScale = _cdiScales[_currentCdiScaleIndex];
 477                                                 }
 478                                         }
 479                                 }
 480                         } else {
 481                                 // Check for approach active - we can only activate approach if it is really armed.
 482                                 if(_activeWaypoint.appType == GPS_FAF) {
 483                                         //cout << "Active waypoint is FAF, id is " << _activeWaypoint.id << '\n';
 484                                         if(GetGreatCircleDistance(_gpsLat, _gpsLon, _activeWaypoint.lat, _activeWaypoint.lon) <= 2.0 && !_obsMode) {
 485                                                 // Assume heading is OK for now
 486                                                 _approachArm = false;   // TODO - check - maybe arm is left on when actv comes on?
 487                                                 _approachReallyArmed = false;
 488                                                 _approachActive = true;
 489                                                 _targetCdiScaleIndex = 2;
 490                                                 if(_currentCdiScaleIndex == 2) {
 491                                                         // No problem!
 492                                                 } else if(_currentCdiScaleIndex == 1) {
 493                                                         _sourceCdiScaleIndex = 1;
 494                                                         _cdiScaleTransition = true;
 495                                                         _cdiTransitionTime = 30.0;      // TODO - compress it if time to FAF < 30sec
 496                                                         _currentCdiScale = _cdiScales[_currentCdiScaleIndex];
 497                                                 } else {
 498                                                         // Abort going active?
 499                                                         _approachActive = false;
 500                                                 }
 501                                         }
 502                                 }
 503                         }
 504                 }
 505
 506                 // CDI scale transition stuff
 507                 if(_cdiScaleTransition) {
 508                         if(fabs(_currentCdiScale - _cdiScales[_targetCdiScaleIndex]) < 0.001) {
 509                                 _currentCdiScale = _cdiScales[_targetCdiScaleIndex];
 510                                 _currentCdiScaleIndex = _targetCdiScaleIndex;
 511                                 _cdiScaleTransition = false;
 512                         } else {
 513                                 double scaleDiff = (_targetCdiScaleIndex > _sourceCdiScaleIndex
 514                                                     ? _cdiScales[_sourceCdiScaleIndex] - _cdiScales[_targetCdiScaleIndex]
 515                                                                         : _cdiScales[_targetCdiScaleIndex] - _cdiScales[_sourceCdiScaleIndex]);
 516                                 //cout << "ScaleDiff = " << scaleDiff << '\n';
 517                                 if(_targetCdiScaleIndex > _sourceCdiScaleIndex) {
 518                                         // Scaling down eg. 5nm -> 1nm
 519                                         _currentCdiScale -= (scaleDiff * dt / _cdiTransitionTime);
 520                                         if(_currentCdiScale < _cdiScales[_targetCdiScaleIndex]) {
 521                                                 _currentCdiScale = _cdiScales[_targetCdiScaleIndex];
 522                                                 _currentCdiScaleIndex = _targetCdiScaleIndex;
 523                                                 _cdiScaleTransition = false;
 524                                         }
 525                                 } else {
 526                                         _currentCdiScale += (scaleDiff * dt / _cdiTransitionTime);
 527                                         if(_currentCdiScale > _cdiScales[_targetCdiScaleIndex]) {
 528                                                 _currentCdiScale = _cdiScales[_targetCdiScaleIndex];
 529                                                 _currentCdiScaleIndex = _targetCdiScaleIndex;
 530                                                 _cdiScaleTransition = false;
 531                                         }
 532                                 }
 533                                 //cout << "_currentCdiScale = " << _currentCdiScale << '\n';
 534                         }
 535                 } else {
 536                         _currentCdiScale = _cdiScales[_currentCdiScaleIndex];
 537                 }
 538
 539
 540                 // Urgh - I've been setting the heading bug based on DTK,
 541                 // bug I think it should be based on heading re. active waypoint
 542                 // based on what the sim does after the final waypoint is passed.
 543                 // (DTK remains the same, but if track is held == DTK heading bug
 544                 // reverses to from once wp is passed).
 545                 /*
 546                 if(_fromWaypoint != NULL) {
 547                         // TODO - how do we handle the change of track with distance over long legs?
 548                         _dtkTrue = GetGreatCircleCourse(_fromWaypoint->lat, _fromWaypoint->lon, _activeWaypoint->lat, _activeWaypoint->lon) * SG_RADIANS_TO_DEGREES;
 549                         _dtkMag = GetMagHeadingFromTo(_fromWaypoint->lat, _fromWaypoint->lon, _activeWaypoint->lat, _activeWaypoint->lon);
 550                         // Don't change the heading bug if speed is too low otherwise it flickers to/from at rest
 551                         if(_groundSpeed_ms > 5) {
 552                                 //cout << "track = " << _track << ", dtk = " << _dtkTrue << '\n';
 553                                 double courseDev = _track - _dtkTrue;
 554                                 //cout << "courseDev = " << courseDev << ", normalized = ";
 555                                 SG_NORMALIZE_RANGE(courseDev, -180.0, 180.0);
 556                                 //cout << courseDev << '\n';
 557                                 _headingBugTo = (fabs(courseDev) > 90.0 ? false : true);
 558                         }
 559                 } else {
 560                         _dtkTrue = 0.0;
 561                         _dtkMag = 0.0;
 562                         // TODO - in DTO operation the position of initiation of DTO defines the "from waypoint".
 563                 }
 564                 */
 565                 if(!_activeWaypoint.id.empty()) {
 566                         double hdgTrue = GetGreatCircleCourse(_gpsLat, _gpsLon, _activeWaypoint.lat, _activeWaypoint.lon) * SG_RADIANS_TO_DEGREES;
 567                         if(_groundSpeed_ms > 5) {
 568                                 //cout << "track = " << _track << ", hdgTrue = " << hdgTrue << '\n';
 569                                 double courseDev = _track - hdgTrue;
 570                                 //cout << "courseDev = " << courseDev << ", normalized = ";
 571                                 SG_NORMALIZE_RANGE(courseDev, -180.0, 180.0);
 572                                 //cout << courseDev << '\n';
 573                                 _headingBugTo = (fabs(courseDev) > 90.0 ? false : true);
 574                         }
 575                         if(!_fromWaypoint.id.empty()) {
 576                                 _dtkTrue = GetGreatCircleCourse(_fromWaypoint.lat, _fromWaypoint.lon, _activeWaypoint.lat, _activeWaypoint.lon) * SG_RADIANS_TO_DEGREES;
 577                                 _dtkMag = GetMagHeadingFromTo(_fromWaypoint.lat, _fromWaypoint.lon, _activeWaypoint.lat, _activeWaypoint.lon);
 578                         } else {
 579                                 _dtkTrue = 0.0;
 580                                 _dtkMag = 0.0;
 581                         }
 582                 }
 583
 584                 _dist2Act = GetGreatCircleDistance(_gpsLat, _gpsLon, _activeWaypoint.lat, _activeWaypoint.lon) * SG_NM_TO_METER;
 585                 if(_groundSpeed_ms > 10.0) {
 586                         _eta = _dist2Act / _groundSpeed_ms;
 587                         if(_eta <= 36) {        // TODO - this is slightly different if turn anticipation is enabled.
 588                                 if(_headingBugTo) {
 589                                         _waypointAlert = true;  // TODO - not if the from flag is set.
 590                                 }
 591                         }
 592                         if(_eta < 60) {
 593                                 // Check if we should sequence to next leg.
 594                                 // Perhaps this should be done on distance instead, but 60s time (about 1 - 2 nm) seems reasonable for now.
 595                                 //double reverseHeading = GetGreatCircleCourse(_activeWaypoint->lat, _activeWaypoint->lon, _fromWaypoint->lat, _fromWaypoint->lon);
 596                                 // Hack - let's cheat and do it on heading bug for now.  TODO - that stops us 'cutting the corner'
 597                                 // when we happen to approach the inside turn of a waypoint - we should probably sequence at the midpoint
 598                                 // of the heading difference between legs in this instance.
 599                                 int idx = GetActiveWaypointIndex();
 600                                 bool finalLeg = (idx == (int)(_activeFP->waypoints.size()) - 1 ? true : false);
 601                                 bool finalDto = (_dto && idx == -1);    // Dto operation to a waypoint not in the flightplan - we don't sequence in this instance
 602                                 if(!_headingBugTo) {
 603                                         if(finalLeg) {
 604                                                 // Do nothing - not sure if Dto should switch off when arriving at the final waypoint of a flightplan
 605                                         } else if(finalDto) {
 606                                                 // Do nothing
 607                                         } else if(_activeWaypoint.appType == GPS_MAP) {
 608                                                 // Don't sequence beyond the missed approach point
 609                                                 //cout << "ACTIVE WAYPOINT is MAP - not sequencing!!!!!\n";
 610                                         } else {
 611                                                 //cout << "Sequencing...\n";
 612                                                 _fromWaypoint = _activeWaypoint;
 613                                                 _activeWaypoint = *_activeFP->waypoints[idx + 1];
 614                                                 _dto = false;
 615                                                 // TODO - course alteration message format is dependent on whether we are slaved HSI/CDI indicator or not.
 616                                                 // For now assume we are not.
 617                                                 string s;
 618                                                 if(fgGetBool("/instrumentation/nav[0]/slaved-to-gps")) {
 619                                                         // TODO - avoid the hardwiring on nav[0]
 620                                                         s = "Adj Nav Crs to ";
 621                                                 } else {
 622                                                         string s = "GPS Course is ";
 623                                                 }
 624                                                 double d = GetMagHeadingFromTo(_fromWaypoint.lat, _fromWaypoint.lon, _activeWaypoint.lat, _activeWaypoint.lon);
 625                                                 while(d < 0.0) d += 360.0;
 626                                                 while(d >= 360.0) d -= 360.0;
 627                                                 char buf[4];
 628                                                 snprintf(buf, 4, "%03i", (int)(d + 0.5));
 629                                                 s += buf;
 630                                                 _messageStack.push_back(s);
 631                                         }
 632                                         _waypointAlert = false;
 633                                 }
 634                         }
 635                 } else {
 636                         _eta = 0.0;
 637                 }
 638
 639                 /*
 640                 // First attempt at a sensible cross-track correction calculation
 641                 // Uh? - I think this is implemented further down the file!
 642                 if(_fromWaypoint != NULL) {
 643
 644                 } else {
 645                         _crosstrackDist = 0.0;
 646                 }
 647                 */
 648         }
 649 }
 650
 651 // I don't yet fully understand all the gotchas about where to source time from.
 652 // This function sets the initial timer before the clock exports properties
 653 // and the one below uses the clock to be consistent with the rest of the code.
 654 // It might change soonish...
 655 void DCLGPS::SetPowerOnTimer() {
 656         struct tm *t = globals->get_time_params()->getGmt();
 657         _powerOnTime.set_hr(t->tm_hour);
 658         _powerOnTime.set_min(t->tm_min);
 659         _powerOnTimerSet = true;
 660 }
 661
 662 void DCLGPS::ResetPowerOnTimer() {
 663         _powerOnTime.set_hr(atoi(fgGetString("/instrumentation/clock/indicated-hour")));
 664         _powerOnTime.set_min(atoi(fgGetString("/instrumentation/clock/indicated-min")));
 665         _powerOnTimerSet = true;
 666 }
 667
 668 double DCLGPS::GetCDIDeflection() const {
 669         double xtd = CalcCrossTrackDeviation(); //nm
 670         return((xtd / _currentCdiScale) * 5.0 * 2.5 * -1.0);
 671 }
 672
 673 void DCLGPS::DtoInitiate(const string& s) {
 674         //cout << "DtoInitiate, s = " << s << '\n';
 675         const GPSWaypoint* wp = FindFirstByExactId(s);
 676         if(wp) {
 677                 //cout << "Waypoint found, starting dto operation!\n";
 678                 _dto = true;
 679                 _activeWaypoint = *wp;
 680                 _fromWaypoint.lat = _gpsLat;
 681                 _fromWaypoint.lon = _gpsLon;
 682                 _fromWaypoint.type = GPS_WP_VIRT;
 683                 _fromWaypoint.id = "DTOWP";
 684     delete wp;
 685         } else {
 686                 //cout << "Waypoint not found, ignoring dto request\n";
 687                 // Should bring up the user waypoint page, but we're not implementing that yet.
 688                 _dto = false;   // TODO - implement this some day.
 689         }
 690 }
 691
 692 void DCLGPS::DtoCancel() {
 693         if(_dto) {
 694                 // i.e. don't bother reorientating if we're just cancelling a DTO button press
 695                 // without having previously initiated DTO.
 696                 OrientateToActiveFlightPlan();
 697         }
 698         _dto = false;
 699 }
 700
 701 void DCLGPS::Knob1Left1() {}
 702 void DCLGPS::Knob1Right1() {}
 703 void DCLGPS::Knob2Left1() {}
 704 void DCLGPS::Knob2Right1() {}
 705 void DCLGPS::CrsrPressed() { _activePage->CrsrPressed(); }
 706 void DCLGPS::EntPressed() { _activePage->EntPressed(); }
 707 void DCLGPS::ClrPressed() { _activePage->ClrPressed(); }
 708 void DCLGPS::DtoPressed() {}
 709 void DCLGPS::NrstPressed() {}
 710 void DCLGPS::AltPressed() {}
 711
 712 void DCLGPS::OBSPressed() {
 713         _obsMode = !_obsMode;
 714         if(_obsMode) {
 715                 if(!_activeWaypoint.id.empty()) {
 716                         _obsHeading = static_cast<int>(_dtkMag);
 717                 }
 718                 // TODO - the _fromWaypoint location will change as the OBS heading changes.
 719                 // Might need to store the OBS initiation position somewhere in case it is needed again.
 720                 SetOBSFromWaypoint();
 721         }
 722 }
 723
 724 // Set the _fromWaypoint position based on the active waypoint and OBS radial.
 725 void DCLGPS::SetOBSFromWaypoint() {
 726         if(!_obsMode) return;
 727         if(_activeWaypoint.id.empty()) return;
 728
 729         // TODO - base the 180 deg correction on the to/from flag.
 730         _fromWaypoint = GetPositionOnMagRadial(_activeWaypoint, 10, _obsHeading + 180.0);
 731         _fromWaypoint.id = "OBSWP";
 732 }
 733
 734 void DCLGPS::MsgPressed() {}
 735
 736 void DCLGPS::CDIFSDIncrease() {
 737         if(_currentCdiScaleIndex == 0) {
 738                 _currentCdiScaleIndex = _cdiScales.size() - 1;
 739         } else {
 740                 _currentCdiScaleIndex--;
 741         }
 742 }
 743
 744 void DCLGPS::CDIFSDDecrease() {
 745         _currentCdiScaleIndex++;
 746         if(_currentCdiScaleIndex == _cdiScales.size()) {
 747                 _currentCdiScaleIndex = 0;
 748         }
 749 }
 750
 751 void DCLGPS::DrawChar(char c, int field, int px, int py, bool bold) {
 752 }
 753
 754 void DCLGPS::DrawText(const string& s, int field, int px, int py, bool bold) {
 755 }
 756
 757 void DCLGPS::SetBaroUnits(int n, bool wrap) {
 758         if(n < 1) {
 759                 _baroUnits = (GPSPressureUnits)(wrap ? 3 : 1);
 760         } else if(n > 3) {
 761                 _baroUnits = (GPSPressureUnits)(wrap ? 1 : 3);
 762         } else {
 763                 _baroUnits = (GPSPressureUnits)n;
 764         }
 765 }
 766
 767 void DCLGPS::CreateDefaultFlightPlans() {}
 768
 769 // Get the time to the active waypoint in seconds.
 770 // Returns -1 if groundspeed < 30 kts
 771 double DCLGPS::GetTimeToActiveWaypoint() {
 772         if(_groundSpeed_kts < 30.0) {
 773                 return(-1.0);
 774         } else {
 775                 return(_eta);
 776         }
 777 }
 778
 779 // Get the time to the final waypoint in seconds.
 780 // Returns -1 if groundspeed < 30 kts
 781 double DCLGPS::GetETE() {
 782         if(_groundSpeed_kts < 30.0) {
 783                 return(-1.0);
 784         } else {
 785                 // TODO - handle OBS / DTO operation appropriately
 786                 if(_activeFP->waypoints.empty()) {
 787                         return(-1.0);
 788                 } else {
 789                         return(GetTimeToWaypoint(_activeFP->waypoints[_activeFP->waypoints.size() - 1]->id));
 790                 }
 791         }
 792 }
 793
 794 // Get the time to a given waypoint (spec'd by ID) in seconds.
 795 // returns -1 if groundspeed is less than 30kts.
 796 // If the waypoint is an unreached part of the active flight plan the time will be via each leg.
 797 // otherwise it will be a direct-to time.
 798 double DCLGPS::GetTimeToWaypoint(const string& id) {
 799         if(_groundSpeed_kts < 30.0) {
 800                 return(-1.0);
 801         }
 802
 803         double eta = 0.0;
 804         int n1 = GetActiveWaypointIndex();
 805         int n2 = GetWaypointIndex(id);
 806         if(n2 > n1) {
 807                 eta = _eta;
 808                 for(unsigned int i=n1+1; i<_activeFP->waypoints.size(); ++i) {
 809                         GPSWaypoint* wp1 = _activeFP->waypoints[i-1];
 810                         GPSWaypoint* wp2 = _activeFP->waypoints[i];
 811                         double distm = GetGreatCircleDistance(wp1->lat, wp1->lon, wp2->lat, wp2->lon) * SG_NM_TO_METER;
 812                         eta += (distm / _groundSpeed_ms);
 813                 }
 814                 return(eta);
 815         } else if(id == _activeWaypoint.id) {
 816                 return(_eta);
 817         } else {
 818                 const GPSWaypoint* wp = FindFirstByExactId(id);
 819                 if(wp == NULL) return(-1.0);
 820                 double distm = GetGreatCircleDistance(_gpsLat, _gpsLon, wp->lat, wp->lon);
 821     delete wp;
 822                 return(distm / _groundSpeed_ms);
 823         }
 824         return(-1.0);   // Hopefully we never get here!
 825 }
 826
 827 // Returns magnetic great-circle heading
 828 // TODO - document units.
 829 float DCLGPS::GetHeadingToActiveWaypoint() {
 830         if(_activeWaypoint.id.empty()) {
 831                 return(-888);
 832         } else {
 833                 double h = GetMagHeadingFromTo(_gpsLat, _gpsLon, _activeWaypoint.lat, _activeWaypoint.lon);
 834                 while(h <= 0.0) h += 360.0;
 835                 while(h > 360.0) h -= 360.0;
 836                 return((float)h);
 837         }
 838 }
 839
 840 // Returns magnetic great-circle heading
 841 // TODO - what units?
 842 float DCLGPS::GetHeadingFromActiveWaypoint() {
 843         if(_activeWaypoint.id.empty()) {
 844                 return(-888);
 845         } else {
 846                 double h = GetMagHeadingFromTo(_activeWaypoint.lat, _activeWaypoint.lon, _gpsLat, _gpsLon);
 847                 while(h <= 0.0) h += 360.0;
 848                 while(h > 360.0) h -= 360.0;
 849                 return(h);
 850         }
 851 }
 852
 853 void DCLGPS::ClearFlightPlan(int n) {
 854         for(unsigned int i=0; i<_flightPlans[n]->waypoints.size(); ++i) {
 855                 delete _flightPlans[n]->waypoints[i];
 856         }
 857         _flightPlans[n]->waypoints.clear();
 858 }
 859
 860 void DCLGPS::ClearFlightPlan(GPSFlightPlan* fp) {
 861         for(unsigned int i=0; i<fp->waypoints.size(); ++i) {
 862                 delete fp->waypoints[i];
 863         }
 864         fp->waypoints.clear();
 865 }
 866
 867 int DCLGPS::GetActiveWaypointIndex() {
 868         for(unsigned int i=0; i<_flightPlans[0]->waypoints.size(); ++i) {
 869                 if(_flightPlans[0]->waypoints[i]->id == _activeWaypoint.id) return((int)i);
 870         }
 871         return(-1);
 872 }
 873
 874 int DCLGPS::GetWaypointIndex(const string& id) {
 875         for(unsigned int i=0; i<_flightPlans[0]->waypoints.size(); ++i) {
 876                 if(_flightPlans[0]->waypoints[i]->id == id) return((int)i);
 877         }
 878         return(-1);
 879 }
 880
 881 void DCLGPS::OrientateToFlightPlan(GPSFlightPlan* fp) {
 882         //cout << "Orientating...\n";
 883         //cout << "_lat = " << _lat << ", _lon = " << _lon << ", _gpsLat = " << _gpsLat << ", gpsLon = " << _gpsLon << '\n';
 884         if(fp->IsEmpty()) {
 885                 _activeWaypoint.id.clear();
 886                 _navFlagged = true;
 887         } else {
 888                 _navFlagged = false;
 889                 if(fp->waypoints.size() == 1) {
 890                         // TODO - may need to flag nav here if not dto or obs, or possibly handle it somewhere else.
 891                         _activeWaypoint = *fp->waypoints[0];
 892                         _fromWaypoint.id.clear();
 893                 } else {
 894                         // FIXME FIXME FIXME
 895                         _fromWaypoint = *fp->waypoints[0];
 896                         _activeWaypoint = *fp->waypoints[1];
 897                         double dmin = 1000000;  // nm!!
 898                         // For now we will simply start on the leg closest to our current position.
 899                         // It's possible that more fancy algorithms may take either heading or track
 900                         // into account when setting inital leg - I'm not sure.
 901                         // This method should handle most cases perfectly OK though.
 902                         for(unsigned int i = 1; i < fp->waypoints.size(); ++i) {
 903                                 //cout << "Pass " << i << ", dmin = " << dmin << ", leg is " << fp->waypoints[i-1]->id << " to " << fp->waypoints[i]->id << '\n';
 904                                 // First get the cross track correction.
 905                                 double d0 = fabs(CalcCrossTrackDeviation(*fp->waypoints[i-1], *fp->waypoints[i]));
 906                                 // That is the shortest distance away we could be though - check for
 907                                 // longer distances if we are 'off the end' of the leg.
 908                                 double ht1 = GetGreatCircleCourse(fp->waypoints[i-1]->lat, fp->waypoints[i-1]->lon,
 909                                                                   fp->waypoints[i]->lat, fp->waypoints[i]->lon)
 910                                                                                                   * SG_RADIANS_TO_DEGREES;
 911                                 // not simply the reverse of the above due to great circle navigation.
 912                                 double ht2 = GetGreatCircleCourse(fp->waypoints[i]->lat, fp->waypoints[i]->lon,
 913                                                                   fp->waypoints[i-1]->lat, fp->waypoints[i-1]->lon)
 914                                                                                                   * SG_RADIANS_TO_DEGREES;
 915                                 double hw1 = GetGreatCircleCourse(_gpsLat, _gpsLon,
 916                                                                   fp->waypoints[i]->lat, fp->waypoints[i]->lon)
 917                                                                                                   * SG_RADIANS_TO_DEGREES;
 918                                 double hw2 = GetGreatCircleCourse(_gpsLat, _gpsLon,
 919                                                                   fp->waypoints[i-1]->lat, fp->waypoints[i-1]->lon)
 920                                                                                                   * SG_RADIANS_TO_DEGREES;
 921                                 double h1 = ht1 - hw1;
 922                                 double h2 = ht2 - hw2;
 923                                 //cout << "d0, h1, h2 = " << d0 << ", " << h1 << ", " << h2 << '\n';
 924                                 //cout << "Normalizing...\n";
 925                                 SG_NORMALIZE_RANGE(h1, -180.0, 180.0);
 926                                 SG_NORMALIZE_RANGE(h2, -180.0, 180.0);
 927                                 //cout << "d0, h1, h2 = " << d0 << ", " << h1 << ", " << h2 << '\n';
 928                                 if(fabs(h1) > 90.0) {
 929                                         // We are past the end of the to waypoint
 930                                         double d = GetGreatCircleDistance(_gpsLat, _gpsLon, fp->waypoints[i]->lat, fp->waypoints[i]->lon);
 931                                         if(d > d0) d0 = d;
 932                                         //cout << "h1 triggered, d0 now = " << d0 << '\n';
 933                                 } else if(fabs(h2) > 90.0) {
 934                                         // We are past the end (not yet at!) the from waypoint
 935                                         double d = GetGreatCircleDistance(_gpsLat, _gpsLon, fp->waypoints[i-1]->lat, fp->waypoints[i-1]->lon);
 936                                         if(d > d0) d0 = d;
 937                                         //cout << "h2 triggered, d0 now = " << d0 << '\n';
 938                                 }
 939                                 if(d0 < dmin) {
 940                                         //cout << "THIS LEG NOW ACTIVE!\n";
 941                                         dmin = d0;
 942                                         _fromWaypoint = *fp->waypoints[i-1];
 943                                         _activeWaypoint = *fp->waypoints[i];
 944                                 }
 945                         }
 946                 }
 947         }
 948 }
 949
 950 void DCLGPS::OrientateToActiveFlightPlan() {
 951         OrientateToFlightPlan(_activeFP);
 952 }
 953
 954 /***************************************/
 955
 956 // Utility function - create a flightplan from a list of waypoint ids and types
 957 void DCLGPS::CreateFlightPlan(GPSFlightPlan* fp, vector<string> ids, vector<GPSWpType> wps) {
 958         if(fp == NULL) fp = new GPSFlightPlan;
 959         unsigned int i;
 960         if(!fp->waypoints.empty()) {
 961                 for(i=0; i<fp->waypoints.size(); ++i) {
 962                         delete fp->waypoints[i];
 963                 }
 964                 fp->waypoints.clear();
 965         }
 966         if(ids.size() != wps.size()) {
 967                 cout << "ID and Waypoint types list size mismatch in GPS::CreateFlightPlan - no flightplan created!\n";
 968                 return;
 969         }
 970         for(i=0; i<ids.size(); ++i) {
 971                 bool multi;
 972                 const FGAirport* ap;
 973                 FGNavRecord* np;
 974                 GPSWaypoint* wp = new GPSWaypoint;
 975                 wp->type = wps[i];
 976                 switch(wp->type) {
 977                 case GPS_WP_APT:
 978                         ap = FindFirstAptById(ids[i], multi, true);
 979                         if(ap == NULL) {
 980                                 // error
 981                                 delete wp;
 982                         } else {
 983                                 wp->lat = ap->getLatitude() * SG_DEGREES_TO_RADIANS;
 984                                 wp->lon = ap->getLongitude() * SG_DEGREES_TO_RADIANS;
 985                                 wp->id = ids[i];
 986                                 fp->waypoints.push_back(wp);
 987                         }
 988                         break;
 989                 case GPS_WP_VOR:
 990                         np = FindFirstVorById(ids[i], multi, true);
 991                         if(np == NULL) {
 992                                 // error
 993                                 delete wp;
 994                         } else {
 995                                 wp->lat = np->get_lat() * SG_DEGREES_TO_RADIANS;
 996                                 wp->lon = np->get_lon() * SG_DEGREES_TO_RADIANS;
 997                                 wp->id = ids[i];
 998                                 fp->waypoints.push_back(wp);
 999                         }
1000                         break;
1001                 case GPS_WP_NDB:
1002                         np = FindFirstNDBById(ids[i], multi, true);
1003                         if(np == NULL) {
1004                                 // error
1005                                 delete wp;
1006                         } else {
1007                                 wp->lat = np->get_lat() * SG_DEGREES_TO_RADIANS;
1008                                 wp->lon = np->get_lon() * SG_DEGREES_TO_RADIANS;
1009                                 wp->id = ids[i];
1010                                 fp->waypoints.push_back(wp);
1011                         }
1012                         break;
1013                 case GPS_WP_INT:
1014                         // TODO TODO
1015                         break;
1016                 case GPS_WP_USR:
1017                         // TODO
1018                         break;
1019                 }
1020         }
1021 }
1022
1023 /***************************************/
1024
1025 /**
1026  * STL functor for use with algorithms. This comapres strings according to
1027  * the KLN-89's notion of ordering, with digits after letters.
1028  * Also implements FGIdentOrdering so it can be passed into the various list
1029  * find helpers.
1030  */
1031
1032 class stringOrderKLN89 : public FGIdentOrdering
1033 {
1034 public:
1035   bool operator()(const gps_waypoint_map::value_type& aA, const std::string& aB) const
1036   {
1037     return compare(aA.first, aB);
1038   }
1039
1040   bool operator()(const std::string& aS1, const std::string& aS2) const
1041   {
1042     return compare(aS1, aS2);
1043   }
1044
1045   virtual bool compare(const std::string& aS1, const std::string& aS2) const
1046   {
1047     if (aS1.empty()) return true;
1048     if (aS2.empty()) return false;
1049
1050     char* a = (char*) aS1.c_str();
1051     char* b = (char*) aS2.c_str();
1052
1053     for ( ; *a && *b; ++a, ++b) {
1054       if (*a == *b) continue;
1055
1056       bool aDigit = isdigit(*a);
1057       bool bDigit = isdigit(*b);
1058
1059       if (aDigit == bDigit) {
1060         return (*a < *b); // we already know they're not equal
1061       }
1062
1063       // digit-ness differs
1064       if (aDigit) return false; // s1 = KS9 goes *after* s2 = KSA
1065       assert(bDigit);
1066       return true; // s1 = KSF, s2 = KS5, s1 is indeed < s2
1067     }
1068
1069     if (*b) return true; // *a == 0, s2 is longer
1070     return false; // s1 is longer, or strings are equal
1071   }
1072 };
1073
1074 GPSWaypoint* DCLGPS::FindFirstById(const string& id) const
1075 {
1076   stringOrderKLN89 ordering;
1077   nav_list_type vors = globals->get_navlist()->findFirstByIdent(id, FG_NAV_VOR, false);
1078   nav_list_type ndbs = globals->get_navlist()->findFirstByIdent(id, FG_NAV_NDB, false);
1079   const FGFix* fix = globals->get_fixlist()->findFirstByIdent(id, &ordering);
1080   const FGAirport* apt = globals->get_airports()->findFirstById(id, &ordering);
1081   // search local gps waypoints (USR)
1082
1083 // pick the best - ugly logic, sorry. This is a temporary fix to getting rid
1084 // of the huge local waypoint table, it'll die when there's a way to query
1085 // this stuff centrally.
1086 // what we're doing is using map inserts to order the result, then using
1087 // the first entry (begin()) as the lowest, hence best, match
1088   map<string, GPSWpType, stringOrderKLN89> sorter;
1089   if (fix) sorter[fix->get_ident()] = GPS_WP_INT;
1090   if (apt) sorter[apt->getId()] = GPS_WP_APT;
1091   if (!vors.empty()) sorter[vors.front()->get_ident()] = GPS_WP_VOR;
1092   if (!ndbs.empty()) sorter[ndbs.front()->get_ident()] = GPS_WP_NDB;
1093
1094   if (sorter.empty()) return NULL; // no results at all
1095   GPSWpType ty = sorter.begin()->second;
1096
1097   switch (ty) {
1098   case GPS_WP_INT:
1099     return GPSWaypoint::createFromFix(fix);
1100
1101   case GPS_WP_APT:
1102     return GPSWaypoint::createFromAirport(apt);
1103
1104   case GPS_WP_VOR:
1105     return GPSWaypoint::createFromNav(vors.front());
1106
1107   case GPS_WP_NDB:
1108     return GPSWaypoint::createFromNav(ndbs.front());
1109   default:
1110     return NULL; // can't happen
1111   }
1112 }
1113
1114 GPSWaypoint* DCLGPS::FindFirstByExactId(const string& id) const
1115 {
1116   if (const FGAirport* apt = globals->get_airports()->search(id)) {
1117     return GPSWaypoint::createFromAirport(apt);
1118   }
1119
1120   if (const FGFix* fix = globals->get_fixlist()->search(id)) {
1121     return GPSWaypoint::createFromFix(fix);
1122   }
1123
1124   nav_list_type vors = globals->get_navlist()->findFirstByIdent(id, FG_NAV_VOR, true);
1125   if (!vors.empty()) {
1126     return GPSWaypoint::createFromNav(vors.front());
1127   }
1128
1129   nav_list_type ndbs = globals->get_navlist()->findFirstByIdent(id, FG_NAV_NDB, true);
1130   if (!ndbs.empty()) {
1131     return GPSWaypoint::createFromNav(ndbs.front());
1132   }
1133
1134   return NULL;
1135 }
1136
1137 // Host specific lookup functions
1138 // TODO - add the ASCII / alphabetical stuff from the Atlas version
1139 FGNavRecord* DCLGPS::FindFirstVorById(const string& id, bool &multi, bool exact) {
1140         // NOTE - at the moment multi is never set.
1141         multi = false;
1142         //if(exact) return(_overlays->FindFirstVorById(id, exact));
1143
1144         nav_list_type nav = globals->get_navlist()->findFirstByIdent(id, FG_NAV_VOR, exact);
1145
1146         if(nav.size() > 1) multi = true;
1147         //return(nav.empty() ? NULL : *(nav.begin()));
1148
1149         // The above is sort of what we want - unfortunately we can't guarantee no NDB/ILS at the moment
1150         if(nav.empty()) return(NULL);
1151
1152         for(nav_list_iterator it = nav.begin(); it != nav.end(); ++it) {
1153                 if((*it)->type() == FGPositioned::VOR) return(*it);
1154         }
1155         return(NULL);   // Shouldn't get here!
1156 }
1157
1158 // TODO - add the ASCII / alphabetical stuff from the Atlas version
1159 FGNavRecord* DCLGPS::FindFirstNDBById(const string& id, bool &multi, bool exact) {
1160         // NOTE - at the moment multi is never set.
1161         multi = false;
1162         //if(exact) return(_overlays->FindFirstVorById(id, exact));
1163
1164         nav_list_type nav = globals->get_navlist()->findFirstByIdent(id, FG_NAV_NDB, exact);
1165
1166         if(nav.size() > 1) multi = true;
1167         //return(nav.empty() ? NULL : *(nav.begin()));
1168
1169         // The above is sort of what we want - unfortunately we can't guarantee no NDB/ILS at the moment
1170         if(nav.empty()) return(NULL);
1171
1172         for(nav_list_iterator it = nav.begin(); it != nav.end(); ++it) {
1173                 if((*it)->type() == FGPositioned::NDB) return(*it);
1174         }
1175         return(NULL);   // Shouldn't get here!
1176 }
1177
1178 const FGFix* DCLGPS::FindFirstIntById(const string& id, bool &multi, bool exact) {
1179         // NOTE - at the moment multi is never set, and indeed can't be
1180         // since FG can only map one Fix per ID at the moment.
1181         multi = false;
1182         if (exact) return globals->get_fixlist()->search(id);
1183
1184   stringOrderKLN89 ordering;
1185   return globals->get_fixlist()->findFirstByIdent(id, &ordering);
1186 }
1187
1188 const FGAirport* DCLGPS::FindFirstAptById(const string& id, bool &multi, bool exact) {
1189         // NOTE - at the moment multi is never set.
1190         //cout << "FindFirstAptById, id = " << id << '\n';
1191         multi = false;
1192         if(exact) return(globals->get_airports()->search(id));
1193
1194   stringOrderKLN89 ordering;
1195   return globals->get_airports()->findFirstById(id, &ordering);
1196 }
1197
1198 FGNavRecord* DCLGPS::FindClosestVor(double lat_rad, double lon_rad) {
1199         return(globals->get_navlist()->findClosest(lon_rad, lat_rad, 0.0, FG_NAV_VOR));
1200 }
1201
1202 //----------------------------------------------------------------------------------------------------------
1203
1204 // Takes lat and lon in RADIANS!!!!!!!
1205 double DCLGPS::GetMagHeadingFromTo(double latA, double lonA, double latB, double lonB) {
1206         double h = GetGreatCircleCourse(latA, lonA, latB, lonB);
1207         h *= SG_RADIANS_TO_DEGREES;
1208         // TODO - use the real altitude below instead of 0.0!
1209         //cout << "MagVar = " << sgGetMagVar(_gpsLon, _gpsLat, 0.0, _time->getJD()) * SG_RADIANS_TO_DEGREES << '\n';
1210         h -= sgGetMagVar(_gpsLon, _gpsLat, 0.0, _time->getJD()) * SG_RADIANS_TO_DEGREES;
1211         while(h >= 360.0) h -= 360.0;
1212         while(h < 0.0) h += 360.0;
1213         return(h);
1214 }
1215
1216 // ---------------- Great Circle formulae from "The Aviation Formulary" -------------
1217 // Note that all of these assume that the world is spherical.
1218
1219 double Rad2Nm(double theta) {
1220         return(((180.0*60.0)/SG_PI)*theta);
1221 }
1222
1223 double Nm2Rad(double d) {
1224         return((SG_PI/(180.0*60.0))*d);
1225 }
1226
1227 /* QUOTE:
1228
1229 The great circle distance d between two points with coordinates {lat1,lon1} and {lat2,lon2} is given by:
1230
1231 d=acos(sin(lat1)*sin(lat2)+cos(lat1)*cos(lat2)*cos(lon1-lon2))
1232
1233 A mathematically equivalent formula, which is less subject to rounding error for short distances is:
1234
1235 d=2*asin(sqrt((sin((lat1-lat2)/2))^2 +
1236                  cos(lat1)*cos(lat2)*(sin((lon1-lon2)/2))^2))
1237
1238 */
1239
1240 // Returns distance in nm, takes lat & lon in RADIANS
1241 double DCLGPS::GetGreatCircleDistance(double lat1, double lon1, double lat2, double lon2) const {
1242         double d = 2.0 * asin(sqrt(((sin((lat1-lat2)/2.0))*(sin((lat1-lat2)/2.0))) +
1243                    cos(lat1)*cos(lat2)*(sin((lon1-lon2)/2.0))*(sin((lon1-lon2)/2.0))));
1244         return(Rad2Nm(d));
1245 }
1246
1247 // fmod dosen't do what we want :-(
1248 static double mod(double d1, double d2) {
1249         return(d1 - d2*floor(d1/d2));
1250 }
1251
1252 // Returns great circle course from point 1 to point 2
1253 // Input and output in RADIANS.
1254 double DCLGPS::GetGreatCircleCourse (double lat1, double lon1, double lat2, double lon2) const {
1255         //double h = 0.0;
1256         /*
1257         // Special case the poles
1258         if(cos(lat1) < SG_EPSILON) {
1259                 if(lat1 > 0) {
1260                         // Starting from North Pole
1261                         h = SG_PI;
1262                 } else {
1263                         // Starting from South Pole
1264                         h = 2.0 * SG_PI;
1265                 }
1266         } else {
1267                 // Urgh - the formula below is for negative lon +ve !!!???
1268                 double d = GetGreatCircleDistance(lat1, lon1, lat2, lon2);
1269                 cout << "d = " << d;
1270                 d = Nm2Rad(d);
1271                 //cout << ", d_theta = " << d;
1272                 //cout << ", and d = " << Rad2Nm(d) << ' ';
1273                 if(sin(lon2 - lon1) < 0) {
1274                         cout << " A ";
1275                         h = acos((sin(lat2)-sin(lat1)*cos(d))/(sin(d)*cos(lat1)));
1276                 } else {
1277                         cout << " B ";
1278                         h = 2.0 * SG_PI - acos((sin(lat2)-sin(lat1)*cos(d))/(sin(d)*cos(lat1)));
1279                 }
1280         }
1281         cout << h * SG_RADIANS_TO_DEGREES << '\n';
1282         */
1283
1284         return( mod(atan2(sin(lon2-lon1)*cos(lat2),
1285             cos(lat1)*sin(lat2)-sin(lat1)*cos(lat2)*cos(lon2-lon1)),
1286             2.0*SG_PI) );
1287 }
1288
1289 // Return a position on a radial from wp1 given distance d (nm) and magnetic heading h (degrees)
1290 // Note that d should be less that 1/4 Earth diameter!
1291 GPSWaypoint DCLGPS::GetPositionOnMagRadial(const GPSWaypoint& wp1, double d, double h) {
1292         h += sgGetMagVar(wp1.lon, wp1.lat, 0.0, _time->getJD()) * SG_RADIANS_TO_DEGREES;
1293         return(GetPositionOnRadial(wp1, d, h));
1294 }
1295
1296 // Return a position on a radial from wp1 given distance d (nm) and TRUE heading h (degrees)
1297 // Note that d should be less that 1/4 Earth diameter!
1298 GPSWaypoint DCLGPS::GetPositionOnRadial(const GPSWaypoint& wp1, double d, double h) {
1299         while(h < 0.0) h += 360.0;
1300         while(h > 360.0) h -= 360.0;
1301
1302         h *= SG_DEGREES_TO_RADIANS;
1303         d *= (SG_PI / (180.0 * 60.0));
1304
1305         double lat=asin(sin(wp1.lat)*cos(d)+cos(wp1.lat)*sin(d)*cos(h));
1306         double lon;
1307         if(cos(lat)==0) {
1308                 lon=wp1.lon;      // endpoint a pole
1309         } else {
1310                 lon=mod(wp1.lon+asin(sin(h)*sin(d)/cos(lat))+SG_PI,2*SG_PI)-SG_PI;
1311         }
1312
1313         GPSWaypoint wp;
1314         wp.lat = lat;
1315         wp.lon = lon;
1316         wp.type = GPS_WP_VIRT;
1317         return(wp);
1318 }
1319
1320 // Returns cross-track deviation in Nm.
1321 double DCLGPS::CalcCrossTrackDeviation() const {
1322         return(CalcCrossTrackDeviation(_fromWaypoint, _activeWaypoint));
1323 }
1324
1325 // Returns cross-track deviation of the current position between two arbitary waypoints in nm.
1326 double DCLGPS::CalcCrossTrackDeviation(const GPSWaypoint& wp1, const GPSWaypoint& wp2) const {
1327         //if(wp1 == NULL || wp2 == NULL) return(0.0);
1328         if(wp1.id.empty() || wp2.id.empty()) return(0.0);
1329         double xtd = asin(sin(Nm2Rad(GetGreatCircleDistance(wp1.lat, wp1.lon, _gpsLat, _gpsLon)))
1330                           * sin(GetGreatCircleCourse(wp1.lat, wp1.lon, _gpsLat, _gpsLon) - GetGreatCircleCourse(wp1.lat, wp1.lon, wp2.lat, wp2.lon)));
1331         return(Rad2Nm(xtd));
1332 }