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