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 - david.luff@nottingham.ac.uk
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., 675 Mass Ave, Cambridge, MA 02139, USA.
28 #include <simgear/sg_inlines.h>
32 //using namespace std;
34 // Command callbacks for FlightGear
36 static bool do_kln89_msg_pressed(const SGPropertyNode* arg) {
37 //cout << "do_kln89_msg_pressed called!\n";
38 DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89");
43 static bool do_kln89_obs_pressed(const SGPropertyNode* arg) {
44 //cout << "do_kln89_obs_pressed called!\n";
45 DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89");
50 static bool do_kln89_alt_pressed(const SGPropertyNode* arg) {
51 //cout << "do_kln89_alt_pressed called!\n";
52 DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89");
57 static bool do_kln89_nrst_pressed(const SGPropertyNode* arg) {
58 DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89");
63 static bool do_kln89_dto_pressed(const SGPropertyNode* arg) {
64 DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89");
69 static bool do_kln89_clr_pressed(const SGPropertyNode* arg) {
70 DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89");
75 static bool do_kln89_ent_pressed(const SGPropertyNode* arg) {
76 DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89");
81 static bool do_kln89_crsr_pressed(const SGPropertyNode* arg) {
82 DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89");
87 static bool do_kln89_knob1left1(const SGPropertyNode* arg) {
88 DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89");
93 static bool do_kln89_knob1right1(const SGPropertyNode* arg) {
94 DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89");
99 static bool do_kln89_knob2left1(const SGPropertyNode* arg) {
100 DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89");
105 static bool do_kln89_knob2right1(const SGPropertyNode* arg) {
106 DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89");
111 // End command callbacks
113 GPSWaypoint::GPSWaypoint() {
114 appType = GPS_APP_NONE;
117 GPSWaypoint::~GPSWaypoint() {}
119 string GPSWaypoint::GetAprId() {
120 if(appType == GPS_IAF) return(id + 'i');
121 else if(appType == GPS_FAF) return(id + 'f');
122 else if(appType == GPS_MAP) return(id + 'm');
123 else if(appType == GPS_MAHP) return(id + 'h');
127 ostream& operator << (ostream& os, GPSAppWpType type) {
129 case(GPS_IAF): return(os << "IAF");
130 case(GPS_IAP): return(os << "IAP");
131 case(GPS_FAF): return(os << "FAF");
132 case(GPS_MAP): return(os << "MAP");
133 case(GPS_MAHP): return(os << "MAHP");
134 case(GPS_HDR): return(os << "HEADER");
135 case(GPS_FENCE): return(os << "FENCE");
136 case(GPS_APP_NONE): return(os << "NONE");
138 return(os << "ERROR - Unknown switch in GPSAppWpType operator << ");
150 FGNPIAP::~FGNPIAP() {
153 GPSPage::GPSPage(DCLGPS* parent) {
158 GPSPage::~GPSPage() {
161 void GPSPage::Update(double dt) {}
163 void GPSPage::Knob1Left1() {}
164 void GPSPage::Knob1Right1() {}
166 void GPSPage::Knob2Left1() {
167 _parent->_activePage->LooseFocus();
169 if(_subPage < 0) _subPage = _nSubPages - 1;
172 void GPSPage::Knob2Right1() {
173 _parent->_activePage->LooseFocus();
175 if(_subPage >= _nSubPages) _subPage = 0;
178 void GPSPage::CrsrPressed() {}
179 void GPSPage::EntPressed() {}
180 void GPSPage::ClrPressed() {}
181 void GPSPage::DtoPressed() {}
182 void GPSPage::NrstPressed() {}
183 void GPSPage::AltPressed() {}
184 void GPSPage::OBSPressed() {}
185 void GPSPage::MsgPressed() {}
187 string GPSPage::GPSitoa(int n) {
189 // TODO - sanity check n!
190 sprintf(buf, "%i", n);
195 void GPSPage::CleanUp() {}
196 void GPSPage::LooseFocus() {}
197 void GPSPage::SetId(string s) {}
198 string GPSPage::GetId() { return(""); }
200 // ------------------------------------------------------------------------------------- //
202 DCLGPS::DCLGPS(RenderArea2D* instrument) {
203 _instrument = instrument;
208 // Units - lets default to US units - FG can set them to other units from config during startup if desired.
209 _altUnits = GPS_ALT_UNITS_FT;
210 _baroUnits = GPS_PRES_UNITS_IN;
211 _velUnits = GPS_VEL_UNITS_KT;
212 _distUnits = GPS_DIST_UNITS_NM;
214 _lon_node = fgGetNode("/instrumentation/gps/indicated-longitude-deg", true);
215 _lat_node = fgGetNode("/instrumentation/gps/indicated-latitude-deg", true);
216 _alt_node = fgGetNode("/instrumentation/gps/indicated-altitude-ft", true);
217 _grnd_speed_node = fgGetNode("/instrumentation/gps/indicated-ground-speed-kt", true);
218 _true_track_node = fgGetNode("/instrumentation/gps/indicated-track-true-deg", true);
219 _mag_track_node = fgGetNode("/instrumentation/gps/indicated-track-magnetic-deg", true);
221 // Use FG's position values at construction in case FG's gps has not run first update yet.
222 _lon = fgGetDouble("/position/longitude-deg") * SG_DEGREES_TO_RADIANS;
223 _lat = fgGetDouble("/position/latitude-deg") * SG_DEGREES_TO_RADIANS;
224 _alt = fgGetDouble("/position/altitude-ft");
225 // Note - we can depriciate _gpsLat and _gpsLon if we implement error handling in FG
226 // gps code and not our own.
231 _groundSpeed_ms = 0.0;
232 _groundSpeed_kts = 0.0;
236 // Sensible defaults. These can be overriden by derived classes if desired.
238 _cdiScales.push_back(5.0);
239 _cdiScales.push_back(1.0);
240 _cdiScales.push_back(0.3);
241 _currentCdiScaleIndex = 0;
242 _targetCdiScaleIndex = 0;
243 _sourceCdiScaleIndex = 0;
244 _cdiScaleTransition = false;
245 _currentCdiScale = 5.0;
249 _activeWaypoint.id.clear();
251 _crosstrackDist = 0.0;
252 _headingBugTo = true;
254 _waypointAlert = false;
256 _departureTimeString = "----";
259 // Configuration Initialisation
260 // Should this be in kln89.cxx ?
261 _turnAnticipationEnabled = false;
262 _suaAlertEnabled = false;
263 _altAlertEnabled = false;
267 _messageStack.clear();
271 _approachLoaded = false;
272 _approachArm = false;
273 _approachReallyArmed = false;
274 _approachActive = false;
275 _approachFP = new GPSFlightPlan;
280 for(gps_waypoint_map_iterator itr = _waypoints.begin(); itr != _waypoints.end(); ++itr) {
281 for(unsigned int i = 0; i < (*itr).second.size(); ++i) {
282 delete(((*itr).second)[i]);
285 delete _approachFP; // Don't need to delete the waypoints inside since they point to
286 // the waypoints in the approach database.
287 // TODO - may need to delete the approach database!!
290 void DCLGPS::draw() {
291 //cout << "draw called!\n";
295 void DCLGPS::init() {
296 globals->get_commands()->addCommand("kln89_msg_pressed", do_kln89_msg_pressed);
297 globals->get_commands()->addCommand("kln89_obs_pressed", do_kln89_obs_pressed);
298 globals->get_commands()->addCommand("kln89_alt_pressed", do_kln89_alt_pressed);
299 globals->get_commands()->addCommand("kln89_nrst_pressed", do_kln89_nrst_pressed);
300 globals->get_commands()->addCommand("kln89_dto_pressed", do_kln89_dto_pressed);
301 globals->get_commands()->addCommand("kln89_clr_pressed", do_kln89_clr_pressed);
302 globals->get_commands()->addCommand("kln89_ent_pressed", do_kln89_ent_pressed);
303 globals->get_commands()->addCommand("kln89_crsr_pressed", do_kln89_crsr_pressed);
304 globals->get_commands()->addCommand("kln89_knob1left1", do_kln89_knob1left1);
305 globals->get_commands()->addCommand("kln89_knob1right1", do_kln89_knob1right1);
306 globals->get_commands()->addCommand("kln89_knob2left1", do_kln89_knob2left1);
307 globals->get_commands()->addCommand("kln89_knob2right1", do_kln89_knob2right1);
309 // Build the GPS-specific databases.
310 // TODO - consider splitting into real life GPS database regions - eg Americas, Europe etc.
311 // Note that this needs to run after FG's airport and nav databases are up and running
313 const airport_list* apts = globals->get_airports()->getAirportList();
314 for(unsigned int i = 0; i < apts->size(); ++i) {
315 FGAirport* a = (*apts)[i];
316 GPSWaypoint* w = new GPSWaypoint;
318 w->lat = a->getLatitude() * SG_DEGREES_TO_RADIANS;
319 w->lon = a->getLongitude() * SG_DEGREES_TO_RADIANS;
320 w->type = GPS_WP_APT;
321 gps_waypoint_map_iterator wtr = _waypoints.find(a->getId());
322 if(wtr == _waypoints.end()) {
323 gps_waypoint_array arr;
325 _waypoints[w->id] = arr;
327 wtr->second.push_back(w);
330 nav_map_type navs = globals->get_navlist()->get_navaids();
331 for(nav_map_iterator itr = navs.begin(); itr != navs.end(); ++itr) {
332 nav_list_type nlst = itr->second;
333 for(unsigned int i = 0; i < nlst.size(); ++i) {
334 FGNavRecord* n = nlst[i];
335 if(n->get_fg_type() == FG_NAV_VOR || n->get_fg_type() == FG_NAV_NDB) { // We don't bother with ILS etc.
336 GPSWaypoint* w = new GPSWaypoint;
337 w->id = n->get_ident();
338 w->lat = n->get_lat() * SG_DEGREES_TO_RADIANS;
339 w->lon = n->get_lon() * SG_DEGREES_TO_RADIANS;
340 w->type = (n->get_fg_type() == FG_NAV_VOR ? GPS_WP_VOR : GPS_WP_NDB);
341 gps_waypoint_map_iterator wtr = _waypoints.find(n->get_ident());
342 if(wtr == _waypoints.end()) {
343 gps_waypoint_array arr;
345 _waypoints[w->id] = arr;
347 wtr->second.push_back(w);
352 const fix_map_type* fixes = globals->get_fixlist()->getFixList();
353 for(fix_map_const_iterator itr = fixes->begin(); itr != fixes->end(); ++itr) {
354 FGFix f = itr->second;
355 GPSWaypoint* w = new GPSWaypoint;
356 w->id = f.get_ident();
357 w->lat = f.get_lat() * SG_DEGREES_TO_RADIANS;
358 w->lon = f.get_lon() * SG_DEGREES_TO_RADIANS;
359 w->type = GPS_WP_INT;
360 gps_waypoint_map_iterator wtr = _waypoints.find(f.get_ident());
361 if(wtr == _waypoints.end()) {
362 gps_waypoint_array arr;
364 _waypoints[w->id] = arr;
366 wtr->second.push_back(w);
369 // TODO - add USR waypoints as well.
371 // Not sure if this should be here, but OK for now.
372 CreateDefaultFlightPlans();
374 // Hack - hardwire some instrument approaches for testing.
375 // TODO - read these from file - either all at startup or as needed.
376 FGNPIAP* iap = new FGNPIAP;
378 iap->_name = "VOR/DME OR GPS-B";
379 iap->_abbrev = "VOR/D";
385 GPSWaypoint* wp = new GPSWaypoint;
388 // Nasty using the find any function here, but it saves converting data from FGFix etc.
389 const GPSWaypoint* fp = FindFirstById(wp->id, multi, true);
391 wp->appType = GPS_IAF;
392 iap->_IAF.push_back(wp);
394 wp = new GPSWaypoint;
396 fp = FindFirstById(wp->id, multi, true);
398 wp->appType = GPS_IAF;
399 iap->_IAF.push_back(wp);
401 wp = new GPSWaypoint;
403 fp = FindFirstById(wp->id, multi, true);
405 wp->appType = GPS_IAP;
406 iap->_IAP.push_back(wp);
408 wp = new GPSWaypoint;
410 fp = FindFirstById(wp->id, multi, true);
412 wp->appType = GPS_FAF;
413 iap->_IAP.push_back(wp);
415 wp = new GPSWaypoint;
417 fp = FindFirstById(wp->id, multi, true);
419 wp->appType = GPS_MAP;
420 iap->_IAP.push_back(wp);
422 wp = new GPSWaypoint;
424 fp = FindFirstById(wp->id, multi, true);
426 wp->appType = GPS_MAHP;
427 iap->_MAP.push_back(wp);
429 _np_iap[iap->_id].push_back(iap);
430 // -----------------------
431 // -----------------------
434 iap->_name = "VOR OR GPS-A";
435 iap->_abbrev = "VOR-";
441 wp = new GPSWaypoint;
443 // Nasty using the find any function here, but it saves converting data from FGFix etc.
444 fp = FindFirstById(wp->id, multi, true);
446 wp->appType = GPS_IAF;
447 iap->_IAF.push_back(wp);
449 wp = new GPSWaypoint;
451 fp = FindFirstById(wp->id, multi, true);
453 wp->appType = GPS_IAF;
454 iap->_IAF.push_back(wp);
456 wp = new GPSWaypoint;
458 fp = FindFirstById(wp->id, multi, true);
460 wp->appType = GPS_IAP;
461 iap->_IAP.push_back(wp);
463 wp = new GPSWaypoint;
465 fp = FindFirstById(wp->id, multi, true);
467 wp->appType = GPS_FAF;
468 iap->_IAP.push_back(wp);
470 wp = new GPSWaypoint;
472 fp = FindFirstById(wp->id, multi, true);
474 wp->appType = GPS_MAP;
475 iap->_IAP.push_back(wp);
477 wp = new GPSWaypoint;
479 fp = FindFirstById(wp->id, multi, true);
481 wp->appType = GPS_MAHP;
482 iap->_MAP.push_back(wp);
484 _np_iap[iap->_id].push_back(iap);
485 // ------------------
486 // ------------------
488 // Ugh - don't load this one - the waypoints required aren't in fix.dat.gz - result: program crash!
489 // TODO - make the IAP loader robust to absent waypoints.
492 iap->_name = "GPS RWY 28L";
493 iap->_abbrev = "GPS";
494 iap->_rwyStr = "28L";
499 wp = new GPSWaypoint;
501 // Nasty using the find any function here, but it saves converting data from FGFix etc.
502 fp = FindFirstById(wp->id, multi, true);
504 wp->appType = GPS_IAF;
505 iap->_IAF.push_back(wp);
507 wp = new GPSWaypoint;
509 fp = FindFirstById(wp->id, multi, true);
511 wp->appType = GPS_IAF;
512 iap->_IAF.push_back(wp);
514 wp = new GPSWaypoint;
516 fp = FindFirstById(wp->id, multi, true);
518 wp->appType = GPS_IAP;
519 iap->_IAP.push_back(wp);
521 wp = new GPSWaypoint;
523 fp = FindFirstById(wp->id, multi, true);
525 wp->appType = GPS_FAF;
526 iap->_IAP.push_back(wp);
528 wp = new GPSWaypoint;
530 wp->appType = GPS_MAP;
531 if(wp->id.substr(0, 2) == "RW" && wp->appType == GPS_MAP) {
532 // Assume that this is a missed-approach point based on the runway number
533 // Get the runway threshold location etc
535 fp = FindFirstById(wp->id, multi, true);
537 cout << "Failed to find waypoint " << wp->id << " in database...\n";
542 iap->_IAP.push_back(wp);
544 wp = new GPSWaypoint;
546 fp = FindFirstById(wp->id, multi, true);
548 wp->appType = GPS_MAHP;
549 iap->_MAP.push_back(wp);
551 _np_iap[iap->_id].push_back(iap);
555 iap->_name = "GPS RWY 30";
556 iap->_abbrev = "GPS";
562 wp = new GPSWaypoint;
564 // Nasty using the find any function here, but it saves converting data from FGFix etc.
565 fp = FindFirstById(wp->id, multi, true);
567 wp->appType = GPS_IAF;
568 iap->_IAF.push_back(wp);
570 wp = new GPSWaypoint;
572 fp = FindFirstById(wp->id, multi, true);
574 wp->appType = GPS_IAF;
575 iap->_IAF.push_back(wp);
577 wp = new GPSWaypoint;
579 fp = FindFirstById(wp->id, multi, true);
581 wp->appType = GPS_IAP;
582 iap->_IAP.push_back(wp);
584 wp = new GPSWaypoint;
586 fp = FindFirstById(wp->id, multi, true);
588 wp->appType = GPS_FAF;
589 iap->_IAP.push_back(wp);
591 wp = new GPSWaypoint;
593 wp->appType = GPS_MAP;
594 if(wp->id.substr(0, 2) == "RW" && wp->appType == GPS_MAP) {
595 // Assume that this is a missed-approach point based on the runway number
596 // TODO: Get the runway threshold location etc
597 cout << "TODO - implement missed-approach point based on rwy no.\n";
599 fp = FindFirstById(wp->id, multi, true);
601 cout << "Failed to find waypoint " << wp->id << " in database...\n";
604 wp->appType = GPS_MAP;
607 iap->_IAP.push_back(wp);
609 wp = new GPSWaypoint;
611 fp = FindFirstById(wp->id, multi, true);
613 wp->appType = GPS_MAHP;
614 iap->_MAP.push_back(wp);
616 _np_iap[iap->_id].push_back(iap);
619 void DCLGPS::bind() {
620 fgTie("/instrumentation/gps/waypoint-alert", this, &DCLGPS::GetWaypointAlert);
621 fgTie("/instrumentation/gps/leg-mode", this, &DCLGPS::GetLegMode);
622 fgTie("/instrumentation/gps/obs-mode", this, &DCLGPS::GetOBSMode);
623 fgTie("/instrumentation/gps/approach-arm", this, &DCLGPS::GetApproachArm);
624 fgTie("/instrumentation/gps/approach-active", this, &DCLGPS::GetApproachActive);
625 fgTie("/instrumentation/gps/cdi-deflection", this, &DCLGPS::GetCDIDeflection);
626 fgTie("/instrumentation/gps/to-flag", this, &DCLGPS::GetToFlag);
629 void DCLGPS::unbind() {
630 fgUntie("/instrumentation/gps/waypoint-alert");
631 fgUntie("/instrumentation/gps/leg-mode");
632 fgUntie("/instrumentation/gps/obs-mode");
633 fgUntie("/instrumentation/gps/approach-arm");
634 fgUntie("/instrumentation/gps/approach-active");
635 fgUntie("/instrumentation/gps/cdi-deflection");
638 void DCLGPS::update(double dt) {
639 //cout << "update called!\n";
641 _lon = _lon_node->getDoubleValue() * SG_DEGREES_TO_RADIANS;
642 _lat = _lat_node->getDoubleValue() * SG_DEGREES_TO_RADIANS;
643 _alt = _alt_node->getDoubleValue();
644 _groundSpeed_kts = _grnd_speed_node->getDoubleValue();
645 _groundSpeed_ms = _groundSpeed_kts * 0.5144444444;
646 _track = _true_track_node->getDoubleValue();
647 _magTrackDeg = _mag_track_node->getDoubleValue();
648 // Note - we can depriciate _gpsLat and _gpsLon if we implement error handling in FG
649 // gps code and not our own.
652 // Check for abnormal position slew
653 if(GetGreatCircleDistance(_gpsLat, _gpsLon, _checkLat, _checkLon) > 1.0) {
654 OrientateToActiveFlightPlan();
660 if(_groundSpeed_kts > 30.0) {
662 string th = fgGetString("/instrumentation/clock/indicated-hour");
663 string tm = fgGetString("/instrumentation/clock/indicated-min");
664 if(th.size() == 1) th = "0" + th;
665 if(tm.size() == 1) tm = "0" + tm;
666 _departureTimeString = th + tm;
669 // TODO - check - is this prone to drift error over time?
670 // Should we difference the departure and current times?
671 // What about when the user resets the time of day from the menu?
675 _time->update(_gpsLon * SG_DEGREES_TO_RADIANS, _gpsLat * SG_DEGREES_TO_RADIANS, 0, 0);
676 // FIXME - currently all the below assumes leg mode and no DTO or OBS cancelled.
677 if(_activeFP->IsEmpty()) {
678 // Not sure if we need to reset these each update or only when fp altered
679 _activeWaypoint.id.clear();
681 } else if(_activeFP->waypoints.size() == 1) {
682 _activeWaypoint.id.clear();
685 if(_activeWaypoint.id.empty() || _fromWaypoint.id.empty()) {
686 //cout << "Error, in leg mode with flightplan of 2 or more waypoints, but either active or from wp is NULL!\n";
687 OrientateToActiveFlightPlan();
691 if(_approachLoaded) {
692 if(!_approachReallyArmed && !_approachActive) {
693 // arm if within 30nm of airport.
694 // TODO - let user cancel approach arm using external GPS-APR switch
696 const FGAirport* ap = FindFirstAptById(_approachID, multi, true);
698 double d = GetGreatCircleDistance(_gpsLat, _gpsLon, ap->getLatitude() * SG_DEGREES_TO_RADIANS, ap->getLongitude() * SG_DEGREES_TO_RADIANS);
701 _approachReallyArmed = true;
702 _messageStack.push_back("*Press ALT To Set Baro");
703 // Not sure what we do if the user has already set CDI to 0.3 nm?
704 _targetCdiScaleIndex = 1;
705 if(_currentCdiScaleIndex == 1) {
707 } else if(_currentCdiScaleIndex == 0) {
708 _sourceCdiScaleIndex = 0;
709 _cdiScaleTransition = true;
710 _cdiTransitionTime = 30.0;
711 _currentCdiScale = _cdiScales[_currentCdiScaleIndex];
716 // Check for approach active - we can only activate approach if it is really armed.
717 if(_activeWaypoint.appType == GPS_FAF) {
718 //cout << "Active waypoint is FAF, id is " << _activeWaypoint.id << '\n';
719 if(GetGreatCircleDistance(_gpsLat, _gpsLon, _activeWaypoint.lat, _activeWaypoint.lon) <= 2.0 && !_obsMode) {
720 // Assume heading is OK for now
721 _approachArm = false; // TODO - check - maybe arm is left on when actv comes on?
722 _approachReallyArmed = false;
723 _approachActive = true;
724 _targetCdiScaleIndex = 2;
725 if(_currentCdiScaleIndex == 2) {
727 } else if(_currentCdiScaleIndex == 1) {
728 _sourceCdiScaleIndex = 1;
729 _cdiScaleTransition = true;
730 _cdiTransitionTime = 30.0; // TODO - compress it if time to FAF < 30sec
731 _currentCdiScale = _cdiScales[_currentCdiScaleIndex];
733 // Abort going active?
734 _approachActive = false;
741 // CDI scale transition stuff
742 if(_cdiScaleTransition) {
743 if(fabs(_currentCdiScale - _cdiScales[_targetCdiScaleIndex]) < 0.001) {
744 _currentCdiScale = _cdiScales[_targetCdiScaleIndex];
745 _currentCdiScaleIndex = _targetCdiScaleIndex;
746 _cdiScaleTransition = false;
748 double scaleDiff = (_targetCdiScaleIndex > _sourceCdiScaleIndex
749 ? _cdiScales[_sourceCdiScaleIndex] - _cdiScales[_targetCdiScaleIndex]
750 : _cdiScales[_targetCdiScaleIndex] - _cdiScales[_sourceCdiScaleIndex]);
751 //cout << "ScaleDiff = " << scaleDiff << '\n';
752 if(_targetCdiScaleIndex > _sourceCdiScaleIndex) {
753 // Scaling down eg. 5nm -> 1nm
754 _currentCdiScale -= (scaleDiff * dt / _cdiTransitionTime);
755 if(_currentCdiScale < _cdiScales[_targetCdiScaleIndex]) {
756 _currentCdiScale = _cdiScales[_targetCdiScaleIndex];
757 _currentCdiScaleIndex = _targetCdiScaleIndex;
758 _cdiScaleTransition = false;
761 _currentCdiScale += (scaleDiff * dt / _cdiTransitionTime);
762 if(_currentCdiScale > _cdiScales[_targetCdiScaleIndex]) {
763 _currentCdiScale = _cdiScales[_targetCdiScaleIndex];
764 _currentCdiScaleIndex = _targetCdiScaleIndex;
765 _cdiScaleTransition = false;
768 //cout << "_currentCdiScale = " << _currentCdiScale << '\n';
771 _currentCdiScale = _cdiScales[_currentCdiScaleIndex];
775 // Urgh - I've been setting the heading bug based on DTK,
776 // bug I think it should be based on heading re. active waypoint
777 // based on what the sim does after the final waypoint is passed.
778 // (DTK remains the same, but if track is held == DTK heading bug
779 // reverses to from once wp is passed).
781 if(_fromWaypoint != NULL) {
782 // TODO - how do we handle the change of track with distance over long legs?
783 _dtkTrue = GetGreatCircleCourse(_fromWaypoint->lat, _fromWaypoint->lon, _activeWaypoint->lat, _activeWaypoint->lon) * SG_RADIANS_TO_DEGREES;
784 _dtkMag = GetMagHeadingFromTo(_fromWaypoint->lat, _fromWaypoint->lon, _activeWaypoint->lat, _activeWaypoint->lon);
785 // Don't change the heading bug if speed is too low otherwise it flickers to/from at rest
786 if(_groundSpeed_ms > 5) {
787 //cout << "track = " << _track << ", dtk = " << _dtkTrue << '\n';
788 double courseDev = _track - _dtkTrue;
789 //cout << "courseDev = " << courseDev << ", normalized = ";
790 SG_NORMALIZE_RANGE(courseDev, -180.0, 180.0);
791 //cout << courseDev << '\n';
792 _headingBugTo = (fabs(courseDev) > 90.0 ? false : true);
797 // TODO - in DTO operation the position of initiation of DTO defines the "from waypoint".
800 if(!_activeWaypoint.id.empty()) {
801 double hdgTrue = GetGreatCircleCourse(_gpsLat, _gpsLon, _activeWaypoint.lat, _activeWaypoint.lon) * SG_RADIANS_TO_DEGREES;
802 if(_groundSpeed_ms > 5) {
803 //cout << "track = " << _track << ", hdgTrue = " << hdgTrue << '\n';
804 double courseDev = _track - hdgTrue;
805 //cout << "courseDev = " << courseDev << ", normalized = ";
806 SG_NORMALIZE_RANGE(courseDev, -180.0, 180.0);
807 //cout << courseDev << '\n';
808 _headingBugTo = (fabs(courseDev) > 90.0 ? false : true);
810 if(!_fromWaypoint.id.empty()) {
811 _dtkTrue = GetGreatCircleCourse(_fromWaypoint.lat, _fromWaypoint.lon, _activeWaypoint.lat, _activeWaypoint.lon) * SG_RADIANS_TO_DEGREES;
812 _dtkMag = GetMagHeadingFromTo(_fromWaypoint.lat, _fromWaypoint.lon, _activeWaypoint.lat, _activeWaypoint.lon);
819 _dist2Act = GetGreatCircleDistance(_gpsLat, _gpsLon, _activeWaypoint.lat, _activeWaypoint.lon) * SG_NM_TO_METER;
820 if(_groundSpeed_ms > 10.0) {
821 _eta = _dist2Act / _groundSpeed_ms;
822 if(_eta <= 36) { // TODO - this is slightly different if turn anticipation is enabled.
824 _waypointAlert = true; // TODO - not if the from flag is set.
828 // Check if we should sequence to next leg.
829 // Perhaps this should be done on distance instead, but 60s time (about 1 - 2 nm) seems reasonable for now.
830 //double reverseHeading = GetGreatCircleCourse(_activeWaypoint->lat, _activeWaypoint->lon, _fromWaypoint->lat, _fromWaypoint->lon);
831 // Hack - let's cheat and do it on heading bug for now. TODO - that stops us 'cutting the corner'
832 // when we happen to approach the inside turn of a waypoint - we should probably sequence at the midpoint
833 // of the heading difference between legs in this instance.
834 int idx = GetActiveWaypointIndex();
835 bool finalLeg = (idx == (int)(_activeFP->waypoints.size()) - 1 ? true : false);
836 bool finalDto = (_dto && idx == -1); // Dto operation to a waypoint not in the flightplan - we don't sequence in this instance
839 // Do nothing - not sure if Dto should switch off when arriving at the final waypoint of a flightplan
840 } else if(finalDto) {
842 } else if(_activeWaypoint.appType == GPS_MAP) {
843 // Don't sequence beyond the missed approach point
844 cout << "ACTIVE WAYPOINT is MAP - not sequencing!!!!!\n";
846 cout << "Sequencing...\n";
847 _fromWaypoint = _activeWaypoint;
848 _activeWaypoint = *_activeFP->waypoints[idx + 1];
850 // TODO - course alteration message format is dependent on whether we are slaved HSI/CDI indicator or not.
851 // For now assume we are not.
853 if(fgGetBool("/instrumentation/nav[0]/slaved-to-gps")) {
854 // TODO - avoid the hardwiring on nav[0]
855 s = "Adj Nav Crs to ";
857 string s = "GPS Course is ";
859 double d = GetMagHeadingFromTo(_fromWaypoint.lat, _fromWaypoint.lon, _activeWaypoint.lat, _activeWaypoint.lon);
860 while(d < 0.0) d += 360.0;
861 while(d >= 360.0) d -= 360.0;
863 snprintf(buf, 4, "%03i", (int)(d + 0.5));
865 _messageStack.push_back(s);
867 _waypointAlert = false;
875 // First attempt at a sensible cross-track correction calculation
876 // Uh? - I think this is implemented further down the file!
877 if(_fromWaypoint != NULL) {
880 _crosstrackDist = 0.0;
886 double DCLGPS::GetCDIDeflection() const {
887 double xtd = CalcCrossTrackDeviation(); //nm
888 return((xtd / _currentCdiScale) * 5.0 * 2.5 * -1.0);
891 void DCLGPS::DtoInitiate(string s) {
892 cout << "DtoInitiate, s = " << s << '\n';
894 const GPSWaypoint* wp = FindFirstById(s, multi, true);
896 cout << "Waypoint found, starting dto operation!\n";
898 _activeWaypoint = *wp;
899 _fromWaypoint.lat = _gpsLat;
900 _fromWaypoint.lon = _gpsLon;
901 _fromWaypoint.type = GPS_WP_VIRT;
902 _fromWaypoint.id = "DTOWP";
904 cout << "Waypoint not found, ignoring dto request\n";
905 // Should bring up the user waypoint page, but we're not implementing that yet.
906 _dto = false; // TODO - implement this some day.
910 void DCLGPS::DtoCancel() {
912 // i.e. don't bother reorientating if we're just cancelling a DTO button press
913 // without having previously initiated DTO.
914 OrientateToActiveFlightPlan();
919 void DCLGPS::Knob1Left1() {}
920 void DCLGPS::Knob1Right1() {}
921 void DCLGPS::Knob2Left1() {}
922 void DCLGPS::Knob2Right1() {}
923 void DCLGPS::CrsrPressed() { _activePage->CrsrPressed(); }
924 void DCLGPS::EntPressed() { _activePage->EntPressed(); }
925 void DCLGPS::ClrPressed() { _activePage->ClrPressed(); }
926 void DCLGPS::DtoPressed() {}
927 void DCLGPS::NrstPressed() {}
928 void DCLGPS::AltPressed() {}
930 void DCLGPS::OBSPressed() {
931 _obsMode = !_obsMode;
933 if(!_activeWaypoint.id.empty()) {
934 _obsHeading = _dtkMag;
936 // TODO - the _fromWaypoint location will change as the OBS heading changes.
937 // Might need to store the OBS initiation position somewhere in case it is needed again.
938 SetOBSFromWaypoint();
942 // Set the _fromWaypoint position based on the active waypoint and OBS radial.
943 void DCLGPS::SetOBSFromWaypoint() {
944 if(!_obsMode) return;
945 if(_activeWaypoint.id.empty()) return;
947 // TODO - base the 180 deg correction on the to/from flag.
948 _fromWaypoint = GetPositionOnMagRadial(_activeWaypoint, 10, _obsHeading + 180.0);
949 _fromWaypoint.id = "OBSWP";
952 void DCLGPS::MsgPressed() {}
954 void DCLGPS::CDIFSDIncrease() {
955 if(_currentCdiScaleIndex == 0) {
956 _currentCdiScaleIndex = _cdiScales.size() - 1;
958 _currentCdiScaleIndex--;
962 void DCLGPS::CDIFSDDecrease() {
963 _currentCdiScaleIndex++;
964 if(_currentCdiScaleIndex == _cdiScales.size()) {
965 _currentCdiScaleIndex = 0;
969 void DCLGPS::DrawChar(char c, int field, int px, int py, bool bold) {
972 void DCLGPS::DrawText(const string& s, int field, int px, int py, bool bold) {
975 void DCLGPS::SetBaroUnits(int n, bool wrap) {
977 _baroUnits = (GPSPressureUnits)(wrap ? 3 : 1);
979 _baroUnits = (GPSPressureUnits)(wrap ? 1 : 3);
981 _baroUnits = (GPSPressureUnits)n;
985 void DCLGPS::CreateDefaultFlightPlans() {}
987 // Get the time to the active waypoint in seconds.
988 // Returns -1 if groundspeed < 30 kts
989 double DCLGPS::GetTimeToActiveWaypoint() {
990 if(_groundSpeed_kts < 30.0) {
997 // Get the time to the final waypoint in seconds.
998 // Returns -1 if groundspeed < 30 kts
999 double DCLGPS::GetETE() {
1000 if(_groundSpeed_kts < 30.0) {
1003 // TODO - handle OBS / DTO operation appropriately
1004 if(_activeFP->waypoints.empty()) {
1007 return(GetTimeToWaypoint(_activeFP->waypoints[_activeFP->waypoints.size() - 1]->id));
1012 // Get the time to a given waypoint (spec'd by ID) in seconds.
1013 // returns -1 if groundspeed is less than 30kts.
1014 // If the waypoint is an unreached part of the active flight plan the time will be via each leg.
1015 // otherwise it will be a direct-to time.
1016 double DCLGPS::GetTimeToWaypoint(string id) {
1017 if(_groundSpeed_kts < 30.0) {
1022 int n1 = GetActiveWaypointIndex();
1023 int n2 = GetWaypointIndex(id);
1026 for(unsigned int i=n1+1; i<_activeFP->waypoints.size(); ++i) {
1027 GPSWaypoint* wp1 = _activeFP->waypoints[i-1];
1028 GPSWaypoint* wp2 = _activeFP->waypoints[i];
1029 double distm = GetGreatCircleDistance(wp1->lat, wp1->lon, wp2->lat, wp2->lon) * SG_NM_TO_METER;
1030 eta += (distm / _groundSpeed_ms);
1033 } else if(id == _activeWaypoint.id) {
1037 const GPSWaypoint* wp = FindFirstById(id, multi, true);
1038 if(wp == NULL) return(-1.0);
1039 double distm = GetGreatCircleDistance(_gpsLat, _gpsLon, wp->lat, wp->lon);
1040 return(distm / _groundSpeed_ms);
1042 return(-1.0); // Hopefully we never get here!
1045 // Returns magnetic great-circle heading
1046 // TODO - document units.
1047 float DCLGPS::GetHeadingToActiveWaypoint() {
1048 if(_activeWaypoint.id.empty()) {
1051 double h = GetMagHeadingFromTo(_gpsLat, _gpsLon, _activeWaypoint.lat, _activeWaypoint.lon);
1052 while(h <= 0.0) h += 360.0;
1053 while(h > 360.0) h -= 360.0;
1058 // Returns magnetic great-circle heading
1059 // TODO - what units?
1060 float DCLGPS::GetHeadingFromActiveWaypoint() {
1061 if(_activeWaypoint.id.empty()) {
1064 double h = GetMagHeadingFromTo(_activeWaypoint.lat, _activeWaypoint.lon, _gpsLat, _gpsLon);
1065 while(h <= 0.0) h += 360.0;
1066 while(h > 360.0) h -= 360.0;
1071 void DCLGPS::ClearFlightPlan(int n) {
1072 for(unsigned int i=0; i<_flightPlans[n]->waypoints.size(); ++i) {
1073 delete _flightPlans[n]->waypoints[i];
1075 _flightPlans[n]->waypoints.clear();
1078 void DCLGPS::ClearFlightPlan(GPSFlightPlan* fp) {
1079 for(unsigned int i=0; i<fp->waypoints.size(); ++i) {
1080 delete fp->waypoints[i];
1082 fp->waypoints.clear();
1085 int DCLGPS::GetActiveWaypointIndex() {
1086 for(unsigned int i=0; i<_flightPlans[0]->waypoints.size(); ++i) {
1087 if(_flightPlans[0]->waypoints[i]->id == _activeWaypoint.id) return((int)i);
1092 int DCLGPS::GetWaypointIndex(string id) {
1093 for(unsigned int i=0; i<_flightPlans[0]->waypoints.size(); ++i) {
1094 if(_flightPlans[0]->waypoints[i]->id == id) return((int)i);
1099 void DCLGPS::OrientateToFlightPlan(GPSFlightPlan* fp) {
1100 //cout << "Orientating...\n";
1101 //cout << "_lat = " << _lat << ", _lon = " << _lon << ", _gpsLat = " << _gpsLat << ", gpsLon = " << _gpsLon << '\n';
1103 _activeWaypoint.id.clear();
1106 _navFlagged = false;
1107 if(fp->waypoints.size() == 1) {
1108 // TODO - may need to flag nav here if not dto or obs, or possibly handle it somewhere else.
1109 _activeWaypoint = *fp->waypoints[0];
1110 _fromWaypoint.id.clear();
1112 // FIXME FIXME FIXME
1113 _fromWaypoint = *fp->waypoints[0];
1114 _activeWaypoint = *fp->waypoints[1];
1115 double dmin = 1000000; // nm!!
1116 // For now we will simply start on the leg closest to our current position.
1117 // It's possible that more fancy algorithms may take either heading or track
1118 // into account when setting inital leg - I'm not sure.
1119 // This method should handle most cases perfectly OK though.
1120 for(unsigned int i = 1; i < fp->waypoints.size(); ++i) {
1121 //cout << "Pass " << i << ", dmin = " << dmin << ", leg is " << fp->waypoints[i-1]->id << " to " << fp->waypoints[i]->id << '\n';
1122 // First get the cross track correction.
1123 double d0 = fabs(CalcCrossTrackDeviation(*fp->waypoints[i-1], *fp->waypoints[i]));
1124 // That is the shortest distance away we could be though - check for
1125 // longer distances if we are 'off the end' of the leg.
1126 double ht1 = GetGreatCircleCourse(fp->waypoints[i-1]->lat, fp->waypoints[i-1]->lon,
1127 fp->waypoints[i]->lat, fp->waypoints[i]->lon)
1128 * SG_RADIANS_TO_DEGREES;
1129 // not simply the reverse of the above due to great circle navigation.
1130 double ht2 = GetGreatCircleCourse(fp->waypoints[i]->lat, fp->waypoints[i]->lon,
1131 fp->waypoints[i-1]->lat, fp->waypoints[i-1]->lon)
1132 * SG_RADIANS_TO_DEGREES;
1133 double hw1 = GetGreatCircleCourse(_gpsLat, _gpsLon,
1134 fp->waypoints[i]->lat, fp->waypoints[i]->lon)
1135 * SG_RADIANS_TO_DEGREES;
1136 double hw2 = GetGreatCircleCourse(_gpsLat, _gpsLon,
1137 fp->waypoints[i-1]->lat, fp->waypoints[i-1]->lon)
1138 * SG_RADIANS_TO_DEGREES;
1139 double h1 = ht1 - hw1;
1140 double h2 = ht2 - hw2;
1141 //cout << "d0, h1, h2 = " << d0 << ", " << h1 << ", " << h2 << '\n';
1142 //cout << "Normalizing...\n";
1143 SG_NORMALIZE_RANGE(h1, -180.0, 180.0);
1144 SG_NORMALIZE_RANGE(h2, -180.0, 180.0);
1145 //cout << "d0, h1, h2 = " << d0 << ", " << h1 << ", " << h2 << '\n';
1146 if(fabs(h1) > 90.0) {
1147 // We are past the end of the to waypoint
1148 double d = GetGreatCircleDistance(_gpsLat, _gpsLon, fp->waypoints[i]->lat, fp->waypoints[i]->lon);
1150 //cout << "h1 triggered, d0 now = " << d0 << '\n';
1151 } else if(fabs(h2) > 90.0) {
1152 // We are past the end (not yet at!) the from waypoint
1153 double d = GetGreatCircleDistance(_gpsLat, _gpsLon, fp->waypoints[i-1]->lat, fp->waypoints[i-1]->lon);
1155 //cout << "h2 triggered, d0 now = " << d0 << '\n';
1158 //cout << "THIS LEG NOW ACTIVE!\n";
1160 _fromWaypoint = *fp->waypoints[i-1];
1161 _activeWaypoint = *fp->waypoints[i];
1168 void DCLGPS::OrientateToActiveFlightPlan() {
1169 OrientateToFlightPlan(_activeFP);
1172 /***************************************/
1174 // Utility function - create a flightplan from a list of waypoint ids and types
1175 void DCLGPS::CreateFlightPlan(GPSFlightPlan* fp, vector<string> ids, vector<GPSWpType> wps) {
1176 if(fp == NULL) fp = new GPSFlightPlan;
1178 if(!fp->waypoints.empty()) {
1179 for(i=0; i<fp->waypoints.size(); ++i) {
1180 delete fp->waypoints[i];
1182 fp->waypoints.clear();
1184 if(ids.size() != wps.size()) {
1185 cout << "ID and Waypoint types list size mismatch in GPS::CreateFlightPlan - no flightplan created!\n";
1188 for(i=0; i<ids.size(); ++i) {
1190 const FGAirport* ap;
1192 GPSWaypoint* wp = new GPSWaypoint;
1196 ap = FindFirstAptById(ids[i], multi, true);
1201 wp->lat = ap->getLatitude() * SG_DEGREES_TO_RADIANS;
1202 wp->lon = ap->getLongitude() * SG_DEGREES_TO_RADIANS;
1204 fp->waypoints.push_back(wp);
1208 np = FindFirstVorById(ids[i], multi, true);
1213 wp->lat = np->get_lat() * SG_DEGREES_TO_RADIANS;
1214 wp->lon = np->get_lon() * SG_DEGREES_TO_RADIANS;
1216 fp->waypoints.push_back(wp);
1220 np = FindFirstNDBById(ids[i], multi, true);
1225 wp->lat = np->get_lat() * SG_DEGREES_TO_RADIANS;
1226 wp->lon = np->get_lon() * SG_DEGREES_TO_RADIANS;
1228 fp->waypoints.push_back(wp);
1241 /***************************************/
1243 const GPSWaypoint* DCLGPS::ActualFindFirstById(string id, bool exact) {
1244 gps_waypoint_map_const_iterator itr;
1246 itr = _waypoints.find(id);
1248 itr = _waypoints.lower_bound(id);
1250 if(itr == _waypoints.end()) {
1253 // TODO - don't just return the first one - either return all or the nearest one.
1254 return((itr->second)[0]);
1258 const GPSWaypoint* DCLGPS::FindFirstById(string id, bool &multi, bool exact) {
1260 if(exact) return(ActualFindFirstById(id, exact));
1262 // OK, that was the easy case, now the fuzzy case
1263 const GPSWaypoint* w1 = ActualFindFirstById(id);
1264 if(w1 == NULL) return(w1);
1266 // The non-trivial code from here to the end of the function is all to deal with the fact that
1267 // the KLN89 alphabetical order (numbers AFTER letters) differs from ASCII order (numbers BEFORE letters).
1269 //string id3 = id+'0';
1270 string id4 = id+'A';
1271 // Increment the last char to provide the boundary. Note that 'Z' -> '[' but we also need to check '0' for all since GPS has numbers after letters
1272 //bool alfa = isalpha(id2[id2.size() - 1]);
1273 id2[id2.size() - 1] = id2[id2.size() - 1] + 1;
1274 const GPSWaypoint* w2 = ActualFindFirstById(id2);
1275 //FGAirport* a3 = globals->get_airports()->findFirstById(id3);
1276 const GPSWaypoint* w4 = ActualFindFirstById(id4);
1277 //cout << "Strings sent were " << id << ", " << id2 << " and " << id4 << '\n';
1278 //cout << "Airports returned were (a1, a2, a4): " << a1->getId() << ", " << a2->getId() << ", " << a4->getId() << '\n';
1279 //cout << "Pointers were " << a1 << ", " << a2 << ", " << a4 << '\n';
1281 // TODO - the below handles the imediately following char OK
1282 // eg id = "KD" returns "KDAA" instead of "KD5"
1283 // but it doesn't handle numbers / letters further down the string,
1284 // eg - id = "I" returns "IA01" instead of "IAN"
1285 // We either need to provide a custom comparison operator, or recurse this function if !isalpha further down the string.
1286 // (Currenly fixed with recursion).
1288 if(w4 != w2) { // A-Z match - preferred
1289 //cout << "A-Z match!\n";
1290 if(w4->id.size() - id.size() > 2) {
1291 // Check for numbers further on
1292 for(unsigned int i=id.size(); i<w4->id.size(); ++i) {
1293 if(!isalpha(w4->id[i])) {
1294 //cout << "SUBSTR is " << (a4->getId()).substr(0, i) << '\n';
1295 return(FindFirstById(w4->id.substr(0, i), multi, exact));
1300 } else if(w1 != w2) { // 0-9 match
1301 //cout << "0-9 match!\n";
1302 if(w1->id.size() - id.size() > 2) {
1303 // Check for numbers further on
1304 for(unsigned int i=id.size(); i<w1->id.size(); ++i) {
1305 if(!isalpha(w1->id[i])) {
1306 //cout << "SUBSTR2 is " << (a4->getId()).substr(0, i) << '\n';
1307 return(FindFirstById(w1->id.substr(0, i), multi, exact));
1312 } else { // No match
1318 // Host specific lookup functions
1319 // TODO - add the ASCII / alphabetical stuff from the Atlas version
1320 FGNavRecord* DCLGPS::FindFirstVorById(string id, bool &multi, bool exact) {
1321 // NOTE - at the moment multi is never set.
1323 //if(exact) return(_overlays->FindFirstVorById(id, exact));
1325 nav_list_type nav = globals->get_navlist()->findFirstByIdent(id, FG_NAV_VOR, exact);
1327 if(nav.size() > 1) multi = true;
1328 //return(nav.empty() ? NULL : *(nav.begin()));
1330 // The above is sort of what we want - unfortunately we can't guarantee no NDB/ILS at the moment
1331 if(nav.empty()) return(NULL);
1333 for(nav_list_iterator it = nav.begin(); it != nav.end(); ++it) {
1334 if((*it)->get_type() == 3) return(*it);
1336 return(NULL); // Shouldn't get here!
1339 Overlays::NAV* DCLGPS::FindFirstVorById(string id, bool &multi, bool exact) {
1340 // NOTE - at the moment multi is never set.
1342 if(exact) return(_overlays->FindFirstVorById(id, exact));
1344 // OK, that was the easy case, now the fuzzy case
1345 Overlays::NAV* n1 = _overlays->FindFirstVorById(id);
1346 if(n1 == NULL) return(n1);
1349 string id3 = id+'0';
1350 string id4 = id+'A';
1351 // Increment the last char to provide the boundary. Note that 'Z' -> '[' but we also need to check '0' for all since GPS has numbers after letters
1352 bool alfa = isalpha(id2[id2.size() - 1]);
1353 id2[id2.size() - 1] = id2[id2.size() - 1] + 1;
1354 Overlays::NAV* n2 = _overlays->FindFirstVorById(id2);
1355 //Overlays::NAV* n3 = _overlays->FindFirstVorById(id3);
1356 //Overlays::NAV* n4 = _overlays->FindFirstVorById(id4);
1357 //cout << "Strings sent were " << id << ", " << id2 << ", " << id3 << ", " << id4 << '\n';
1361 if(n1 != n2) { // match
1370 // Something matches - the problem is the number/letter preference order is reversed between the GPS and the STL
1372 // There's a letter match - return that
1375 // By definition we must have a number match
1376 if(n3 == n2) cout << "HELP - LOGIC FLAW in find VOR!\n";
1388 // TODO - add the ASCII / alphabetical stuff from the Atlas version
1389 FGNavRecord* DCLGPS::FindFirstNDBById(string id, bool &multi, bool exact) {
1390 // NOTE - at the moment multi is never set.
1392 //if(exact) return(_overlays->FindFirstVorById(id, exact));
1394 nav_list_type nav = globals->get_navlist()->findFirstByIdent(id, FG_NAV_NDB, exact);
1396 if(nav.size() > 1) multi = true;
1397 //return(nav.empty() ? NULL : *(nav.begin()));
1399 // The above is sort of what we want - unfortunately we can't guarantee no NDB/ILS at the moment
1400 if(nav.empty()) return(NULL);
1402 for(nav_list_iterator it = nav.begin(); it != nav.end(); ++it) {
1403 if((*it)->get_type() == 2) return(*it);
1405 return(NULL); // Shouldn't get here!
1408 Overlays::NAV* DCLGPS::FindFirstNDBById(string id, bool &multi, bool exact) {
1409 // NOTE - at the moment multi is never set.
1411 if(exact) return(_overlays->FindFirstNDBById(id, exact));
1413 // OK, that was the easy case, now the fuzzy case
1414 Overlays::NAV* n1 = _overlays->FindFirstNDBById(id);
1415 if(n1 == NULL) return(n1);
1418 string id3 = id+'0';
1419 string id4 = id+'A';
1420 // Increment the last char to provide the boundary. Note that 'Z' -> '[' but we also need to check '0' for all since GPS has numbers after letters
1421 bool alfa = isalpha(id2[id2.size() - 1]);
1422 id2[id2.size() - 1] = id2[id2.size() - 1] + 1;
1423 Overlays::NAV* n2 = _overlays->FindFirstNDBById(id2);
1424 //Overlays::NAV* n3 = _overlays->FindFirstNDBById(id3);
1425 //Overlays::NAV* n4 = _overlays->FindFirstNDBById(id4);
1426 //cout << "Strings sent were " << id << ", " << id2 << ", " << id3 << ", " << id4 << '\n';
1430 if(n1 != n2) { // match
1439 // Something matches - the problem is the number/letter preference order is reversed between the GPS and the STL
1441 // There's a letter match - return that
1444 // By definition we must have a number match
1445 if(n3 == n2) cout << "HELP - LOGIC FLAW in find VOR!\n";
1457 // TODO - add the ASCII / alphabetical stuff from the Atlas version
1458 const FGFix* DCLGPS::FindFirstIntById(string id, bool &multi, bool exact) {
1459 // NOTE - at the moment multi is never set, and indeed can't be
1460 // since FG can only map one Fix per ID at the moment.
1462 if(exact) return(globals->get_fixlist()->findFirstByIdent(id, exact));
1464 const FGFix* f1 = globals->get_fixlist()->findFirstByIdent(id, exact);
1465 if(f1 == NULL) return(f1);
1467 // The non-trivial code from here to the end of the function is all to deal with the fact that
1468 // the KLN89 alphabetical order (numbers AFTER letters) differs from ASCII order (numbers BEFORE letters).
1469 // It is copied from the airport version which is definately needed, but at present I'm not actually
1470 // sure if any fixes in FG or real-life have numbers in them!
1472 //string id3 = id+'0';
1473 string id4 = id+'A';
1474 // Increment the last char to provide the boundary. Note that 'Z' -> '[' but we also need to check '0' for all since GPS has numbers after letters
1475 //bool alfa = isalpha(id2[id2.size() - 1]);
1476 id2[id2.size() - 1] = id2[id2.size() - 1] + 1;
1477 const FGFix* f2 = globals->get_fixlist()->findFirstByIdent(id2);
1478 //const FGFix* a3 = globals->get_fixlist()->findFirstByIdent(id3);
1479 const FGFix* f4 = globals->get_fixlist()->findFirstByIdent(id4);
1481 // TODO - the below handles the imediately following char OK
1482 // eg id = "KD" returns "KDAA" instead of "KD5"
1483 // but it doesn't handle numbers / letters further down the string,
1484 // eg - id = "I" returns "IA01" instead of "IAN"
1485 // We either need to provide a custom comparison operator, or recurse this function if !isalpha further down the string.
1486 // (Currenly fixed with recursion).
1488 if(f4 != f2) { // A-Z match - preferred
1489 //cout << "A-Z match!\n";
1490 if(f4->get_ident().size() - id.size() > 2) {
1491 // Check for numbers further on
1492 for(unsigned int i=id.size(); i<f4->get_ident().size(); ++i) {
1493 if(!isalpha(f4->get_ident()[i])) {
1494 //cout << "SUBSTR is " << (a4->getId()).substr(0, i) << '\n';
1495 return(FindFirstIntById(f4->get_ident().substr(0, i), multi, exact));
1500 } else if(f1 != f2) { // 0-9 match
1501 //cout << "0-9 match!\n";
1502 if(f1->get_ident().size() - id.size() > 2) {
1503 // Check for numbers further on
1504 for(unsigned int i=id.size(); i<f1->get_ident().size(); ++i) {
1505 if(!isalpha(f1->get_ident()[i])) {
1506 //cout << "SUBSTR2 is " << (a4->getId()).substr(0, i) << '\n';
1507 return(FindFirstIntById(f1->get_ident().substr(0, i), multi, exact));
1512 } else { // No match
1516 return NULL; // Don't think we can ever get here.
1519 const FGAirport* DCLGPS::FindFirstAptById(string id, bool &multi, bool exact) {
1520 // NOTE - at the moment multi is never set.
1521 //cout << "FindFirstAptById, id = " << id << '\n';
1523 if(exact) return(globals->get_airports()->findFirstById(id, exact));
1525 // OK, that was the easy case, now the fuzzy case
1526 const FGAirport* a1 = globals->get_airports()->findFirstById(id);
1527 if(a1 == NULL) return(a1);
1529 // The non-trivial code from here to the end of the function is all to deal with the fact that
1530 // the KLN89 alphabetical order (numbers AFTER letters) differs from ASCII order (numbers BEFORE letters).
1532 //string id3 = id+'0';
1533 string id4 = id+'A';
1534 // Increment the last char to provide the boundary. Note that 'Z' -> '[' but we also need to check '0' for all since GPS has numbers after letters
1535 //bool alfa = isalpha(id2[id2.size() - 1]);
1536 id2[id2.size() - 1] = id2[id2.size() - 1] + 1;
1537 const FGAirport* a2 = globals->get_airports()->findFirstById(id2);
1538 //FGAirport* a3 = globals->get_airports()->findFirstById(id3);
1539 const FGAirport* a4 = globals->get_airports()->findFirstById(id4);
1540 //cout << "Strings sent were " << id << ", " << id2 << " and " << id4 << '\n';
1541 //cout << "Airports returned were (a1, a2, a4): " << a1->getId() << ", " << a2->getId() << ", " << a4->getId() << '\n';
1542 //cout << "Pointers were " << a1 << ", " << a2 << ", " << a4 << '\n';
1544 // TODO - the below handles the imediately following char OK
1545 // eg id = "KD" returns "KDAA" instead of "KD5"
1546 // but it doesn't handle numbers / letters further down the string,
1547 // eg - id = "I" returns "IA01" instead of "IAN"
1548 // We either need to provide a custom comparison operator, or recurse this function if !isalpha further down the string.
1549 // (Currenly fixed with recursion).
1551 if(a4 != a2) { // A-Z match - preferred
1552 //cout << "A-Z match!\n";
1553 if(a4->getId().size() - id.size() > 2) {
1554 // Check for numbers further on
1555 for(unsigned int i=id.size(); i<a4->getId().size(); ++i) {
1556 if(!isalpha(a4->getId()[i])) {
1557 //cout << "SUBSTR is " << (a4->getId()).substr(0, i) << '\n';
1558 return(FindFirstAptById(a4->getId().substr(0, i), multi, exact));
1563 } else if(a1 != a2) { // 0-9 match
1564 //cout << "0-9 match!\n";
1565 if(a1->getId().size() - id.size() > 2) {
1566 // Check for numbers further on
1567 for(unsigned int i=id.size(); i<a1->getId().size(); ++i) {
1568 if(!isalpha(a1->getId()[i])) {
1569 //cout << "SUBSTR2 is " << (a4->getId()).substr(0, i) << '\n';
1570 return(FindFirstAptById(a1->getId().substr(0, i), multi, exact));
1575 } else { // No match
1582 FGNavRecord* DCLGPS::FindClosestVor(double lat_rad, double lon_rad) {
1583 return(globals->get_navlist()->findClosest(lon_rad, lat_rad, 0.0, FG_NAV_VOR));
1586 //----------------------------------------------------------------------------------------------------------
1588 // Takes lat and lon in RADIANS!!!!!!!
1589 double DCLGPS::GetMagHeadingFromTo(double latA, double lonA, double latB, double lonB) {
1590 double h = GetGreatCircleCourse(latA, lonA, latB, lonB);
1591 h *= SG_RADIANS_TO_DEGREES;
1592 // TODO - use the real altitude below instead of 0.0!
1593 //cout << "MagVar = " << sgGetMagVar(_gpsLon, _gpsLat, 0.0, _time->getJD()) * SG_RADIANS_TO_DEGREES << '\n';
1594 h -= sgGetMagVar(_gpsLon, _gpsLat, 0.0, _time->getJD()) * SG_RADIANS_TO_DEGREES;
1595 while(h >= 360.0) h -= 360.0;
1596 while(h < 0.0) h += 360.0;
1600 // ---------------- Great Circle formulae from "The Aviation Formulary" -------------
1601 // Note that all of these assume that the world is spherical.
1603 double Rad2Nm(double theta) {
1604 return(((180.0*60.0)/SG_PI)*theta);
1607 double Nm2Rad(double d) {
1608 return((SG_PI/(180.0*60.0))*d);
1613 The great circle distance d between two points with coordinates {lat1,lon1} and {lat2,lon2} is given by:
1615 d=acos(sin(lat1)*sin(lat2)+cos(lat1)*cos(lat2)*cos(lon1-lon2))
1617 A mathematically equivalent formula, which is less subject to rounding error for short distances is:
1619 d=2*asin(sqrt((sin((lat1-lat2)/2))^2 +
1620 cos(lat1)*cos(lat2)*(sin((lon1-lon2)/2))^2))
1624 // Returns distance in nm, takes lat & lon in RADIANS
1625 double DCLGPS::GetGreatCircleDistance(double lat1, double lon1, double lat2, double lon2) const {
1626 double d = 2.0 * asin(sqrt(((sin((lat1-lat2)/2.0))*(sin((lat1-lat2)/2.0))) +
1627 cos(lat1)*cos(lat2)*(sin((lon1-lon2)/2.0))*(sin((lon1-lon2)/2.0))));
1631 // fmod dosen't do what we want :-(
1632 static double mod(double d1, double d2) {
1633 return(d1 - d2*floor(d1/d2));
1636 // Returns great circle course from point 1 to point 2
1637 // Input and output in RADIANS.
1638 double DCLGPS::GetGreatCircleCourse (double lat1, double lon1, double lat2, double lon2) const {
1641 // Special case the poles
1642 if(cos(lat1) < SG_EPSILON) {
1644 // Starting from North Pole
1647 // Starting from South Pole
1651 // Urgh - the formula below is for negative lon +ve !!!???
1652 double d = GetGreatCircleDistance(lat1, lon1, lat2, lon2);
1653 cout << "d = " << d;
1655 //cout << ", d_theta = " << d;
1656 //cout << ", and d = " << Rad2Nm(d) << ' ';
1657 if(sin(lon2 - lon1) < 0) {
1659 h = acos((sin(lat2)-sin(lat1)*cos(d))/(sin(d)*cos(lat1)));
1662 h = 2.0 * SG_PI - acos((sin(lat2)-sin(lat1)*cos(d))/(sin(d)*cos(lat1)));
1665 cout << h * SG_RADIANS_TO_DEGREES << '\n';
1668 return( mod(atan2(sin(lon2-lon1)*cos(lat2),
1669 cos(lat1)*sin(lat2)-sin(lat1)*cos(lat2)*cos(lon2-lon1)),
1673 // Return a position on a radial from wp1 given distance d (nm) and magnetic heading h (degrees)
1674 // Note that d should be less that 1/4 Earth diameter!
1675 GPSWaypoint DCLGPS::GetPositionOnMagRadial(const GPSWaypoint& wp1, double d, double h) {
1676 h += sgGetMagVar(wp1.lon, wp1.lat, 0.0, _time->getJD()) * SG_RADIANS_TO_DEGREES;
1677 return(GetPositionOnRadial(wp1, d, h));
1680 // Return a position on a radial from wp1 given distance d (nm) and TRUE heading h (degrees)
1681 // Note that d should be less that 1/4 Earth diameter!
1682 GPSWaypoint DCLGPS::GetPositionOnRadial(const GPSWaypoint& wp1, double d, double h) {
1683 while(h < 0.0) h += 360.0;
1684 while(h > 360.0) h -= 360.0;
1686 h *= SG_DEGREES_TO_RADIANS;
1687 d *= (SG_PI / (180.0 * 60.0));
1689 double lat=asin(sin(wp1.lat)*cos(d)+cos(wp1.lat)*sin(d)*cos(h));
1692 lon=wp1.lon; // endpoint a pole
1694 lon=mod(wp1.lon+asin(sin(h)*sin(d)/cos(lat))+SG_PI,2*SG_PI)-SG_PI;
1700 wp.type = GPS_WP_VIRT;
1704 // Returns cross-track deviation in Nm.
1705 double DCLGPS::CalcCrossTrackDeviation() const {
1706 return(CalcCrossTrackDeviation(_fromWaypoint, _activeWaypoint));
1709 // Returns cross-track deviation of the current position between two arbitary waypoints in nm.
1710 double DCLGPS::CalcCrossTrackDeviation(const GPSWaypoint& wp1, const GPSWaypoint& wp2) const {
1711 //if(wp1 == NULL || wp2 == NULL) return(0.0);
1712 if(wp1.id.empty() || wp2.id.empty()) return(0.0);
1713 double xtd = asin(sin(Nm2Rad(GetGreatCircleDistance(wp1.lat, wp1.lon, _gpsLat, _gpsLon)))
1714 * sin(GetGreatCircleCourse(wp1.lat, wp1.lon, _gpsLat, _gpsLon) - GetGreatCircleCourse(wp1.lat, wp1.lon, wp2.lat, wp2.lon)));
1715 return(Rad2Nm(xtd));