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