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.
6 // Written by David Luff, started 2005.
8 // Copyright (C) 2005 - David C Luff: daveluff --AT-- ntlworld --D0T-- com
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.
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.
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.
28 #include <simgear/sg_inlines.h>
29 #include <simgear/structure/commands.hxx>
30 #include <simgear/timing/sg_time.hxx>
31 #include <simgear/magvar/magvar.hxx>
33 #include <Main/fg_props.hxx>
34 #include <Navaids/fix.hxx>
39 //using namespace std;
41 // Command callbacks for FlightGear
43 static bool do_kln89_msg_pressed(const SGPropertyNode* arg) {
44 //cout << "do_kln89_msg_pressed called!\n";
45 DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89");
50 static bool do_kln89_obs_pressed(const SGPropertyNode* arg) {
51 //cout << "do_kln89_obs_pressed called!\n";
52 DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89");
57 static bool do_kln89_alt_pressed(const SGPropertyNode* arg) {
58 //cout << "do_kln89_alt_pressed called!\n";
59 DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89");
64 static bool do_kln89_nrst_pressed(const SGPropertyNode* arg) {
65 DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89");
70 static bool do_kln89_dto_pressed(const SGPropertyNode* arg) {
71 DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89");
76 static bool do_kln89_clr_pressed(const SGPropertyNode* arg) {
77 DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89");
82 static bool do_kln89_ent_pressed(const SGPropertyNode* arg) {
83 DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89");
88 static bool do_kln89_crsr_pressed(const SGPropertyNode* arg) {
89 DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89");
94 static bool do_kln89_knob1left1(const SGPropertyNode* arg) {
95 DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89");
100 static bool do_kln89_knob1right1(const SGPropertyNode* arg) {
101 DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89");
106 static bool do_kln89_knob2left1(const SGPropertyNode* arg) {
107 DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89");
112 static bool do_kln89_knob2right1(const SGPropertyNode* arg) {
113 DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89");
118 // End command callbacks
120 GPSWaypoint::GPSWaypoint() {
121 appType = GPS_APP_NONE;
124 GPSWaypoint::GPSWaypoint(const std::string& aIdent, float aLat, float aLon, GPSWpType aType) :
129 appType(GPS_APP_NONE)
133 GPSWaypoint::~GPSWaypoint() {}
135 string GPSWaypoint::GetAprId() {
136 if(appType == GPS_IAF) return(id + 'i');
137 else if(appType == GPS_FAF) return(id + 'f');
138 else if(appType == GPS_MAP) return(id + 'm');
139 else if(appType == GPS_MAHP) return(id + 'h');
143 GPSWaypoint* GPSWaypoint::createFromFix(const FGFix* aFix)
146 return new GPSWaypoint(aFix->get_ident(),
147 aFix->get_lat() * SG_DEGREES_TO_RADIANS,
148 aFix->get_lon() * SG_DEGREES_TO_RADIANS,
152 GPSWaypoint* GPSWaypoint::createFromNav(const FGNavRecord* aNav)
155 return new GPSWaypoint(aNav->get_ident(),
156 aNav->get_lat() * SG_DEGREES_TO_RADIANS,
157 aNav->get_lon() * SG_DEGREES_TO_RADIANS,
158 (aNav->get_fg_type() == FG_NAV_VOR ? GPS_WP_VOR : GPS_WP_NDB));
161 GPSWaypoint* GPSWaypoint::createFromAirport(const FGAirport* aApt)
164 return new GPSWaypoint(aApt->getId(),
165 aApt->getLatitude() * SG_DEGREES_TO_RADIANS,
166 aApt->getLongitude() * SG_DEGREES_TO_RADIANS,
170 ostream& operator << (ostream& os, GPSAppWpType type) {
172 case(GPS_IAF): return(os << "IAF");
173 case(GPS_IAP): return(os << "IAP");
174 case(GPS_FAF): return(os << "FAF");
175 case(GPS_MAP): return(os << "MAP");
176 case(GPS_MAHP): return(os << "MAHP");
177 case(GPS_HDR): return(os << "HEADER");
178 case(GPS_FENCE): return(os << "FENCE");
179 case(GPS_APP_NONE): return(os << "NONE");
181 return(os << "ERROR - Unknown switch in GPSAppWpType operator << ");
193 FGNPIAP::~FGNPIAP() {
196 ClockTime::ClockTime() {
201 ClockTime::ClockTime(int hr, int min) {
202 while(hr < 0) { hr += 24; }
204 while(min < 0) { min += 60; }
205 while(min > 60) { min -= 60; }
209 ClockTime::~ClockTime() {
212 GPSPage::GPSPage(DCLGPS* parent) {
217 GPSPage::~GPSPage() {
220 void GPSPage::Update(double dt) {}
222 void GPSPage::Knob1Left1() {}
223 void GPSPage::Knob1Right1() {}
225 void GPSPage::Knob2Left1() {
226 _parent->_activePage->LooseFocus();
228 if(_subPage < 0) _subPage = _nSubPages - 1;
231 void GPSPage::Knob2Right1() {
232 _parent->_activePage->LooseFocus();
234 if(_subPage >= _nSubPages) _subPage = 0;
237 void GPSPage::CrsrPressed() {}
238 void GPSPage::EntPressed() {}
239 void GPSPage::ClrPressed() {}
240 void GPSPage::DtoPressed() {}
241 void GPSPage::NrstPressed() {}
242 void GPSPage::AltPressed() {}
243 void GPSPage::OBSPressed() {}
244 void GPSPage::MsgPressed() {}
246 string GPSPage::GPSitoa(int n) {
248 // TODO - sanity check n!
249 sprintf(buf, "%i", n);
254 void GPSPage::CleanUp() {}
255 void GPSPage::LooseFocus() {}
256 void GPSPage::SetId(const string& s) {}
258 // ------------------------------------------------------------------------------------- //
260 DCLGPS::DCLGPS(RenderArea2D* instrument) {
261 _instrument = instrument;
266 // Units - lets default to US units - FG can set them to other units from config during startup if desired.
267 _altUnits = GPS_ALT_UNITS_FT;
268 _baroUnits = GPS_PRES_UNITS_IN;
269 _velUnits = GPS_VEL_UNITS_KT;
270 _distUnits = GPS_DIST_UNITS_NM;
272 _lon_node = fgGetNode("/instrumentation/gps/indicated-longitude-deg", true);
273 _lat_node = fgGetNode("/instrumentation/gps/indicated-latitude-deg", true);
274 _alt_node = fgGetNode("/instrumentation/gps/indicated-altitude-ft", true);
275 _grnd_speed_node = fgGetNode("/instrumentation/gps/indicated-ground-speed-kt", true);
276 _true_track_node = fgGetNode("/instrumentation/gps/indicated-track-true-deg", true);
277 _mag_track_node = fgGetNode("/instrumentation/gps/indicated-track-magnetic-deg", true);
279 // Use FG's position values at construction in case FG's gps has not run first update yet.
280 _lon = fgGetDouble("/position/longitude-deg") * SG_DEGREES_TO_RADIANS;
281 _lat = fgGetDouble("/position/latitude-deg") * SG_DEGREES_TO_RADIANS;
282 _alt = fgGetDouble("/position/altitude-ft");
283 // Note - we can depriciate _gpsLat and _gpsLon if we implement error handling in FG
284 // gps code and not our own.
289 _groundSpeed_ms = 0.0;
290 _groundSpeed_kts = 0.0;
294 // Sensible defaults. These can be overriden by derived classes if desired.
296 _cdiScales.push_back(5.0);
297 _cdiScales.push_back(1.0);
298 _cdiScales.push_back(0.3);
299 _currentCdiScaleIndex = 0;
300 _targetCdiScaleIndex = 0;
301 _sourceCdiScaleIndex = 0;
302 _cdiScaleTransition = false;
303 _currentCdiScale = 5.0;
307 _activeWaypoint.id.clear();
309 _crosstrackDist = 0.0;
310 _headingBugTo = true;
312 _waypointAlert = false;
314 _departureTimeString = "----";
316 _powerOnTime.set_hr(0);
317 _powerOnTime.set_min(0);
318 _powerOnTimerSet = false;
321 // Configuration Initialisation
322 // Should this be in kln89.cxx ?
323 _turnAnticipationEnabled = false;
324 _suaAlertEnabled = false;
325 _altAlertEnabled = false;
329 _messageStack.clear();
333 _approachLoaded = false;
334 _approachArm = false;
335 _approachReallyArmed = false;
336 _approachActive = false;
337 _approachFP = new GPSFlightPlan;
342 delete _approachFP; // Don't need to delete the waypoints inside since they point to
343 // the waypoints in the approach database.
344 // TODO - may need to delete the approach database!!
347 void DCLGPS::draw() {
348 //cout << "draw called!\n";
352 void DCLGPS::init() {
353 globals->get_commands()->addCommand("kln89_msg_pressed", do_kln89_msg_pressed);
354 globals->get_commands()->addCommand("kln89_obs_pressed", do_kln89_obs_pressed);
355 globals->get_commands()->addCommand("kln89_alt_pressed", do_kln89_alt_pressed);
356 globals->get_commands()->addCommand("kln89_nrst_pressed", do_kln89_nrst_pressed);
357 globals->get_commands()->addCommand("kln89_dto_pressed", do_kln89_dto_pressed);
358 globals->get_commands()->addCommand("kln89_clr_pressed", do_kln89_clr_pressed);
359 globals->get_commands()->addCommand("kln89_ent_pressed", do_kln89_ent_pressed);
360 globals->get_commands()->addCommand("kln89_crsr_pressed", do_kln89_crsr_pressed);
361 globals->get_commands()->addCommand("kln89_knob1left1", do_kln89_knob1left1);
362 globals->get_commands()->addCommand("kln89_knob1right1", do_kln89_knob1right1);
363 globals->get_commands()->addCommand("kln89_knob2left1", do_kln89_knob2left1);
364 globals->get_commands()->addCommand("kln89_knob2right1", do_kln89_knob2right1);
366 // Not sure if this should be here, but OK for now.
367 CreateDefaultFlightPlans();
370 void DCLGPS::bind() {
371 fgTie("/instrumentation/gps/waypoint-alert", this, &DCLGPS::GetWaypointAlert);
372 fgTie("/instrumentation/gps/leg-mode", this, &DCLGPS::GetLegMode);
373 fgTie("/instrumentation/gps/obs-mode", this, &DCLGPS::GetOBSMode);
374 fgTie("/instrumentation/gps/approach-arm", this, &DCLGPS::GetApproachArm);
375 fgTie("/instrumentation/gps/approach-active", this, &DCLGPS::GetApproachActive);
376 fgTie("/instrumentation/gps/cdi-deflection", this, &DCLGPS::GetCDIDeflection);
377 fgTie("/instrumentation/gps/to-flag", this, &DCLGPS::GetToFlag);
380 void DCLGPS::unbind() {
381 fgUntie("/instrumentation/gps/waypoint-alert");
382 fgUntie("/instrumentation/gps/leg-mode");
383 fgUntie("/instrumentation/gps/obs-mode");
384 fgUntie("/instrumentation/gps/approach-arm");
385 fgUntie("/instrumentation/gps/approach-active");
386 fgUntie("/instrumentation/gps/cdi-deflection");
389 void DCLGPS::update(double dt) {
390 //cout << "update called!\n";
392 _lon = _lon_node->getDoubleValue() * SG_DEGREES_TO_RADIANS;
393 _lat = _lat_node->getDoubleValue() * SG_DEGREES_TO_RADIANS;
394 _alt = _alt_node->getDoubleValue();
395 _groundSpeed_kts = _grnd_speed_node->getDoubleValue();
396 _groundSpeed_ms = _groundSpeed_kts * 0.5144444444;
397 _track = _true_track_node->getDoubleValue();
398 _magTrackDeg = _mag_track_node->getDoubleValue();
399 // Note - we can depriciate _gpsLat and _gpsLon if we implement error handling in FG
400 // gps code and not our own.
403 // Check for abnormal position slew
404 if(GetGreatCircleDistance(_gpsLat, _gpsLon, _checkLat, _checkLon) > 1.0) {
405 OrientateToActiveFlightPlan();
410 // TODO - check for unit power before running this.
411 if(!_powerOnTimerSet) {
415 // Check if an alarm timer has expired
417 if(_alarmTime.hr() == atoi(fgGetString("/instrumentation/clock/indicated-hour"))
418 && _alarmTime.min() == atoi(fgGetString("/instrumentation/clock/indicated-min"))) {
419 _messageStack.push_back("*Timer Expired");
425 if(_groundSpeed_kts > 30.0) {
427 string th = fgGetString("/instrumentation/clock/indicated-hour");
428 string tm = fgGetString("/instrumentation/clock/indicated-min");
429 if(th.size() == 1) th = "0" + th;
430 if(tm.size() == 1) tm = "0" + tm;
431 _departureTimeString = th + tm;
434 // TODO - check - is this prone to drift error over time?
435 // Should we difference the departure and current times?
436 // What about when the user resets the time of day from the menu?
440 _time->update(_gpsLon * SG_DEGREES_TO_RADIANS, _gpsLat * SG_DEGREES_TO_RADIANS, 0, 0);
441 // FIXME - currently all the below assumes leg mode and no DTO or OBS cancelled.
442 if(_activeFP->IsEmpty()) {
443 // Not sure if we need to reset these each update or only when fp altered
444 _activeWaypoint.id.clear();
446 } else if(_activeFP->waypoints.size() == 1) {
447 _activeWaypoint.id.clear();
450 if(_activeWaypoint.id.empty() || _fromWaypoint.id.empty()) {
451 //cout << "Error, in leg mode with flightplan of 2 or more waypoints, but either active or from wp is NULL!\n";
452 OrientateToActiveFlightPlan();
456 if(_approachLoaded) {
457 if(!_approachReallyArmed && !_approachActive) {
458 // arm if within 30nm of airport.
459 // TODO - let user cancel approach arm using external GPS-APR switch
461 const FGAirport* ap = FindFirstAptById(_approachID, multi, true);
463 double d = GetGreatCircleDistance(_gpsLat, _gpsLon, ap->getLatitude() * SG_DEGREES_TO_RADIANS, ap->getLongitude() * SG_DEGREES_TO_RADIANS);
466 _approachReallyArmed = true;
467 _messageStack.push_back("*Press ALT To Set Baro");
468 // Not sure what we do if the user has already set CDI to 0.3 nm?
469 _targetCdiScaleIndex = 1;
470 if(_currentCdiScaleIndex == 1) {
472 } else if(_currentCdiScaleIndex == 0) {
473 _sourceCdiScaleIndex = 0;
474 _cdiScaleTransition = true;
475 _cdiTransitionTime = 30.0;
476 _currentCdiScale = _cdiScales[_currentCdiScaleIndex];
481 // Check for approach active - we can only activate approach if it is really armed.
482 if(_activeWaypoint.appType == GPS_FAF) {
483 //cout << "Active waypoint is FAF, id is " << _activeWaypoint.id << '\n';
484 if(GetGreatCircleDistance(_gpsLat, _gpsLon, _activeWaypoint.lat, _activeWaypoint.lon) <= 2.0 && !_obsMode) {
485 // Assume heading is OK for now
486 _approachArm = false; // TODO - check - maybe arm is left on when actv comes on?
487 _approachReallyArmed = false;
488 _approachActive = true;
489 _targetCdiScaleIndex = 2;
490 if(_currentCdiScaleIndex == 2) {
492 } else if(_currentCdiScaleIndex == 1) {
493 _sourceCdiScaleIndex = 1;
494 _cdiScaleTransition = true;
495 _cdiTransitionTime = 30.0; // TODO - compress it if time to FAF < 30sec
496 _currentCdiScale = _cdiScales[_currentCdiScaleIndex];
498 // Abort going active?
499 _approachActive = false;
506 // CDI scale transition stuff
507 if(_cdiScaleTransition) {
508 if(fabs(_currentCdiScale - _cdiScales[_targetCdiScaleIndex]) < 0.001) {
509 _currentCdiScale = _cdiScales[_targetCdiScaleIndex];
510 _currentCdiScaleIndex = _targetCdiScaleIndex;
511 _cdiScaleTransition = false;
513 double scaleDiff = (_targetCdiScaleIndex > _sourceCdiScaleIndex
514 ? _cdiScales[_sourceCdiScaleIndex] - _cdiScales[_targetCdiScaleIndex]
515 : _cdiScales[_targetCdiScaleIndex] - _cdiScales[_sourceCdiScaleIndex]);
516 //cout << "ScaleDiff = " << scaleDiff << '\n';
517 if(_targetCdiScaleIndex > _sourceCdiScaleIndex) {
518 // Scaling down eg. 5nm -> 1nm
519 _currentCdiScale -= (scaleDiff * dt / _cdiTransitionTime);
520 if(_currentCdiScale < _cdiScales[_targetCdiScaleIndex]) {
521 _currentCdiScale = _cdiScales[_targetCdiScaleIndex];
522 _currentCdiScaleIndex = _targetCdiScaleIndex;
523 _cdiScaleTransition = false;
526 _currentCdiScale += (scaleDiff * dt / _cdiTransitionTime);
527 if(_currentCdiScale > _cdiScales[_targetCdiScaleIndex]) {
528 _currentCdiScale = _cdiScales[_targetCdiScaleIndex];
529 _currentCdiScaleIndex = _targetCdiScaleIndex;
530 _cdiScaleTransition = false;
533 //cout << "_currentCdiScale = " << _currentCdiScale << '\n';
536 _currentCdiScale = _cdiScales[_currentCdiScaleIndex];
540 // Urgh - I've been setting the heading bug based on DTK,
541 // bug I think it should be based on heading re. active waypoint
542 // based on what the sim does after the final waypoint is passed.
543 // (DTK remains the same, but if track is held == DTK heading bug
544 // reverses to from once wp is passed).
546 if(_fromWaypoint != NULL) {
547 // TODO - how do we handle the change of track with distance over long legs?
548 _dtkTrue = GetGreatCircleCourse(_fromWaypoint->lat, _fromWaypoint->lon, _activeWaypoint->lat, _activeWaypoint->lon) * SG_RADIANS_TO_DEGREES;
549 _dtkMag = GetMagHeadingFromTo(_fromWaypoint->lat, _fromWaypoint->lon, _activeWaypoint->lat, _activeWaypoint->lon);
550 // Don't change the heading bug if speed is too low otherwise it flickers to/from at rest
551 if(_groundSpeed_ms > 5) {
552 //cout << "track = " << _track << ", dtk = " << _dtkTrue << '\n';
553 double courseDev = _track - _dtkTrue;
554 //cout << "courseDev = " << courseDev << ", normalized = ";
555 SG_NORMALIZE_RANGE(courseDev, -180.0, 180.0);
556 //cout << courseDev << '\n';
557 _headingBugTo = (fabs(courseDev) > 90.0 ? false : true);
562 // TODO - in DTO operation the position of initiation of DTO defines the "from waypoint".
565 if(!_activeWaypoint.id.empty()) {
566 double hdgTrue = GetGreatCircleCourse(_gpsLat, _gpsLon, _activeWaypoint.lat, _activeWaypoint.lon) * SG_RADIANS_TO_DEGREES;
567 if(_groundSpeed_ms > 5) {
568 //cout << "track = " << _track << ", hdgTrue = " << hdgTrue << '\n';
569 double courseDev = _track - hdgTrue;
570 //cout << "courseDev = " << courseDev << ", normalized = ";
571 SG_NORMALIZE_RANGE(courseDev, -180.0, 180.0);
572 //cout << courseDev << '\n';
573 _headingBugTo = (fabs(courseDev) > 90.0 ? false : true);
575 if(!_fromWaypoint.id.empty()) {
576 _dtkTrue = GetGreatCircleCourse(_fromWaypoint.lat, _fromWaypoint.lon, _activeWaypoint.lat, _activeWaypoint.lon) * SG_RADIANS_TO_DEGREES;
577 _dtkMag = GetMagHeadingFromTo(_fromWaypoint.lat, _fromWaypoint.lon, _activeWaypoint.lat, _activeWaypoint.lon);
584 _dist2Act = GetGreatCircleDistance(_gpsLat, _gpsLon, _activeWaypoint.lat, _activeWaypoint.lon) * SG_NM_TO_METER;
585 if(_groundSpeed_ms > 10.0) {
586 _eta = _dist2Act / _groundSpeed_ms;
587 if(_eta <= 36) { // TODO - this is slightly different if turn anticipation is enabled.
589 _waypointAlert = true; // TODO - not if the from flag is set.
593 // Check if we should sequence to next leg.
594 // Perhaps this should be done on distance instead, but 60s time (about 1 - 2 nm) seems reasonable for now.
595 //double reverseHeading = GetGreatCircleCourse(_activeWaypoint->lat, _activeWaypoint->lon, _fromWaypoint->lat, _fromWaypoint->lon);
596 // Hack - let's cheat and do it on heading bug for now. TODO - that stops us 'cutting the corner'
597 // when we happen to approach the inside turn of a waypoint - we should probably sequence at the midpoint
598 // of the heading difference between legs in this instance.
599 int idx = GetActiveWaypointIndex();
600 bool finalLeg = (idx == (int)(_activeFP->waypoints.size()) - 1 ? true : false);
601 bool finalDto = (_dto && idx == -1); // Dto operation to a waypoint not in the flightplan - we don't sequence in this instance
604 // Do nothing - not sure if Dto should switch off when arriving at the final waypoint of a flightplan
605 } else if(finalDto) {
607 } else if(_activeWaypoint.appType == GPS_MAP) {
608 // Don't sequence beyond the missed approach point
609 //cout << "ACTIVE WAYPOINT is MAP - not sequencing!!!!!\n";
611 //cout << "Sequencing...\n";
612 _fromWaypoint = _activeWaypoint;
613 _activeWaypoint = *_activeFP->waypoints[idx + 1];
615 // TODO - course alteration message format is dependent on whether we are slaved HSI/CDI indicator or not.
616 // For now assume we are not.
618 if(fgGetBool("/instrumentation/nav[0]/slaved-to-gps")) {
619 // TODO - avoid the hardwiring on nav[0]
620 s = "Adj Nav Crs to ";
622 string s = "GPS Course is ";
624 double d = GetMagHeadingFromTo(_fromWaypoint.lat, _fromWaypoint.lon, _activeWaypoint.lat, _activeWaypoint.lon);
625 while(d < 0.0) d += 360.0;
626 while(d >= 360.0) d -= 360.0;
628 snprintf(buf, 4, "%03i", (int)(d + 0.5));
630 _messageStack.push_back(s);
632 _waypointAlert = false;
640 // First attempt at a sensible cross-track correction calculation
641 // Uh? - I think this is implemented further down the file!
642 if(_fromWaypoint != NULL) {
645 _crosstrackDist = 0.0;
651 // I don't yet fully understand all the gotchas about where to source time from.
652 // This function sets the initial timer before the clock exports properties
653 // and the one below uses the clock to be consistent with the rest of the code.
654 // It might change soonish...
655 void DCLGPS::SetPowerOnTimer() {
656 struct tm *t = globals->get_time_params()->getGmt();
657 _powerOnTime.set_hr(t->tm_hour);
658 _powerOnTime.set_min(t->tm_min);
659 _powerOnTimerSet = true;
662 void DCLGPS::ResetPowerOnTimer() {
663 _powerOnTime.set_hr(atoi(fgGetString("/instrumentation/clock/indicated-hour")));
664 _powerOnTime.set_min(atoi(fgGetString("/instrumentation/clock/indicated-min")));
665 _powerOnTimerSet = true;
668 double DCLGPS::GetCDIDeflection() const {
669 double xtd = CalcCrossTrackDeviation(); //nm
670 return((xtd / _currentCdiScale) * 5.0 * 2.5 * -1.0);
673 void DCLGPS::DtoInitiate(const string& s) {
674 //cout << "DtoInitiate, s = " << s << '\n';
675 const GPSWaypoint* wp = FindFirstByExactId(s);
677 //cout << "Waypoint found, starting dto operation!\n";
679 _activeWaypoint = *wp;
680 _fromWaypoint.lat = _gpsLat;
681 _fromWaypoint.lon = _gpsLon;
682 _fromWaypoint.type = GPS_WP_VIRT;
683 _fromWaypoint.id = "DTOWP";
686 //cout << "Waypoint not found, ignoring dto request\n";
687 // Should bring up the user waypoint page, but we're not implementing that yet.
688 _dto = false; // TODO - implement this some day.
692 void DCLGPS::DtoCancel() {
694 // i.e. don't bother reorientating if we're just cancelling a DTO button press
695 // without having previously initiated DTO.
696 OrientateToActiveFlightPlan();
701 void DCLGPS::Knob1Left1() {}
702 void DCLGPS::Knob1Right1() {}
703 void DCLGPS::Knob2Left1() {}
704 void DCLGPS::Knob2Right1() {}
705 void DCLGPS::CrsrPressed() { _activePage->CrsrPressed(); }
706 void DCLGPS::EntPressed() { _activePage->EntPressed(); }
707 void DCLGPS::ClrPressed() { _activePage->ClrPressed(); }
708 void DCLGPS::DtoPressed() {}
709 void DCLGPS::NrstPressed() {}
710 void DCLGPS::AltPressed() {}
712 void DCLGPS::OBSPressed() {
713 _obsMode = !_obsMode;
715 if(!_activeWaypoint.id.empty()) {
716 _obsHeading = static_cast<int>(_dtkMag);
718 // TODO - the _fromWaypoint location will change as the OBS heading changes.
719 // Might need to store the OBS initiation position somewhere in case it is needed again.
720 SetOBSFromWaypoint();
724 // Set the _fromWaypoint position based on the active waypoint and OBS radial.
725 void DCLGPS::SetOBSFromWaypoint() {
726 if(!_obsMode) return;
727 if(_activeWaypoint.id.empty()) return;
729 // TODO - base the 180 deg correction on the to/from flag.
730 _fromWaypoint = GetPositionOnMagRadial(_activeWaypoint, 10, _obsHeading + 180.0);
731 _fromWaypoint.id = "OBSWP";
734 void DCLGPS::MsgPressed() {}
736 void DCLGPS::CDIFSDIncrease() {
737 if(_currentCdiScaleIndex == 0) {
738 _currentCdiScaleIndex = _cdiScales.size() - 1;
740 _currentCdiScaleIndex--;
744 void DCLGPS::CDIFSDDecrease() {
745 _currentCdiScaleIndex++;
746 if(_currentCdiScaleIndex == _cdiScales.size()) {
747 _currentCdiScaleIndex = 0;
751 void DCLGPS::DrawChar(char c, int field, int px, int py, bool bold) {
754 void DCLGPS::DrawText(const string& s, int field, int px, int py, bool bold) {
757 void DCLGPS::SetBaroUnits(int n, bool wrap) {
759 _baroUnits = (GPSPressureUnits)(wrap ? 3 : 1);
761 _baroUnits = (GPSPressureUnits)(wrap ? 1 : 3);
763 _baroUnits = (GPSPressureUnits)n;
767 void DCLGPS::CreateDefaultFlightPlans() {}
769 // Get the time to the active waypoint in seconds.
770 // Returns -1 if groundspeed < 30 kts
771 double DCLGPS::GetTimeToActiveWaypoint() {
772 if(_groundSpeed_kts < 30.0) {
779 // Get the time to the final waypoint in seconds.
780 // Returns -1 if groundspeed < 30 kts
781 double DCLGPS::GetETE() {
782 if(_groundSpeed_kts < 30.0) {
785 // TODO - handle OBS / DTO operation appropriately
786 if(_activeFP->waypoints.empty()) {
789 return(GetTimeToWaypoint(_activeFP->waypoints[_activeFP->waypoints.size() - 1]->id));
794 // Get the time to a given waypoint (spec'd by ID) in seconds.
795 // returns -1 if groundspeed is less than 30kts.
796 // If the waypoint is an unreached part of the active flight plan the time will be via each leg.
797 // otherwise it will be a direct-to time.
798 double DCLGPS::GetTimeToWaypoint(const string& id) {
799 if(_groundSpeed_kts < 30.0) {
804 int n1 = GetActiveWaypointIndex();
805 int n2 = GetWaypointIndex(id);
808 for(unsigned int i=n1+1; i<_activeFP->waypoints.size(); ++i) {
809 GPSWaypoint* wp1 = _activeFP->waypoints[i-1];
810 GPSWaypoint* wp2 = _activeFP->waypoints[i];
811 double distm = GetGreatCircleDistance(wp1->lat, wp1->lon, wp2->lat, wp2->lon) * SG_NM_TO_METER;
812 eta += (distm / _groundSpeed_ms);
815 } else if(id == _activeWaypoint.id) {
818 const GPSWaypoint* wp = FindFirstByExactId(id);
819 if(wp == NULL) return(-1.0);
820 double distm = GetGreatCircleDistance(_gpsLat, _gpsLon, wp->lat, wp->lon);
822 return(distm / _groundSpeed_ms);
824 return(-1.0); // Hopefully we never get here!
827 // Returns magnetic great-circle heading
828 // TODO - document units.
829 float DCLGPS::GetHeadingToActiveWaypoint() {
830 if(_activeWaypoint.id.empty()) {
833 double h = GetMagHeadingFromTo(_gpsLat, _gpsLon, _activeWaypoint.lat, _activeWaypoint.lon);
834 while(h <= 0.0) h += 360.0;
835 while(h > 360.0) h -= 360.0;
840 // Returns magnetic great-circle heading
841 // TODO - what units?
842 float DCLGPS::GetHeadingFromActiveWaypoint() {
843 if(_activeWaypoint.id.empty()) {
846 double h = GetMagHeadingFromTo(_activeWaypoint.lat, _activeWaypoint.lon, _gpsLat, _gpsLon);
847 while(h <= 0.0) h += 360.0;
848 while(h > 360.0) h -= 360.0;
853 void DCLGPS::ClearFlightPlan(int n) {
854 for(unsigned int i=0; i<_flightPlans[n]->waypoints.size(); ++i) {
855 delete _flightPlans[n]->waypoints[i];
857 _flightPlans[n]->waypoints.clear();
860 void DCLGPS::ClearFlightPlan(GPSFlightPlan* fp) {
861 for(unsigned int i=0; i<fp->waypoints.size(); ++i) {
862 delete fp->waypoints[i];
864 fp->waypoints.clear();
867 int DCLGPS::GetActiveWaypointIndex() {
868 for(unsigned int i=0; i<_flightPlans[0]->waypoints.size(); ++i) {
869 if(_flightPlans[0]->waypoints[i]->id == _activeWaypoint.id) return((int)i);
874 int DCLGPS::GetWaypointIndex(const string& id) {
875 for(unsigned int i=0; i<_flightPlans[0]->waypoints.size(); ++i) {
876 if(_flightPlans[0]->waypoints[i]->id == id) return((int)i);
881 void DCLGPS::OrientateToFlightPlan(GPSFlightPlan* fp) {
882 //cout << "Orientating...\n";
883 //cout << "_lat = " << _lat << ", _lon = " << _lon << ", _gpsLat = " << _gpsLat << ", gpsLon = " << _gpsLon << '\n';
885 _activeWaypoint.id.clear();
889 if(fp->waypoints.size() == 1) {
890 // TODO - may need to flag nav here if not dto or obs, or possibly handle it somewhere else.
891 _activeWaypoint = *fp->waypoints[0];
892 _fromWaypoint.id.clear();
895 _fromWaypoint = *fp->waypoints[0];
896 _activeWaypoint = *fp->waypoints[1];
897 double dmin = 1000000; // nm!!
898 // For now we will simply start on the leg closest to our current position.
899 // It's possible that more fancy algorithms may take either heading or track
900 // into account when setting inital leg - I'm not sure.
901 // This method should handle most cases perfectly OK though.
902 for(unsigned int i = 1; i < fp->waypoints.size(); ++i) {
903 //cout << "Pass " << i << ", dmin = " << dmin << ", leg is " << fp->waypoints[i-1]->id << " to " << fp->waypoints[i]->id << '\n';
904 // First get the cross track correction.
905 double d0 = fabs(CalcCrossTrackDeviation(*fp->waypoints[i-1], *fp->waypoints[i]));
906 // That is the shortest distance away we could be though - check for
907 // longer distances if we are 'off the end' of the leg.
908 double ht1 = GetGreatCircleCourse(fp->waypoints[i-1]->lat, fp->waypoints[i-1]->lon,
909 fp->waypoints[i]->lat, fp->waypoints[i]->lon)
910 * SG_RADIANS_TO_DEGREES;
911 // not simply the reverse of the above due to great circle navigation.
912 double ht2 = GetGreatCircleCourse(fp->waypoints[i]->lat, fp->waypoints[i]->lon,
913 fp->waypoints[i-1]->lat, fp->waypoints[i-1]->lon)
914 * SG_RADIANS_TO_DEGREES;
915 double hw1 = GetGreatCircleCourse(_gpsLat, _gpsLon,
916 fp->waypoints[i]->lat, fp->waypoints[i]->lon)
917 * SG_RADIANS_TO_DEGREES;
918 double hw2 = GetGreatCircleCourse(_gpsLat, _gpsLon,
919 fp->waypoints[i-1]->lat, fp->waypoints[i-1]->lon)
920 * SG_RADIANS_TO_DEGREES;
921 double h1 = ht1 - hw1;
922 double h2 = ht2 - hw2;
923 //cout << "d0, h1, h2 = " << d0 << ", " << h1 << ", " << h2 << '\n';
924 //cout << "Normalizing...\n";
925 SG_NORMALIZE_RANGE(h1, -180.0, 180.0);
926 SG_NORMALIZE_RANGE(h2, -180.0, 180.0);
927 //cout << "d0, h1, h2 = " << d0 << ", " << h1 << ", " << h2 << '\n';
928 if(fabs(h1) > 90.0) {
929 // We are past the end of the to waypoint
930 double d = GetGreatCircleDistance(_gpsLat, _gpsLon, fp->waypoints[i]->lat, fp->waypoints[i]->lon);
932 //cout << "h1 triggered, d0 now = " << d0 << '\n';
933 } else if(fabs(h2) > 90.0) {
934 // We are past the end (not yet at!) the from waypoint
935 double d = GetGreatCircleDistance(_gpsLat, _gpsLon, fp->waypoints[i-1]->lat, fp->waypoints[i-1]->lon);
937 //cout << "h2 triggered, d0 now = " << d0 << '\n';
940 //cout << "THIS LEG NOW ACTIVE!\n";
942 _fromWaypoint = *fp->waypoints[i-1];
943 _activeWaypoint = *fp->waypoints[i];
950 void DCLGPS::OrientateToActiveFlightPlan() {
951 OrientateToFlightPlan(_activeFP);
954 /***************************************/
956 // Utility function - create a flightplan from a list of waypoint ids and types
957 void DCLGPS::CreateFlightPlan(GPSFlightPlan* fp, vector<string> ids, vector<GPSWpType> wps) {
958 if(fp == NULL) fp = new GPSFlightPlan;
960 if(!fp->waypoints.empty()) {
961 for(i=0; i<fp->waypoints.size(); ++i) {
962 delete fp->waypoints[i];
964 fp->waypoints.clear();
966 if(ids.size() != wps.size()) {
967 cout << "ID and Waypoint types list size mismatch in GPS::CreateFlightPlan - no flightplan created!\n";
970 for(i=0; i<ids.size(); ++i) {
974 GPSWaypoint* wp = new GPSWaypoint;
978 ap = FindFirstAptById(ids[i], multi, true);
983 wp->lat = ap->getLatitude() * SG_DEGREES_TO_RADIANS;
984 wp->lon = ap->getLongitude() * SG_DEGREES_TO_RADIANS;
986 fp->waypoints.push_back(wp);
990 np = FindFirstVorById(ids[i], multi, true);
995 wp->lat = np->get_lat() * SG_DEGREES_TO_RADIANS;
996 wp->lon = np->get_lon() * SG_DEGREES_TO_RADIANS;
998 fp->waypoints.push_back(wp);
1002 np = FindFirstNDBById(ids[i], multi, true);
1007 wp->lat = np->get_lat() * SG_DEGREES_TO_RADIANS;
1008 wp->lon = np->get_lon() * SG_DEGREES_TO_RADIANS;
1010 fp->waypoints.push_back(wp);
1023 /***************************************/
1026 * STL functor for use with algorithms. This comapres strings according to
1027 * the KLN-89's notion of ordering, with digits after letters.
1028 * Also implements FGIdentOrdering so it can be passed into the various list
1032 class stringOrderKLN89 : public FGIdentOrdering
1035 bool operator()(const gps_waypoint_map::value_type& aA, const std::string& aB) const
1037 return compare(aA.first, aB);
1040 bool operator()(const std::string& aS1, const std::string& aS2) const
1042 return compare(aS1, aS2);
1045 virtual bool compare(const std::string& aS1, const std::string& aS2) const
1047 if (aS1.empty()) return true;
1048 if (aS2.empty()) return false;
1050 char* a = (char*) aS1.c_str();
1051 char* b = (char*) aS2.c_str();
1053 for ( ; *a && *b; ++a, ++b) {
1054 if (*a == *b) continue;
1056 bool aDigit = isdigit(*a);
1057 bool bDigit = isdigit(*b);
1059 if (aDigit == bDigit) {
1060 return (*a < *b); // we already know they're not equal
1063 // digit-ness differs
1064 if (aDigit) return false; // s1 = KS9 goes *after* s2 = KSA
1066 return true; // s1 = KSF, s2 = KS5, s1 is indeed < s2
1069 if (*b) return true; // *a == 0, s2 is longer
1070 return false; // s1 is longer, or strings are equal
1074 GPSWaypoint* DCLGPS::FindFirstById(const string& id) const
1076 stringOrderKLN89 ordering;
1077 nav_list_type vors = globals->get_navlist()->findFirstByIdent(id, FG_NAV_VOR, false);
1078 nav_list_type ndbs = globals->get_navlist()->findFirstByIdent(id, FG_NAV_NDB, false);
1079 const FGFix* fix = globals->get_fixlist()->findFirstByIdent(id, &ordering);
1080 const FGAirport* apt = globals->get_airports()->findFirstById(id, &ordering);
1081 // search local gps waypoints (USR)
1083 // pick the best - ugly logic, sorry. This is a temporary fix to getting rid
1084 // of the huge local waypoint table, it'll die when there's a way to query
1085 // this stuff centrally.
1086 // what we're doing is using map inserts to order the result, then using
1087 // the first entry (begin()) as the lowest, hence best, match
1088 map<string, GPSWpType, stringOrderKLN89> sorter;
1089 if (fix) sorter[fix->get_ident()] = GPS_WP_INT;
1090 if (apt) sorter[apt->getId()] = GPS_WP_APT;
1091 if (!vors.empty()) sorter[vors.front()->get_ident()] = GPS_WP_VOR;
1092 if (!ndbs.empty()) sorter[ndbs.front()->get_ident()] = GPS_WP_NDB;
1094 if (sorter.empty()) return NULL; // no results at all
1095 GPSWpType ty = sorter.begin()->second;
1099 return GPSWaypoint::createFromFix(fix);
1102 return GPSWaypoint::createFromAirport(apt);
1105 return GPSWaypoint::createFromNav(vors.front());
1108 return GPSWaypoint::createFromNav(ndbs.front());
1110 return NULL; // can't happen
1114 GPSWaypoint* DCLGPS::FindFirstByExactId(const string& id) const
1116 if (const FGAirport* apt = globals->get_airports()->search(id)) {
1117 return GPSWaypoint::createFromAirport(apt);
1120 if (const FGFix* fix = globals->get_fixlist()->search(id)) {
1121 return GPSWaypoint::createFromFix(fix);
1124 nav_list_type vors = globals->get_navlist()->findFirstByIdent(id, FG_NAV_VOR, true);
1125 if (!vors.empty()) {
1126 return GPSWaypoint::createFromNav(vors.front());
1129 nav_list_type ndbs = globals->get_navlist()->findFirstByIdent(id, FG_NAV_NDB, true);
1130 if (!ndbs.empty()) {
1131 return GPSWaypoint::createFromNav(ndbs.front());
1137 // Host specific lookup functions
1138 // TODO - add the ASCII / alphabetical stuff from the Atlas version
1139 FGNavRecord* DCLGPS::FindFirstVorById(const string& id, bool &multi, bool exact) {
1140 // NOTE - at the moment multi is never set.
1142 //if(exact) return(_overlays->FindFirstVorById(id, exact));
1144 nav_list_type nav = globals->get_navlist()->findFirstByIdent(id, FG_NAV_VOR, exact);
1146 if(nav.size() > 1) multi = true;
1147 //return(nav.empty() ? NULL : *(nav.begin()));
1149 // The above is sort of what we want - unfortunately we can't guarantee no NDB/ILS at the moment
1150 if(nav.empty()) return(NULL);
1152 for(nav_list_iterator it = nav.begin(); it != nav.end(); ++it) {
1153 if((*it)->get_type() == 3) return(*it);
1155 return(NULL); // Shouldn't get here!
1158 // TODO - add the ASCII / alphabetical stuff from the Atlas version
1159 FGNavRecord* DCLGPS::FindFirstNDBById(const string& id, bool &multi, bool exact) {
1160 // NOTE - at the moment multi is never set.
1162 //if(exact) return(_overlays->FindFirstVorById(id, exact));
1164 nav_list_type nav = globals->get_navlist()->findFirstByIdent(id, FG_NAV_NDB, exact);
1166 if(nav.size() > 1) multi = true;
1167 //return(nav.empty() ? NULL : *(nav.begin()));
1169 // The above is sort of what we want - unfortunately we can't guarantee no NDB/ILS at the moment
1170 if(nav.empty()) return(NULL);
1172 for(nav_list_iterator it = nav.begin(); it != nav.end(); ++it) {
1173 if((*it)->get_type() == 2) return(*it);
1175 return(NULL); // Shouldn't get here!
1178 const FGFix* DCLGPS::FindFirstIntById(const string& id, bool &multi, bool exact) {
1179 // NOTE - at the moment multi is never set, and indeed can't be
1180 // since FG can only map one Fix per ID at the moment.
1182 if (exact) return globals->get_fixlist()->search(id);
1184 stringOrderKLN89 ordering;
1185 return globals->get_fixlist()->findFirstByIdent(id, &ordering);
1188 const FGAirport* DCLGPS::FindFirstAptById(const string& id, bool &multi, bool exact) {
1189 // NOTE - at the moment multi is never set.
1190 //cout << "FindFirstAptById, id = " << id << '\n';
1192 if(exact) return(globals->get_airports()->search(id));
1194 stringOrderKLN89 ordering;
1195 return globals->get_airports()->findFirstById(id, &ordering);
1198 FGNavRecord* DCLGPS::FindClosestVor(double lat_rad, double lon_rad) {
1199 return(globals->get_navlist()->findClosest(lon_rad, lat_rad, 0.0, FG_NAV_VOR));
1202 //----------------------------------------------------------------------------------------------------------
1204 // Takes lat and lon in RADIANS!!!!!!!
1205 double DCLGPS::GetMagHeadingFromTo(double latA, double lonA, double latB, double lonB) {
1206 double h = GetGreatCircleCourse(latA, lonA, latB, lonB);
1207 h *= SG_RADIANS_TO_DEGREES;
1208 // TODO - use the real altitude below instead of 0.0!
1209 //cout << "MagVar = " << sgGetMagVar(_gpsLon, _gpsLat, 0.0, _time->getJD()) * SG_RADIANS_TO_DEGREES << '\n';
1210 h -= sgGetMagVar(_gpsLon, _gpsLat, 0.0, _time->getJD()) * SG_RADIANS_TO_DEGREES;
1211 while(h >= 360.0) h -= 360.0;
1212 while(h < 0.0) h += 360.0;
1216 // ---------------- Great Circle formulae from "The Aviation Formulary" -------------
1217 // Note that all of these assume that the world is spherical.
1219 double Rad2Nm(double theta) {
1220 return(((180.0*60.0)/SG_PI)*theta);
1223 double Nm2Rad(double d) {
1224 return((SG_PI/(180.0*60.0))*d);
1229 The great circle distance d between two points with coordinates {lat1,lon1} and {lat2,lon2} is given by:
1231 d=acos(sin(lat1)*sin(lat2)+cos(lat1)*cos(lat2)*cos(lon1-lon2))
1233 A mathematically equivalent formula, which is less subject to rounding error for short distances is:
1235 d=2*asin(sqrt((sin((lat1-lat2)/2))^2 +
1236 cos(lat1)*cos(lat2)*(sin((lon1-lon2)/2))^2))
1240 // Returns distance in nm, takes lat & lon in RADIANS
1241 double DCLGPS::GetGreatCircleDistance(double lat1, double lon1, double lat2, double lon2) const {
1242 double d = 2.0 * asin(sqrt(((sin((lat1-lat2)/2.0))*(sin((lat1-lat2)/2.0))) +
1243 cos(lat1)*cos(lat2)*(sin((lon1-lon2)/2.0))*(sin((lon1-lon2)/2.0))));
1247 // fmod dosen't do what we want :-(
1248 static double mod(double d1, double d2) {
1249 return(d1 - d2*floor(d1/d2));
1252 // Returns great circle course from point 1 to point 2
1253 // Input and output in RADIANS.
1254 double DCLGPS::GetGreatCircleCourse (double lat1, double lon1, double lat2, double lon2) const {
1257 // Special case the poles
1258 if(cos(lat1) < SG_EPSILON) {
1260 // Starting from North Pole
1263 // Starting from South Pole
1267 // Urgh - the formula below is for negative lon +ve !!!???
1268 double d = GetGreatCircleDistance(lat1, lon1, lat2, lon2);
1269 cout << "d = " << d;
1271 //cout << ", d_theta = " << d;
1272 //cout << ", and d = " << Rad2Nm(d) << ' ';
1273 if(sin(lon2 - lon1) < 0) {
1275 h = acos((sin(lat2)-sin(lat1)*cos(d))/(sin(d)*cos(lat1)));
1278 h = 2.0 * SG_PI - acos((sin(lat2)-sin(lat1)*cos(d))/(sin(d)*cos(lat1)));
1281 cout << h * SG_RADIANS_TO_DEGREES << '\n';
1284 return( mod(atan2(sin(lon2-lon1)*cos(lat2),
1285 cos(lat1)*sin(lat2)-sin(lat1)*cos(lat2)*cos(lon2-lon1)),
1289 // Return a position on a radial from wp1 given distance d (nm) and magnetic heading h (degrees)
1290 // Note that d should be less that 1/4 Earth diameter!
1291 GPSWaypoint DCLGPS::GetPositionOnMagRadial(const GPSWaypoint& wp1, double d, double h) {
1292 h += sgGetMagVar(wp1.lon, wp1.lat, 0.0, _time->getJD()) * SG_RADIANS_TO_DEGREES;
1293 return(GetPositionOnRadial(wp1, d, h));
1296 // Return a position on a radial from wp1 given distance d (nm) and TRUE heading h (degrees)
1297 // Note that d should be less that 1/4 Earth diameter!
1298 GPSWaypoint DCLGPS::GetPositionOnRadial(const GPSWaypoint& wp1, double d, double h) {
1299 while(h < 0.0) h += 360.0;
1300 while(h > 360.0) h -= 360.0;
1302 h *= SG_DEGREES_TO_RADIANS;
1303 d *= (SG_PI / (180.0 * 60.0));
1305 double lat=asin(sin(wp1.lat)*cos(d)+cos(wp1.lat)*sin(d)*cos(h));
1308 lon=wp1.lon; // endpoint a pole
1310 lon=mod(wp1.lon+asin(sin(h)*sin(d)/cos(lat))+SG_PI,2*SG_PI)-SG_PI;
1316 wp.type = GPS_WP_VIRT;
1320 // Returns cross-track deviation in Nm.
1321 double DCLGPS::CalcCrossTrackDeviation() const {
1322 return(CalcCrossTrackDeviation(_fromWaypoint, _activeWaypoint));
1325 // Returns cross-track deviation of the current position between two arbitary waypoints in nm.
1326 double DCLGPS::CalcCrossTrackDeviation(const GPSWaypoint& wp1, const GPSWaypoint& wp2) const {
1327 //if(wp1 == NULL || wp2 == NULL) return(0.0);
1328 if(wp1.id.empty() || wp2.id.empty()) return(0.0);
1329 double xtd = asin(sin(Nm2Rad(GetGreatCircleDistance(wp1.lat, wp1.lon, _gpsLat, _gpsLon)))
1330 * sin(GetGreatCircleCourse(wp1.lat, wp1.lon, _gpsLat, _gpsLon) - GetGreatCircleCourse(wp1.lat, wp1.lon, wp2.lat, wp2.lon)));
1331 return(Rad2Nm(xtd));