src/Instrumentation/dclgps.cxx

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