src/Instrumentation/dclgps.cxx

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