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/timing/sg_time.hxx>
  30 #include <simgear/magvar/magvar.hxx>
  31
  32 #include <Main/fg_props.hxx>
  33 #include <Navaids/fix.hxx>
  34 #include <Navaids/navrecord.hxx>
  35 #include <Airports/simple.hxx>
  36 #include <Airports/runways.hxx>
  37
  38 #include <iostream>
  39
  40 using namespace std;
  41
  42 GPSWaypoint::GPSWaypoint() {
  43     appType = GPS_APP_NONE;
  44 }
  45
  46 GPSWaypoint::GPSWaypoint(const std::string& aIdent, float aLat, float aLon, GPSWpType aType) :
  47   id(aIdent),
  48   lat(aLat),
  49   lon(aLon),
  50   type(aType),
  51   appType(GPS_APP_NONE)
  52 {
  53 }
  54
  55 GPSWaypoint::~GPSWaypoint() {}
  56
  57 string GPSWaypoint::GetAprId() {
  58         if(appType == GPS_IAF) return(id + 'i');
  59         else if(appType == GPS_FAF) return(id + 'f');
  60         else if(appType == GPS_MAP) return(id + 'm');
  61         else if(appType == GPS_MAHP) return(id + 'h');
  62         else return(id);
  63 }
  64
  65 static GPSWpType
  66 GPSWpTypeFromFGPosType(FGPositioned::Type aType)
  67 {
  68   switch (aType) {
  69   case FGPositioned::AIRPORT:
  70   case FGPositioned::SEAPORT:
  71   case FGPositioned::HELIPORT:
  72     return GPS_WP_APT;
  73
  74   case FGPositioned::VOR:
  75     return GPS_WP_VOR;
  76
  77   case FGPositioned::NDB:
  78     return GPS_WP_NDB;
  79
  80   case FGPositioned::WAYPOINT:
  81     return GPS_WP_USR;
  82
  83   case FGPositioned::FIX:
  84     return GPS_WP_INT;
  85
  86   default:
  87     return GPS_WP_USR;
  88   }
  89 }
  90
  91 GPSWaypoint* GPSWaypoint::createFromPositioned(const FGPositioned* aPos)
  92 {
  93   if (!aPos) {
  94     return NULL; // happens if find returns no match
  95   }
  96
  97   return new GPSWaypoint(aPos->ident(),
  98     aPos->latitude() * SG_DEGREES_TO_RADIANS,
  99     aPos->longitude() * SG_DEGREES_TO_RADIANS,
 100     GPSWpTypeFromFGPosType(aPos->type())
 101   );
 102 }
 103
 104 ostream& operator << (ostream& os, GPSAppWpType type) {
 105         switch(type) {
 106                 case(GPS_IAF):       return(os << "IAF");
 107                 case(GPS_IAP):       return(os << "IAP");
 108                 case(GPS_FAF):       return(os << "FAF");
 109                 case(GPS_MAP):       return(os << "MAP");
 110                 case(GPS_MAHP):      return(os << "MAHP");
 111                 case(GPS_HDR):       return(os << "HEADER");
 112                 case(GPS_FENCE):     return(os << "FENCE");
 113                 case(GPS_APP_NONE):  return(os << "NONE");
 114         }
 115         return(os << "ERROR - Unknown switch in GPSAppWpType operator << ");
 116 }
 117
 118 FGIAP::FGIAP() {
 119 }
 120
 121 FGIAP::~FGIAP() {
 122 }
 123
 124 FGNPIAP::FGNPIAP() {
 125 }
 126
 127 FGNPIAP::~FGNPIAP() {
 128 }
 129
 130 ClockTime::ClockTime() {
 131     _hr = 0;
 132     _min = 0;
 133 }
 134
 135 ClockTime::ClockTime(int hr, int min) {
 136     while(hr < 0) { hr += 24; }
 137     _hr = hr % 24;
 138     while(min < 0) { min += 60; }
 139     while(min > 60) { min -= 60; }
 140         _min = min;
 141 }
 142
 143 ClockTime::~ClockTime() {
 144 }
 145
 146 // ------------------------------------------------------------------------------------- //
 147
 148 DCLGPS::DCLGPS(RenderArea2D* instrument) {
 149         _instrument = instrument;
 150         _nFields = 1;
 151         _maxFields = 2;
 152
 153         // Units - lets default to US units - FG can set them to other units from config during startup if desired.
 154         _altUnits = GPS_ALT_UNITS_FT;
 155         _baroUnits = GPS_PRES_UNITS_IN;
 156         _velUnits = GPS_VEL_UNITS_KT;
 157         _distUnits = GPS_DIST_UNITS_NM;
 158
 159         _lon_node = fgGetNode("/instrumentation/gps/indicated-longitude-deg", true);
 160         _lat_node = fgGetNode("/instrumentation/gps/indicated-latitude-deg", true);
 161         _alt_node = fgGetNode("/instrumentation/gps/indicated-altitude-ft", true);
 162         _grnd_speed_node = fgGetNode("/instrumentation/gps/indicated-ground-speed-kt", true);
 163         _true_track_node = fgGetNode("/instrumentation/gps/indicated-track-true-deg", true);
 164         _mag_track_node = fgGetNode("/instrumentation/gps/indicated-track-magnetic-deg", true);
 165
 166         // Use FG's position values at construction in case FG's gps has not run first update yet.
 167         _lon = fgGetDouble("/position/longitude-deg") * SG_DEGREES_TO_RADIANS;
 168         _lat = fgGetDouble("/position/latitude-deg") * SG_DEGREES_TO_RADIANS;
 169         _alt = fgGetDouble("/position/altitude-ft");
 170         // Note - we can depriciate _gpsLat and _gpsLon if we implement error handling in FG
 171         // gps code and not our own.
 172         _gpsLon = _lon;
 173         _gpsLat = _lat;
 174         _checkLon = _gpsLon;
 175         _checkLat = _gpsLat;
 176         _groundSpeed_ms = 0.0;
 177         _groundSpeed_kts = 0.0;
 178         _track = 0.0;
 179         _magTrackDeg = 0.0;
 180
 181         // Sensible defaults.  These can be overriden by derived classes if desired.
 182         _cdiScales.clear();
 183         _cdiScales.push_back(5.0);
 184         _cdiScales.push_back(1.0);
 185         _cdiScales.push_back(0.3);
 186         _currentCdiScaleIndex = 0;
 187         _targetCdiScaleIndex = 0;
 188         _sourceCdiScaleIndex = 0;
 189         _cdiScaleTransition = false;
 190         _currentCdiScale = 5.0;
 191
 192         _cleanUpPage = -1;
 193
 194         _activeWaypoint.id.clear();
 195         _dist2Act = 0.0;
 196         _crosstrackDist = 0.0;
 197         _headingBugTo = true;
 198         _navFlagged = true;
 199         _waypointAlert = false;
 200         _departed = false;
 201         _departureTimeString = "----";
 202         _elapsedTime = 0.0;
 203         _powerOnTime.set_hr(0);
 204         _powerOnTime.set_min(0);
 205         _powerOnTimerSet = false;
 206         _alarmSet = false;
 207
 208         // Configuration Initialisation
 209         // Should this be in kln89.cxx ?
 210         _turnAnticipationEnabled = false;
 211         _suaAlertEnabled = false;
 212         _altAlertEnabled = false;
 213
 214         _time = new SGTime;
 215
 216         _messageStack.clear();
 217
 218         _dto = false;
 219
 220         _approachLoaded = false;
 221         _approachArm = false;
 222         _approachReallyArmed = false;
 223         _approachActive = false;
 224         _approachFP = new GPSFlightPlan;
 225 }
 226
 227 DCLGPS::~DCLGPS() {
 228         delete _time;
 229   delete _approachFP;           // Don't need to delete the waypoints inside since they point to
 230                                                         // the waypoints in the approach database.
 231         // TODO - may need to delete the approach database!!
 232 }
 233
 234 void DCLGPS::draw(osg::State& state) {
 235         _instrument->draw(state);
 236 }
 237
 238 void DCLGPS::init() {
 239
 240         // Not sure if this should be here, but OK for now.
 241         CreateDefaultFlightPlans();
 242
 243         // Hack - hardwire some instrument approaches for development.
 244         // These will shortly be replaced by a routine to read ARINC data from file instead.
 245         FGNPIAP* iap;
 246         GPSWaypoint* wp;
 247         GPSFlightPlan* fp;
 248         const GPSWaypoint* cwp;
 249
 250         iap = new FGNPIAP;
 251         iap->_aptIdent = "KHAF";
 252         iap->_ident = "R12-Y";
 253         iap->_name = ExpandSIAPIdent(iap->_ident);
 254         iap->_rwyStr = "12";
 255         iap->_approachRoutes.clear();
 256         iap->_IAP.clear();
 257         // -------
 258         wp = new GPSWaypoint;
 259         wp->id = "GOBBS";
 260         // Nasty using the find any function here, but it saves converting data from FGFix etc.
 261         cwp = FindFirstByExactId(wp->id);
 262         if(cwp) {
 263                 *wp = *cwp;
 264                 wp->appType = GPS_IAF;
 265                 fp = new GPSFlightPlan;
 266                 fp->waypoints.push_back(wp);
 267         } else {
 268                 //cout << "Unable to find waypoint " << wp->id << '\n';
 269         }
 270         // -------
 271         wp = new GPSWaypoint;
 272         wp->id = "FUJCE";
 273         cwp = FindFirstByExactId(wp->id);
 274         if(cwp) {
 275                 *wp = *cwp;
 276                 wp->appType = GPS_IAP;
 277                 fp->waypoints.push_back(wp);
 278                 iap->_approachRoutes.push_back(fp);
 279                 iap->_IAP.push_back(wp);
 280         } else {
 281                 //cout << "Unable to find waypoint " << wp->id << '\n';
 282         }
 283         // -------
 284         wp = new GPSWaypoint;
 285         wp->id = "JEVXY";
 286         cwp = FindFirstByExactId(wp->id);
 287         if(cwp) {
 288                 *wp = *cwp;
 289                 wp->appType = GPS_FAF;
 290                 iap->_IAP.push_back(wp);
 291         } else {
 292                 //cout << "Unable to find waypoint " << wp->id << '\n';
 293         }
 294         // -------
 295         wp = new GPSWaypoint;
 296         wp->id = "RW12";
 297         wp->appType = GPS_MAP;
 298         if(wp->id.substr(0, 2) == "RW" && wp->appType == GPS_MAP) {
 299                 // Assume that this is a missed-approach point based on the runway number, which appears to be standard for most approaches.
 300                 const FGAirport* apt = fgFindAirportID(iap->_aptIdent);
 301                 if(apt) {
 302                         // TODO - sanity check the waypoint ID to ensure we have a double digit number
 303                         FGRunway* rwy = apt->getRunwayByIdent(wp->id.substr(2, 2));
 304                         if(rwy) {
 305                                 wp->lat = rwy->begin().getLatitudeRad();
 306                                 wp->lon = rwy->begin().getLongitudeRad();
 307                         }
 308                 }
 309         } else {
 310                 cwp = FindFirstByExactId(wp->id);
 311                 if(cwp) {
 312                         *wp = *cwp;
 313                         wp->appType = GPS_MAP;
 314                 } else {
 315                         //cout << "Unable to find waypoint " << wp->id << '\n';
 316                 }
 317         }
 318         iap->_IAP.push_back(wp);
 319         // -------
 320         wp = new GPSWaypoint;
 321         wp->id = "SEEMS";
 322         cwp = FindFirstByExactId(wp->id);
 323         if(cwp) {
 324                 *wp = *cwp;
 325                 wp->appType = GPS_MAHP;
 326                 iap->_IAP.push_back(wp);
 327         } else {
 328                 //cout << "Unable to find waypoint " << wp->id << '\n';
 329         }
 330         // -------
 331         _np_iap[iap->_aptIdent].push_back(iap);
 332 }
 333
 334 void DCLGPS::bind() {
 335         fgTie("/instrumentation/gps/waypoint-alert", this, &DCLGPS::GetWaypointAlert);
 336         fgTie("/instrumentation/gps/leg-mode", this, &DCLGPS::GetLegMode);
 337         fgTie("/instrumentation/gps/obs-mode", this, &DCLGPS::GetOBSMode);
 338         fgTie("/instrumentation/gps/approach-arm", this, &DCLGPS::GetApproachArm);
 339         fgTie("/instrumentation/gps/approach-active", this, &DCLGPS::GetApproachActive);
 340         fgTie("/instrumentation/gps/cdi-deflection", this, &DCLGPS::GetCDIDeflection);
 341         fgTie("/instrumentation/gps/to-flag", this, &DCLGPS::GetToFlag);
 342 }
 343
 344 void DCLGPS::unbind() {
 345         fgUntie("/instrumentation/gps/waypoint-alert");
 346         fgUntie("/instrumentation/gps/leg-mode");
 347         fgUntie("/instrumentation/gps/obs-mode");
 348         fgUntie("/instrumentation/gps/approach-arm");
 349         fgUntie("/instrumentation/gps/approach-active");
 350         fgUntie("/instrumentation/gps/cdi-deflection");
 351 }
 352
 353 void DCLGPS::update(double dt) {
 354         //cout << "update called!\n";
 355
 356         _lon = _lon_node->getDoubleValue() * SG_DEGREES_TO_RADIANS;
 357         _lat = _lat_node->getDoubleValue() * SG_DEGREES_TO_RADIANS;
 358         _alt = _alt_node->getDoubleValue();
 359         _groundSpeed_kts = _grnd_speed_node->getDoubleValue();
 360         _groundSpeed_ms = _groundSpeed_kts * 0.5144444444;
 361         _track = _true_track_node->getDoubleValue();
 362         _magTrackDeg = _mag_track_node->getDoubleValue();
 363         // Note - we can depriciate _gpsLat and _gpsLon if we implement error handling in FG
 364         // gps code and not our own.
 365         _gpsLon = _lon;
 366         _gpsLat = _lat;
 367         // Check for abnormal position slew
 368         if(GetGreatCircleDistance(_gpsLat, _gpsLon, _checkLat, _checkLon) > 1.0) {
 369                 OrientateToActiveFlightPlan();
 370         }
 371         _checkLon = _gpsLon;
 372         _checkLat = _gpsLat;
 373
 374         // TODO - check for unit power before running this.
 375         if(!_powerOnTimerSet) {
 376                 SetPowerOnTimer();
 377         }
 378
 379         // Check if an alarm timer has expired
 380         if(_alarmSet) {
 381                 if(_alarmTime.hr() == atoi(fgGetString("/instrumentation/clock/indicated-hour"))
 382                 && _alarmTime.min() == atoi(fgGetString("/instrumentation/clock/indicated-min"))) {
 383                         _messageStack.push_back("*Timer Expired");
 384                         _alarmSet = false;
 385                 }
 386         }
 387
 388         if(!_departed) {
 389                 if(_groundSpeed_kts > 30.0) {
 390                         _departed = true;
 391                         string th = fgGetString("/instrumentation/clock/indicated-hour");
 392                         string tm = fgGetString("/instrumentation/clock/indicated-min");
 393                         if(th.size() == 1) th = "0" + th;
 394                         if(tm.size() == 1) tm = "0" + tm;
 395                         _departureTimeString = th + tm;
 396                 }
 397         } else {
 398                 // TODO - check - is this prone to drift error over time?
 399                 // Should we difference the departure and current times?
 400                 // What about when the user resets the time of day from the menu?
 401                 _elapsedTime += dt;
 402         }
 403
 404         _time->update(_gpsLon * SG_DEGREES_TO_RADIANS, _gpsLat * SG_DEGREES_TO_RADIANS, 0, 0);
 405         // FIXME - currently all the below assumes leg mode and no DTO or OBS cancelled.
 406         if(_activeFP->IsEmpty()) {
 407                 // Not sure if we need to reset these each update or only when fp altered
 408                 _activeWaypoint.id.clear();
 409                 _navFlagged = true;
 410         } else if(_activeFP->waypoints.size() == 1) {
 411                 _activeWaypoint.id.clear();
 412         } else {
 413                 _navFlagged = false;
 414                 if(_activeWaypoint.id.empty() || _fromWaypoint.id.empty()) {
 415                         //cout << "Error, in leg mode with flightplan of 2 or more waypoints, but either active or from wp is NULL!\n";
 416                         OrientateToActiveFlightPlan();
 417                 }
 418
 419                 // Approach stuff
 420                 if(_approachLoaded) {
 421                         if(!_approachReallyArmed && !_approachActive) {
 422                                 // arm if within 30nm of airport.
 423                                 // TODO - let user cancel approach arm using external GPS-APR switch
 424                                 bool multi;
 425                                 const FGAirport* ap = FindFirstAptById(_approachID, multi, true);
 426                                 if(ap != NULL) {
 427                                         double d = GetGreatCircleDistance(_gpsLat, _gpsLon, ap->getLatitude() * SG_DEGREES_TO_RADIANS, ap->getLongitude() * SG_DEGREES_TO_RADIANS);
 428                                         if(d <= 30) {
 429                                                 _approachArm = true;
 430                                                 _approachReallyArmed = true;
 431                                                 _messageStack.push_back("*Press ALT To Set Baro");
 432                                                 // Not sure what we do if the user has already set CDI to 0.3 nm?
 433                                                 _targetCdiScaleIndex = 1;
 434                                                 if(_currentCdiScaleIndex == 1) {
 435                                                         // No problem!
 436                                                 } else if(_currentCdiScaleIndex == 0) {
 437                                                         _sourceCdiScaleIndex = 0;
 438                                                         _cdiScaleTransition = true;
 439                                                         _cdiTransitionTime = 30.0;
 440                                                         _currentCdiScale = _cdiScales[_currentCdiScaleIndex];
 441                                                 }
 442                                         }
 443                                 }
 444                         } else {
 445                                 // Check for approach active - we can only activate approach if it is really armed.
 446                                 if(_activeWaypoint.appType == GPS_FAF) {
 447                                         //cout << "Active waypoint is FAF, id is " << _activeWaypoint.id << '\n';
 448                                         if(GetGreatCircleDistance(_gpsLat, _gpsLon, _activeWaypoint.lat, _activeWaypoint.lon) <= 2.0 && !_obsMode) {
 449                                                 // Assume heading is OK for now
 450                                                 _approachArm = false;   // TODO - check - maybe arm is left on when actv comes on?
 451                                                 _approachReallyArmed = false;
 452                                                 _approachActive = true;
 453                                                 _targetCdiScaleIndex = 2;
 454                                                 if(_currentCdiScaleIndex == 2) {
 455                                                         // No problem!
 456                                                 } else if(_currentCdiScaleIndex == 1) {
 457                                                         _sourceCdiScaleIndex = 1;
 458                                                         _cdiScaleTransition = true;
 459                                                         _cdiTransitionTime = 30.0;      // TODO - compress it if time to FAF < 30sec
 460                                                         _currentCdiScale = _cdiScales[_currentCdiScaleIndex];
 461                                                 } else {
 462                                                         // Abort going active?
 463                                                         _approachActive = false;
 464                                                 }
 465                                         }
 466                                 }
 467                         }
 468                 }
 469
 470                 // CDI scale transition stuff
 471                 if(_cdiScaleTransition) {
 472                         if(fabs(_currentCdiScale - _cdiScales[_targetCdiScaleIndex]) < 0.001) {
 473                                 _currentCdiScale = _cdiScales[_targetCdiScaleIndex];
 474                                 _currentCdiScaleIndex = _targetCdiScaleIndex;
 475                                 _cdiScaleTransition = false;
 476                         } else {
 477                                 double scaleDiff = (_targetCdiScaleIndex > _sourceCdiScaleIndex
 478                                                     ? _cdiScales[_sourceCdiScaleIndex] - _cdiScales[_targetCdiScaleIndex]
 479                                                                         : _cdiScales[_targetCdiScaleIndex] - _cdiScales[_sourceCdiScaleIndex]);
 480                                 //cout << "ScaleDiff = " << scaleDiff << '\n';
 481                                 if(_targetCdiScaleIndex > _sourceCdiScaleIndex) {
 482                                         // Scaling down eg. 5nm -> 1nm
 483                                         _currentCdiScale -= (scaleDiff * dt / _cdiTransitionTime);
 484                                         if(_currentCdiScale < _cdiScales[_targetCdiScaleIndex]) {
 485                                                 _currentCdiScale = _cdiScales[_targetCdiScaleIndex];
 486                                                 _currentCdiScaleIndex = _targetCdiScaleIndex;
 487                                                 _cdiScaleTransition = false;
 488                                         }
 489                                 } else {
 490                                         _currentCdiScale += (scaleDiff * dt / _cdiTransitionTime);
 491                                         if(_currentCdiScale > _cdiScales[_targetCdiScaleIndex]) {
 492                                                 _currentCdiScale = _cdiScales[_targetCdiScaleIndex];
 493                                                 _currentCdiScaleIndex = _targetCdiScaleIndex;
 494                                                 _cdiScaleTransition = false;
 495                                         }
 496                                 }
 497                                 //cout << "_currentCdiScale = " << _currentCdiScale << '\n';
 498                         }
 499                 } else {
 500                         _currentCdiScale = _cdiScales[_currentCdiScaleIndex];
 501                 }
 502
 503
 504                 // Urgh - I've been setting the heading bug based on DTK,
 505                 // bug I think it should be based on heading re. active waypoint
 506                 // based on what the sim does after the final waypoint is passed.
 507                 // (DTK remains the same, but if track is held == DTK heading bug
 508                 // reverses to from once wp is passed).
 509                 /*
 510                 if(_fromWaypoint != NULL) {
 511                         // TODO - how do we handle the change of track with distance over long legs?
 512                         _dtkTrue = GetGreatCircleCourse(_fromWaypoint->lat, _fromWaypoint->lon, _activeWaypoint->lat, _activeWaypoint->lon) * SG_RADIANS_TO_DEGREES;
 513                         _dtkMag = GetMagHeadingFromTo(_fromWaypoint->lat, _fromWaypoint->lon, _activeWaypoint->lat, _activeWaypoint->lon);
 514                         // Don't change the heading bug if speed is too low otherwise it flickers to/from at rest
 515                         if(_groundSpeed_ms > 5) {
 516                                 //cout << "track = " << _track << ", dtk = " << _dtkTrue << '\n';
 517                                 double courseDev = _track - _dtkTrue;
 518                                 //cout << "courseDev = " << courseDev << ", normalized = ";
 519                                 SG_NORMALIZE_RANGE(courseDev, -180.0, 180.0);
 520                                 //cout << courseDev << '\n';
 521                                 _headingBugTo = (fabs(courseDev) > 90.0 ? false : true);
 522                         }
 523                 } else {
 524                         _dtkTrue = 0.0;
 525                         _dtkMag = 0.0;
 526                         // TODO - in DTO operation the position of initiation of DTO defines the "from waypoint".
 527                 }
 528                 */
 529                 if(!_activeWaypoint.id.empty()) {
 530                         double hdgTrue = GetGreatCircleCourse(_gpsLat, _gpsLon, _activeWaypoint.lat, _activeWaypoint.lon) * SG_RADIANS_TO_DEGREES;
 531                         if(_groundSpeed_ms > 5) {
 532                                 //cout << "track = " << _track << ", hdgTrue = " << hdgTrue << '\n';
 533                                 double courseDev = _track - hdgTrue;
 534                                 //cout << "courseDev = " << courseDev << ", normalized = ";
 535                                 SG_NORMALIZE_RANGE(courseDev, -180.0, 180.0);
 536                                 //cout << courseDev << '\n';
 537                                 _headingBugTo = (fabs(courseDev) > 90.0 ? false : true);
 538                         }
 539                         if(!_fromWaypoint.id.empty()) {
 540                                 _dtkTrue = GetGreatCircleCourse(_fromWaypoint.lat, _fromWaypoint.lon, _activeWaypoint.lat, _activeWaypoint.lon) * SG_RADIANS_TO_DEGREES;
 541                                 _dtkMag = GetMagHeadingFromTo(_fromWaypoint.lat, _fromWaypoint.lon, _activeWaypoint.lat, _activeWaypoint.lon);
 542                         } else {
 543                                 _dtkTrue = 0.0;
 544                                 _dtkMag = 0.0;
 545                         }
 546                 }
 547
 548                 _dist2Act = GetGreatCircleDistance(_gpsLat, _gpsLon, _activeWaypoint.lat, _activeWaypoint.lon) * SG_NM_TO_METER;
 549                 if(_groundSpeed_ms > 10.0) {
 550                         _eta = _dist2Act / _groundSpeed_ms;
 551                         if(_eta <= 36) {        // TODO - this is slightly different if turn anticipation is enabled.
 552                                 if(_headingBugTo) {
 553                                         _waypointAlert = true;  // TODO - not if the from flag is set.
 554                                 }
 555                         }
 556                         if(_eta < 60) {
 557                                 // Check if we should sequence to next leg.
 558                                 // Perhaps this should be done on distance instead, but 60s time (about 1 - 2 nm) seems reasonable for now.
 559                                 //double reverseHeading = GetGreatCircleCourse(_activeWaypoint->lat, _activeWaypoint->lon, _fromWaypoint->lat, _fromWaypoint->lon);
 560                                 // Hack - let's cheat and do it on heading bug for now.  TODO - that stops us 'cutting the corner'
 561                                 // when we happen to approach the inside turn of a waypoint - we should probably sequence at the midpoint
 562                                 // of the heading difference between legs in this instance.
 563                                 int idx = GetActiveWaypointIndex();
 564                                 bool finalLeg = (idx == (int)(_activeFP->waypoints.size()) - 1 ? true : false);
 565                                 bool finalDto = (_dto && idx == -1);    // Dto operation to a waypoint not in the flightplan - we don't sequence in this instance
 566                                 if(!_headingBugTo) {
 567                                         if(finalLeg) {
 568                                                 // Do nothing - not sure if Dto should switch off when arriving at the final waypoint of a flightplan
 569                                         } else if(finalDto) {
 570                                                 // Do nothing
 571                                         } else if(_activeWaypoint.appType == GPS_MAP) {
 572                                                 // Don't sequence beyond the missed approach point
 573                                                 //cout << "ACTIVE WAYPOINT is MAP - not sequencing!!!!!\n";
 574                                         } else {
 575                                                 //cout << "Sequencing...\n";
 576                                                 _fromWaypoint = _activeWaypoint;
 577                                                 _activeWaypoint = *_activeFP->waypoints[idx + 1];
 578                                                 _dto = false;
 579                                                 // TODO - course alteration message format is dependent on whether we are slaved HSI/CDI indicator or not.
 580                                                 // For now assume we are not.
 581                                                 string s;
 582                                                 if(fgGetBool("/instrumentation/nav[0]/slaved-to-gps")) {
 583                                                         // TODO - avoid the hardwiring on nav[0]
 584                                                         s = "Adj Nav Crs to ";
 585                                                 } else {
 586                                                         string s = "GPS Course is ";
 587                                                 }
 588                                                 double d = GetMagHeadingFromTo(_fromWaypoint.lat, _fromWaypoint.lon, _activeWaypoint.lat, _activeWaypoint.lon);
 589                                                 while(d < 0.0) d += 360.0;
 590                                                 while(d >= 360.0) d -= 360.0;
 591                                                 char buf[4];
 592                                                 snprintf(buf, 4, "%03i", (int)(d + 0.5));
 593                                                 s += buf;
 594                                                 _messageStack.push_back(s);
 595                                         }
 596                                         _waypointAlert = false;
 597                                 }
 598                         }
 599                 } else {
 600                         _eta = 0.0;
 601                 }
 602
 603                 /*
 604                 // First attempt at a sensible cross-track correction calculation
 605                 // Uh? - I think this is implemented further down the file!
 606                 if(_fromWaypoint != NULL) {
 607
 608                 } else {
 609                         _crosstrackDist = 0.0;
 610                 }
 611                 */
 612         }
 613 }
 614
 615 /*
 616         Expand a SIAP ident to the full procedure name (as shown on the approach chart).
 617         NOTE: Some of this is inferred from data, some is from documentation.
 618
 619         Example expansions from ARINC 424-18 [and the airport they're taken from]:
 620         "R10LY" <--> "RNAV (GPS) Y RWY 10 L"    [KBOI]
 621         "R10-Z" <--> "RNAV (GPS) Z RWY 10"              [KHTO]
 622         "S25"   <--> "VOR or GPS RWY 25"                [KHHR]
 623         "P20"   <--> "GPS RWY 20"                               [KDAN]
 624         "NDB-B" <--> "NDB or GPS-B"                             [KDAW]
 625         "NDBC"  <--> "NDB or GPS-C"                             [KEMT]
 626         "VDMA"  <--> "VOR/DME or GPS-A"                 [KDAW]
 627         "VDM-A" <--> "VOR/DME or GPS-A"                 [KEAG]
 628         "VDMB"  <--> "VOR/DME or GPS-B"                 [KDKX]
 629         "VORA"  <--> "VOR or GPS-A"                             [KEMT]
 630
 631         It seems that there are 2 basic types of expansions; those that include
 632         the runway and those that don't.  Of those that don't, it seems that 2
 633         different positions within the string to encode the identifying letter
 634         are used, i.e. with a dash and without.
 635 */
 636 string DCLGPS::ExpandSIAPIdent(const string& ident) {
 637         string name;
 638         bool has_rwy = false;
 639
 640         switch(ident[0]) {
 641         case 'N': name = "NDB or GPS"; has_rwy = false; break;
 642         case 'P': name = "GPS"; has_rwy = true; break;
 643         case 'R': name = "RNAV (GPS)"; has_rwy = true; break;
 644         case 'S': name = "VOR or GPS"; has_rwy = true; break;
 645         case 'V':
 646                 if(ident[1] == 'D') name = "VOR/DME or GPS";
 647                 else name = "VOR or GPS";
 648                 has_rwy = false;
 649                 break;
 650         default: // TODO output a log message
 651                 break;
 652         }
 653
 654         if(has_rwy) {
 655                 // Add the identifying letter if present
 656                 if(ident.size() == 5) {
 657                         name += ' ';
 658                         name += ident[4];
 659                 }
 660
 661                 // Add the runway
 662                 name += " RWY ";
 663                 name += ident.substr(1, 2);
 664
 665                 // Add a left/right/centre indication if present.
 666                 if(ident.size() > 3) {
 667                         if((ident[3] != '-') && (ident[3] != ' ')) {    // Early versions of the spec allowed a blank instead of a dash so check for both
 668                                 name += ' ';
 669                                 name += ident[3];
 670                         }
 671                 }
 672         } else {
 673                 // Add the identifying letter, which I *think* should always be present, but seems to be inconsistent as to whether a dash is used.
 674                 if(ident.size() == 5) {
 675                         name += '-';
 676                         name += ident[4];
 677                 } else if(ident.size() == 4) {
 678                         name += '-';
 679                         name += ident[3];
 680                 } else {
 681                         // No suffix letter
 682                 }
 683         }
 684
 685         return(name);
 686 }
 687
 688 GPSWaypoint* DCLGPS::GetActiveWaypoint() {
 689         return &_activeWaypoint;
 690 }
 691
 692 // Returns meters
 693 float DCLGPS::GetDistToActiveWaypoint() {
 694         return _dist2Act;
 695 }
 696
 697 // I don't yet fully understand all the gotchas about where to source time from.
 698 // This function sets the initial timer before the clock exports properties
 699 // and the one below uses the clock to be consistent with the rest of the code.
 700 // It might change soonish...
 701 void DCLGPS::SetPowerOnTimer() {
 702         struct tm *t = globals->get_time_params()->getGmt();
 703         _powerOnTime.set_hr(t->tm_hour);
 704         _powerOnTime.set_min(t->tm_min);
 705         _powerOnTimerSet = true;
 706 }
 707
 708 void DCLGPS::ResetPowerOnTimer() {
 709         _powerOnTime.set_hr(atoi(fgGetString("/instrumentation/clock/indicated-hour")));
 710         _powerOnTime.set_min(atoi(fgGetString("/instrumentation/clock/indicated-min")));
 711         _powerOnTimerSet = true;
 712 }
 713
 714 double DCLGPS::GetCDIDeflection() const {
 715         double xtd = CalcCrossTrackDeviation(); //nm
 716         return((xtd / _currentCdiScale) * 5.0 * 2.5 * -1.0);
 717 }
 718
 719 void DCLGPS::DtoInitiate(const string& s) {
 720         const GPSWaypoint* wp = FindFirstByExactId(s);
 721         if(wp) {
 722                 // TODO - Currently we start DTO operation unconditionally, regardless of which mode we are in.
 723                 // In fact, the following rules apply:
 724                 // In LEG mode, start DTO as we currently do.
 725                 // In OBS mode, set the active waypoint to the requested waypoint, and then:
 726                 // If the KLN89 is not connected to an external HSI or CDI, set the OBS course to go direct to the waypoint.
 727                 // If the KLN89 *is* connected to an external HSI or CDI, it cannot set the course itself, and will display
 728                 // a scratchpad message with the course to set manually on the HSI/CDI.
 729                 // In both OBS cases, leave _dto false, since we don't need the virtual waypoint created.
 730                 _dto = true;
 731                 _activeWaypoint = *wp;
 732                 _fromWaypoint.lat = _gpsLat;
 733                 _fromWaypoint.lon = _gpsLon;
 734                 _fromWaypoint.type = GPS_WP_VIRT;
 735                 _fromWaypoint.id = "DTOWP";
 736                 delete wp;
 737         } else {
 738                 _dto = false;
 739         }
 740 }
 741
 742 void DCLGPS::DtoCancel() {
 743         if(_dto) {
 744                 // i.e. don't bother reorientating if we're just cancelling a DTO button press
 745                 // without having previously initiated DTO.
 746                 OrientateToActiveFlightPlan();
 747         }
 748         _dto = false;
 749 }
 750
 751 void DCLGPS::ToggleOBSMode() {
 752         _obsMode = !_obsMode;
 753         if(_obsMode) {
 754                 if(!_activeWaypoint.id.empty()) {
 755                         _obsHeading = static_cast<int>(_dtkMag);
 756                 }
 757                 // TODO - the _fromWaypoint location will change as the OBS heading changes.
 758                 // Might need to store the OBS initiation position somewhere in case it is needed again.
 759                 SetOBSFromWaypoint();
 760         }
 761 }
 762
 763 // Set the _fromWaypoint position based on the active waypoint and OBS radial.
 764 void DCLGPS::SetOBSFromWaypoint() {
 765         if(!_obsMode) return;
 766         if(_activeWaypoint.id.empty()) return;
 767
 768         // TODO - base the 180 deg correction on the to/from flag.
 769         _fromWaypoint = GetPositionOnMagRadial(_activeWaypoint, 10, _obsHeading + 180.0);
 770         _fromWaypoint.id = "OBSWP";
 771 }
 772
 773 void DCLGPS::CDIFSDIncrease() {
 774         if(_currentCdiScaleIndex == 0) {
 775                 _currentCdiScaleIndex = _cdiScales.size() - 1;
 776         } else {
 777                 _currentCdiScaleIndex--;
 778         }
 779 }
 780
 781 void DCLGPS::CDIFSDDecrease() {
 782         _currentCdiScaleIndex++;
 783         if(_currentCdiScaleIndex == _cdiScales.size()) {
 784                 _currentCdiScaleIndex = 0;
 785         }
 786 }
 787
 788 void DCLGPS::DrawChar(char c, int field, int px, int py, bool bold) {
 789 }
 790
 791 void DCLGPS::DrawText(const string& s, int field, int px, int py, bool bold) {
 792 }
 793
 794 void DCLGPS::SetBaroUnits(int n, bool wrap) {
 795         if(n < 1) {
 796                 _baroUnits = (GPSPressureUnits)(wrap ? 3 : 1);
 797         } else if(n > 3) {
 798                 _baroUnits = (GPSPressureUnits)(wrap ? 1 : 3);
 799         } else {
 800                 _baroUnits = (GPSPressureUnits)n;
 801         }
 802 }
 803
 804 void DCLGPS::CreateDefaultFlightPlans() {}
 805
 806 // Get the time to the active waypoint in seconds.
 807 // Returns -1 if groundspeed < 30 kts
 808 double DCLGPS::GetTimeToActiveWaypoint() {
 809         if(_groundSpeed_kts < 30.0) {
 810                 return(-1.0);
 811         } else {
 812                 return(_eta);
 813         }
 814 }
 815
 816 // Get the time to the final waypoint in seconds.
 817 // Returns -1 if groundspeed < 30 kts
 818 double DCLGPS::GetETE() {
 819         if(_groundSpeed_kts < 30.0) {
 820                 return(-1.0);
 821         } else {
 822                 // TODO - handle OBS / DTO operation appropriately
 823                 if(_activeFP->waypoints.empty()) {
 824                         return(-1.0);
 825                 } else {
 826                         return(GetTimeToWaypoint(_activeFP->waypoints[_activeFP->waypoints.size() - 1]->id));
 827                 }
 828         }
 829 }
 830
 831 // Get the time to a given waypoint (spec'd by ID) in seconds.
 832 // returns -1 if groundspeed is less than 30kts.
 833 // If the waypoint is an unreached part of the active flight plan the time will be via each leg.
 834 // otherwise it will be a direct-to time.
 835 double DCLGPS::GetTimeToWaypoint(const string& id) {
 836         if(_groundSpeed_kts < 30.0) {
 837                 return(-1.0);
 838         }
 839
 840         double eta = 0.0;
 841         int n1 = GetActiveWaypointIndex();
 842         int n2 = GetWaypointIndex(id);
 843         if(n2 > n1) {
 844                 eta = _eta;
 845                 for(unsigned int i=n1+1; i<_activeFP->waypoints.size(); ++i) {
 846                         GPSWaypoint* wp1 = _activeFP->waypoints[i-1];
 847                         GPSWaypoint* wp2 = _activeFP->waypoints[i];
 848                         double distm = GetGreatCircleDistance(wp1->lat, wp1->lon, wp2->lat, wp2->lon) * SG_NM_TO_METER;
 849                         eta += (distm / _groundSpeed_ms);
 850                 }
 851                 return(eta);
 852         } else if(id == _activeWaypoint.id) {
 853                 return(_eta);
 854         } else {
 855                 const GPSWaypoint* wp = FindFirstByExactId(id);
 856                 if(wp == NULL) return(-1.0);
 857                 double distm = GetGreatCircleDistance(_gpsLat, _gpsLon, wp->lat, wp->lon);
 858     delete wp;
 859                 return(distm / _groundSpeed_ms);
 860         }
 861         return(-1.0);   // Hopefully we never get here!
 862 }
 863
 864 // Returns magnetic great-circle heading
 865 // TODO - document units.
 866 float DCLGPS::GetHeadingToActiveWaypoint() {
 867         if(_activeWaypoint.id.empty()) {
 868                 return(-888);
 869         } else {
 870                 double h = GetMagHeadingFromTo(_gpsLat, _gpsLon, _activeWaypoint.lat, _activeWaypoint.lon);
 871                 while(h <= 0.0) h += 360.0;
 872                 while(h > 360.0) h -= 360.0;
 873                 return((float)h);
 874         }
 875 }
 876
 877 // Returns magnetic great-circle heading
 878 // TODO - what units?
 879 float DCLGPS::GetHeadingFromActiveWaypoint() {
 880         if(_activeWaypoint.id.empty()) {
 881                 return(-888);
 882         } else {
 883                 double h = GetMagHeadingFromTo(_activeWaypoint.lat, _activeWaypoint.lon, _gpsLat, _gpsLon);
 884                 while(h <= 0.0) h += 360.0;
 885                 while(h > 360.0) h -= 360.0;
 886                 return(h);
 887         }
 888 }
 889
 890 void DCLGPS::ClearFlightPlan(int n) {
 891         for(unsigned int i=0; i<_flightPlans[n]->waypoints.size(); ++i) {
 892                 delete _flightPlans[n]->waypoints[i];
 893         }
 894         _flightPlans[n]->waypoints.clear();
 895 }
 896
 897 void DCLGPS::ClearFlightPlan(GPSFlightPlan* fp) {
 898         for(unsigned int i=0; i<fp->waypoints.size(); ++i) {
 899                 delete fp->waypoints[i];
 900         }
 901         fp->waypoints.clear();
 902 }
 903
 904 int DCLGPS::GetActiveWaypointIndex() {
 905         for(unsigned int i=0; i<_flightPlans[0]->waypoints.size(); ++i) {
 906                 if(_flightPlans[0]->waypoints[i]->id == _activeWaypoint.id) return((int)i);
 907         }
 908         return(-1);
 909 }
 910
 911 int DCLGPS::GetWaypointIndex(const string& id) {
 912         for(unsigned int i=0; i<_flightPlans[0]->waypoints.size(); ++i) {
 913                 if(_flightPlans[0]->waypoints[i]->id == id) return((int)i);
 914         }
 915         return(-1);
 916 }
 917
 918 void DCLGPS::OrientateToFlightPlan(GPSFlightPlan* fp) {
 919         //cout << "Orientating...\n";
 920         //cout << "_lat = " << _lat << ", _lon = " << _lon << ", _gpsLat = " << _gpsLat << ", gpsLon = " << _gpsLon << '\n';
 921         if(fp->IsEmpty()) {
 922                 _activeWaypoint.id.clear();
 923                 _navFlagged = true;
 924         } else {
 925                 _navFlagged = false;
 926                 if(fp->waypoints.size() == 1) {
 927                         // TODO - may need to flag nav here if not dto or obs, or possibly handle it somewhere else.
 928                         _activeWaypoint = *fp->waypoints[0];
 929                         _fromWaypoint.id.clear();
 930                 } else {
 931                         // FIXME FIXME FIXME
 932                         _fromWaypoint = *fp->waypoints[0];
 933                         _activeWaypoint = *fp->waypoints[1];
 934                         double dmin = 1000000;  // nm!!
 935                         // For now we will simply start on the leg closest to our current position.
 936                         // It's possible that more fancy algorithms may take either heading or track
 937                         // into account when setting inital leg - I'm not sure.
 938                         // This method should handle most cases perfectly OK though.
 939                         for(unsigned int i = 1; i < fp->waypoints.size(); ++i) {
 940                                 //cout << "Pass " << i << ", dmin = " << dmin << ", leg is " << fp->waypoints[i-1]->id << " to " << fp->waypoints[i]->id << '\n';
 941                                 // First get the cross track correction.
 942                                 double d0 = fabs(CalcCrossTrackDeviation(*fp->waypoints[i-1], *fp->waypoints[i]));
 943                                 // That is the shortest distance away we could be though - check for
 944                                 // longer distances if we are 'off the end' of the leg.
 945                                 double ht1 = GetGreatCircleCourse(fp->waypoints[i-1]->lat, fp->waypoints[i-1]->lon,
 946                                                                   fp->waypoints[i]->lat, fp->waypoints[i]->lon)
 947                                                                                                   * SG_RADIANS_TO_DEGREES;
 948                                 // not simply the reverse of the above due to great circle navigation.
 949                                 double ht2 = GetGreatCircleCourse(fp->waypoints[i]->lat, fp->waypoints[i]->lon,
 950                                                                   fp->waypoints[i-1]->lat, fp->waypoints[i-1]->lon)
 951                                                                                                   * SG_RADIANS_TO_DEGREES;
 952                                 double hw1 = GetGreatCircleCourse(_gpsLat, _gpsLon,
 953                                                                   fp->waypoints[i]->lat, fp->waypoints[i]->lon)
 954                                                                                                   * SG_RADIANS_TO_DEGREES;
 955                                 double hw2 = GetGreatCircleCourse(_gpsLat, _gpsLon,
 956                                                                   fp->waypoints[i-1]->lat, fp->waypoints[i-1]->lon)
 957                                                                                                   * SG_RADIANS_TO_DEGREES;
 958                                 double h1 = ht1 - hw1;
 959                                 double h2 = ht2 - hw2;
 960                                 //cout << "d0, h1, h2 = " << d0 << ", " << h1 << ", " << h2 << '\n';
 961                                 //cout << "Normalizing...\n";
 962                                 SG_NORMALIZE_RANGE(h1, -180.0, 180.0);
 963                                 SG_NORMALIZE_RANGE(h2, -180.0, 180.0);
 964                                 //cout << "d0, h1, h2 = " << d0 << ", " << h1 << ", " << h2 << '\n';
 965                                 if(fabs(h1) > 90.0) {
 966                                         // We are past the end of the to waypoint
 967                                         double d = GetGreatCircleDistance(_gpsLat, _gpsLon, fp->waypoints[i]->lat, fp->waypoints[i]->lon);
 968                                         if(d > d0) d0 = d;
 969                                         //cout << "h1 triggered, d0 now = " << d0 << '\n';
 970                                 } else if(fabs(h2) > 90.0) {
 971                                         // We are past the end (not yet at!) the from waypoint
 972                                         double d = GetGreatCircleDistance(_gpsLat, _gpsLon, fp->waypoints[i-1]->lat, fp->waypoints[i-1]->lon);
 973                                         if(d > d0) d0 = d;
 974                                         //cout << "h2 triggered, d0 now = " << d0 << '\n';
 975                                 }
 976                                 if(d0 < dmin) {
 977                                         //cout << "THIS LEG NOW ACTIVE!\n";
 978                                         dmin = d0;
 979                                         _fromWaypoint = *fp->waypoints[i-1];
 980                                         _activeWaypoint = *fp->waypoints[i];
 981                                 }
 982                         }
 983                 }
 984         }
 985 }
 986
 987 void DCLGPS::OrientateToActiveFlightPlan() {
 988         OrientateToFlightPlan(_activeFP);
 989 }
 990
 991 /***************************************/
 992
 993 // Utility function - create a flightplan from a list of waypoint ids and types
 994 void DCLGPS::CreateFlightPlan(GPSFlightPlan* fp, vector<string> ids, vector<GPSWpType> wps) {
 995         if(fp == NULL) fp = new GPSFlightPlan;
 996         unsigned int i;
 997         if(!fp->waypoints.empty()) {
 998                 for(i=0; i<fp->waypoints.size(); ++i) {
 999                         delete fp->waypoints[i];
1000                 }
1001                 fp->waypoints.clear();
1002         }
1003         if(ids.size() != wps.size()) {
1004                 cout << "ID and Waypoint types list size mismatch in GPS::CreateFlightPlan - no flightplan created!\n";
1005                 return;
1006         }
1007         for(i=0; i<ids.size(); ++i) {
1008                 bool multi;
1009                 const FGAirport* ap;
1010                 FGNavRecord* np;
1011                 GPSWaypoint* wp = new GPSWaypoint;
1012                 wp->type = wps[i];
1013                 switch(wp->type) {
1014                 case GPS_WP_APT:
1015                         ap = FindFirstAptById(ids[i], multi, true);
1016                         if(ap == NULL) {
1017                                 // error
1018                                 delete wp;
1019                         } else {
1020                                 wp->lat = ap->getLatitude() * SG_DEGREES_TO_RADIANS;
1021                                 wp->lon = ap->getLongitude() * SG_DEGREES_TO_RADIANS;
1022                                 wp->id = ids[i];
1023                                 fp->waypoints.push_back(wp);
1024                         }
1025                         break;
1026                 case GPS_WP_VOR:
1027                         np = FindFirstVorById(ids[i], multi, true);
1028                         if(np == NULL) {
1029                                 // error
1030                                 delete wp;
1031                         } else {
1032                                 wp->lat = np->get_lat() * SG_DEGREES_TO_RADIANS;
1033                                 wp->lon = np->get_lon() * SG_DEGREES_TO_RADIANS;
1034                                 wp->id = ids[i];
1035                                 fp->waypoints.push_back(wp);
1036                         }
1037                         break;
1038                 case GPS_WP_NDB:
1039                         np = FindFirstNDBById(ids[i], multi, true);
1040                         if(np == NULL) {
1041                                 // error
1042                                 delete wp;
1043                         } else {
1044                                 wp->lat = np->get_lat() * SG_DEGREES_TO_RADIANS;
1045                                 wp->lon = np->get_lon() * SG_DEGREES_TO_RADIANS;
1046                                 wp->id = ids[i];
1047                                 fp->waypoints.push_back(wp);
1048                         }
1049                         break;
1050                 case GPS_WP_INT:
1051                         // TODO TODO
1052                         break;
1053                 case GPS_WP_USR:
1054                         // TODO
1055                         break;
1056                 case GPS_WP_VIRT:
1057                         // TODO
1058                         break;
1059                 }
1060         }
1061 }
1062
1063 /***************************************/
1064
1065 class DCLGPSFilter : public FGPositioned::Filter
1066 {
1067 public:
1068   virtual bool pass(const FGPositioned* aPos) const {
1069     switch (aPos->type()) {
1070     case FGPositioned::AIRPORT:
1071     // how about heliports and seaports?
1072     case FGPositioned::NDB:
1073     case FGPositioned::VOR:
1074     case FGPositioned::WAYPOINT:
1075     case FGPositioned::FIX:
1076       break;
1077     default: return false; // reject all other types
1078     }
1079     return true;
1080   }
1081 };
1082
1083 GPSWaypoint* DCLGPS::FindFirstById(const string& id) const
1084 {
1085   DCLGPSFilter filter;
1086   FGPositionedRef result = FGPositioned::findNextWithPartialId(NULL, id, &filter);
1087   return GPSWaypoint::createFromPositioned(result);
1088 }
1089
1090 GPSWaypoint* DCLGPS::FindFirstByExactId(const string& id) const
1091 {
1092   SGGeod pos(SGGeod::fromRad(_lon, _lat));
1093   FGPositionedRef result = FGPositioned::findClosestWithIdent(id, pos);
1094   return GPSWaypoint::createFromPositioned(result);
1095 }
1096
1097 // TODO - add the ASCII / alphabetical stuff from the Atlas version
1098 FGPositioned* DCLGPS::FindTypedFirstById(const string& id, FGPositioned::Type ty, bool &multi, bool exact)
1099 {
1100   multi = false;
1101   FGPositioned::TypeFilter filter(ty);
1102
1103   if (exact) {
1104     FGPositioned::List matches =
1105       FGPositioned::findAllWithIdent(id, &filter);
1106     FGPositioned::sortByRange(matches, SGGeod::fromRad(_lon, _lat));
1107     multi = (matches.size() > 1);
1108     return matches.empty() ? NULL : matches.front().ptr();
1109   }
1110
1111   return FGPositioned::findNextWithPartialId(NULL, id, &filter);
1112 }
1113
1114 FGNavRecord* DCLGPS::FindFirstVorById(const string& id, bool &multi, bool exact)
1115 {
1116   return dynamic_cast<FGNavRecord*>(FindTypedFirstById(id, FGPositioned::VOR, multi, exact));
1117 }
1118
1119 FGNavRecord* DCLGPS::FindFirstNDBById(const string& id, bool &multi, bool exact)
1120 {
1121   return dynamic_cast<FGNavRecord*>(FindTypedFirstById(id, FGPositioned::NDB, multi, exact));
1122 }
1123
1124 const FGFix* DCLGPS::FindFirstIntById(const string& id, bool &multi, bool exact)
1125 {
1126   return dynamic_cast<FGFix*>(FindTypedFirstById(id, FGPositioned::FIX, multi, exact));
1127 }
1128
1129 const FGAirport* DCLGPS::FindFirstAptById(const string& id, bool &multi, bool exact)
1130 {
1131   return dynamic_cast<FGAirport*>(FindTypedFirstById(id, FGPositioned::AIRPORT, multi, exact));
1132 }
1133
1134 FGNavRecord* DCLGPS::FindClosestVor(double lat_rad, double lon_rad) {
1135   FGPositioned::TypeFilter filter(FGPositioned::VOR);
1136   double cutoff = 1000; // nautical miles
1137   FGPositionedRef v = FGPositioned::findClosest(SGGeod::fromRad(lon_rad, lat_rad), cutoff, &filter);
1138   if (!v) {
1139     return NULL;
1140   }
1141
1142   return dynamic_cast<FGNavRecord*>(v.ptr());
1143 }
1144
1145 //----------------------------------------------------------------------------------------------------------
1146
1147 // Takes lat and lon in RADIANS!!!!!!!
1148 double DCLGPS::GetMagHeadingFromTo(double latA, double lonA, double latB, double lonB) {
1149         double h = GetGreatCircleCourse(latA, lonA, latB, lonB);
1150         h *= SG_RADIANS_TO_DEGREES;
1151         // TODO - use the real altitude below instead of 0.0!
1152         //cout << "MagVar = " << sgGetMagVar(_gpsLon, _gpsLat, 0.0, _time->getJD()) * SG_RADIANS_TO_DEGREES << '\n';
1153         h -= sgGetMagVar(_gpsLon, _gpsLat, 0.0, _time->getJD()) * SG_RADIANS_TO_DEGREES;
1154         while(h >= 360.0) h -= 360.0;
1155         while(h < 0.0) h += 360.0;
1156         return(h);
1157 }
1158
1159 // ---------------- Great Circle formulae from "The Aviation Formulary" -------------
1160 // Note that all of these assume that the world is spherical.
1161
1162 double Rad2Nm(double theta) {
1163         return(((180.0*60.0)/SG_PI)*theta);
1164 }
1165
1166 double Nm2Rad(double d) {
1167         return((SG_PI/(180.0*60.0))*d);
1168 }
1169
1170 /* QUOTE:
1171
1172 The great circle distance d between two points with coordinates {lat1,lon1} and {lat2,lon2} is given by:
1173
1174 d=acos(sin(lat1)*sin(lat2)+cos(lat1)*cos(lat2)*cos(lon1-lon2))
1175
1176 A mathematically equivalent formula, which is less subject to rounding error for short distances is:
1177
1178 d=2*asin(sqrt((sin((lat1-lat2)/2))^2 +
1179                  cos(lat1)*cos(lat2)*(sin((lon1-lon2)/2))^2))
1180
1181 */
1182
1183 // Returns distance in nm, takes lat & lon in RADIANS
1184 double DCLGPS::GetGreatCircleDistance(double lat1, double lon1, double lat2, double lon2) const {
1185         double d = 2.0 * asin(sqrt(((sin((lat1-lat2)/2.0))*(sin((lat1-lat2)/2.0))) +
1186                    cos(lat1)*cos(lat2)*(sin((lon1-lon2)/2.0))*(sin((lon1-lon2)/2.0))));
1187         return(Rad2Nm(d));
1188 }
1189
1190 // fmod dosen't do what we want :-(
1191 static double mod(double d1, double d2) {
1192         return(d1 - d2*floor(d1/d2));
1193 }
1194
1195 // Returns great circle course from point 1 to point 2
1196 // Input and output in RADIANS.
1197 double DCLGPS::GetGreatCircleCourse (double lat1, double lon1, double lat2, double lon2) const {
1198         //double h = 0.0;
1199         /*
1200         // Special case the poles
1201         if(cos(lat1) < SG_EPSILON) {
1202                 if(lat1 > 0) {
1203                         // Starting from North Pole
1204                         h = SG_PI;
1205                 } else {
1206                         // Starting from South Pole
1207                         h = 2.0 * SG_PI;
1208                 }
1209         } else {
1210                 // Urgh - the formula below is for negative lon +ve !!!???
1211                 double d = GetGreatCircleDistance(lat1, lon1, lat2, lon2);
1212                 cout << "d = " << d;
1213                 d = Nm2Rad(d);
1214                 //cout << ", d_theta = " << d;
1215                 //cout << ", and d = " << Rad2Nm(d) << ' ';
1216                 if(sin(lon2 - lon1) < 0) {
1217                         cout << " A ";
1218                         h = acos((sin(lat2)-sin(lat1)*cos(d))/(sin(d)*cos(lat1)));
1219                 } else {
1220                         cout << " B ";
1221                         h = 2.0 * SG_PI - acos((sin(lat2)-sin(lat1)*cos(d))/(sin(d)*cos(lat1)));
1222                 }
1223         }
1224         cout << h * SG_RADIANS_TO_DEGREES << '\n';
1225         */
1226
1227         return( mod(atan2(sin(lon2-lon1)*cos(lat2),
1228             cos(lat1)*sin(lat2)-sin(lat1)*cos(lat2)*cos(lon2-lon1)),
1229             2.0*SG_PI) );
1230 }
1231
1232 // Return a position on a radial from wp1 given distance d (nm) and magnetic heading h (degrees)
1233 // Note that d should be less that 1/4 Earth diameter!
1234 GPSWaypoint DCLGPS::GetPositionOnMagRadial(const GPSWaypoint& wp1, double d, double h) {
1235         h += sgGetMagVar(wp1.lon, wp1.lat, 0.0, _time->getJD()) * SG_RADIANS_TO_DEGREES;
1236         return(GetPositionOnRadial(wp1, d, h));
1237 }
1238
1239 // Return a position on a radial from wp1 given distance d (nm) and TRUE heading h (degrees)
1240 // Note that d should be less that 1/4 Earth diameter!
1241 GPSWaypoint DCLGPS::GetPositionOnRadial(const GPSWaypoint& wp1, double d, double h) {
1242         while(h < 0.0) h += 360.0;
1243         while(h > 360.0) h -= 360.0;
1244
1245         h *= SG_DEGREES_TO_RADIANS;
1246         d *= (SG_PI / (180.0 * 60.0));
1247
1248         double lat=asin(sin(wp1.lat)*cos(d)+cos(wp1.lat)*sin(d)*cos(h));
1249         double lon;
1250         if(cos(lat)==0) {
1251                 lon=wp1.lon;      // endpoint a pole
1252         } else {
1253                 lon=mod(wp1.lon+asin(sin(h)*sin(d)/cos(lat))+SG_PI,2*SG_PI)-SG_PI;
1254         }
1255
1256         GPSWaypoint wp;
1257         wp.lat = lat;
1258         wp.lon = lon;
1259         wp.type = GPS_WP_VIRT;
1260         return(wp);
1261 }
1262
1263 // Returns cross-track deviation in Nm.
1264 double DCLGPS::CalcCrossTrackDeviation() const {
1265         return(CalcCrossTrackDeviation(_fromWaypoint, _activeWaypoint));
1266 }
1267
1268 // Returns cross-track deviation of the current position between two arbitary waypoints in nm.
1269 double DCLGPS::CalcCrossTrackDeviation(const GPSWaypoint& wp1, const GPSWaypoint& wp2) const {
1270         //if(wp1 == NULL || wp2 == NULL) return(0.0);
1271         if(wp1.id.empty() || wp2.id.empty()) return(0.0);
1272         double xtd = asin(sin(Nm2Rad(GetGreatCircleDistance(wp1.lat, wp1.lon, _gpsLat, _gpsLon)))
1273                           * sin(GetGreatCircleCourse(wp1.lat, wp1.lon, _gpsLat, _gpsLon) - GetGreatCircleCourse(wp1.lat, wp1.lon, wp2.lat, wp2.lon)));
1274         return(Rad2Nm(xtd));
1275 }