src/Instrumentation/dclgps.cxx

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