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/misc/sg_path.hxx>
  30 #include <simgear/timing/sg_time.hxx>
  31 #include <simgear/magvar/magvar.hxx>
  32 #include <simgear/structure/exception.hxx>
  33
  34 #include <Main/fg_props.hxx>
  35 #include <Navaids/fix.hxx>
  36 #include <Navaids/navrecord.hxx>
  37 #include <Airports/simple.hxx>
  38 #include <Airports/runways.hxx>
  39
  40 #include <fstream>
  41 #include <iostream>
  42
  43 using namespace std;
  44
  45 GPSWaypoint::GPSWaypoint() {
  46     appType = GPS_APP_NONE;
  47 }
  48
  49 GPSWaypoint::GPSWaypoint(const std::string& aIdent, float aLat, float aLon, GPSWpType aType) :
  50   id(aIdent),
  51   lat(aLat),
  52   lon(aLon),
  53   type(aType),
  54   appType(GPS_APP_NONE)
  55 {
  56 }
  57
  58 GPSWaypoint::~GPSWaypoint() {}
  59
  60 string GPSWaypoint::GetAprId() {
  61         if(appType == GPS_IAF) return(id + 'i');
  62         else if(appType == GPS_FAF) return(id + 'f');
  63         else if(appType == GPS_MAP) return(id + 'm');
  64         else if(appType == GPS_MAHP) return(id + 'h');
  65         else return(id);
  66 }
  67
  68 static GPSWpType
  69 GPSWpTypeFromFGPosType(FGPositioned::Type aType)
  70 {
  71   switch (aType) {
  72   case FGPositioned::AIRPORT:
  73   case FGPositioned::SEAPORT:
  74   case FGPositioned::HELIPORT:
  75     return GPS_WP_APT;
  76
  77   case FGPositioned::VOR:
  78     return GPS_WP_VOR;
  79
  80   case FGPositioned::NDB:
  81     return GPS_WP_NDB;
  82
  83   case FGPositioned::WAYPOINT:
  84     return GPS_WP_USR;
  85
  86   case FGPositioned::FIX:
  87     return GPS_WP_INT;
  88
  89   default:
  90     return GPS_WP_USR;
  91   }
  92 }
  93
  94 GPSWaypoint* GPSWaypoint::createFromPositioned(const FGPositioned* aPos)
  95 {
  96   if (!aPos) {
  97     return NULL; // happens if find returns no match
  98   }
  99
 100   return new GPSWaypoint(aPos->ident(),
 101     aPos->latitude() * SG_DEGREES_TO_RADIANS,
 102     aPos->longitude() * SG_DEGREES_TO_RADIANS,
 103     GPSWpTypeFromFGPosType(aPos->type())
 104   );
 105 }
 106
 107 ostream& operator << (ostream& os, GPSAppWpType type) {
 108         switch(type) {
 109                 case(GPS_IAF):       return(os << "IAF");
 110                 case(GPS_IAP):       return(os << "IAP");
 111                 case(GPS_FAF):       return(os << "FAF");
 112                 case(GPS_MAP):       return(os << "MAP");
 113                 case(GPS_MAHP):      return(os << "MAHP");
 114                 case(GPS_HDR):       return(os << "HEADER");
 115                 case(GPS_FENCE):     return(os << "FENCE");
 116                 case(GPS_APP_NONE):  return(os << "NONE");
 117         }
 118         return(os << "ERROR - Unknown switch in GPSAppWpType operator << ");
 119 }
 120
 121 FGIAP::FGIAP() {
 122 }
 123
 124 FGIAP::~FGIAP() {
 125 }
 126
 127 FGNPIAP::FGNPIAP() {
 128 }
 129
 130 FGNPIAP::~FGNPIAP() {
 131 }
 132
 133 ClockTime::ClockTime() {
 134     _hr = 0;
 135     _min = 0;
 136 }
 137
 138 ClockTime::ClockTime(int hr, int min) {
 139     while(hr < 0) { hr += 24; }
 140     _hr = hr % 24;
 141     while(min < 0) { min += 60; }
 142     while(min > 60) { min -= 60; }
 143         _min = min;
 144 }
 145
 146 ClockTime::~ClockTime() {
 147 }
 148
 149 // ------------------------------------------------------------------------------------- //
 150
 151 DCLGPS::DCLGPS(RenderArea2D* instrument) {
 152         _instrument = instrument;
 153         _nFields = 1;
 154         _maxFields = 2;
 155
 156         // Units - lets default to US units - FG can set them to other units from config during startup if desired.
 157         _altUnits = GPS_ALT_UNITS_FT;
 158         _baroUnits = GPS_PRES_UNITS_IN;
 159         _velUnits = GPS_VEL_UNITS_KT;
 160         _distUnits = GPS_DIST_UNITS_NM;
 161
 162         _lon_node = fgGetNode("/instrumentation/gps/indicated-longitude-deg", true);
 163         _lat_node = fgGetNode("/instrumentation/gps/indicated-latitude-deg", true);
 164         _alt_node = fgGetNode("/instrumentation/gps/indicated-altitude-ft", true);
 165         _grnd_speed_node = fgGetNode("/instrumentation/gps/indicated-ground-speed-kt", true);
 166         _true_track_node = fgGetNode("/instrumentation/gps/indicated-track-true-deg", true);
 167         _mag_track_node = fgGetNode("/instrumentation/gps/indicated-track-magnetic-deg", true);
 168
 169         // Use FG's position values at construction in case FG's gps has not run first update yet.
 170         _lon = fgGetDouble("/position/longitude-deg") * SG_DEGREES_TO_RADIANS;
 171         _lat = fgGetDouble("/position/latitude-deg") * SG_DEGREES_TO_RADIANS;
 172         _alt = fgGetDouble("/position/altitude-ft");
 173         // Note - we can depriciate _gpsLat and _gpsLon if we implement error handling in FG
 174         // gps code and not our own.
 175         _gpsLon = _lon;
 176         _gpsLat = _lat;
 177         _checkLon = _gpsLon;
 178         _checkLat = _gpsLat;
 179         _groundSpeed_ms = 0.0;
 180         _groundSpeed_kts = 0.0;
 181         _track = 0.0;
 182         _magTrackDeg = 0.0;
 183
 184         // Sensible defaults.  These can be overriden by derived classes if desired.
 185         _cdiScales.clear();
 186         _cdiScales.push_back(5.0);
 187         _cdiScales.push_back(1.0);
 188         _cdiScales.push_back(0.3);
 189         _currentCdiScaleIndex = 0;
 190         _targetCdiScaleIndex = 0;
 191         _sourceCdiScaleIndex = 0;
 192         _cdiScaleTransition = false;
 193         _currentCdiScale = 5.0;
 194
 195         _cleanUpPage = -1;
 196
 197         _activeWaypoint.id.clear();
 198         _dist2Act = 0.0;
 199         _crosstrackDist = 0.0;
 200         _headingBugTo = true;
 201         _navFlagged = true;
 202         _waypointAlert = false;
 203         _departed = false;
 204         _departureTimeString = "----";
 205         _elapsedTime = 0.0;
 206         _powerOnTime.set_hr(0);
 207         _powerOnTime.set_min(0);
 208         _powerOnTimerSet = false;
 209         _alarmSet = false;
 210
 211         // Configuration Initialisation
 212         // Should this be in kln89.cxx ?
 213         _turnAnticipationEnabled = false;
 214
 215         _time = new SGTime;
 216
 217         _messageStack.clear();
 218
 219         _dto = false;
 220
 221         _approachLoaded = false;
 222         _approachArm = false;
 223         _approachReallyArmed = false;
 224         _approachActive = false;
 225         _approachFP = new GPSFlightPlan;
 226 }
 227
 228 DCLGPS::~DCLGPS() {
 229         delete _time;
 230   delete _approachFP;           // Don't need to delete the waypoints inside since they point to
 231                                                         // the waypoints in the approach database.
 232         // TODO - may need to delete the approach database!!
 233 }
 234
 235 void DCLGPS::draw(osg::State& state) {
 236         _instrument->Draw(state);
 237 }
 238
 239 void DCLGPS::init() {
 240
 241         // Not sure if this should be here, but OK for now.
 242         CreateDefaultFlightPlans();
 243
 244         LoadApproachData();
 245 }
 246
 247 void DCLGPS::bind() {
 248         fgTie("/instrumentation/gps/waypoint-alert", this, &DCLGPS::GetWaypointAlert);
 249         fgTie("/instrumentation/gps/leg-mode", this, &DCLGPS::GetLegMode);
 250         fgTie("/instrumentation/gps/obs-mode", this, &DCLGPS::GetOBSMode);
 251         fgTie("/instrumentation/gps/approach-arm", this, &DCLGPS::GetApproachArm);
 252         fgTie("/instrumentation/gps/approach-active", this, &DCLGPS::GetApproachActive);
 253         fgTie("/instrumentation/gps/cdi-deflection", this, &DCLGPS::GetCDIDeflection);
 254         fgTie("/instrumentation/gps/to-flag", this, &DCLGPS::GetToFlag);
 255 }
 256
 257 void DCLGPS::unbind() {
 258         fgUntie("/instrumentation/gps/waypoint-alert");
 259         fgUntie("/instrumentation/gps/leg-mode");
 260         fgUntie("/instrumentation/gps/obs-mode");
 261         fgUntie("/instrumentation/gps/approach-arm");
 262         fgUntie("/instrumentation/gps/approach-active");
 263         fgUntie("/instrumentation/gps/cdi-deflection");
 264 }
 265
 266 void DCLGPS::update(double dt) {
 267         //cout << "update called!\n";
 268
 269         _lon = _lon_node->getDoubleValue() * SG_DEGREES_TO_RADIANS;
 270         _lat = _lat_node->getDoubleValue() * SG_DEGREES_TO_RADIANS;
 271         _alt = _alt_node->getDoubleValue();
 272         _groundSpeed_kts = _grnd_speed_node->getDoubleValue();
 273         _groundSpeed_ms = _groundSpeed_kts * 0.5144444444;
 274         _track = _true_track_node->getDoubleValue();
 275         _magTrackDeg = _mag_track_node->getDoubleValue();
 276         // Note - we can depriciate _gpsLat and _gpsLon if we implement error handling in FG
 277         // gps code and not our own.
 278         _gpsLon = _lon;
 279         _gpsLat = _lat;
 280         // Check for abnormal position slew
 281         if(GetGreatCircleDistance(_gpsLat, _gpsLon, _checkLat, _checkLon) > 1.0) {
 282                 OrientateToActiveFlightPlan();
 283         }
 284         _checkLon = _gpsLon;
 285         _checkLat = _gpsLat;
 286
 287         // TODO - check for unit power before running this.
 288         if(!_powerOnTimerSet) {
 289                 SetPowerOnTimer();
 290         }
 291
 292         // Check if an alarm timer has expired
 293         if(_alarmSet) {
 294                 if(_alarmTime.hr() == atoi(fgGetString("/instrumentation/clock/indicated-hour"))
 295                 && _alarmTime.min() == atoi(fgGetString("/instrumentation/clock/indicated-min"))) {
 296                         _messageStack.push_back("*Timer Expired");
 297                         _alarmSet = false;
 298                 }
 299         }
 300
 301         if(!_departed) {
 302                 if(_groundSpeed_kts > 30.0) {
 303                         _departed = true;
 304                         string th = fgGetString("/instrumentation/clock/indicated-hour");
 305                         string tm = fgGetString("/instrumentation/clock/indicated-min");
 306                         if(th.size() == 1) th = "0" + th;
 307                         if(tm.size() == 1) tm = "0" + tm;
 308                         _departureTimeString = th + tm;
 309                 }
 310         } else {
 311                 // TODO - check - is this prone to drift error over time?
 312                 // Should we difference the departure and current times?
 313                 // What about when the user resets the time of day from the menu?
 314                 _elapsedTime += dt;
 315         }
 316
 317         _time->update(_gpsLon * SG_DEGREES_TO_RADIANS, _gpsLat * SG_DEGREES_TO_RADIANS, 0, 0);
 318         // FIXME - currently all the below assumes leg mode and no DTO or OBS cancelled.
 319         if(_activeFP->IsEmpty()) {
 320                 // Not sure if we need to reset these each update or only when fp altered
 321                 _activeWaypoint.id.clear();
 322                 _navFlagged = true;
 323         } else if(_activeFP->waypoints.size() == 1) {
 324                 _activeWaypoint.id.clear();
 325         } else {
 326                 _navFlagged = false;
 327                 if(_activeWaypoint.id.empty() || _fromWaypoint.id.empty()) {
 328                         //cout << "Error, in leg mode with flightplan of 2 or more waypoints, but either active or from wp is NULL!\n";
 329                         OrientateToActiveFlightPlan();
 330                 }
 331
 332                 // Approach stuff
 333                 if(_approachLoaded) {
 334                         if(!_approachReallyArmed && !_approachActive) {
 335                                 // arm if within 30nm of airport.
 336                                 // TODO - let user cancel approach arm using external GPS-APR switch
 337                                 bool multi;
 338                                 const FGAirport* ap = FindFirstAptById(_approachID, multi, true);
 339                                 if(ap != NULL) {
 340                                         double d = GetGreatCircleDistance(_gpsLat, _gpsLon, ap->getLatitude() * SG_DEGREES_TO_RADIANS, ap->getLongitude() * SG_DEGREES_TO_RADIANS);
 341                                         if(d <= 30) {
 342                                                 _approachArm = true;
 343                                                 _approachReallyArmed = true;
 344                                                 _messageStack.push_back("*Press ALT To Set Baro");
 345                                                 // Not sure what we do if the user has already set CDI to 0.3 nm?
 346                                                 _targetCdiScaleIndex = 1;
 347                                                 if(_currentCdiScaleIndex == 1) {
 348                                                         // No problem!
 349                                                 } else if(_currentCdiScaleIndex == 0) {
 350                                                         _sourceCdiScaleIndex = 0;
 351                                                         _cdiScaleTransition = true;
 352                                                         _cdiTransitionTime = 30.0;
 353                                                         _currentCdiScale = _cdiScales[_currentCdiScaleIndex];
 354                                                 }
 355                                         }
 356                                 }
 357                         } else {
 358                                 // Check for approach active - we can only activate approach if it is really armed.
 359                                 if(_activeWaypoint.appType == GPS_FAF) {
 360                                         //cout << "Active waypoint is FAF, id is " << _activeWaypoint.id << '\n';
 361                                         if(GetGreatCircleDistance(_gpsLat, _gpsLon, _activeWaypoint.lat, _activeWaypoint.lon) <= 2.0 && !_obsMode) {
 362                                                 // Assume heading is OK for now
 363                                                 _approachArm = false;   // TODO - check - maybe arm is left on when actv comes on?
 364                                                 _approachReallyArmed = false;
 365                                                 _approachActive = true;
 366                                                 _targetCdiScaleIndex = 2;
 367                                                 if(_currentCdiScaleIndex == 2) {
 368                                                         // No problem!
 369                                                 } else if(_currentCdiScaleIndex == 1) {
 370                                                         _sourceCdiScaleIndex = 1;
 371                                                         _cdiScaleTransition = true;
 372                                                         _cdiTransitionTime = 30.0;      // TODO - compress it if time to FAF < 30sec
 373                                                         _currentCdiScale = _cdiScales[_currentCdiScaleIndex];
 374                                                 } else {
 375                                                         // Abort going active?
 376                                                         _approachActive = false;
 377                                                 }
 378                                         }
 379                                 }
 380                         }
 381                 }
 382
 383                 // CDI scale transition stuff
 384                 if(_cdiScaleTransition) {
 385                         if(fabs(_currentCdiScale - _cdiScales[_targetCdiScaleIndex]) < 0.001) {
 386                                 _currentCdiScale = _cdiScales[_targetCdiScaleIndex];
 387                                 _currentCdiScaleIndex = _targetCdiScaleIndex;
 388                                 _cdiScaleTransition = false;
 389                         } else {
 390                                 double scaleDiff = (_targetCdiScaleIndex > _sourceCdiScaleIndex
 391                                                     ? _cdiScales[_sourceCdiScaleIndex] - _cdiScales[_targetCdiScaleIndex]
 392                                                                         : _cdiScales[_targetCdiScaleIndex] - _cdiScales[_sourceCdiScaleIndex]);
 393                                 //cout << "ScaleDiff = " << scaleDiff << '\n';
 394                                 if(_targetCdiScaleIndex > _sourceCdiScaleIndex) {
 395                                         // Scaling down eg. 5nm -> 1nm
 396                                         _currentCdiScale -= (scaleDiff * dt / _cdiTransitionTime);
 397                                         if(_currentCdiScale < _cdiScales[_targetCdiScaleIndex]) {
 398                                                 _currentCdiScale = _cdiScales[_targetCdiScaleIndex];
 399                                                 _currentCdiScaleIndex = _targetCdiScaleIndex;
 400                                                 _cdiScaleTransition = false;
 401                                         }
 402                                 } else {
 403                                         _currentCdiScale += (scaleDiff * dt / _cdiTransitionTime);
 404                                         if(_currentCdiScale > _cdiScales[_targetCdiScaleIndex]) {
 405                                                 _currentCdiScale = _cdiScales[_targetCdiScaleIndex];
 406                                                 _currentCdiScaleIndex = _targetCdiScaleIndex;
 407                                                 _cdiScaleTransition = false;
 408                                         }
 409                                 }
 410                                 //cout << "_currentCdiScale = " << _currentCdiScale << '\n';
 411                         }
 412                 } else {
 413                         _currentCdiScale = _cdiScales[_currentCdiScaleIndex];
 414                 }
 415
 416
 417                 // Urgh - I've been setting the heading bug based on DTK,
 418                 // bug I think it should be based on heading re. active waypoint
 419                 // based on what the sim does after the final waypoint is passed.
 420                 // (DTK remains the same, but if track is held == DTK heading bug
 421                 // reverses to from once wp is passed).
 422                 /*
 423                 if(_fromWaypoint != NULL) {
 424                         // TODO - how do we handle the change of track with distance over long legs?
 425                         _dtkTrue = GetGreatCircleCourse(_fromWaypoint->lat, _fromWaypoint->lon, _activeWaypoint->lat, _activeWaypoint->lon) * SG_RADIANS_TO_DEGREES;
 426                         _dtkMag = GetMagHeadingFromTo(_fromWaypoint->lat, _fromWaypoint->lon, _activeWaypoint->lat, _activeWaypoint->lon);
 427                         // Don't change the heading bug if speed is too low otherwise it flickers to/from at rest
 428                         if(_groundSpeed_ms > 5) {
 429                                 //cout << "track = " << _track << ", dtk = " << _dtkTrue << '\n';
 430                                 double courseDev = _track - _dtkTrue;
 431                                 //cout << "courseDev = " << courseDev << ", normalized = ";
 432                                 SG_NORMALIZE_RANGE(courseDev, -180.0, 180.0);
 433                                 //cout << courseDev << '\n';
 434                                 _headingBugTo = (fabs(courseDev) > 90.0 ? false : true);
 435                         }
 436                 } else {
 437                         _dtkTrue = 0.0;
 438                         _dtkMag = 0.0;
 439                         // TODO - in DTO operation the position of initiation of DTO defines the "from waypoint".
 440                 }
 441                 */
 442                 if(!_activeWaypoint.id.empty()) {
 443                         double hdgTrue = GetGreatCircleCourse(_gpsLat, _gpsLon, _activeWaypoint.lat, _activeWaypoint.lon) * SG_RADIANS_TO_DEGREES;
 444                         if(_groundSpeed_ms > 5) {
 445                                 //cout << "track = " << _track << ", hdgTrue = " << hdgTrue << '\n';
 446                                 double courseDev = _track - hdgTrue;
 447                                 //cout << "courseDev = " << courseDev << ", normalized = ";
 448                                 SG_NORMALIZE_RANGE(courseDev, -180.0, 180.0);
 449                                 //cout << courseDev << '\n';
 450                                 _headingBugTo = (fabs(courseDev) > 90.0 ? false : true);
 451                         }
 452                         if(!_fromWaypoint.id.empty()) {
 453                                 _dtkTrue = GetGreatCircleCourse(_fromWaypoint.lat, _fromWaypoint.lon, _activeWaypoint.lat, _activeWaypoint.lon) * SG_RADIANS_TO_DEGREES;
 454                                 _dtkMag = GetMagHeadingFromTo(_fromWaypoint.lat, _fromWaypoint.lon, _activeWaypoint.lat, _activeWaypoint.lon);
 455                         } else {
 456                                 _dtkTrue = 0.0;
 457                                 _dtkMag = 0.0;
 458                         }
 459                 }
 460
 461                 _dist2Act = GetGreatCircleDistance(_gpsLat, _gpsLon, _activeWaypoint.lat, _activeWaypoint.lon) * SG_NM_TO_METER;
 462                 if(_groundSpeed_ms > 10.0) {
 463                         _eta = _dist2Act / _groundSpeed_ms;
 464                         if(_eta <= 36) {        // TODO - this is slightly different if turn anticipation is enabled.
 465                                 if(_headingBugTo) {
 466                                         _waypointAlert = true;  // TODO - not if the from flag is set.
 467                                 }
 468                         }
 469                         if(_eta < 60) {
 470                                 // Check if we should sequence to next leg.
 471                                 // Perhaps this should be done on distance instead, but 60s time (about 1 - 2 nm) seems reasonable for now.
 472                                 //double reverseHeading = GetGreatCircleCourse(_activeWaypoint->lat, _activeWaypoint->lon, _fromWaypoint->lat, _fromWaypoint->lon);
 473                                 // Hack - let's cheat and do it on heading bug for now.  TODO - that stops us 'cutting the corner'
 474                                 // when we happen to approach the inside turn of a waypoint - we should probably sequence at the midpoint
 475                                 // of the heading difference between legs in this instance.
 476                                 int idx = GetActiveWaypointIndex();
 477                                 bool finalLeg = (idx == (int)(_activeFP->waypoints.size()) - 1 ? true : false);
 478                                 bool finalDto = (_dto && idx == -1);    // Dto operation to a waypoint not in the flightplan - we don't sequence in this instance
 479                                 if(!_headingBugTo) {
 480                                         if(finalLeg) {
 481                                                 // Do nothing - not sure if Dto should switch off when arriving at the final waypoint of a flightplan
 482                                         } else if(finalDto) {
 483                                                 // Do nothing
 484                                         } else if(_activeWaypoint.appType == GPS_MAP) {
 485                                                 // Don't sequence beyond the missed approach point
 486                                                 //cout << "ACTIVE WAYPOINT is MAP - not sequencing!!!!!\n";
 487                                         } else {
 488                                                 //cout << "Sequencing...\n";
 489                                                 _fromWaypoint = _activeWaypoint;
 490                                                 _activeWaypoint = *_activeFP->waypoints[idx + 1];
 491                                                 _dto = false;
 492                                                 // Course alteration message format is dependent on whether we are slaved HSI/CDI indicator or not.
 493                                                 string s;
 494                                                 if(fgGetBool("/instrumentation/nav[0]/slaved-to-gps")) {
 495                                                         // TODO - avoid the hardwiring on nav[0]
 496                                                         s = "Adj Nav Crs to ";
 497                                                 } else {
 498                                                         string s = "GPS Course is ";
 499                                                 }
 500                                                 double d = GetMagHeadingFromTo(_fromWaypoint.lat, _fromWaypoint.lon, _activeWaypoint.lat, _activeWaypoint.lon);
 501                                                 while(d < 0.0) d += 360.0;
 502                                                 while(d >= 360.0) d -= 360.0;
 503                                                 char buf[4];
 504                                                 snprintf(buf, 4, "%03i", (int)(d + 0.5));
 505                                                 s += buf;
 506                                                 _messageStack.push_back(s);
 507                                         }
 508                                         _waypointAlert = false;
 509                                 }
 510                         }
 511                 } else {
 512                         _eta = 0.0;
 513                 }
 514
 515                 /*
 516                 // First attempt at a sensible cross-track correction calculation
 517                 // Uh? - I think this is implemented further down the file!
 518                 if(_fromWaypoint != NULL) {
 519
 520                 } else {
 521                         _crosstrackDist = 0.0;
 522                 }
 523                 */
 524         }
 525 }
 526
 527 /*
 528         Expand a SIAP ident to the full procedure name (as shown on the approach chart).
 529         NOTE: Some of this is inferred from data, some is from documentation.
 530
 531         Example expansions from ARINC 424-18 [and the airport they're taken from]:
 532         "R10LY" <--> "RNAV (GPS) Y RWY 10 L"    [KBOI]
 533         "R10-Z" <--> "RNAV (GPS) Z RWY 10"              [KHTO]
 534         "S25"   <--> "VOR or GPS RWY 25"                [KHHR]
 535         "P20"   <--> "GPS RWY 20"                               [KDAN]
 536         "NDB-B" <--> "NDB or GPS-B"                             [KDAW]
 537         "NDBC"  <--> "NDB or GPS-C"                             [KEMT]
 538         "VDMA"  <--> "VOR/DME or GPS-A"                 [KDAW]
 539         "VDM-A" <--> "VOR/DME or GPS-A"                 [KEAG]
 540         "VDMB"  <--> "VOR/DME or GPS-B"                 [KDKX]
 541         "VORA"  <--> "VOR or GPS-A"                             [KEMT]
 542
 543         It seems that there are 2 basic types of expansions; those that include
 544         the runway and those that don't.  Of those that don't, it seems that 2
 545         different positions within the string to encode the identifying letter
 546         are used, i.e. with a dash and without.
 547 */
 548 string DCLGPS::ExpandSIAPIdent(const string& ident) {
 549         string name;
 550         bool has_rwy = false;
 551
 552         switch(ident[0]) {
 553         case 'N': name = "NDB or GPS"; has_rwy = false; break;
 554         case 'P': name = "GPS"; has_rwy = true; break;
 555         case 'R': name = "RNAV (GPS)"; has_rwy = true; break;
 556         case 'S': name = "VOR or GPS"; has_rwy = true; break;
 557         case 'V':
 558                 if(ident[1] == 'D') name = "VOR/DME or GPS";
 559                 else name = "VOR or GPS";
 560                 has_rwy = false;
 561                 break;
 562         default: // TODO output a log message
 563                 break;
 564         }
 565
 566         if(has_rwy) {
 567                 // Add the identifying letter if present
 568                 if(ident.size() == 5) {
 569                         name += ' ';
 570                         name += ident[4];
 571                 }
 572
 573                 // Add the runway
 574                 name += " RWY ";
 575                 name += ident.substr(1, 2);
 576
 577                 // Add a left/right/centre indication if present.
 578                 if(ident.size() > 3) {
 579                         if((ident[3] != '-') && (ident[3] != ' ')) {    // Early versions of the spec allowed a blank instead of a dash so check for both
 580                                 name += ' ';
 581                                 name += ident[3];
 582                         }
 583                 }
 584         } else {
 585                 // Add the identifying letter, which I *think* should always be present, but seems to be inconsistent as to whether a dash is used.
 586                 if(ident.size() == 5) {
 587                         name += '-';
 588                         name += ident[4];
 589                 } else if(ident.size() == 4) {
 590                         name += '-';
 591                         name += ident[3];
 592                 } else {
 593                         // No suffix letter
 594                 }
 595         }
 596
 597         return(name);
 598 }
 599
 600 /*
 601         Load instrument approaches from an ARINC 424 file.
 602         Tested on ARINC 424-18.
 603         Known / current best guess at the format:
 604         Col 1:          Always 'S'.  If it isn't, ditch it.
 605         Col 2-4:        "Customer area" code, eg "USA", "CAN".  I think that CAN is used for Alaska.
 606         Col 5:          Section code.  Used in conjunction with sub-section code.  Definitions are with sub-section code.
 607         Col 6:          Always blank.
 608         Col 7-10:       ICAO (or FAA) airport ident. Left justified if < 4 chars.
 609         Col 11-12:      Based on ICAO geographical region.
 610         Col 13:         Sub-section code.  Used in conjunction with section code.
 611                                 "HD/E/F" => Helicopter record.
 612                                 "HS" => Helicopter minimum safe altitude.
 613                                 "PA" => Airport record.
 614                                 "PF" => Approach segment.
 615                                 "PG" => Runway record.
 616                                 "PP" => Path point record.      ???
 617                                 "PS" => MSA record (minimum safe altitude).
 618
 619         ------ The following is for "PF", approach segment -------
 620
 621         Col 14-19:      SIAP ident for this approach (left justified).  This is a standardised abbreviated approach name.
 622                                 e.g. "R10LZ" expands to "RNAV (GPS) Z RWY 10 L".  See the comment block for ExpandSIAPIdent for full details.
 623         Col 20:         Route type.  This is tricky - I don't have full documentation and am having to guess a bit.
 624                                 'A'     => Arrival route?       This seems to be used to encode arrival routes from the IAF to the approach proper.
 625                                                                                 Note that the final fix of the arrival route is duplicated in the approach proper.
 626                                 'D'     => VOR/DME or GPS
 627                                 'N' => NDB or GPS
 628                                 'P'     => GPS (ARINC 424-18), GPS and RNAV (GPS) (ARINC 424-15 and before).
 629                                 'R' => RNAV (GPS) (ARINC 424-18).
 630                                 'S'     => VOR or GPS
 631         Col 21-25:      Transition identifier.  AFAICT, this is the ident of the IAF for this initial approach route, and left blank for the final approach course.  See col 30-34 for the actual fix ident.
 632         Col 26:         BLANK
 633         Col 27-29:      Sequence number - position within the route segment.  Rule: 10-20-30 etc.
 634         Col 30-34:      Fix identifer.  The ident of the waypoint.
 635         Col 35-36:      ICAO geographical region code.  I think we can ignore this for now.
 636         Col 37:         Section code - ??? I don't know what this means
 637         Col 38          Subsection code - ??? ditto - no idea!
 638         Col 40:         Waypoint type.
 639                                 'A' => Airport as waypoint
 640                                 'E' => Essential waypoint (e.g. change of heading at this waypoint, etc).
 641                                 'G' => Runway or helipad as waypoint
 642                                 'H' => Heliport as waypoint
 643                                 'N' => NDB as waypoint
 644                                 'P' => Phantom waypoint (not sure if this is used in rev 18?)
 645                                 'V' => VOR as waypoint
 646         Col 41:         Waypoint type.
 647                                 'B' => Flyover, approach transition, or final approach.
 648                                 'E' => end of route segment (transition waypoint). (Actually "End of terminal procedure route type" in the docs).
 649                                 'N' => ??? I've also seen 'N' in this column, but don't know what it indicates.
 650                                 'Y' => Flyover.
 651         Col 43:         Waypoint type.  May also be blank when none of the below.
 652                                 'A' => Initial approach fix (IAF)
 653                                 'F' => Final approach fix
 654                                 'H' => Holding fix
 655                                 'I' => Final approach course fix
 656                                 'M' => Missed approach point
 657                                 'P' => ??? This is present, but I don't know what this means and it wasn't in the FAA docs that I found the above in!
 658                                            ??? Possibly procedure turn?
 659                                 'C' => ??? This is also present in the data, but missing from the docs.  Is at airport 00R.
 660         Col 107-111     MSA center fix.  We can ignore this.
 661 */
 662 void DCLGPS::LoadApproachData() {
 663         FGNPIAP* iap;
 664         GPSWaypoint* wp;
 665         GPSFlightPlan* fp;
 666         const GPSWaypoint* cwp;
 667
 668         std::ifstream fin;
 669         SGPath path = globals->get_fg_root();
 670         path.append("Navaids/rnav.dat");
 671         fin.open(path.c_str(), ios::in);
 672         if(!fin) {
 673                 //cout << "Unable to open input file " << path.c_str() << '\n';
 674                 return;
 675         } else {
 676                 //cout << "Opened " << path.c_str() << " for reading\n";
 677         }
 678         char tmp[256];
 679         string s;
 680
 681         string apt_ident;    // This gets set to the ICAO code of the current airport being processed.
 682         string iap_ident;    // The abbreviated name of the current approach being processed.
 683         string wp_ident;     // The ident of the waypoint of the current line
 684         string last_apt_ident;
 685         string last_iap_ident;
 686         string last_wp_ident;
 687         // There is no need to save the full name - it can be generated on the fly from the abbreviated name as and when needed.
 688         bool apt_in_progress = false;    // Set true whilst loading all the approaches for a given airport.
 689         bool iap_in_progress = false;    // Set true whilst loading a given approach.
 690         bool iap_error = false;                  // Set true if there is an error loading a given approach.
 691         bool route_in_progress = false;  // Set true when we are loading a "route" segment of the approach.
 692         int last_sequence_number = 0;    // Position within the route, rule (rev 18): 10, 20, 30 etc.
 693         int sequence_number;
 694         char last_route_type = 0;
 695         char route_type;
 696         char waypoint_fix_type;  // This is the waypoint type from col 43, i.e. the type of fix.  May be blank.
 697
 698         int j;
 699
 700         // Debugging info
 701         unsigned int nLoaded = 0;
 702         unsigned int nErrors = 0;
 703
 704         //for(i=0; i<64; ++i) {
 705         while(!fin.eof()) {
 706                 fin.getline(tmp, 256);
 707                 //s = Fake_rnav_dat[i];
 708                 s = tmp;
 709                 if(s.size() < 132) continue;
 710                 if(s[0] == 'S') {    // Valid line
 711                         string country_code = s.substr(1, 3);
 712                         if(country_code == "USA") {    // For now we'll stick to US procedures in case there are unknown gotchas with others
 713                                 if(s[4] == 'P') {    // Includes approaches.
 714                                         if(s[12] == 'A') {      // Airport record
 715                                                 apt_ident = s.substr(6, 4);
 716                                                 // Trim any whitespace from the ident.  The ident is left justified,
 717                                                 // so any space will be at the end.
 718                                                 if(apt_ident[3] == ' ') apt_ident = apt_ident.substr(0, 3);
 719                                                 // I think that all idents are either 3 or 4 chars - could check this though!
 720                                                 if(!apt_in_progress) {
 721                                                         last_apt_ident = apt_ident;
 722                                                         apt_in_progress = 1;
 723                                                 } else {
 724                                                         if(last_apt_ident != apt_ident) {
 725                                                                 if(iap_in_progress) {
 726                                                                         if(iap_error) {
 727                                                                                 //cout << "ERROR: Unable to load approach " << iap->_ident << " at " << iap->_aptIdent << '\n';
 728                                                                                 nErrors++;
 729                                                                         } else {
 730                                                                                 _np_iap[iap->_aptIdent].push_back(iap);
 731                                                                                 //cout << "** Loaded " << iap->_aptIdent << "\t" << iap->_ident << '\n';
 732                                                                                 nLoaded++;
 733                                                                         }
 734                                                                         iap_in_progress = false;
 735                                                                 }
 736                                                         }
 737                                                         last_apt_ident = apt_ident;
 738                                                 }
 739                                                 iap_in_progress = 0;
 740                                         } else if(s[12] == 'F') {       // Approach segment
 741                                                 if(apt_in_progress) {
 742                                                         iap_ident = s.substr(13, 6);
 743                                                         // Trim any whitespace from the RH end.
 744                                                         for(j=0; j<6; ++j) {
 745                                                                 if(iap_ident[5-j] == ' ') {
 746                                                                         iap_ident = iap_ident.substr(0, 5-j);
 747                                                                 } else {
 748                                                                         // It's important to break here, since earlier versions of ARINC 424 allowed spaces in the ident.
 749                                                                         break;
 750                                                                 }
 751                                                         }
 752                                                         if(iap_in_progress) {
 753                                                                 if(iap_ident != last_iap_ident) {
 754                                                                         // This is a new approach - store the last one and trigger
 755                                                                         // starting afresh by setting the in progress flag to false.
 756                                                                         if(iap_error) {
 757                                                                                 //cout << "ERROR: Unable to load approach " << iap->_ident << " at " << iap->_aptIdent << '\n';
 758                                                                                 nErrors++;
 759                                                                         } else {
 760                                                                                 _np_iap[iap->_aptIdent].push_back(iap);
 761                                                                                 //cout << "Loaded " << iap->_aptIdent << "\t" << iap->_ident << '\n';
 762                                                                                 nLoaded++;
 763                                                                         }
 764                                                                         iap_in_progress = false;
 765                                                                 }
 766                                                         }
 767                                                         if(!iap_in_progress) {
 768                                                                 iap = new FGNPIAP;
 769                                                                 iap->_aptIdent = apt_ident;
 770                                                                 iap->_ident = iap_ident;
 771                                                                 iap->_name = ExpandSIAPIdent(iap_ident); // I suspect that it's probably better to just store idents, and to expand the names as needed.
 772                                                                 // Note, we haven't set iap->_rwyStr yet.
 773                                                                 last_iap_ident = iap_ident;
 774                                                                 iap_in_progress = true;
 775                                                                 iap_error = false;
 776                                                         }
 777
 778                                                         // Route type
 779                                                         route_type = s[19];
 780                                                         sequence_number = atoi(s.substr(26,3).c_str());
 781                                                         wp_ident = s.substr(29, 5);
 782                                                         waypoint_fix_type = s[42];
 783                                                         // Trim any whitespace from the RH end
 784                                                         for(j=0; j<5; ++j) {
 785                                                                 if(wp_ident[4-j] == ' ') {
 786                                                                         wp_ident = wp_ident.substr(0, 4-j);
 787                                                                 } else {
 788                                                                         break;
 789                                                                 }
 790                                                         }
 791
 792                                                         // Ignore lines with no waypoint ID for now - these are normally part of the
 793                                                         // missed approach procedure, and we don't use them in the KLN89.
 794                                                         if(!wp_ident.empty()) {
 795                                                                 // Make a local copy of the waypoint for now, since we're not yet sure if we'll be using it
 796                                                                 GPSWaypoint w;
 797                                                                 w.id = wp_ident;
 798                                                                 bool wp_error = false;
 799                                                                 if(w.id.substr(0, 2) == "RW" && waypoint_fix_type == 'M') {
 800                                                                         // Assume that this is a missed-approach point based on the runway number, which appears to be standard for most approaches.
 801                                                                         // Note: Currently fgFindAirportID returns NULL on error, but getRunwayByIdent throws an exception.
 802                                                                         const FGAirport* apt = fgFindAirportID(iap->_aptIdent);
 803                                                                         if(apt) {
 804                                                                                 string rwystr;
 805                                                                                 try {
 806                                                                                         rwystr = w.id.substr(2, 2);
 807                                                                                         // TODO - sanity check the rwystr at this point to ensure we have a double digit number
 808                                                                                         if(w.id.size() > 4) {
 809                                                                                                 if(w.id[4] == 'L' || w.id[4] == 'C' || w.id[4] == 'R') {
 810                                                                                                         rwystr += w.id[4];
 811                                                                                                 }
 812                                                                                         }
 813                                                                                         FGRunway* rwy = apt->getRunwayByIdent(rwystr);
 814                                                                                         w.lat = rwy->begin().getLatitudeRad();
 815                                                                                         w.lon = rwy->begin().getLongitudeRad();
 816                                                                                 } catch(const sg_exception&) {
 817                                                                                         SG_LOG(SG_GENERAL, SG_WARN, "Unable to find runway " << w.id.substr(2, 2) << " at airport " << iap->_aptIdent);
 818                                                                                         //cout << "Unable to find runway " << w.id.substr(2, 2) << " at airport " << iap->_aptIdent << " ( w.id = " << w.id << ", rwystr = " << rwystr << " )\n";
 819                                                                                         wp_error = true;
 820                                                                                 }
 821                                                                         } else {
 822                                                                                 wp_error = true;
 823                                                                         }
 824                                                                 } else {
 825                                                                         cwp = FindFirstByExactId(w.id);
 826                                                                         if(cwp) {
 827                                                                                 w = *cwp;
 828                                                                         } else {
 829                                                                                 wp_error = true;
 830                                                                         }
 831                                                                 }
 832                                                                 switch(waypoint_fix_type) {
 833                                                                 case 'A': w.appType = GPS_IAF; break;
 834                                                                 case 'F': w.appType = GPS_FAF; break;
 835                                                                 case 'H': w.appType = GPS_MAHP; break;
 836                                                                 case 'I': w.appType = GPS_IAP; break;
 837                                                                 case 'M': w.appType = GPS_MAP; break;
 838                                                                 case ' ': w.appType = GPS_APP_NONE; break;
 839                                                                 //default: cout << "Unknown waypoint_fix_type: \'" << waypoint_fix_type << "\' [" << apt_ident << ", " << iap_ident << "]\n";
 840                                                                 }
 841
 842                                                                 if(wp_error) {
 843                                                                         //cout << "Unable to find waypoint " << w.id << " [" << apt_ident << ", " << iap_ident << "]\n";
 844                                                                         iap_error = true;
 845                                                                 }
 846
 847                                                                 if(!wp_error) {
 848                                                                         if(route_in_progress) {
 849                                                                                 if(sequence_number > last_sequence_number) {
 850                                                                                         // TODO - add a check for runway numbers
 851                                                                                         // Check for the waypoint ID being the same as the previous line.
 852                                                                                         // This is often the case for the missed approach holding point.
 853                                                                                         if(wp_ident == last_wp_ident) {
 854                                                                                                 if(waypoint_fix_type == 'H') {
 855                                                                                                         if(!iap->_IAP.empty()) {
 856                                                                                                                 if(iap->_IAP[iap->_IAP.size() - 1]->appType == GPS_APP_NONE) {
 857                                                                                                                         iap->_IAP[iap->_IAP.size() - 1]->appType = GPS_MAHP;
 858                                                                                                                 } else {
 859                                                                                                                         //cout << "Waypoint is MAHP and another type! " << w.id << " [" << apt_ident << ", " << iap_ident << "]\n";
 860                                                                                                                 }
 861                                                                                                         }
 862                                                                                                 }
 863                                                                                         } else {
 864                                                                                                 // Create a new waypoint on the heap, copy the local copy into it, and push it onto the approach.
 865                                                                                                 wp = new GPSWaypoint;
 866                                                                                                 *wp = w;
 867                                                                                                 if(route_type == 'A') {
 868                                                                                                         fp->waypoints.push_back(wp);
 869                                                                                                 } else {
 870                                                                                                         iap->_IAP.push_back(wp);
 871                                                                                                 }
 872                                                                                         }
 873                                                                                 } else if(sequence_number == last_sequence_number) {
 874                                                                                         // This seems to happen once per final approach route - one of the waypoints
 875                                                                                         // is duplicated with the same sequence number - I'm not sure what information
 876                                                                                         // the second line give yet so ignore it for now.
 877                                                                                         // TODO - figure this out!
 878                                                                                 } else {
 879                                                                                         // Finalise the current route and start a new one
 880                                                                                         //
 881                                                                                         // Finalise the current route
 882                                                                                         if(last_route_type == 'A') {
 883                                                                                                 // Push the flightplan onto the approach
 884                                                                                                 iap->_approachRoutes.push_back(fp);
 885                                                                                         } else {
 886                                                                                                 // All the waypoints get pushed individually - don't need to do it.
 887                                                                                         }
 888                                                                                         // Start a new one
 889                                                                                         // There are basically 2 possibilities here - either it's one of the arrival transitions,
 890                                                                                         // or it's the core final approach course.
 891                                                                                         wp = new GPSWaypoint;
 892                                                                                         *wp = w;
 893                                                                                         if(route_type == 'A') { // It's one of the arrival transition(s)
 894                                                                                                 fp = new GPSFlightPlan;
 895                                                                                                 fp->waypoints.push_back(wp);
 896                                                                                         } else {
 897                                                                                                 iap->_IAP.push_back(wp);
 898                                                                                         }
 899                                                                                         route_in_progress = true;
 900                                                                                 }
 901                                                                         } else {
 902                                                                                 // Start a new route.
 903                                                                                 // There are basically 2 possibilities here - either it's one of the arrival transitions,
 904                                                                                 // or it's the core final approach course.
 905                                                                                 wp = new GPSWaypoint;
 906                                                                                 *wp = w;
 907                                                                                 if(route_type == 'A') { // It's one of the arrival transition(s)
 908                                                                                         fp = new GPSFlightPlan;
 909                                                                                         fp->waypoints.push_back(wp);
 910                                                                                 } else {
 911                                                                                         iap->_IAP.push_back(wp);
 912                                                                                 }
 913                                                                                 route_in_progress = true;
 914                                                                         }
 915                                                                         last_route_type = route_type;
 916                                                                         last_wp_ident = wp_ident;
 917                                                                         last_sequence_number = sequence_number;
 918                                                                 }
 919                                                         }
 920                                                 } else {
 921                                                         // ERROR - no airport record read.
 922                                                 }
 923                                         }
 924                                 } else {
 925                                         // Check and finalise any approaches in progress
 926                                         // TODO - sanity check that the approach has all the required elements
 927                                         if(iap_in_progress) {
 928                                                 // This is a new approach - store the last one and trigger
 929                                                 // starting afresh by setting the in progress flag to false.
 930                                                 if(iap_error) {
 931                                                         //cout << "ERROR: Unable to load approach " << iap->_ident << " at " << iap->_aptIdent << '\n';
 932                                                         nErrors++;
 933                                                 } else {
 934                                                         _np_iap[iap->_aptIdent].push_back(iap);
 935                                                         //cout << "* Loaded " << iap->_aptIdent << "\t" << iap->_ident << '\n';
 936                                                         nLoaded++;
 937                                                 }
 938                                                 iap_in_progress = false;
 939                                         }
 940                                 }
 941                         }
 942                 }
 943         }
 944
 945         // If we get to the end of the file, load any approach that is still in progress
 946         // TODO - sanity check that the approach has all the required elements
 947         if(iap_in_progress) {
 948                 if(iap_error) {
 949                         //cout << "ERROR: Unable to load approach " << iap->_ident << " at " << iap->_aptIdent << '\n';
 950                         nErrors++;
 951                 } else {
 952                         _np_iap[iap->_aptIdent].push_back(iap);
 953                         //cout << "*** Loaded " << iap->_aptIdent << "\t" << iap->_ident << '\n';
 954                         nLoaded++;
 955                 }
 956         }
 957
 958         //cout << "Done loading approach database\n";
 959         //cout << "Loaded: " << nLoaded << '\n';
 960         //cout << "Failed: " << nErrors << '\n';
 961
 962         fin.close();
 963 }
 964
 965 GPSWaypoint* DCLGPS::GetActiveWaypoint() {
 966         return &_activeWaypoint;
 967 }
 968
 969 // Returns meters
 970 float DCLGPS::GetDistToActiveWaypoint() {
 971         return _dist2Act;
 972 }
 973
 974 // I don't yet fully understand all the gotchas about where to source time from.
 975 // This function sets the initial timer before the clock exports properties
 976 // and the one below uses the clock to be consistent with the rest of the code.
 977 // It might change soonish...
 978 void DCLGPS::SetPowerOnTimer() {
 979         struct tm *t = globals->get_time_params()->getGmt();
 980         _powerOnTime.set_hr(t->tm_hour);
 981         _powerOnTime.set_min(t->tm_min);
 982         _powerOnTimerSet = true;
 983 }
 984
 985 void DCLGPS::ResetPowerOnTimer() {
 986         _powerOnTime.set_hr(atoi(fgGetString("/instrumentation/clock/indicated-hour")));
 987         _powerOnTime.set_min(atoi(fgGetString("/instrumentation/clock/indicated-min")));
 988         _powerOnTimerSet = true;
 989 }
 990
 991 double DCLGPS::GetCDIDeflection() const {
 992         double xtd = CalcCrossTrackDeviation(); //nm
 993         return((xtd / _currentCdiScale) * 5.0 * 2.5 * -1.0);
 994 }
 995
 996 void DCLGPS::DtoInitiate(const string& s) {
 997         const GPSWaypoint* wp = FindFirstByExactId(s);
 998         if(wp) {
 999                 // TODO - Currently we start DTO operation unconditionally, regardless of which mode we are in.
1000                 // In fact, the following rules apply:
1001                 // In LEG mode, start DTO as we currently do.
1002                 // In OBS mode, set the active waypoint to the requested waypoint, and then:
1003                 // If the KLN89 is not connected to an external HSI or CDI, set the OBS course to go direct to the waypoint.
1004                 // If the KLN89 *is* connected to an external HSI or CDI, it cannot set the course itself, and will display
1005                 // a scratchpad message with the course to set manually on the HSI/CDI.
1006                 // In both OBS cases, leave _dto false, since we don't need the virtual waypoint created.
1007                 _dto = true;
1008                 _activeWaypoint = *wp;
1009                 _fromWaypoint.lat = _gpsLat;
1010                 _fromWaypoint.lon = _gpsLon;
1011                 _fromWaypoint.type = GPS_WP_VIRT;
1012                 _fromWaypoint.id = "_DTOWP_";
1013                 delete wp;
1014         } else {
1015                 // TODO - Should bring up the user waypoint page.
1016                 _dto = false;
1017         }
1018 }
1019
1020 void DCLGPS::DtoCancel() {
1021         if(_dto) {
1022                 // i.e. don't bother reorientating if we're just cancelling a DTO button press
1023                 // without having previously initiated DTO.
1024                 OrientateToActiveFlightPlan();
1025         }
1026         _dto = false;
1027 }
1028
1029 void DCLGPS::ToggleOBSMode() {
1030         _obsMode = !_obsMode;
1031         if(_obsMode) {
1032                 // We need to set the OBS heading.  The rules for this are:
1033                 //
1034                 // If the KLN89 is connected to an external HSI or CDI, then the OBS heading must be set
1035                 // to the OBS radial on the external indicator, and cannot be changed in the KLN89.
1036                 //
1037                 // If the KLN89 is not connected to an external indicator, then:
1038                 //              If there is an active waypoint, the OBS heading is set such that the
1039                 //              deviation indicator remains at the same deviation (i.e. set to DTK,
1040                 //              although there may be some small difference).
1041                 //
1042                 //              If there is not an active waypoint, I am not sure what value should be
1043                 //              set.
1044                 //
1045                 if(fgGetBool("/instrumentation/nav/slaved-to-gps")) {
1046                         _obsHeading = static_cast<int>(fgGetDouble("/instrumentation/nav/radials/selected-deg") + 0.5);
1047                 } else if(!_activeWaypoint.id.empty()) {
1048                         // NOTE: I am not sure if this is completely correct.  There are sometimes small differences
1049                         // between DTK and default OBS heading in the simulator (~ 1 or 2 degrees).  It might also
1050                         // be that OBS heading should be stored as float and only displayed as int, in order to maintain
1051                         // the initial bar deviation?
1052                         _obsHeading = static_cast<int>(_dtkMag + 0.5);
1053                         //cout << "_dtkMag = " << _dtkMag << ", _dtkTrue = " << _dtkTrue << ", _obsHeading = " << _obsHeading << '\n';
1054                 } else {
1055                         // TODO - what should we really do here?
1056                         _obsHeading = 0;
1057                 }
1058
1059                 // Valid OBS heading values are 0 -> 359 degrees inclusive (from kln89 simulator).
1060                 if(_obsHeading > 359) _obsHeading -= 360;
1061                 if(_obsHeading < 0) _obsHeading += 360;
1062
1063                 // TODO - the _fromWaypoint location will change as the OBS heading changes.
1064                 // Might need to store the OBS initiation position somewhere in case it is needed again.
1065                 SetOBSFromWaypoint();
1066         }
1067 }
1068
1069 // Set the _fromWaypoint position based on the active waypoint and OBS radial.
1070 void DCLGPS::SetOBSFromWaypoint() {
1071         if(!_obsMode) return;
1072         if(_activeWaypoint.id.empty()) return;
1073
1074         // TODO - base the 180 deg correction on the to/from flag.
1075         _fromWaypoint = GetPositionOnMagRadial(_activeWaypoint, 10, _obsHeading + 180.0);
1076         _fromWaypoint.type = GPS_WP_VIRT;
1077         _fromWaypoint.id = "_OBSWP_";
1078 }
1079
1080 void DCLGPS::CDIFSDIncrease() {
1081         if(_currentCdiScaleIndex == 0) {
1082                 _currentCdiScaleIndex = _cdiScales.size() - 1;
1083         } else {
1084                 _currentCdiScaleIndex--;
1085         }
1086 }
1087
1088 void DCLGPS::CDIFSDDecrease() {
1089         _currentCdiScaleIndex++;
1090         if(_currentCdiScaleIndex == _cdiScales.size()) {
1091                 _currentCdiScaleIndex = 0;
1092         }
1093 }
1094
1095 void DCLGPS::DrawChar(char c, int field, int px, int py, bool bold) {
1096 }
1097
1098 void DCLGPS::DrawText(const string& s, int field, int px, int py, bool bold) {
1099 }
1100
1101 void DCLGPS::SetBaroUnits(int n, bool wrap) {
1102         if(n < 1) {
1103                 _baroUnits = (GPSPressureUnits)(wrap ? 3 : 1);
1104         } else if(n > 3) {
1105                 _baroUnits = (GPSPressureUnits)(wrap ? 1 : 3);
1106         } else {
1107                 _baroUnits = (GPSPressureUnits)n;
1108         }
1109 }
1110
1111 void DCLGPS::CreateDefaultFlightPlans() {}
1112
1113 // Get the time to the active waypoint in seconds.
1114 // Returns -1 if groundspeed < 30 kts
1115 double DCLGPS::GetTimeToActiveWaypoint() {
1116         if(_groundSpeed_kts < 30.0) {
1117                 return(-1.0);
1118         } else {
1119                 return(_eta);
1120         }
1121 }
1122
1123 // Get the time to the final waypoint in seconds.
1124 // Returns -1 if groundspeed < 30 kts
1125 double DCLGPS::GetETE() {
1126         if(_groundSpeed_kts < 30.0) {
1127                 return(-1.0);
1128         } else {
1129                 // TODO - handle OBS / DTO operation appropriately
1130                 if(_activeFP->waypoints.empty()) {
1131                         return(-1.0);
1132                 } else {
1133                         return(GetTimeToWaypoint(_activeFP->waypoints[_activeFP->waypoints.size() - 1]->id));
1134                 }
1135         }
1136 }
1137
1138 // Get the time to a given waypoint (spec'd by ID) in seconds.
1139 // returns -1 if groundspeed is less than 30kts.
1140 // If the waypoint is an unreached part of the active flight plan the time will be via each leg.
1141 // otherwise it will be a direct-to time.
1142 double DCLGPS::GetTimeToWaypoint(const string& id) {
1143         if(_groundSpeed_kts < 30.0) {
1144                 return(-1.0);
1145         }
1146
1147         double eta = 0.0;
1148         int n1 = GetActiveWaypointIndex();
1149         int n2 = GetWaypointIndex(id);
1150         if(n2 > n1) {
1151                 eta = _eta;
1152                 for(unsigned int i=n1+1; i<_activeFP->waypoints.size(); ++i) {
1153                         GPSWaypoint* wp1 = _activeFP->waypoints[i-1];
1154                         GPSWaypoint* wp2 = _activeFP->waypoints[i];
1155                         double distm = GetGreatCircleDistance(wp1->lat, wp1->lon, wp2->lat, wp2->lon) * SG_NM_TO_METER;
1156                         eta += (distm / _groundSpeed_ms);
1157                 }
1158                 return(eta);
1159         } else if(id == _activeWaypoint.id) {
1160                 return(_eta);
1161         } else {
1162                 const GPSWaypoint* wp = FindFirstByExactId(id);
1163                 if(wp == NULL) return(-1.0);
1164                 double distm = GetGreatCircleDistance(_gpsLat, _gpsLon, wp->lat, wp->lon);
1165     delete wp;
1166                 return(distm / _groundSpeed_ms);
1167         }
1168         return(-1.0);   // Hopefully we never get here!
1169 }
1170
1171 // Returns magnetic great-circle heading
1172 // TODO - document units.
1173 float DCLGPS::GetHeadingToActiveWaypoint() {
1174         if(_activeWaypoint.id.empty()) {
1175                 return(-888);
1176         } else {
1177                 double h = GetMagHeadingFromTo(_gpsLat, _gpsLon, _activeWaypoint.lat, _activeWaypoint.lon);
1178                 while(h <= 0.0) h += 360.0;
1179                 while(h > 360.0) h -= 360.0;
1180                 return((float)h);
1181         }
1182 }
1183
1184 // Returns magnetic great-circle heading
1185 // TODO - what units?
1186 float DCLGPS::GetHeadingFromActiveWaypoint() {
1187         if(_activeWaypoint.id.empty()) {
1188                 return(-888);
1189         } else {
1190                 double h = GetMagHeadingFromTo(_activeWaypoint.lat, _activeWaypoint.lon, _gpsLat, _gpsLon);
1191                 while(h <= 0.0) h += 360.0;
1192                 while(h > 360.0) h -= 360.0;
1193                 return(h);
1194         }
1195 }
1196
1197 void DCLGPS::ClearFlightPlan(int n) {
1198         for(unsigned int i=0; i<_flightPlans[n]->waypoints.size(); ++i) {
1199                 delete _flightPlans[n]->waypoints[i];
1200         }
1201         _flightPlans[n]->waypoints.clear();
1202 }
1203
1204 void DCLGPS::ClearFlightPlan(GPSFlightPlan* fp) {
1205         for(unsigned int i=0; i<fp->waypoints.size(); ++i) {
1206                 delete fp->waypoints[i];
1207         }
1208         fp->waypoints.clear();
1209 }
1210
1211 int DCLGPS::GetActiveWaypointIndex() {
1212         for(unsigned int i=0; i<_flightPlans[0]->waypoints.size(); ++i) {
1213                 if(_flightPlans[0]->waypoints[i]->id == _activeWaypoint.id) return((int)i);
1214         }
1215         return(-1);
1216 }
1217
1218 int DCLGPS::GetWaypointIndex(const string& id) {
1219         for(unsigned int i=0; i<_flightPlans[0]->waypoints.size(); ++i) {
1220                 if(_flightPlans[0]->waypoints[i]->id == id) return((int)i);
1221         }
1222         return(-1);
1223 }
1224
1225 void DCLGPS::OrientateToFlightPlan(GPSFlightPlan* fp) {
1226         //cout << "Orientating...\n";
1227         //cout << "_lat = " << _lat << ", _lon = " << _lon << ", _gpsLat = " << _gpsLat << ", gpsLon = " << _gpsLon << '\n';
1228         if(fp->IsEmpty()) {
1229                 _activeWaypoint.id.clear();
1230                 _navFlagged = true;
1231         } else {
1232                 _navFlagged = false;
1233                 if(fp->waypoints.size() == 1) {
1234                         // TODO - may need to flag nav here if not dto or obs, or possibly handle it somewhere else.
1235                         _activeWaypoint = *fp->waypoints[0];
1236                         _fromWaypoint.id.clear();
1237                 } else {
1238                         // FIXME FIXME FIXME
1239                         _fromWaypoint = *fp->waypoints[0];
1240                         _activeWaypoint = *fp->waypoints[1];
1241                         double dmin = 1000000;  // nm!!
1242                         // For now we will simply start on the leg closest to our current position.
1243                         // It's possible that more fancy algorithms may take either heading or track
1244                         // into account when setting inital leg - I'm not sure.
1245                         // This method should handle most cases perfectly OK though.
1246                         for(unsigned int i = 1; i < fp->waypoints.size(); ++i) {
1247                                 //cout << "Pass " << i << ", dmin = " << dmin << ", leg is " << fp->waypoints[i-1]->id << " to " << fp->waypoints[i]->id << '\n';
1248                                 // First get the cross track correction.
1249                                 double d0 = fabs(CalcCrossTrackDeviation(*fp->waypoints[i-1], *fp->waypoints[i]));
1250                                 // That is the shortest distance away we could be though - check for
1251                                 // longer distances if we are 'off the end' of the leg.
1252                                 double ht1 = GetGreatCircleCourse(fp->waypoints[i-1]->lat, fp->waypoints[i-1]->lon,
1253                                                                   fp->waypoints[i]->lat, fp->waypoints[i]->lon)
1254                                                                                                   * SG_RADIANS_TO_DEGREES;
1255                                 // not simply the reverse of the above due to great circle navigation.
1256                                 double ht2 = GetGreatCircleCourse(fp->waypoints[i]->lat, fp->waypoints[i]->lon,
1257                                                                   fp->waypoints[i-1]->lat, fp->waypoints[i-1]->lon)
1258                                                                                                   * SG_RADIANS_TO_DEGREES;
1259                                 double hw1 = GetGreatCircleCourse(_gpsLat, _gpsLon,
1260                                                                   fp->waypoints[i]->lat, fp->waypoints[i]->lon)
1261                                                                                                   * SG_RADIANS_TO_DEGREES;
1262                                 double hw2 = GetGreatCircleCourse(_gpsLat, _gpsLon,
1263                                                                   fp->waypoints[i-1]->lat, fp->waypoints[i-1]->lon)
1264                                                                                                   * SG_RADIANS_TO_DEGREES;
1265                                 double h1 = ht1 - hw1;
1266                                 double h2 = ht2 - hw2;
1267                                 //cout << "d0, h1, h2 = " << d0 << ", " << h1 << ", " << h2 << '\n';
1268                                 //cout << "Normalizing...\n";
1269                                 SG_NORMALIZE_RANGE(h1, -180.0, 180.0);
1270                                 SG_NORMALIZE_RANGE(h2, -180.0, 180.0);
1271                                 //cout << "d0, h1, h2 = " << d0 << ", " << h1 << ", " << h2 << '\n';
1272                                 if(fabs(h1) > 90.0) {
1273                                         // We are past the end of the to waypoint
1274                                         double d = GetGreatCircleDistance(_gpsLat, _gpsLon, fp->waypoints[i]->lat, fp->waypoints[i]->lon);
1275                                         if(d > d0) d0 = d;
1276                                         //cout << "h1 triggered, d0 now = " << d0 << '\n';
1277                                 } else if(fabs(h2) > 90.0) {
1278                                         // We are past the end (not yet at!) the from waypoint
1279                                         double d = GetGreatCircleDistance(_gpsLat, _gpsLon, fp->waypoints[i-1]->lat, fp->waypoints[i-1]->lon);
1280                                         if(d > d0) d0 = d;
1281                                         //cout << "h2 triggered, d0 now = " << d0 << '\n';
1282                                 }
1283                                 if(d0 < dmin) {
1284                                         //cout << "THIS LEG NOW ACTIVE!\n";
1285                                         dmin = d0;
1286                                         _fromWaypoint = *fp->waypoints[i-1];
1287                                         _activeWaypoint = *fp->waypoints[i];
1288                                 }
1289                         }
1290                 }
1291         }
1292 }
1293
1294 void DCLGPS::OrientateToActiveFlightPlan() {
1295         OrientateToFlightPlan(_activeFP);
1296 }
1297
1298 /***************************************/
1299
1300 // Utility function - create a flightplan from a list of waypoint ids and types
1301 void DCLGPS::CreateFlightPlan(GPSFlightPlan* fp, vector<string> ids, vector<GPSWpType> wps) {
1302         if(fp == NULL) fp = new GPSFlightPlan;
1303         unsigned int i;
1304         if(!fp->waypoints.empty()) {
1305                 for(i=0; i<fp->waypoints.size(); ++i) {
1306                         delete fp->waypoints[i];
1307                 }
1308                 fp->waypoints.clear();
1309         }
1310         if(ids.size() != wps.size()) {
1311                 cout << "ID and Waypoint types list size mismatch in GPS::CreateFlightPlan - no flightplan created!\n";
1312                 return;
1313         }
1314         for(i=0; i<ids.size(); ++i) {
1315                 bool multi;
1316                 const FGAirport* ap;
1317                 FGNavRecord* np;
1318                 GPSWaypoint* wp = new GPSWaypoint;
1319                 wp->type = wps[i];
1320                 switch(wp->type) {
1321                 case GPS_WP_APT:
1322                         ap = FindFirstAptById(ids[i], multi, true);
1323                         if(ap == NULL) {
1324                                 // error
1325                                 delete wp;
1326                         } else {
1327                                 wp->lat = ap->getLatitude() * SG_DEGREES_TO_RADIANS;
1328                                 wp->lon = ap->getLongitude() * SG_DEGREES_TO_RADIANS;
1329                                 wp->id = ids[i];
1330                                 fp->waypoints.push_back(wp);
1331                         }
1332                         break;
1333                 case GPS_WP_VOR:
1334                         np = FindFirstVorById(ids[i], multi, true);
1335                         if(np == NULL) {
1336                                 // error
1337                                 delete wp;
1338                         } else {
1339                                 wp->lat = np->get_lat() * SG_DEGREES_TO_RADIANS;
1340                                 wp->lon = np->get_lon() * SG_DEGREES_TO_RADIANS;
1341                                 wp->id = ids[i];
1342                                 fp->waypoints.push_back(wp);
1343                         }
1344                         break;
1345                 case GPS_WP_NDB:
1346                         np = FindFirstNDBById(ids[i], multi, true);
1347                         if(np == NULL) {
1348                                 // error
1349                                 delete wp;
1350                         } else {
1351                                 wp->lat = np->get_lat() * SG_DEGREES_TO_RADIANS;
1352                                 wp->lon = np->get_lon() * SG_DEGREES_TO_RADIANS;
1353                                 wp->id = ids[i];
1354                                 fp->waypoints.push_back(wp);
1355                         }
1356                         break;
1357                 case GPS_WP_INT:
1358                         // TODO TODO
1359                         break;
1360                 case GPS_WP_USR:
1361                         // TODO
1362                         break;
1363                 case GPS_WP_VIRT:
1364                         // TODO
1365                         break;
1366                 }
1367         }
1368 }
1369
1370 /***************************************/
1371
1372 class DCLGPSFilter : public FGPositioned::Filter
1373 {
1374 public:
1375   virtual bool pass(const FGPositioned* aPos) const {
1376     switch (aPos->type()) {
1377     case FGPositioned::AIRPORT:
1378     // how about heliports and seaports?
1379     case FGPositioned::NDB:
1380     case FGPositioned::VOR:
1381     case FGPositioned::WAYPOINT:
1382     case FGPositioned::FIX:
1383       break;
1384     default: return false; // reject all other types
1385     }
1386     return true;
1387   }
1388 };
1389
1390 GPSWaypoint* DCLGPS::FindFirstById(const string& id) const
1391 {
1392   DCLGPSFilter filter;
1393   FGPositionedRef result = FGPositioned::findNextWithPartialId(NULL, id, &filter);
1394   return GPSWaypoint::createFromPositioned(result);
1395 }
1396
1397 GPSWaypoint* DCLGPS::FindFirstByExactId(const string& id) const
1398 {
1399   SGGeod pos(SGGeod::fromRad(_lon, _lat));
1400   FGPositionedRef result = FGPositioned::findClosestWithIdent(id, pos);
1401   return GPSWaypoint::createFromPositioned(result);
1402 }
1403
1404 // TODO - add the ASCII / alphabetical stuff from the Atlas version
1405 FGPositioned* DCLGPS::FindTypedFirstById(const string& id, FGPositioned::Type ty, bool &multi, bool exact)
1406 {
1407   multi = false;
1408   FGPositioned::TypeFilter filter(ty);
1409
1410   if (exact) {
1411     FGPositioned::List matches =
1412       FGPositioned::findAllWithIdent(id, &filter);
1413     FGPositioned::sortByRange(matches, SGGeod::fromRad(_lon, _lat));
1414     multi = (matches.size() > 1);
1415     return matches.empty() ? NULL : matches.front().ptr();
1416   }
1417
1418   return FGPositioned::findNextWithPartialId(NULL, id, &filter);
1419 }
1420
1421 FGNavRecord* DCLGPS::FindFirstVorById(const string& id, bool &multi, bool exact)
1422 {
1423   return dynamic_cast<FGNavRecord*>(FindTypedFirstById(id, FGPositioned::VOR, multi, exact));
1424 }
1425
1426 FGNavRecord* DCLGPS::FindFirstNDBById(const string& id, bool &multi, bool exact)
1427 {
1428   return dynamic_cast<FGNavRecord*>(FindTypedFirstById(id, FGPositioned::NDB, multi, exact));
1429 }
1430
1431 const FGFix* DCLGPS::FindFirstIntById(const string& id, bool &multi, bool exact)
1432 {
1433   return dynamic_cast<FGFix*>(FindTypedFirstById(id, FGPositioned::FIX, multi, exact));
1434 }
1435
1436 const FGAirport* DCLGPS::FindFirstAptById(const string& id, bool &multi, bool exact)
1437 {
1438   return dynamic_cast<FGAirport*>(FindTypedFirstById(id, FGPositioned::AIRPORT, multi, exact));
1439 }
1440
1441 FGNavRecord* DCLGPS::FindClosestVor(double lat_rad, double lon_rad) {
1442   FGPositioned::TypeFilter filter(FGPositioned::VOR);
1443   double cutoff = 1000; // nautical miles
1444   FGPositionedRef v = FGPositioned::findClosest(SGGeod::fromRad(lon_rad, lat_rad), cutoff, &filter);
1445   if (!v) {
1446     return NULL;
1447   }
1448
1449   return dynamic_cast<FGNavRecord*>(v.ptr());
1450 }
1451
1452 //----------------------------------------------------------------------------------------------------------
1453
1454 // Takes lat and lon in RADIANS!!!!!!!
1455 double DCLGPS::GetMagHeadingFromTo(double latA, double lonA, double latB, double lonB) {
1456         double h = GetGreatCircleCourse(latA, lonA, latB, lonB);
1457         h *= SG_RADIANS_TO_DEGREES;
1458         // TODO - use the real altitude below instead of 0.0!
1459         //cout << "MagVar = " << sgGetMagVar(_gpsLon, _gpsLat, 0.0, _time->getJD()) * SG_RADIANS_TO_DEGREES << '\n';
1460         h -= sgGetMagVar(_gpsLon, _gpsLat, 0.0, _time->getJD()) * SG_RADIANS_TO_DEGREES;
1461         while(h >= 360.0) h -= 360.0;
1462         while(h < 0.0) h += 360.0;
1463         return(h);
1464 }
1465
1466 // ---------------- Great Circle formulae from "The Aviation Formulary" -------------
1467 // Note that all of these assume that the world is spherical.
1468
1469 double Rad2Nm(double theta) {
1470         return(((180.0*60.0)/SG_PI)*theta);
1471 }
1472
1473 double Nm2Rad(double d) {
1474         return((SG_PI/(180.0*60.0))*d);
1475 }
1476
1477 /* QUOTE:
1478
1479 The great circle distance d between two points with coordinates {lat1,lon1} and {lat2,lon2} is given by:
1480
1481 d=acos(sin(lat1)*sin(lat2)+cos(lat1)*cos(lat2)*cos(lon1-lon2))
1482
1483 A mathematically equivalent formula, which is less subject to rounding error for short distances is:
1484
1485 d=2*asin(sqrt((sin((lat1-lat2)/2))^2 +
1486                  cos(lat1)*cos(lat2)*(sin((lon1-lon2)/2))^2))
1487
1488 */
1489
1490 // Returns distance in nm, takes lat & lon in RADIANS
1491 double DCLGPS::GetGreatCircleDistance(double lat1, double lon1, double lat2, double lon2) const {
1492         double d = 2.0 * asin(sqrt(((sin((lat1-lat2)/2.0))*(sin((lat1-lat2)/2.0))) +
1493                    cos(lat1)*cos(lat2)*(sin((lon1-lon2)/2.0))*(sin((lon1-lon2)/2.0))));
1494         return(Rad2Nm(d));
1495 }
1496
1497 // fmod dosen't do what we want :-(
1498 static double mod(double d1, double d2) {
1499         return(d1 - d2*floor(d1/d2));
1500 }
1501
1502 // Returns great circle course from point 1 to point 2
1503 // Input and output in RADIANS.
1504 double DCLGPS::GetGreatCircleCourse (double lat1, double lon1, double lat2, double lon2) const {
1505         //double h = 0.0;
1506         /*
1507         // Special case the poles
1508         if(cos(lat1) < SG_EPSILON) {
1509                 if(lat1 > 0) {
1510                         // Starting from North Pole
1511                         h = SG_PI;
1512                 } else {
1513                         // Starting from South Pole
1514                         h = 2.0 * SG_PI;
1515                 }
1516         } else {
1517                 // Urgh - the formula below is for negative lon +ve !!!???
1518                 double d = GetGreatCircleDistance(lat1, lon1, lat2, lon2);
1519                 cout << "d = " << d;
1520                 d = Nm2Rad(d);
1521                 //cout << ", d_theta = " << d;
1522                 //cout << ", and d = " << Rad2Nm(d) << ' ';
1523                 if(sin(lon2 - lon1) < 0) {
1524                         cout << " A ";
1525                         h = acos((sin(lat2)-sin(lat1)*cos(d))/(sin(d)*cos(lat1)));
1526                 } else {
1527                         cout << " B ";
1528                         h = 2.0 * SG_PI - acos((sin(lat2)-sin(lat1)*cos(d))/(sin(d)*cos(lat1)));
1529                 }
1530         }
1531         cout << h * SG_RADIANS_TO_DEGREES << '\n';
1532         */
1533
1534         return( mod(atan2(sin(lon2-lon1)*cos(lat2),
1535             cos(lat1)*sin(lat2)-sin(lat1)*cos(lat2)*cos(lon2-lon1)),
1536             2.0*SG_PI) );
1537 }
1538
1539 // Return a position on a radial from wp1 given distance d (nm) and magnetic heading h (degrees)
1540 // Note that d should be less that 1/4 Earth diameter!
1541 GPSWaypoint DCLGPS::GetPositionOnMagRadial(const GPSWaypoint& wp1, double d, double h) {
1542         h += sgGetMagVar(wp1.lon, wp1.lat, 0.0, _time->getJD()) * SG_RADIANS_TO_DEGREES;
1543         return(GetPositionOnRadial(wp1, d, h));
1544 }
1545
1546 // Return a position on a radial from wp1 given distance d (nm) and TRUE heading h (degrees)
1547 // Note that d should be less that 1/4 Earth diameter!
1548 GPSWaypoint DCLGPS::GetPositionOnRadial(const GPSWaypoint& wp1, double d, double h) {
1549         while(h < 0.0) h += 360.0;
1550         while(h > 360.0) h -= 360.0;
1551
1552         h *= SG_DEGREES_TO_RADIANS;
1553         d *= (SG_PI / (180.0 * 60.0));
1554
1555         double lat=asin(sin(wp1.lat)*cos(d)+cos(wp1.lat)*sin(d)*cos(h));
1556         double lon;
1557         if(cos(lat)==0) {
1558                 lon=wp1.lon;      // endpoint a pole
1559         } else {
1560                 lon=mod(wp1.lon+asin(sin(h)*sin(d)/cos(lat))+SG_PI,2*SG_PI)-SG_PI;
1561         }
1562
1563         GPSWaypoint wp;
1564         wp.lat = lat;
1565         wp.lon = lon;
1566         wp.type = GPS_WP_VIRT;
1567         return(wp);
1568 }
1569
1570 // Returns cross-track deviation in Nm.
1571 double DCLGPS::CalcCrossTrackDeviation() const {
1572         return(CalcCrossTrackDeviation(_fromWaypoint, _activeWaypoint));
1573 }
1574
1575 // Returns cross-track deviation of the current position between two arbitary waypoints in nm.
1576 double DCLGPS::CalcCrossTrackDeviation(const GPSWaypoint& wp1, const GPSWaypoint& wp2) const {
1577         //if(wp1 == NULL || wp2 == NULL) return(0.0);
1578         if(wp1.id.empty() || wp2.id.empty()) return(0.0);
1579         double xtd = asin(sin(Nm2Rad(GetGreatCircleDistance(wp1.lat, wp1.lon, _gpsLat, _gpsLon)))
1580                           * sin(GetGreatCircleCourse(wp1.lat, wp1.lon, _gpsLat, _gpsLon) - GetGreatCircleCourse(wp1.lat, wp1.lon, wp2.lat, wp2.lon)));
1581         return(Rad2Nm(xtd));
1582 }