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 file.
 605         Tested on ARINC 424-18.
 606         Known / current best guess at the format:
 607         Col 1:          Always 'S'.  If it isn't, ditch it.
 608         Col 2-4:        "Customer area" code, eg "USA", "CAN".  I think that CAN is used for Alaska.
 609         Col 5:          Section code.  Used in conjunction with sub-section code.  Definitions are with sub-section code.
 610         Col 6:          Always blank.
 611         Col 7-10:       ICAO (or FAA) airport ident. Left justified if < 4 chars.
 612         Col 11-12:      Based on ICAO geographical region.
 613         Col 13:         Sub-section code.  Used in conjunction with section code.
 614                                 "HD/E/F" => Helicopter record.
 615                                 "HS" => Helicopter minimum safe altitude.
 616                                 "PA" => Airport record.
 617                                 "PF" => Approach segment.
 618                                 "PG" => Runway record.
 619                                 "PP" => Path point record.      ???
 620                                 "PS" => MSA record (minimum safe altitude).
 621
 622         ------ The following is for "PF", approach segment -------
 623
 624         Col 14-19:      SIAP ident for this approach (left justified).  This is a standardised abbreviated approach name.
 625                                 e.g. "R10LZ" expands to "RNAV (GPS) Z RWY 10 L".  See the comment block for ExpandSIAPIdent for full details.
 626         Col 20:         Route type.  This is tricky - I don't have full documentation and am having to guess a bit.
 627                                 'A'     => Arrival route?       This seems to be used to encode arrival routes from the IAF to the approach proper.
 628                                                                                 Note that the final fix of the arrival route is duplicated in the approach proper.
 629                                 'D'     => VOR/DME or GPS
 630                                 'N' => NDB or GPS
 631                                 'P'     => GPS (ARINC 424-18), GPS and RNAV (GPS) (ARINC 424-15 and before).
 632                                 'R' => RNAV (GPS) (ARINC 424-18).
 633                                 'S'     => VOR or GPS
 634         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.
 635         Col 26:         BLANK
 636         Col 27-29:      Sequence number - position within the route segment.  Rule: 10-20-30 etc.
 637         Col 30-34:      Fix identifer.  The ident of the waypoint.
 638         Col 35-36:      ICAO geographical region code.  I think we can ignore this for now.
 639         Col 37:         Section code - ??? I don't know what this means
 640         Col 38          Subsection code - ??? ditto - no idea!
 641         Col 40:         Waypoint type.
 642                                 'A' => Airport as waypoint
 643                                 'E' => Essential waypoint (e.g. change of heading at this waypoint, etc).
 644                                 'G' => Runway or helipad as waypoint
 645                                 'H' => Heliport as waypoint
 646                                 'N' => NDB as waypoint
 647                                 'P' => Phantom waypoint (not sure if this is used in rev 18?)
 648                                 'V' => VOR as waypoint
 649         Col 41:         Waypoint type.
 650                                 'B' => Flyover, approach transition, or final approach.
 651                                 'E' => end of route segment (transition waypoint). (Actually "End of terminal procedure route type" in the docs).
 652                                 'N' => ??? I've also seen 'N' in this column, but don't know what it indicates.
 653                                 'Y' => Flyover.
 654         Col 43:         Waypoint type.  May also be blank when none of the below.
 655                                 'A' => Initial approach fix (IAF)
 656                                 'F' => Final approach fix
 657                                 'H' => Holding fix
 658                                 'I' => Final approach course fix
 659                                 'M' => Missed approach point
 660                                 '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!
 661                                            ??? Possibly procedure turn?
 662                                 'C' => ??? This is also present in the data, but missing from the docs.  Is at airport 00R.
 663         Col 107-111     MSA center fix.  We can ignore this.
 664 */
 665 void DCLGPS::LoadApproachData() {
 666         FGNPIAP* iap;
 667         GPSWaypoint* wp;
 668         GPSFlightPlan* fp;
 669         const GPSWaypoint* cwp;
 670
 671         std::ifstream fin;
 672         SGPath path = globals->get_fg_root();
 673         path.append("Navaids/rnav.dat");
 674         fin.open(path.c_str(), ios::in);
 675         if(!fin) {
 676                 //cout << "Unable to open input file " << path.c_str() << '\n';
 677                 return;
 678         } else {
 679                 //cout << "Opened " << path.c_str() << " for reading\n";
 680         }
 681         char tmp[256];
 682         string s;
 683
 684         string apt_ident;    // This gets set to the ICAO code of the current airport being processed.
 685         string iap_ident;    // The abbreviated name of the current approach being processed.
 686         string wp_ident;     // The ident of the waypoint of the current line
 687         string last_apt_ident;
 688         string last_iap_ident;
 689         string last_wp_ident;
 690         // There is no need to save the full name - it can be generated on the fly from the abbreviated name as and when needed.
 691         bool apt_in_progress = false;    // Set true whilst loading all the approaches for a given airport.
 692         bool iap_in_progress = false;    // Set true whilst loading a given approach.
 693         bool iap_error = false;                  // Set true if there is an error loading a given approach.
 694         bool route_in_progress = false;  // Set true when we are loading a "route" segment of the approach.
 695         int last_sequence_number = 0;    // Position within the route, rule (rev 18): 10, 20, 30 etc.
 696         int sequence_number;
 697         char last_route_type = 0;
 698         char route_type;
 699         char waypoint_fix_type;  // This is the waypoint type from col 43, i.e. the type of fix.  May be blank.
 700
 701         int j;
 702
 703         // Debugging info
 704         unsigned int nLoaded = 0;
 705         unsigned int nErrors = 0;
 706
 707         //for(i=0; i<64; ++i) {
 708         while(!fin.eof()) {
 709                 fin.getline(tmp, 256);
 710                 //s = Fake_rnav_dat[i];
 711                 s = tmp;
 712                 if(s.size() < 132) continue;
 713                 if(s[0] == 'S') {    // Valid line
 714                         string country_code = s.substr(1, 3);
 715                         if(country_code == "USA") {    // For now we'll stick to US procedures in case there are unknown gotchas with others
 716                                 if(s[4] == 'P') {    // Includes approaches.
 717                                         if(s[12] == 'A') {      // Airport record
 718                                                 apt_ident = s.substr(6, 4);
 719                                                 // Trim any whitespace from the ident.  The ident is left justified,
 720                                                 // so any space will be at the end.
 721                                                 if(apt_ident[3] == ' ') apt_ident = apt_ident.substr(0, 3);
 722                                                 // I think that all idents are either 3 or 4 chars - could check this though!
 723                                                 if(!apt_in_progress) {
 724                                                         last_apt_ident = apt_ident;
 725                                                         apt_in_progress = 1;
 726                                                 } else {
 727                                                         if(last_apt_ident != apt_ident) {
 728                                                                 if(iap_in_progress) {
 729                                                                         if(iap_error) {
 730                                                                                 //cout << "ERROR: Unable to load approach " << iap->_ident << " at " << iap->_aptIdent << '\n';
 731                                                                                 nErrors++;
 732                                                                         } else {
 733                                                                                 _np_iap[iap->_aptIdent].push_back(iap);
 734                                                                                 //cout << "** Loaded " << iap->_aptIdent << "\t" << iap->_ident << '\n';
 735                                                                                 nLoaded++;
 736                                                                         }
 737                                                                         iap_in_progress = false;
 738                                                                 }
 739                                                         }
 740                                                         last_apt_ident = apt_ident;
 741                                                 }
 742                                                 iap_in_progress = 0;
 743                                         } else if(s[12] == 'F') {       // Approach segment
 744                                                 if(apt_in_progress) {
 745                                                         iap_ident = s.substr(13, 6);
 746                                                         // Trim any whitespace from the RH end.
 747                                                         for(j=0; j<6; ++j) {
 748                                                                 if(iap_ident[5-j] == ' ') {
 749                                                                         iap_ident = iap_ident.substr(0, 5-j);
 750                                                                 } else {
 751                                                                         // It's important to break here, since earlier versions of ARINC 424 allowed spaces in the ident.
 752                                                                         break;
 753                                                                 }
 754                                                         }
 755                                                         if(iap_in_progress) {
 756                                                                 if(iap_ident != last_iap_ident) {
 757                                                                         // This is a new approach - store the last one and trigger
 758                                                                         // starting afresh by setting the in progress flag to false.
 759                                                                         if(iap_error) {
 760                                                                                 //cout << "ERROR: Unable to load approach " << iap->_ident << " at " << iap->_aptIdent << '\n';
 761                                                                                 nErrors++;
 762                                                                         } else {
 763                                                                                 _np_iap[iap->_aptIdent].push_back(iap);
 764                                                                                 //cout << "Loaded " << iap->_aptIdent << "\t" << iap->_ident << '\n';
 765                                                                                 nLoaded++;
 766                                                                         }
 767                                                                         iap_in_progress = false;
 768                                                                 }
 769                                                         }
 770                                                         if(!iap_in_progress) {
 771                                                                 iap = new FGNPIAP;
 772                                                                 iap->_aptIdent = apt_ident;
 773                                                                 iap->_ident = iap_ident;
 774                                                                 iap->_name = ExpandSIAPIdent(iap_ident); // I suspect that it's probably better to just store idents, and to expand the names as needed.
 775                                                                 // Note, we haven't set iap->_rwyStr yet.
 776                                                                 last_iap_ident = iap_ident;
 777                                                                 iap_in_progress = true;
 778                                                                 iap_error = false;
 779                                                         }
 780
 781                                                         // Route type
 782                                                         route_type = s[19];
 783                                                         sequence_number = atoi(s.substr(26,3).c_str());
 784                                                         wp_ident = s.substr(29, 5);
 785                                                         waypoint_fix_type = s[42];
 786                                                         // Trim any whitespace from the RH end
 787                                                         for(j=0; j<5; ++j) {
 788                                                                 if(wp_ident[4-j] == ' ') {
 789                                                                         wp_ident = wp_ident.substr(0, 4-j);
 790                                                                 } else {
 791                                                                         break;
 792                                                                 }
 793                                                         }
 794
 795                                                         // Ignore lines with no waypoint ID for now - these are normally part of the
 796                                                         // missed approach procedure, and we don't use them in the KLN89.
 797                                                         if(!wp_ident.empty()) {
 798                                                                 // Make a local copy of the waypoint for now, since we're not yet sure if we'll be using it
 799                                                                 GPSWaypoint w;
 800                                                                 w.id = wp_ident;
 801                                                                 bool wp_error = false;
 802                                                                 if(w.id.substr(0, 2) == "RW" && waypoint_fix_type == 'M') {
 803                                                                         // Assume that this is a missed-approach point based on the runway number, which appears to be standard for most approaches.
 804                                                                         // Note: Currently fgFindAirportID returns NULL on error, but getRunwayByIdent throws an exception.
 805                                                                         const FGAirport* apt = fgFindAirportID(iap->_aptIdent);
 806                                                                         if(apt) {
 807                                                                                 string rwystr;
 808                                                                                 try {
 809                                                                                         rwystr = w.id.substr(2, 2);
 810                                                                                         // TODO - sanity check the rwystr at this point to ensure we have a double digit number
 811                                                                                         if(w.id.size() > 4) {
 812                                                                                                 if(w.id[4] == 'L' || w.id[4] == 'C' || w.id[4] == 'R') {
 813                                                                                                         rwystr += w.id[4];
 814                                                                                                 }
 815                                                                                         }
 816                                                                                         FGRunway* rwy = apt->getRunwayByIdent(rwystr);
 817                                                                                         w.lat = rwy->begin().getLatitudeRad();
 818                                                                                         w.lon = rwy->begin().getLongitudeRad();
 819                                                                                 } catch(const sg_exception&) {
 820                                                                                         SG_LOG(SG_GENERAL, SG_WARN, "Unable to find runway " << w.id.substr(2, 2) << " at airport " << iap->_aptIdent);
 821                                                                                         //cout << "Unable to find runway " << w.id.substr(2, 2) << " at airport " << iap->_aptIdent << " ( w.id = " << w.id << ", rwystr = " << rwystr << " )\n";
 822                                                                                         wp_error = true;
 823                                                                                 }
 824                                                                         } else {
 825                                                                                 wp_error = true;
 826                                                                         }
 827                                                                 } else {
 828                                                                         cwp = FindFirstByExactId(w.id);
 829                                                                         if(cwp) {
 830                                                                                 w = *cwp;
 831                                                                         } else {
 832                                                                                 wp_error = true;
 833                                                                         }
 834                                                                 }
 835                                                                 switch(waypoint_fix_type) {
 836                                                                 case 'A': w.appType = GPS_IAF; break;
 837                                                                 case 'F': w.appType = GPS_FAF; break;
 838                                                                 case 'H': w.appType = GPS_MAHP; break;
 839                                                                 case 'I': w.appType = GPS_IAP; break;
 840                                                                 case 'M': w.appType = GPS_MAP; break;
 841                                                                 case ' ': w.appType = GPS_APP_NONE; break;
 842                                                                 //default: cout << "Unknown waypoint_fix_type: \'" << waypoint_fix_type << "\' [" << apt_ident << ", " << iap_ident << "]\n";
 843                                                                 }
 844
 845                                                                 if(wp_error) {
 846                                                                         //cout << "Unable to find waypoint " << w.id << " [" << apt_ident << ", " << iap_ident << "]\n";
 847                                                                         iap_error = true;
 848                                                                 }
 849
 850                                                                 if(!wp_error) {
 851                                                                         if(route_in_progress) {
 852                                                                                 if(sequence_number > last_sequence_number) {
 853                                                                                         // TODO - add a check for runway numbers
 854                                                                                         // Check for the waypoint ID being the same as the previous line.
 855                                                                                         // This is often the case for the missed approach holding point.
 856                                                                                         if(wp_ident == last_wp_ident) {
 857                                                                                                 if(waypoint_fix_type == 'H') {
 858                                                                                                         if(!iap->_IAP.empty()) {
 859                                                                                                                 if(iap->_IAP[iap->_IAP.size() - 1]->appType == GPS_APP_NONE) {
 860                                                                                                                         iap->_IAP[iap->_IAP.size() - 1]->appType = GPS_MAHP;
 861                                                                                                                 } else {
 862                                                                                                                         //cout << "Waypoint is MAHP and another type! " << w.id << " [" << apt_ident << ", " << iap_ident << "]\n";
 863                                                                                                                 }
 864                                                                                                         }
 865                                                                                                 }
 866                                                                                         } else {
 867                                                                                                 // Create a new waypoint on the heap, copy the local copy into it, and push it onto the approach.
 868                                                                                                 wp = new GPSWaypoint;
 869                                                                                                 *wp = w;
 870                                                                                                 if(route_type == 'A') {
 871                                                                                                         fp->waypoints.push_back(wp);
 872                                                                                                 } else {
 873                                                                                                         iap->_IAP.push_back(wp);
 874                                                                                                 }
 875                                                                                         }
 876                                                                                 } else if(sequence_number == last_sequence_number) {
 877                                                                                         // This seems to happen once per final approach route - one of the waypoints
 878                                                                                         // is duplicated with the same sequence number - I'm not sure what information
 879                                                                                         // the second line give yet so ignore it for now.
 880                                                                                         // TODO - figure this out!
 881                                                                                 } else {
 882                                                                                         // Finalise the current route and start a new one
 883                                                                                         //
 884                                                                                         // Finalise the current route
 885                                                                                         if(last_route_type == 'A') {
 886                                                                                                 // Push the flightplan onto the approach
 887                                                                                                 iap->_approachRoutes.push_back(fp);
 888                                                                                         } else {
 889                                                                                                 // All the waypoints get pushed individually - don't need to do it.
 890                                                                                         }
 891                                                                                         // Start a new one
 892                                                                                         // There are basically 2 possibilities here - either it's one of the arrival transitions,
 893                                                                                         // or it's the core final approach course.
 894                                                                                         wp = new GPSWaypoint;
 895                                                                                         *wp = w;
 896                                                                                         if(route_type == 'A') { // It's one of the arrival transition(s)
 897                                                                                                 fp = new GPSFlightPlan;
 898                                                                                                 fp->waypoints.push_back(wp);
 899                                                                                         } else {
 900                                                                                                 iap->_IAP.push_back(wp);
 901                                                                                         }
 902                                                                                         route_in_progress = true;
 903                                                                                 }
 904                                                                         } else {
 905                                                                                 // Start a new route.
 906                                                                                 // There are basically 2 possibilities here - either it's one of the arrival transitions,
 907                                                                                 // or it's the core final approach course.
 908                                                                                 wp = new GPSWaypoint;
 909                                                                                 *wp = w;
 910                                                                                 if(route_type == 'A') { // It's one of the arrival transition(s)
 911                                                                                         fp = new GPSFlightPlan;
 912                                                                                         fp->waypoints.push_back(wp);
 913                                                                                 } else {
 914                                                                                         iap->_IAP.push_back(wp);
 915                                                                                 }
 916                                                                                 route_in_progress = true;
 917                                                                         }
 918                                                                         last_route_type = route_type;
 919                                                                         last_wp_ident = wp_ident;
 920                                                                         last_sequence_number = sequence_number;
 921                                                                 }
 922                                                         }
 923                                                 } else {
 924                                                         // ERROR - no airport record read.
 925                                                 }
 926                                         }
 927                                 } else {
 928                                         // Check and finalise any approaches in progress
 929                                         // TODO - sanity check that the approach has all the required elements
 930                                         if(iap_in_progress) {
 931                                                 // This is a new approach - store the last one and trigger
 932                                                 // starting afresh by setting the in progress flag to false.
 933                                                 if(iap_error) {
 934                                                         //cout << "ERROR: Unable to load approach " << iap->_ident << " at " << iap->_aptIdent << '\n';
 935                                                         nErrors++;
 936                                                 } else {
 937                                                         _np_iap[iap->_aptIdent].push_back(iap);
 938                                                         //cout << "* Loaded " << iap->_aptIdent << "\t" << iap->_ident << '\n';
 939                                                         nLoaded++;
 940                                                 }
 941                                                 iap_in_progress = false;
 942                                         }
 943                                 }
 944                         }
 945                 }
 946         }
 947
 948         // If we get to the end of the file, load any approach that is still in progress
 949         // TODO - sanity check that the approach has all the required elements
 950         if(iap_in_progress) {
 951                 if(iap_error) {
 952                         //cout << "ERROR: Unable to load approach " << iap->_ident << " at " << iap->_aptIdent << '\n';
 953                         nErrors++;
 954                 } else {
 955                         _np_iap[iap->_aptIdent].push_back(iap);
 956                         //cout << "*** Loaded " << iap->_aptIdent << "\t" << iap->_ident << '\n';
 957                         nLoaded++;
 958                 }
 959         }
 960
 961         //cout << "Done loading approach database\n";
 962         //cout << "Loaded: " << nLoaded << '\n';
 963         //cout << "Failed: " << nErrors << '\n';
 964
 965         fin.close();
 966 }
 967
 968 GPSWaypoint* DCLGPS::GetActiveWaypoint() {
 969         return &_activeWaypoint;
 970 }
 971
 972 // Returns meters
 973 float DCLGPS::GetDistToActiveWaypoint() {
 974         return _dist2Act;
 975 }
 976
 977 // I don't yet fully understand all the gotchas about where to source time from.
 978 // This function sets the initial timer before the clock exports properties
 979 // and the one below uses the clock to be consistent with the rest of the code.
 980 // It might change soonish...
 981 void DCLGPS::SetPowerOnTimer() {
 982         struct tm *t = globals->get_time_params()->getGmt();
 983         _powerOnTime.set_hr(t->tm_hour);
 984         _powerOnTime.set_min(t->tm_min);
 985         _powerOnTimerSet = true;
 986 }
 987
 988 void DCLGPS::ResetPowerOnTimer() {
 989         _powerOnTime.set_hr(atoi(fgGetString("/instrumentation/clock/indicated-hour")));
 990         _powerOnTime.set_min(atoi(fgGetString("/instrumentation/clock/indicated-min")));
 991         _powerOnTimerSet = true;
 992 }
 993
 994 double DCLGPS::GetCDIDeflection() const {
 995         double xtd = CalcCrossTrackDeviation(); //nm
 996         return((xtd / _currentCdiScale) * 5.0 * 2.5 * -1.0);
 997 }
 998
 999 void DCLGPS::DtoInitiate(const string& s) {
1000         const GPSWaypoint* wp = FindFirstByExactId(s);
1001         if(wp) {
1002                 // TODO - Currently we start DTO operation unconditionally, regardless of which mode we are in.
1003                 // In fact, the following rules apply:
1004                 // In LEG mode, start DTO as we currently do.
1005                 // In OBS mode, set the active waypoint to the requested waypoint, and then:
1006                 // If the KLN89 is not connected to an external HSI or CDI, set the OBS course to go direct to the waypoint.
1007                 // If the KLN89 *is* connected to an external HSI or CDI, it cannot set the course itself, and will display
1008                 // a scratchpad message with the course to set manually on the HSI/CDI.
1009                 // In both OBS cases, leave _dto false, since we don't need the virtual waypoint created.
1010                 _dto = true;
1011                 _activeWaypoint = *wp;
1012                 _fromWaypoint.lat = _gpsLat;
1013                 _fromWaypoint.lon = _gpsLon;
1014                 _fromWaypoint.type = GPS_WP_VIRT;
1015                 _fromWaypoint.id = "DTOWP";
1016                 delete wp;
1017         } else {
1018                 // TODO - Should bring up the user waypoint page.
1019                 _dto = false;
1020         }
1021 }
1022
1023 void DCLGPS::DtoCancel() {
1024         if(_dto) {
1025                 // i.e. don't bother reorientating if we're just cancelling a DTO button press
1026                 // without having previously initiated DTO.
1027                 OrientateToActiveFlightPlan();
1028         }
1029         _dto = false;
1030 }
1031
1032 void DCLGPS::ToggleOBSMode() {
1033         _obsMode = !_obsMode;
1034         if(_obsMode) {
1035                 if(!_activeWaypoint.id.empty()) {
1036                         _obsHeading = static_cast<int>(_dtkMag);
1037                 }
1038                 // TODO - the _fromWaypoint location will change as the OBS heading changes.
1039                 // Might need to store the OBS initiation position somewhere in case it is needed again.
1040                 SetOBSFromWaypoint();
1041         }
1042 }
1043
1044 // Set the _fromWaypoint position based on the active waypoint and OBS radial.
1045 void DCLGPS::SetOBSFromWaypoint() {
1046         if(!_obsMode) return;
1047         if(_activeWaypoint.id.empty()) return;
1048
1049         // TODO - base the 180 deg correction on the to/from flag.
1050         _fromWaypoint = GetPositionOnMagRadial(_activeWaypoint, 10, _obsHeading + 180.0);
1051         _fromWaypoint.id = "OBSWP";
1052 }
1053
1054 void DCLGPS::CDIFSDIncrease() {
1055         if(_currentCdiScaleIndex == 0) {
1056                 _currentCdiScaleIndex = _cdiScales.size() - 1;
1057         } else {
1058                 _currentCdiScaleIndex--;
1059         }
1060 }
1061
1062 void DCLGPS::CDIFSDDecrease() {
1063         _currentCdiScaleIndex++;
1064         if(_currentCdiScaleIndex == _cdiScales.size()) {
1065                 _currentCdiScaleIndex = 0;
1066         }
1067 }
1068
1069 void DCLGPS::DrawChar(char c, int field, int px, int py, bool bold) {
1070 }
1071
1072 void DCLGPS::DrawText(const string& s, int field, int px, int py, bool bold) {
1073 }
1074
1075 void DCLGPS::SetBaroUnits(int n, bool wrap) {
1076         if(n < 1) {
1077                 _baroUnits = (GPSPressureUnits)(wrap ? 3 : 1);
1078         } else if(n > 3) {
1079                 _baroUnits = (GPSPressureUnits)(wrap ? 1 : 3);
1080         } else {
1081                 _baroUnits = (GPSPressureUnits)n;
1082         }
1083 }
1084
1085 void DCLGPS::CreateDefaultFlightPlans() {}
1086
1087 // Get the time to the active waypoint in seconds.
1088 // Returns -1 if groundspeed < 30 kts
1089 double DCLGPS::GetTimeToActiveWaypoint() {
1090         if(_groundSpeed_kts < 30.0) {
1091                 return(-1.0);
1092         } else {
1093                 return(_eta);
1094         }
1095 }
1096
1097 // Get the time to the final waypoint in seconds.
1098 // Returns -1 if groundspeed < 30 kts
1099 double DCLGPS::GetETE() {
1100         if(_groundSpeed_kts < 30.0) {
1101                 return(-1.0);
1102         } else {
1103                 // TODO - handle OBS / DTO operation appropriately
1104                 if(_activeFP->waypoints.empty()) {
1105                         return(-1.0);
1106                 } else {
1107                         return(GetTimeToWaypoint(_activeFP->waypoints[_activeFP->waypoints.size() - 1]->id));
1108                 }
1109         }
1110 }
1111
1112 // Get the time to a given waypoint (spec'd by ID) in seconds.
1113 // returns -1 if groundspeed is less than 30kts.
1114 // If the waypoint is an unreached part of the active flight plan the time will be via each leg.
1115 // otherwise it will be a direct-to time.
1116 double DCLGPS::GetTimeToWaypoint(const string& id) {
1117         if(_groundSpeed_kts < 30.0) {
1118                 return(-1.0);
1119         }
1120
1121         double eta = 0.0;
1122         int n1 = GetActiveWaypointIndex();
1123         int n2 = GetWaypointIndex(id);
1124         if(n2 > n1) {
1125                 eta = _eta;
1126                 for(unsigned int i=n1+1; i<_activeFP->waypoints.size(); ++i) {
1127                         GPSWaypoint* wp1 = _activeFP->waypoints[i-1];
1128                         GPSWaypoint* wp2 = _activeFP->waypoints[i];
1129                         double distm = GetGreatCircleDistance(wp1->lat, wp1->lon, wp2->lat, wp2->lon) * SG_NM_TO_METER;
1130                         eta += (distm / _groundSpeed_ms);
1131                 }
1132                 return(eta);
1133         } else if(id == _activeWaypoint.id) {
1134                 return(_eta);
1135         } else {
1136                 const GPSWaypoint* wp = FindFirstByExactId(id);
1137                 if(wp == NULL) return(-1.0);
1138                 double distm = GetGreatCircleDistance(_gpsLat, _gpsLon, wp->lat, wp->lon);
1139     delete wp;
1140                 return(distm / _groundSpeed_ms);
1141         }
1142         return(-1.0);   // Hopefully we never get here!
1143 }
1144
1145 // Returns magnetic great-circle heading
1146 // TODO - document units.
1147 float DCLGPS::GetHeadingToActiveWaypoint() {
1148         if(_activeWaypoint.id.empty()) {
1149                 return(-888);
1150         } else {
1151                 double h = GetMagHeadingFromTo(_gpsLat, _gpsLon, _activeWaypoint.lat, _activeWaypoint.lon);
1152                 while(h <= 0.0) h += 360.0;
1153                 while(h > 360.0) h -= 360.0;
1154                 return((float)h);
1155         }
1156 }
1157
1158 // Returns magnetic great-circle heading
1159 // TODO - what units?
1160 float DCLGPS::GetHeadingFromActiveWaypoint() {
1161         if(_activeWaypoint.id.empty()) {
1162                 return(-888);
1163         } else {
1164                 double h = GetMagHeadingFromTo(_activeWaypoint.lat, _activeWaypoint.lon, _gpsLat, _gpsLon);
1165                 while(h <= 0.0) h += 360.0;
1166                 while(h > 360.0) h -= 360.0;
1167                 return(h);
1168         }
1169 }
1170
1171 void DCLGPS::ClearFlightPlan(int n) {
1172         for(unsigned int i=0; i<_flightPlans[n]->waypoints.size(); ++i) {
1173                 delete _flightPlans[n]->waypoints[i];
1174         }
1175         _flightPlans[n]->waypoints.clear();
1176 }
1177
1178 void DCLGPS::ClearFlightPlan(GPSFlightPlan* fp) {
1179         for(unsigned int i=0; i<fp->waypoints.size(); ++i) {
1180                 delete fp->waypoints[i];
1181         }
1182         fp->waypoints.clear();
1183 }
1184
1185 int DCLGPS::GetActiveWaypointIndex() {
1186         for(unsigned int i=0; i<_flightPlans[0]->waypoints.size(); ++i) {
1187                 if(_flightPlans[0]->waypoints[i]->id == _activeWaypoint.id) return((int)i);
1188         }
1189         return(-1);
1190 }
1191
1192 int DCLGPS::GetWaypointIndex(const string& id) {
1193         for(unsigned int i=0; i<_flightPlans[0]->waypoints.size(); ++i) {
1194                 if(_flightPlans[0]->waypoints[i]->id == id) return((int)i);
1195         }
1196         return(-1);
1197 }
1198
1199 void DCLGPS::OrientateToFlightPlan(GPSFlightPlan* fp) {
1200         //cout << "Orientating...\n";
1201         //cout << "_lat = " << _lat << ", _lon = " << _lon << ", _gpsLat = " << _gpsLat << ", gpsLon = " << _gpsLon << '\n';
1202         if(fp->IsEmpty()) {
1203                 _activeWaypoint.id.clear();
1204                 _navFlagged = true;
1205         } else {
1206                 _navFlagged = false;
1207                 if(fp->waypoints.size() == 1) {
1208                         // TODO - may need to flag nav here if not dto or obs, or possibly handle it somewhere else.
1209                         _activeWaypoint = *fp->waypoints[0];
1210                         _fromWaypoint.id.clear();
1211                 } else {
1212                         // FIXME FIXME FIXME
1213                         _fromWaypoint = *fp->waypoints[0];
1214                         _activeWaypoint = *fp->waypoints[1];
1215                         double dmin = 1000000;  // nm!!
1216                         // For now we will simply start on the leg closest to our current position.
1217                         // It's possible that more fancy algorithms may take either heading or track
1218                         // into account when setting inital leg - I'm not sure.
1219                         // This method should handle most cases perfectly OK though.
1220                         for(unsigned int i = 1; i < fp->waypoints.size(); ++i) {
1221                                 //cout << "Pass " << i << ", dmin = " << dmin << ", leg is " << fp->waypoints[i-1]->id << " to " << fp->waypoints[i]->id << '\n';
1222                                 // First get the cross track correction.
1223                                 double d0 = fabs(CalcCrossTrackDeviation(*fp->waypoints[i-1], *fp->waypoints[i]));
1224                                 // That is the shortest distance away we could be though - check for
1225                                 // longer distances if we are 'off the end' of the leg.
1226                                 double ht1 = GetGreatCircleCourse(fp->waypoints[i-1]->lat, fp->waypoints[i-1]->lon,
1227                                                                   fp->waypoints[i]->lat, fp->waypoints[i]->lon)
1228                                                                                                   * SG_RADIANS_TO_DEGREES;
1229                                 // not simply the reverse of the above due to great circle navigation.
1230                                 double ht2 = GetGreatCircleCourse(fp->waypoints[i]->lat, fp->waypoints[i]->lon,
1231                                                                   fp->waypoints[i-1]->lat, fp->waypoints[i-1]->lon)
1232                                                                                                   * SG_RADIANS_TO_DEGREES;
1233                                 double hw1 = GetGreatCircleCourse(_gpsLat, _gpsLon,
1234                                                                   fp->waypoints[i]->lat, fp->waypoints[i]->lon)
1235                                                                                                   * SG_RADIANS_TO_DEGREES;
1236                                 double hw2 = GetGreatCircleCourse(_gpsLat, _gpsLon,
1237                                                                   fp->waypoints[i-1]->lat, fp->waypoints[i-1]->lon)
1238                                                                                                   * SG_RADIANS_TO_DEGREES;
1239                                 double h1 = ht1 - hw1;
1240                                 double h2 = ht2 - hw2;
1241                                 //cout << "d0, h1, h2 = " << d0 << ", " << h1 << ", " << h2 << '\n';
1242                                 //cout << "Normalizing...\n";
1243                                 SG_NORMALIZE_RANGE(h1, -180.0, 180.0);
1244                                 SG_NORMALIZE_RANGE(h2, -180.0, 180.0);
1245                                 //cout << "d0, h1, h2 = " << d0 << ", " << h1 << ", " << h2 << '\n';
1246                                 if(fabs(h1) > 90.0) {
1247                                         // We are past the end of the to waypoint
1248                                         double d = GetGreatCircleDistance(_gpsLat, _gpsLon, fp->waypoints[i]->lat, fp->waypoints[i]->lon);
1249                                         if(d > d0) d0 = d;
1250                                         //cout << "h1 triggered, d0 now = " << d0 << '\n';
1251                                 } else if(fabs(h2) > 90.0) {
1252                                         // We are past the end (not yet at!) the from waypoint
1253                                         double d = GetGreatCircleDistance(_gpsLat, _gpsLon, fp->waypoints[i-1]->lat, fp->waypoints[i-1]->lon);
1254                                         if(d > d0) d0 = d;
1255                                         //cout << "h2 triggered, d0 now = " << d0 << '\n';
1256                                 }
1257                                 if(d0 < dmin) {
1258                                         //cout << "THIS LEG NOW ACTIVE!\n";
1259                                         dmin = d0;
1260                                         _fromWaypoint = *fp->waypoints[i-1];
1261                                         _activeWaypoint = *fp->waypoints[i];
1262                                 }
1263                         }
1264                 }
1265         }
1266 }
1267
1268 void DCLGPS::OrientateToActiveFlightPlan() {
1269         OrientateToFlightPlan(_activeFP);
1270 }
1271
1272 /***************************************/
1273
1274 // Utility function - create a flightplan from a list of waypoint ids and types
1275 void DCLGPS::CreateFlightPlan(GPSFlightPlan* fp, vector<string> ids, vector<GPSWpType> wps) {
1276         if(fp == NULL) fp = new GPSFlightPlan;
1277         unsigned int i;
1278         if(!fp->waypoints.empty()) {
1279                 for(i=0; i<fp->waypoints.size(); ++i) {
1280                         delete fp->waypoints[i];
1281                 }
1282                 fp->waypoints.clear();
1283         }
1284         if(ids.size() != wps.size()) {
1285                 cout << "ID and Waypoint types list size mismatch in GPS::CreateFlightPlan - no flightplan created!\n";
1286                 return;
1287         }
1288         for(i=0; i<ids.size(); ++i) {
1289                 bool multi;
1290                 const FGAirport* ap;
1291                 FGNavRecord* np;
1292                 GPSWaypoint* wp = new GPSWaypoint;
1293                 wp->type = wps[i];
1294                 switch(wp->type) {
1295                 case GPS_WP_APT:
1296                         ap = FindFirstAptById(ids[i], multi, true);
1297                         if(ap == NULL) {
1298                                 // error
1299                                 delete wp;
1300                         } else {
1301                                 wp->lat = ap->getLatitude() * SG_DEGREES_TO_RADIANS;
1302                                 wp->lon = ap->getLongitude() * SG_DEGREES_TO_RADIANS;
1303                                 wp->id = ids[i];
1304                                 fp->waypoints.push_back(wp);
1305                         }
1306                         break;
1307                 case GPS_WP_VOR:
1308                         np = FindFirstVorById(ids[i], multi, true);
1309                         if(np == NULL) {
1310                                 // error
1311                                 delete wp;
1312                         } else {
1313                                 wp->lat = np->get_lat() * SG_DEGREES_TO_RADIANS;
1314                                 wp->lon = np->get_lon() * SG_DEGREES_TO_RADIANS;
1315                                 wp->id = ids[i];
1316                                 fp->waypoints.push_back(wp);
1317                         }
1318                         break;
1319                 case GPS_WP_NDB:
1320                         np = FindFirstNDBById(ids[i], multi, true);
1321                         if(np == NULL) {
1322                                 // error
1323                                 delete wp;
1324                         } else {
1325                                 wp->lat = np->get_lat() * SG_DEGREES_TO_RADIANS;
1326                                 wp->lon = np->get_lon() * SG_DEGREES_TO_RADIANS;
1327                                 wp->id = ids[i];
1328                                 fp->waypoints.push_back(wp);
1329                         }
1330                         break;
1331                 case GPS_WP_INT:
1332                         // TODO TODO
1333                         break;
1334                 case GPS_WP_USR:
1335                         // TODO
1336                         break;
1337                 case GPS_WP_VIRT:
1338                         // TODO
1339                         break;
1340                 }
1341         }
1342 }
1343
1344 /***************************************/
1345
1346 class DCLGPSFilter : public FGPositioned::Filter
1347 {
1348 public:
1349   virtual bool pass(const FGPositioned* aPos) const {
1350     switch (aPos->type()) {
1351     case FGPositioned::AIRPORT:
1352     // how about heliports and seaports?
1353     case FGPositioned::NDB:
1354     case FGPositioned::VOR:
1355     case FGPositioned::WAYPOINT:
1356     case FGPositioned::FIX:
1357       break;
1358     default: return false; // reject all other types
1359     }
1360     return true;
1361   }
1362 };
1363
1364 GPSWaypoint* DCLGPS::FindFirstById(const string& id) const
1365 {
1366   DCLGPSFilter filter;
1367   FGPositionedRef result = FGPositioned::findNextWithPartialId(NULL, id, &filter);
1368   return GPSWaypoint::createFromPositioned(result);
1369 }
1370
1371 GPSWaypoint* DCLGPS::FindFirstByExactId(const string& id) const
1372 {
1373   SGGeod pos(SGGeod::fromRad(_lon, _lat));
1374   FGPositionedRef result = FGPositioned::findClosestWithIdent(id, pos);
1375   return GPSWaypoint::createFromPositioned(result);
1376 }
1377
1378 // TODO - add the ASCII / alphabetical stuff from the Atlas version
1379 FGPositioned* DCLGPS::FindTypedFirstById(const string& id, FGPositioned::Type ty, bool &multi, bool exact)
1380 {
1381   multi = false;
1382   FGPositioned::TypeFilter filter(ty);
1383
1384   if (exact) {
1385     FGPositioned::List matches =
1386       FGPositioned::findAllWithIdent(id, &filter);
1387     FGPositioned::sortByRange(matches, SGGeod::fromRad(_lon, _lat));
1388     multi = (matches.size() > 1);
1389     return matches.empty() ? NULL : matches.front().ptr();
1390   }
1391
1392   return FGPositioned::findNextWithPartialId(NULL, id, &filter);
1393 }
1394
1395 FGNavRecord* DCLGPS::FindFirstVorById(const string& id, bool &multi, bool exact)
1396 {
1397   return dynamic_cast<FGNavRecord*>(FindTypedFirstById(id, FGPositioned::VOR, multi, exact));
1398 }
1399
1400 FGNavRecord* DCLGPS::FindFirstNDBById(const string& id, bool &multi, bool exact)
1401 {
1402   return dynamic_cast<FGNavRecord*>(FindTypedFirstById(id, FGPositioned::NDB, multi, exact));
1403 }
1404
1405 const FGFix* DCLGPS::FindFirstIntById(const string& id, bool &multi, bool exact)
1406 {
1407   return dynamic_cast<FGFix*>(FindTypedFirstById(id, FGPositioned::FIX, multi, exact));
1408 }
1409
1410 const FGAirport* DCLGPS::FindFirstAptById(const string& id, bool &multi, bool exact)
1411 {
1412   return dynamic_cast<FGAirport*>(FindTypedFirstById(id, FGPositioned::AIRPORT, multi, exact));
1413 }
1414
1415 FGNavRecord* DCLGPS::FindClosestVor(double lat_rad, double lon_rad) {
1416   FGPositioned::TypeFilter filter(FGPositioned::VOR);
1417   double cutoff = 1000; // nautical miles
1418   FGPositionedRef v = FGPositioned::findClosest(SGGeod::fromRad(lon_rad, lat_rad), cutoff, &filter);
1419   if (!v) {
1420     return NULL;
1421   }
1422
1423   return dynamic_cast<FGNavRecord*>(v.ptr());
1424 }
1425
1426 //----------------------------------------------------------------------------------------------------------
1427
1428 // Takes lat and lon in RADIANS!!!!!!!
1429 double DCLGPS::GetMagHeadingFromTo(double latA, double lonA, double latB, double lonB) {
1430         double h = GetGreatCircleCourse(latA, lonA, latB, lonB);
1431         h *= SG_RADIANS_TO_DEGREES;
1432         // TODO - use the real altitude below instead of 0.0!
1433         //cout << "MagVar = " << sgGetMagVar(_gpsLon, _gpsLat, 0.0, _time->getJD()) * SG_RADIANS_TO_DEGREES << '\n';
1434         h -= sgGetMagVar(_gpsLon, _gpsLat, 0.0, _time->getJD()) * SG_RADIANS_TO_DEGREES;
1435         while(h >= 360.0) h -= 360.0;
1436         while(h < 0.0) h += 360.0;
1437         return(h);
1438 }
1439
1440 // ---------------- Great Circle formulae from "The Aviation Formulary" -------------
1441 // Note that all of these assume that the world is spherical.
1442
1443 double Rad2Nm(double theta) {
1444         return(((180.0*60.0)/SG_PI)*theta);
1445 }
1446
1447 double Nm2Rad(double d) {
1448         return((SG_PI/(180.0*60.0))*d);
1449 }
1450
1451 /* QUOTE:
1452
1453 The great circle distance d between two points with coordinates {lat1,lon1} and {lat2,lon2} is given by:
1454
1455 d=acos(sin(lat1)*sin(lat2)+cos(lat1)*cos(lat2)*cos(lon1-lon2))
1456
1457 A mathematically equivalent formula, which is less subject to rounding error for short distances is:
1458
1459 d=2*asin(sqrt((sin((lat1-lat2)/2))^2 +
1460                  cos(lat1)*cos(lat2)*(sin((lon1-lon2)/2))^2))
1461
1462 */
1463
1464 // Returns distance in nm, takes lat & lon in RADIANS
1465 double DCLGPS::GetGreatCircleDistance(double lat1, double lon1, double lat2, double lon2) const {
1466         double d = 2.0 * asin(sqrt(((sin((lat1-lat2)/2.0))*(sin((lat1-lat2)/2.0))) +
1467                    cos(lat1)*cos(lat2)*(sin((lon1-lon2)/2.0))*(sin((lon1-lon2)/2.0))));
1468         return(Rad2Nm(d));
1469 }
1470
1471 // fmod dosen't do what we want :-(
1472 static double mod(double d1, double d2) {
1473         return(d1 - d2*floor(d1/d2));
1474 }
1475
1476 // Returns great circle course from point 1 to point 2
1477 // Input and output in RADIANS.
1478 double DCLGPS::GetGreatCircleCourse (double lat1, double lon1, double lat2, double lon2) const {
1479         //double h = 0.0;
1480         /*
1481         // Special case the poles
1482         if(cos(lat1) < SG_EPSILON) {
1483                 if(lat1 > 0) {
1484                         // Starting from North Pole
1485                         h = SG_PI;
1486                 } else {
1487                         // Starting from South Pole
1488                         h = 2.0 * SG_PI;
1489                 }
1490         } else {
1491                 // Urgh - the formula below is for negative lon +ve !!!???
1492                 double d = GetGreatCircleDistance(lat1, lon1, lat2, lon2);
1493                 cout << "d = " << d;
1494                 d = Nm2Rad(d);
1495                 //cout << ", d_theta = " << d;
1496                 //cout << ", and d = " << Rad2Nm(d) << ' ';
1497                 if(sin(lon2 - lon1) < 0) {
1498                         cout << " A ";
1499                         h = acos((sin(lat2)-sin(lat1)*cos(d))/(sin(d)*cos(lat1)));
1500                 } else {
1501                         cout << " B ";
1502                         h = 2.0 * SG_PI - acos((sin(lat2)-sin(lat1)*cos(d))/(sin(d)*cos(lat1)));
1503                 }
1504         }
1505         cout << h * SG_RADIANS_TO_DEGREES << '\n';
1506         */
1507
1508         return( mod(atan2(sin(lon2-lon1)*cos(lat2),
1509             cos(lat1)*sin(lat2)-sin(lat1)*cos(lat2)*cos(lon2-lon1)),
1510             2.0*SG_PI) );
1511 }
1512
1513 // Return a position on a radial from wp1 given distance d (nm) and magnetic heading h (degrees)
1514 // Note that d should be less that 1/4 Earth diameter!
1515 GPSWaypoint DCLGPS::GetPositionOnMagRadial(const GPSWaypoint& wp1, double d, double h) {
1516         h += sgGetMagVar(wp1.lon, wp1.lat, 0.0, _time->getJD()) * SG_RADIANS_TO_DEGREES;
1517         return(GetPositionOnRadial(wp1, d, h));
1518 }
1519
1520 // Return a position on a radial from wp1 given distance d (nm) and TRUE heading h (degrees)
1521 // Note that d should be less that 1/4 Earth diameter!
1522 GPSWaypoint DCLGPS::GetPositionOnRadial(const GPSWaypoint& wp1, double d, double h) {
1523         while(h < 0.0) h += 360.0;
1524         while(h > 360.0) h -= 360.0;
1525
1526         h *= SG_DEGREES_TO_RADIANS;
1527         d *= (SG_PI / (180.0 * 60.0));
1528
1529         double lat=asin(sin(wp1.lat)*cos(d)+cos(wp1.lat)*sin(d)*cos(h));
1530         double lon;
1531         if(cos(lat)==0) {
1532                 lon=wp1.lon;      // endpoint a pole
1533         } else {
1534                 lon=mod(wp1.lon+asin(sin(h)*sin(d)/cos(lat))+SG_PI,2*SG_PI)-SG_PI;
1535         }
1536
1537         GPSWaypoint wp;
1538         wp.lat = lat;
1539         wp.lon = lon;
1540         wp.type = GPS_WP_VIRT;
1541         return(wp);
1542 }
1543
1544 // Returns cross-track deviation in Nm.
1545 double DCLGPS::CalcCrossTrackDeviation() const {
1546         return(CalcCrossTrackDeviation(_fromWaypoint, _activeWaypoint));
1547 }
1548
1549 // Returns cross-track deviation of the current position between two arbitary waypoints in nm.
1550 double DCLGPS::CalcCrossTrackDeviation(const GPSWaypoint& wp1, const GPSWaypoint& wp2) const {
1551         //if(wp1 == NULL || wp2 == NULL) return(0.0);
1552         if(wp1.id.empty() || wp2.id.empty()) return(0.0);
1553         double xtd = asin(sin(Nm2Rad(GetGreatCircleDistance(wp1.lat, wp1.lon, _gpsLat, _gpsLon)))
1554                           * sin(GetGreatCircleCourse(wp1.lat, wp1.lon, _gpsLat, _gpsLon) - GetGreatCircleCourse(wp1.lat, wp1.lon, wp2.lat, wp2.lon)));
1555         return(Rad2Nm(xtd));
1556 }