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