]> git.mxchange.org Git - flightgear.git/blob - src/Instrumentation/dclgps.cxx
Merge branch 'durk/ai-atc'
[flightgear.git] / 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;
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::findAllWithIdentSortedByRange(id, SGGeod::fromRad(_lon, _lat), &filter);
1106     multi = (matches.size() > 1);
1107     return matches.empty() ? NULL : matches.front().ptr();
1108   }
1109   
1110   return FGPositioned::findNextWithPartialId(NULL, id, &filter);
1111 }
1112
1113 FGNavRecord* DCLGPS::FindFirstVorById(const string& id, bool &multi, bool exact)
1114 {
1115   return dynamic_cast<FGNavRecord*>(FindTypedFirstById(id, FGPositioned::VOR, multi, exact));
1116 }
1117
1118 FGNavRecord* DCLGPS::FindFirstNDBById(const string& id, bool &multi, bool exact)
1119 {
1120   return dynamic_cast<FGNavRecord*>(FindTypedFirstById(id, FGPositioned::NDB, multi, exact));
1121 }
1122
1123 const FGFix* DCLGPS::FindFirstIntById(const string& id, bool &multi, bool exact)
1124 {
1125   return dynamic_cast<FGFix*>(FindTypedFirstById(id, FGPositioned::FIX, multi, exact));
1126 }
1127
1128 const FGAirport* DCLGPS::FindFirstAptById(const string& id, bool &multi, bool exact)
1129 {
1130   return dynamic_cast<FGAirport*>(FindTypedFirstById(id, FGPositioned::AIRPORT, multi, exact));
1131 }
1132
1133 FGNavRecord* DCLGPS::FindClosestVor(double lat_rad, double lon_rad) {  
1134   FGPositioned::TypeFilter filter(FGPositioned::VOR);
1135   double cutoff = 1000; // nautical miles
1136   FGPositionedRef v = FGPositioned::findClosest(SGGeod::fromRad(lon_rad, lat_rad), cutoff, &filter);
1137   if (!v) {
1138     return NULL;
1139   }
1140   
1141   return dynamic_cast<FGNavRecord*>(v.ptr());
1142 }
1143
1144 //----------------------------------------------------------------------------------------------------------
1145
1146 // Takes lat and lon in RADIANS!!!!!!!
1147 double DCLGPS::GetMagHeadingFromTo(double latA, double lonA, double latB, double lonB) {
1148         double h = GetGreatCircleCourse(latA, lonA, latB, lonB);
1149         h *= SG_RADIANS_TO_DEGREES;
1150         // TODO - use the real altitude below instead of 0.0!
1151         //cout << "MagVar = " << sgGetMagVar(_gpsLon, _gpsLat, 0.0, _time->getJD()) * SG_RADIANS_TO_DEGREES << '\n';
1152         h -= sgGetMagVar(_gpsLon, _gpsLat, 0.0, _time->getJD()) * SG_RADIANS_TO_DEGREES;
1153         while(h >= 360.0) h -= 360.0;
1154         while(h < 0.0) h += 360.0;
1155         return(h);
1156 }
1157
1158 // ---------------- Great Circle formulae from "The Aviation Formulary" -------------
1159 // Note that all of these assume that the world is spherical.
1160
1161 double Rad2Nm(double theta) {
1162         return(((180.0*60.0)/SG_PI)*theta);
1163 }
1164
1165 double Nm2Rad(double d) {
1166         return((SG_PI/(180.0*60.0))*d);
1167 }
1168
1169 /* QUOTE:
1170
1171 The great circle distance d between two points with coordinates {lat1,lon1} and {lat2,lon2} is given by:
1172
1173 d=acos(sin(lat1)*sin(lat2)+cos(lat1)*cos(lat2)*cos(lon1-lon2))
1174
1175 A mathematically equivalent formula, which is less subject to rounding error for short distances is:
1176
1177 d=2*asin(sqrt((sin((lat1-lat2)/2))^2 + 
1178                  cos(lat1)*cos(lat2)*(sin((lon1-lon2)/2))^2))
1179                                  
1180 */
1181
1182 // Returns distance in nm, takes lat & lon in RADIANS
1183 double DCLGPS::GetGreatCircleDistance(double lat1, double lon1, double lat2, double lon2) const {
1184         double d = 2.0 * asin(sqrt(((sin((lat1-lat2)/2.0))*(sin((lat1-lat2)/2.0))) +
1185                    cos(lat1)*cos(lat2)*(sin((lon1-lon2)/2.0))*(sin((lon1-lon2)/2.0))));
1186         return(Rad2Nm(d));
1187 }
1188
1189 // fmod dosen't do what we want :-( 
1190 static double mod(double d1, double d2) {
1191         return(d1 - d2*floor(d1/d2));
1192 }
1193
1194 // Returns great circle course from point 1 to point 2
1195 // Input and output in RADIANS.
1196 double DCLGPS::GetGreatCircleCourse (double lat1, double lon1, double lat2, double lon2) const {
1197         //double h = 0.0;
1198         /*
1199         // Special case the poles
1200         if(cos(lat1) < SG_EPSILON) {
1201                 if(lat1 > 0) {
1202                         // Starting from North Pole
1203                         h = SG_PI;
1204                 } else {
1205                         // Starting from South Pole
1206                         h = 2.0 * SG_PI;
1207                 }
1208         } else {
1209                 // Urgh - the formula below is for negative lon +ve !!!???
1210                 double d = GetGreatCircleDistance(lat1, lon1, lat2, lon2);
1211                 cout << "d = " << d;
1212                 d = Nm2Rad(d);
1213                 //cout << ", d_theta = " << d;
1214                 //cout << ", and d = " << Rad2Nm(d) << ' ';
1215                 if(sin(lon2 - lon1) < 0) {
1216                         cout << " A ";
1217                         h = acos((sin(lat2)-sin(lat1)*cos(d))/(sin(d)*cos(lat1)));
1218                 } else {
1219                         cout << " B ";
1220                         h = 2.0 * SG_PI - acos((sin(lat2)-sin(lat1)*cos(d))/(sin(d)*cos(lat1)));
1221                 }
1222         }
1223         cout << h * SG_RADIANS_TO_DEGREES << '\n';
1224         */
1225         
1226         return( mod(atan2(sin(lon2-lon1)*cos(lat2),
1227             cos(lat1)*sin(lat2)-sin(lat1)*cos(lat2)*cos(lon2-lon1)),
1228             2.0*SG_PI) );
1229 }
1230
1231 // Return a position on a radial from wp1 given distance d (nm) and magnetic heading h (degrees)
1232 // Note that d should be less that 1/4 Earth diameter!
1233 GPSWaypoint DCLGPS::GetPositionOnMagRadial(const GPSWaypoint& wp1, double d, double h) {
1234         h += sgGetMagVar(wp1.lon, wp1.lat, 0.0, _time->getJD()) * SG_RADIANS_TO_DEGREES;
1235         return(GetPositionOnRadial(wp1, d, h));
1236 }
1237
1238 // Return a position on a radial from wp1 given distance d (nm) and TRUE heading h (degrees)
1239 // Note that d should be less that 1/4 Earth diameter!
1240 GPSWaypoint DCLGPS::GetPositionOnRadial(const GPSWaypoint& wp1, double d, double h) {
1241         while(h < 0.0) h += 360.0;
1242         while(h > 360.0) h -= 360.0;
1243         
1244         h *= SG_DEGREES_TO_RADIANS;
1245         d *= (SG_PI / (180.0 * 60.0));
1246         
1247         double lat=asin(sin(wp1.lat)*cos(d)+cos(wp1.lat)*sin(d)*cos(h));
1248         double lon;
1249         if(cos(lat)==0) {
1250                 lon=wp1.lon;      // endpoint a pole
1251         } else {
1252                 lon=mod(wp1.lon+asin(sin(h)*sin(d)/cos(lat))+SG_PI,2*SG_PI)-SG_PI;
1253         }
1254         
1255         GPSWaypoint wp;
1256         wp.lat = lat;
1257         wp.lon = lon;
1258         wp.type = GPS_WP_VIRT;
1259         return(wp);
1260 }
1261
1262 // Returns cross-track deviation in Nm.
1263 double DCLGPS::CalcCrossTrackDeviation() const {
1264         return(CalcCrossTrackDeviation(_fromWaypoint, _activeWaypoint));
1265 }
1266
1267 // Returns cross-track deviation of the current position between two arbitary waypoints in nm.
1268 double DCLGPS::CalcCrossTrackDeviation(const GPSWaypoint& wp1, const GPSWaypoint& wp2) const {
1269         //if(wp1 == NULL || wp2 == NULL) return(0.0);
1270         if(wp1.id.empty() || wp2.id.empty()) return(0.0);
1271         double xtd = asin(sin(Nm2Rad(GetGreatCircleDistance(wp1.lat, wp1.lon, _gpsLat, _gpsLon))) 
1272                           * sin(GetGreatCircleCourse(wp1.lat, wp1.lon, _gpsLat, _gpsLon) - GetGreatCircleCourse(wp1.lat, wp1.lon, wp2.lat, wp2.lon)));
1273         return(Rad2Nm(xtd));
1274 }