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