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>
35 #include <Navaids/navrecord.hxx>
36 #include <Airports/simple.hxx>
42 // Command callbacks for FlightGear
44 static bool do_kln89_msg_pressed(const SGPropertyNode* arg) {
45 //cout << "do_kln89_msg_pressed called!\n";
46 DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89");
51 static bool do_kln89_obs_pressed(const SGPropertyNode* arg) {
52 //cout << "do_kln89_obs_pressed called!\n";
53 DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89");
58 static bool do_kln89_alt_pressed(const SGPropertyNode* arg) {
59 //cout << "do_kln89_alt_pressed called!\n";
60 DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89");
65 static bool do_kln89_nrst_pressed(const SGPropertyNode* arg) {
66 DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89");
71 static bool do_kln89_dto_pressed(const SGPropertyNode* arg) {
72 DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89");
77 static bool do_kln89_clr_pressed(const SGPropertyNode* arg) {
78 DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89");
83 static bool do_kln89_ent_pressed(const SGPropertyNode* arg) {
84 DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89");
89 static bool do_kln89_crsr_pressed(const SGPropertyNode* arg) {
90 DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89");
95 static bool do_kln89_knob1left1(const SGPropertyNode* arg) {
96 DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89");
101 static bool do_kln89_knob1right1(const SGPropertyNode* arg) {
102 DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89");
107 static bool do_kln89_knob2left1(const SGPropertyNode* arg) {
108 DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89");
113 static bool do_kln89_knob2right1(const SGPropertyNode* arg) {
114 DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89");
119 // End command callbacks
121 GPSWaypoint::GPSWaypoint() {
122 appType = GPS_APP_NONE;
125 GPSWaypoint::GPSWaypoint(const std::string& aIdent, float aLat, float aLon, GPSWpType aType) :
130 appType(GPS_APP_NONE)
134 GPSWaypoint::~GPSWaypoint() {}
136 string GPSWaypoint::GetAprId() {
137 if(appType == GPS_IAF) return(id + 'i');
138 else if(appType == GPS_FAF) return(id + 'f');
139 else if(appType == GPS_MAP) return(id + 'm');
140 else if(appType == GPS_MAHP) return(id + 'h');
145 GPSWpTypeFromFGPosType(FGPositioned::Type aType)
148 case FGPositioned::AIRPORT:
149 case FGPositioned::SEAPORT:
150 case FGPositioned::HELIPORT:
153 case FGPositioned::VOR:
156 case FGPositioned::NDB:
159 case FGPositioned::WAYPOINT:
162 case FGPositioned::FIX:
170 GPSWaypoint* GPSWaypoint::createFromPositioned(const FGPositioned* aPos)
173 return NULL; // happens if find returns no match
176 return new GPSWaypoint(aPos->ident(),
177 aPos->latitude() * SG_DEGREES_TO_RADIANS,
178 aPos->longitude() * SG_DEGREES_TO_RADIANS,
179 GPSWpTypeFromFGPosType(aPos->type())
183 ostream& operator << (ostream& os, GPSAppWpType type) {
185 case(GPS_IAF): return(os << "IAF");
186 case(GPS_IAP): return(os << "IAP");
187 case(GPS_FAF): return(os << "FAF");
188 case(GPS_MAP): return(os << "MAP");
189 case(GPS_MAHP): return(os << "MAHP");
190 case(GPS_HDR): return(os << "HEADER");
191 case(GPS_FENCE): return(os << "FENCE");
192 case(GPS_APP_NONE): return(os << "NONE");
194 return(os << "ERROR - Unknown switch in GPSAppWpType operator << ");
206 FGNPIAP::~FGNPIAP() {
209 ClockTime::ClockTime() {
214 ClockTime::ClockTime(int hr, int min) {
215 while(hr < 0) { hr += 24; }
217 while(min < 0) { min += 60; }
218 while(min > 60) { min -= 60; }
222 ClockTime::~ClockTime() {
225 GPSPage::GPSPage(DCLGPS* parent) {
230 GPSPage::~GPSPage() {
233 void GPSPage::Update(double dt) {}
235 void GPSPage::Knob1Left1() {}
236 void GPSPage::Knob1Right1() {}
238 void GPSPage::Knob2Left1() {
239 _parent->_activePage->LooseFocus();
241 if(_subPage < 0) _subPage = _nSubPages - 1;
244 void GPSPage::Knob2Right1() {
245 _parent->_activePage->LooseFocus();
247 if(_subPage >= _nSubPages) _subPage = 0;
250 void GPSPage::CrsrPressed() {}
251 void GPSPage::EntPressed() {}
252 void GPSPage::ClrPressed() {}
253 void GPSPage::DtoPressed() {}
254 void GPSPage::NrstPressed() {}
255 void GPSPage::AltPressed() {}
256 void GPSPage::OBSPressed() {}
257 void GPSPage::MsgPressed() {}
259 string GPSPage::GPSitoa(int n) {
261 snprintf(buf, 6, "%i", n);
266 void GPSPage::CleanUp() {}
267 void GPSPage::LooseFocus() {}
268 void GPSPage::SetId(const string& s) {}
270 // ------------------------------------------------------------------------------------- //
272 DCLGPS::DCLGPS(RenderArea2D* instrument) {
273 _instrument = instrument;
278 // Units - lets default to US units - FG can set them to other units from config during startup if desired.
279 _altUnits = GPS_ALT_UNITS_FT;
280 _baroUnits = GPS_PRES_UNITS_IN;
281 _velUnits = GPS_VEL_UNITS_KT;
282 _distUnits = GPS_DIST_UNITS_NM;
284 _lon_node = fgGetNode("/instrumentation/gps/indicated-longitude-deg", true);
285 _lat_node = fgGetNode("/instrumentation/gps/indicated-latitude-deg", true);
286 _alt_node = fgGetNode("/instrumentation/gps/indicated-altitude-ft", true);
287 _grnd_speed_node = fgGetNode("/instrumentation/gps/indicated-ground-speed-kt", true);
288 _true_track_node = fgGetNode("/instrumentation/gps/indicated-track-true-deg", true);
289 _mag_track_node = fgGetNode("/instrumentation/gps/indicated-track-magnetic-deg", true);
291 // Use FG's position values at construction in case FG's gps has not run first update yet.
292 _lon = fgGetDouble("/position/longitude-deg") * SG_DEGREES_TO_RADIANS;
293 _lat = fgGetDouble("/position/latitude-deg") * SG_DEGREES_TO_RADIANS;
294 _alt = fgGetDouble("/position/altitude-ft");
295 // Note - we can depriciate _gpsLat and _gpsLon if we implement error handling in FG
296 // gps code and not our own.
301 _groundSpeed_ms = 0.0;
302 _groundSpeed_kts = 0.0;
306 // Sensible defaults. These can be overriden by derived classes if desired.
308 _cdiScales.push_back(5.0);
309 _cdiScales.push_back(1.0);
310 _cdiScales.push_back(0.3);
311 _currentCdiScaleIndex = 0;
312 _targetCdiScaleIndex = 0;
313 _sourceCdiScaleIndex = 0;
314 _cdiScaleTransition = false;
315 _currentCdiScale = 5.0;
319 _activeWaypoint.id.clear();
321 _crosstrackDist = 0.0;
322 _headingBugTo = true;
324 _waypointAlert = false;
326 _departureTimeString = "----";
328 _powerOnTime.set_hr(0);
329 _powerOnTime.set_min(0);
330 _powerOnTimerSet = false;
333 // Configuration Initialisation
334 // Should this be in kln89.cxx ?
335 _turnAnticipationEnabled = false;
336 _suaAlertEnabled = false;
337 _altAlertEnabled = false;
341 _messageStack.clear();
345 _approachLoaded = false;
346 _approachArm = false;
347 _approachReallyArmed = false;
348 _approachActive = false;
349 _approachFP = new GPSFlightPlan;
354 delete _approachFP; // Don't need to delete the waypoints inside since they point to
355 // the waypoints in the approach database.
356 // TODO - may need to delete the approach database!!
359 void DCLGPS::draw(osg::State& state) {
360 _instrument->draw(state);
363 void DCLGPS::init() {
364 globals->get_commands()->addCommand("kln89_msg_pressed", do_kln89_msg_pressed);
365 globals->get_commands()->addCommand("kln89_obs_pressed", do_kln89_obs_pressed);
366 globals->get_commands()->addCommand("kln89_alt_pressed", do_kln89_alt_pressed);
367 globals->get_commands()->addCommand("kln89_nrst_pressed", do_kln89_nrst_pressed);
368 globals->get_commands()->addCommand("kln89_dto_pressed", do_kln89_dto_pressed);
369 globals->get_commands()->addCommand("kln89_clr_pressed", do_kln89_clr_pressed);
370 globals->get_commands()->addCommand("kln89_ent_pressed", do_kln89_ent_pressed);
371 globals->get_commands()->addCommand("kln89_crsr_pressed", do_kln89_crsr_pressed);
372 globals->get_commands()->addCommand("kln89_knob1left1", do_kln89_knob1left1);
373 globals->get_commands()->addCommand("kln89_knob1right1", do_kln89_knob1right1);
374 globals->get_commands()->addCommand("kln89_knob2left1", do_kln89_knob2left1);
375 globals->get_commands()->addCommand("kln89_knob2right1", do_kln89_knob2right1);
377 // Not sure if this should be here, but OK for now.
378 CreateDefaultFlightPlans();
381 void DCLGPS::bind() {
382 fgTie("/instrumentation/gps/waypoint-alert", this, &DCLGPS::GetWaypointAlert);
383 fgTie("/instrumentation/gps/leg-mode", this, &DCLGPS::GetLegMode);
384 fgTie("/instrumentation/gps/obs-mode", this, &DCLGPS::GetOBSMode);
385 fgTie("/instrumentation/gps/approach-arm", this, &DCLGPS::GetApproachArm);
386 fgTie("/instrumentation/gps/approach-active", this, &DCLGPS::GetApproachActive);
387 fgTie("/instrumentation/gps/cdi-deflection", this, &DCLGPS::GetCDIDeflection);
388 fgTie("/instrumentation/gps/to-flag", this, &DCLGPS::GetToFlag);
391 void DCLGPS::unbind() {
392 fgUntie("/instrumentation/gps/waypoint-alert");
393 fgUntie("/instrumentation/gps/leg-mode");
394 fgUntie("/instrumentation/gps/obs-mode");
395 fgUntie("/instrumentation/gps/approach-arm");
396 fgUntie("/instrumentation/gps/approach-active");
397 fgUntie("/instrumentation/gps/cdi-deflection");
400 void DCLGPS::update(double dt) {
401 //cout << "update called!\n";
403 _lon = _lon_node->getDoubleValue() * SG_DEGREES_TO_RADIANS;
404 _lat = _lat_node->getDoubleValue() * SG_DEGREES_TO_RADIANS;
405 _alt = _alt_node->getDoubleValue();
406 _groundSpeed_kts = _grnd_speed_node->getDoubleValue();
407 _groundSpeed_ms = _groundSpeed_kts * 0.5144444444;
408 _track = _true_track_node->getDoubleValue();
409 _magTrackDeg = _mag_track_node->getDoubleValue();
410 // Note - we can depriciate _gpsLat and _gpsLon if we implement error handling in FG
411 // gps code and not our own.
414 // Check for abnormal position slew
415 if(GetGreatCircleDistance(_gpsLat, _gpsLon, _checkLat, _checkLon) > 1.0) {
416 OrientateToActiveFlightPlan();
421 // TODO - check for unit power before running this.
422 if(!_powerOnTimerSet) {
426 // Check if an alarm timer has expired
428 if(_alarmTime.hr() == atoi(fgGetString("/instrumentation/clock/indicated-hour"))
429 && _alarmTime.min() == atoi(fgGetString("/instrumentation/clock/indicated-min"))) {
430 _messageStack.push_back("*Timer Expired");
436 if(_groundSpeed_kts > 30.0) {
438 string th = fgGetString("/instrumentation/clock/indicated-hour");
439 string tm = fgGetString("/instrumentation/clock/indicated-min");
440 if(th.size() == 1) th = "0" + th;
441 if(tm.size() == 1) tm = "0" + tm;
442 _departureTimeString = th + tm;
445 // TODO - check - is this prone to drift error over time?
446 // Should we difference the departure and current times?
447 // What about when the user resets the time of day from the menu?
451 _time->update(_gpsLon * SG_DEGREES_TO_RADIANS, _gpsLat * SG_DEGREES_TO_RADIANS, 0, 0);
452 // FIXME - currently all the below assumes leg mode and no DTO or OBS cancelled.
453 if(_activeFP->IsEmpty()) {
454 // Not sure if we need to reset these each update or only when fp altered
455 _activeWaypoint.id.clear();
457 } else if(_activeFP->waypoints.size() == 1) {
458 _activeWaypoint.id.clear();
461 if(_activeWaypoint.id.empty() || _fromWaypoint.id.empty()) {
462 //cout << "Error, in leg mode with flightplan of 2 or more waypoints, but either active or from wp is NULL!\n";
463 OrientateToActiveFlightPlan();
467 if(_approachLoaded) {
468 if(!_approachReallyArmed && !_approachActive) {
469 // arm if within 30nm of airport.
470 // TODO - let user cancel approach arm using external GPS-APR switch
472 const FGAirport* ap = FindFirstAptById(_approachID, multi, true);
474 double d = GetGreatCircleDistance(_gpsLat, _gpsLon, ap->getLatitude() * SG_DEGREES_TO_RADIANS, ap->getLongitude() * SG_DEGREES_TO_RADIANS);
477 _approachReallyArmed = true;
478 _messageStack.push_back("*Press ALT To Set Baro");
479 // Not sure what we do if the user has already set CDI to 0.3 nm?
480 _targetCdiScaleIndex = 1;
481 if(_currentCdiScaleIndex == 1) {
483 } else if(_currentCdiScaleIndex == 0) {
484 _sourceCdiScaleIndex = 0;
485 _cdiScaleTransition = true;
486 _cdiTransitionTime = 30.0;
487 _currentCdiScale = _cdiScales[_currentCdiScaleIndex];
492 // Check for approach active - we can only activate approach if it is really armed.
493 if(_activeWaypoint.appType == GPS_FAF) {
494 //cout << "Active waypoint is FAF, id is " << _activeWaypoint.id << '\n';
495 if(GetGreatCircleDistance(_gpsLat, _gpsLon, _activeWaypoint.lat, _activeWaypoint.lon) <= 2.0 && !_obsMode) {
496 // Assume heading is OK for now
497 _approachArm = false; // TODO - check - maybe arm is left on when actv comes on?
498 _approachReallyArmed = false;
499 _approachActive = true;
500 _targetCdiScaleIndex = 2;
501 if(_currentCdiScaleIndex == 2) {
503 } else if(_currentCdiScaleIndex == 1) {
504 _sourceCdiScaleIndex = 1;
505 _cdiScaleTransition = true;
506 _cdiTransitionTime = 30.0; // TODO - compress it if time to FAF < 30sec
507 _currentCdiScale = _cdiScales[_currentCdiScaleIndex];
509 // Abort going active?
510 _approachActive = false;
517 // CDI scale transition stuff
518 if(_cdiScaleTransition) {
519 if(fabs(_currentCdiScale - _cdiScales[_targetCdiScaleIndex]) < 0.001) {
520 _currentCdiScale = _cdiScales[_targetCdiScaleIndex];
521 _currentCdiScaleIndex = _targetCdiScaleIndex;
522 _cdiScaleTransition = false;
524 double scaleDiff = (_targetCdiScaleIndex > _sourceCdiScaleIndex
525 ? _cdiScales[_sourceCdiScaleIndex] - _cdiScales[_targetCdiScaleIndex]
526 : _cdiScales[_targetCdiScaleIndex] - _cdiScales[_sourceCdiScaleIndex]);
527 //cout << "ScaleDiff = " << scaleDiff << '\n';
528 if(_targetCdiScaleIndex > _sourceCdiScaleIndex) {
529 // Scaling down eg. 5nm -> 1nm
530 _currentCdiScale -= (scaleDiff * dt / _cdiTransitionTime);
531 if(_currentCdiScale < _cdiScales[_targetCdiScaleIndex]) {
532 _currentCdiScale = _cdiScales[_targetCdiScaleIndex];
533 _currentCdiScaleIndex = _targetCdiScaleIndex;
534 _cdiScaleTransition = false;
537 _currentCdiScale += (scaleDiff * dt / _cdiTransitionTime);
538 if(_currentCdiScale > _cdiScales[_targetCdiScaleIndex]) {
539 _currentCdiScale = _cdiScales[_targetCdiScaleIndex];
540 _currentCdiScaleIndex = _targetCdiScaleIndex;
541 _cdiScaleTransition = false;
544 //cout << "_currentCdiScale = " << _currentCdiScale << '\n';
547 _currentCdiScale = _cdiScales[_currentCdiScaleIndex];
551 // Urgh - I've been setting the heading bug based on DTK,
552 // bug I think it should be based on heading re. active waypoint
553 // based on what the sim does after the final waypoint is passed.
554 // (DTK remains the same, but if track is held == DTK heading bug
555 // reverses to from once wp is passed).
557 if(_fromWaypoint != NULL) {
558 // TODO - how do we handle the change of track with distance over long legs?
559 _dtkTrue = GetGreatCircleCourse(_fromWaypoint->lat, _fromWaypoint->lon, _activeWaypoint->lat, _activeWaypoint->lon) * SG_RADIANS_TO_DEGREES;
560 _dtkMag = GetMagHeadingFromTo(_fromWaypoint->lat, _fromWaypoint->lon, _activeWaypoint->lat, _activeWaypoint->lon);
561 // Don't change the heading bug if speed is too low otherwise it flickers to/from at rest
562 if(_groundSpeed_ms > 5) {
563 //cout << "track = " << _track << ", dtk = " << _dtkTrue << '\n';
564 double courseDev = _track - _dtkTrue;
565 //cout << "courseDev = " << courseDev << ", normalized = ";
566 SG_NORMALIZE_RANGE(courseDev, -180.0, 180.0);
567 //cout << courseDev << '\n';
568 _headingBugTo = (fabs(courseDev) > 90.0 ? false : true);
573 // TODO - in DTO operation the position of initiation of DTO defines the "from waypoint".
576 if(!_activeWaypoint.id.empty()) {
577 double hdgTrue = GetGreatCircleCourse(_gpsLat, _gpsLon, _activeWaypoint.lat, _activeWaypoint.lon) * SG_RADIANS_TO_DEGREES;
578 if(_groundSpeed_ms > 5) {
579 //cout << "track = " << _track << ", hdgTrue = " << hdgTrue << '\n';
580 double courseDev = _track - hdgTrue;
581 //cout << "courseDev = " << courseDev << ", normalized = ";
582 SG_NORMALIZE_RANGE(courseDev, -180.0, 180.0);
583 //cout << courseDev << '\n';
584 _headingBugTo = (fabs(courseDev) > 90.0 ? false : true);
586 if(!_fromWaypoint.id.empty()) {
587 _dtkTrue = GetGreatCircleCourse(_fromWaypoint.lat, _fromWaypoint.lon, _activeWaypoint.lat, _activeWaypoint.lon) * SG_RADIANS_TO_DEGREES;
588 _dtkMag = GetMagHeadingFromTo(_fromWaypoint.lat, _fromWaypoint.lon, _activeWaypoint.lat, _activeWaypoint.lon);
595 _dist2Act = GetGreatCircleDistance(_gpsLat, _gpsLon, _activeWaypoint.lat, _activeWaypoint.lon) * SG_NM_TO_METER;
596 if(_groundSpeed_ms > 10.0) {
597 _eta = _dist2Act / _groundSpeed_ms;
598 if(_eta <= 36) { // TODO - this is slightly different if turn anticipation is enabled.
600 _waypointAlert = true; // TODO - not if the from flag is set.
604 // Check if we should sequence to next leg.
605 // Perhaps this should be done on distance instead, but 60s time (about 1 - 2 nm) seems reasonable for now.
606 //double reverseHeading = GetGreatCircleCourse(_activeWaypoint->lat, _activeWaypoint->lon, _fromWaypoint->lat, _fromWaypoint->lon);
607 // Hack - let's cheat and do it on heading bug for now. TODO - that stops us 'cutting the corner'
608 // when we happen to approach the inside turn of a waypoint - we should probably sequence at the midpoint
609 // of the heading difference between legs in this instance.
610 int idx = GetActiveWaypointIndex();
611 bool finalLeg = (idx == (int)(_activeFP->waypoints.size()) - 1 ? true : false);
612 bool finalDto = (_dto && idx == -1); // Dto operation to a waypoint not in the flightplan - we don't sequence in this instance
615 // Do nothing - not sure if Dto should switch off when arriving at the final waypoint of a flightplan
616 } else if(finalDto) {
618 } else if(_activeWaypoint.appType == GPS_MAP) {
619 // Don't sequence beyond the missed approach point
620 //cout << "ACTIVE WAYPOINT is MAP - not sequencing!!!!!\n";
622 //cout << "Sequencing...\n";
623 _fromWaypoint = _activeWaypoint;
624 _activeWaypoint = *_activeFP->waypoints[idx + 1];
626 // TODO - course alteration message format is dependent on whether we are slaved HSI/CDI indicator or not.
627 // For now assume we are not.
629 if(fgGetBool("/instrumentation/nav[0]/slaved-to-gps")) {
630 // TODO - avoid the hardwiring on nav[0]
631 s = "Adj Nav Crs to ";
633 string s = "GPS Course is ";
635 double d = GetMagHeadingFromTo(_fromWaypoint.lat, _fromWaypoint.lon, _activeWaypoint.lat, _activeWaypoint.lon);
636 while(d < 0.0) d += 360.0;
637 while(d >= 360.0) d -= 360.0;
639 snprintf(buf, 4, "%03i", (int)(d + 0.5));
641 _messageStack.push_back(s);
643 _waypointAlert = false;
651 // First attempt at a sensible cross-track correction calculation
652 // Uh? - I think this is implemented further down the file!
653 if(_fromWaypoint != NULL) {
656 _crosstrackDist = 0.0;
662 // I don't yet fully understand all the gotchas about where to source time from.
663 // This function sets the initial timer before the clock exports properties
664 // and the one below uses the clock to be consistent with the rest of the code.
665 // It might change soonish...
666 void DCLGPS::SetPowerOnTimer() {
667 struct tm *t = globals->get_time_params()->getGmt();
668 _powerOnTime.set_hr(t->tm_hour);
669 _powerOnTime.set_min(t->tm_min);
670 _powerOnTimerSet = true;
673 void DCLGPS::ResetPowerOnTimer() {
674 _powerOnTime.set_hr(atoi(fgGetString("/instrumentation/clock/indicated-hour")));
675 _powerOnTime.set_min(atoi(fgGetString("/instrumentation/clock/indicated-min")));
676 _powerOnTimerSet = true;
679 double DCLGPS::GetCDIDeflection() const {
680 double xtd = CalcCrossTrackDeviation(); //nm
681 return((xtd / _currentCdiScale) * 5.0 * 2.5 * -1.0);
684 void DCLGPS::DtoInitiate(const string& s) {
685 //cout << "DtoInitiate, s = " << s << '\n';
686 const GPSWaypoint* wp = FindFirstByExactId(s);
688 //cout << "Waypoint found, starting dto operation!\n";
690 _activeWaypoint = *wp;
691 _fromWaypoint.lat = _gpsLat;
692 _fromWaypoint.lon = _gpsLon;
693 _fromWaypoint.type = GPS_WP_VIRT;
694 _fromWaypoint.id = "DTOWP";
697 //cout << "Waypoint not found, ignoring dto request\n";
698 // Should bring up the user waypoint page, but we're not implementing that yet.
699 _dto = false; // TODO - implement this some day.
703 void DCLGPS::DtoCancel() {
705 // i.e. don't bother reorientating if we're just cancelling a DTO button press
706 // without having previously initiated DTO.
707 OrientateToActiveFlightPlan();
712 void DCLGPS::Knob1Left1() {}
713 void DCLGPS::Knob1Right1() {}
714 void DCLGPS::Knob2Left1() {}
715 void DCLGPS::Knob2Right1() {}
716 void DCLGPS::CrsrPressed() { _activePage->CrsrPressed(); }
717 void DCLGPS::EntPressed() { _activePage->EntPressed(); }
718 void DCLGPS::ClrPressed() { _activePage->ClrPressed(); }
719 void DCLGPS::DtoPressed() {}
720 void DCLGPS::NrstPressed() {}
721 void DCLGPS::AltPressed() {}
723 void DCLGPS::OBSPressed() {
724 _obsMode = !_obsMode;
726 if(!_activeWaypoint.id.empty()) {
727 _obsHeading = static_cast<int>(_dtkMag);
729 // TODO - the _fromWaypoint location will change as the OBS heading changes.
730 // Might need to store the OBS initiation position somewhere in case it is needed again.
731 SetOBSFromWaypoint();
735 // Set the _fromWaypoint position based on the active waypoint and OBS radial.
736 void DCLGPS::SetOBSFromWaypoint() {
737 if(!_obsMode) return;
738 if(_activeWaypoint.id.empty()) return;
740 // TODO - base the 180 deg correction on the to/from flag.
741 _fromWaypoint = GetPositionOnMagRadial(_activeWaypoint, 10, _obsHeading + 180.0);
742 _fromWaypoint.id = "OBSWP";
745 void DCLGPS::MsgPressed() {}
747 void DCLGPS::CDIFSDIncrease() {
748 if(_currentCdiScaleIndex == 0) {
749 _currentCdiScaleIndex = _cdiScales.size() - 1;
751 _currentCdiScaleIndex--;
755 void DCLGPS::CDIFSDDecrease() {
756 _currentCdiScaleIndex++;
757 if(_currentCdiScaleIndex == _cdiScales.size()) {
758 _currentCdiScaleIndex = 0;
762 void DCLGPS::DrawChar(char c, int field, int px, int py, bool bold) {
765 void DCLGPS::DrawText(const string& s, int field, int px, int py, bool bold) {
768 void DCLGPS::SetBaroUnits(int n, bool wrap) {
770 _baroUnits = (GPSPressureUnits)(wrap ? 3 : 1);
772 _baroUnits = (GPSPressureUnits)(wrap ? 1 : 3);
774 _baroUnits = (GPSPressureUnits)n;
778 void DCLGPS::CreateDefaultFlightPlans() {}
780 // Get the time to the active waypoint in seconds.
781 // Returns -1 if groundspeed < 30 kts
782 double DCLGPS::GetTimeToActiveWaypoint() {
783 if(_groundSpeed_kts < 30.0) {
790 // Get the time to the final waypoint in seconds.
791 // Returns -1 if groundspeed < 30 kts
792 double DCLGPS::GetETE() {
793 if(_groundSpeed_kts < 30.0) {
796 // TODO - handle OBS / DTO operation appropriately
797 if(_activeFP->waypoints.empty()) {
800 return(GetTimeToWaypoint(_activeFP->waypoints[_activeFP->waypoints.size() - 1]->id));
805 // Get the time to a given waypoint (spec'd by ID) in seconds.
806 // returns -1 if groundspeed is less than 30kts.
807 // If the waypoint is an unreached part of the active flight plan the time will be via each leg.
808 // otherwise it will be a direct-to time.
809 double DCLGPS::GetTimeToWaypoint(const string& id) {
810 if(_groundSpeed_kts < 30.0) {
815 int n1 = GetActiveWaypointIndex();
816 int n2 = GetWaypointIndex(id);
819 for(unsigned int i=n1+1; i<_activeFP->waypoints.size(); ++i) {
820 GPSWaypoint* wp1 = _activeFP->waypoints[i-1];
821 GPSWaypoint* wp2 = _activeFP->waypoints[i];
822 double distm = GetGreatCircleDistance(wp1->lat, wp1->lon, wp2->lat, wp2->lon) * SG_NM_TO_METER;
823 eta += (distm / _groundSpeed_ms);
826 } else if(id == _activeWaypoint.id) {
829 const GPSWaypoint* wp = FindFirstByExactId(id);
830 if(wp == NULL) return(-1.0);
831 double distm = GetGreatCircleDistance(_gpsLat, _gpsLon, wp->lat, wp->lon);
833 return(distm / _groundSpeed_ms);
835 return(-1.0); // Hopefully we never get here!
838 // Returns magnetic great-circle heading
839 // TODO - document units.
840 float DCLGPS::GetHeadingToActiveWaypoint() {
841 if(_activeWaypoint.id.empty()) {
844 double h = GetMagHeadingFromTo(_gpsLat, _gpsLon, _activeWaypoint.lat, _activeWaypoint.lon);
845 while(h <= 0.0) h += 360.0;
846 while(h > 360.0) h -= 360.0;
851 // Returns magnetic great-circle heading
852 // TODO - what units?
853 float DCLGPS::GetHeadingFromActiveWaypoint() {
854 if(_activeWaypoint.id.empty()) {
857 double h = GetMagHeadingFromTo(_activeWaypoint.lat, _activeWaypoint.lon, _gpsLat, _gpsLon);
858 while(h <= 0.0) h += 360.0;
859 while(h > 360.0) h -= 360.0;
864 void DCLGPS::ClearFlightPlan(int n) {
865 for(unsigned int i=0; i<_flightPlans[n]->waypoints.size(); ++i) {
866 delete _flightPlans[n]->waypoints[i];
868 _flightPlans[n]->waypoints.clear();
871 void DCLGPS::ClearFlightPlan(GPSFlightPlan* fp) {
872 for(unsigned int i=0; i<fp->waypoints.size(); ++i) {
873 delete fp->waypoints[i];
875 fp->waypoints.clear();
878 int DCLGPS::GetActiveWaypointIndex() {
879 for(unsigned int i=0; i<_flightPlans[0]->waypoints.size(); ++i) {
880 if(_flightPlans[0]->waypoints[i]->id == _activeWaypoint.id) return((int)i);
885 int DCLGPS::GetWaypointIndex(const string& id) {
886 for(unsigned int i=0; i<_flightPlans[0]->waypoints.size(); ++i) {
887 if(_flightPlans[0]->waypoints[i]->id == id) return((int)i);
892 void DCLGPS::OrientateToFlightPlan(GPSFlightPlan* fp) {
893 //cout << "Orientating...\n";
894 //cout << "_lat = " << _lat << ", _lon = " << _lon << ", _gpsLat = " << _gpsLat << ", gpsLon = " << _gpsLon << '\n';
896 _activeWaypoint.id.clear();
900 if(fp->waypoints.size() == 1) {
901 // TODO - may need to flag nav here if not dto or obs, or possibly handle it somewhere else.
902 _activeWaypoint = *fp->waypoints[0];
903 _fromWaypoint.id.clear();
906 _fromWaypoint = *fp->waypoints[0];
907 _activeWaypoint = *fp->waypoints[1];
908 double dmin = 1000000; // nm!!
909 // For now we will simply start on the leg closest to our current position.
910 // It's possible that more fancy algorithms may take either heading or track
911 // into account when setting inital leg - I'm not sure.
912 // This method should handle most cases perfectly OK though.
913 for(unsigned int i = 1; i < fp->waypoints.size(); ++i) {
914 //cout << "Pass " << i << ", dmin = " << dmin << ", leg is " << fp->waypoints[i-1]->id << " to " << fp->waypoints[i]->id << '\n';
915 // First get the cross track correction.
916 double d0 = fabs(CalcCrossTrackDeviation(*fp->waypoints[i-1], *fp->waypoints[i]));
917 // That is the shortest distance away we could be though - check for
918 // longer distances if we are 'off the end' of the leg.
919 double ht1 = GetGreatCircleCourse(fp->waypoints[i-1]->lat, fp->waypoints[i-1]->lon,
920 fp->waypoints[i]->lat, fp->waypoints[i]->lon)
921 * SG_RADIANS_TO_DEGREES;
922 // not simply the reverse of the above due to great circle navigation.
923 double ht2 = GetGreatCircleCourse(fp->waypoints[i]->lat, fp->waypoints[i]->lon,
924 fp->waypoints[i-1]->lat, fp->waypoints[i-1]->lon)
925 * SG_RADIANS_TO_DEGREES;
926 double hw1 = GetGreatCircleCourse(_gpsLat, _gpsLon,
927 fp->waypoints[i]->lat, fp->waypoints[i]->lon)
928 * SG_RADIANS_TO_DEGREES;
929 double hw2 = GetGreatCircleCourse(_gpsLat, _gpsLon,
930 fp->waypoints[i-1]->lat, fp->waypoints[i-1]->lon)
931 * SG_RADIANS_TO_DEGREES;
932 double h1 = ht1 - hw1;
933 double h2 = ht2 - hw2;
934 //cout << "d0, h1, h2 = " << d0 << ", " << h1 << ", " << h2 << '\n';
935 //cout << "Normalizing...\n";
936 SG_NORMALIZE_RANGE(h1, -180.0, 180.0);
937 SG_NORMALIZE_RANGE(h2, -180.0, 180.0);
938 //cout << "d0, h1, h2 = " << d0 << ", " << h1 << ", " << h2 << '\n';
939 if(fabs(h1) > 90.0) {
940 // We are past the end of the to waypoint
941 double d = GetGreatCircleDistance(_gpsLat, _gpsLon, fp->waypoints[i]->lat, fp->waypoints[i]->lon);
943 //cout << "h1 triggered, d0 now = " << d0 << '\n';
944 } else if(fabs(h2) > 90.0) {
945 // We are past the end (not yet at!) the from waypoint
946 double d = GetGreatCircleDistance(_gpsLat, _gpsLon, fp->waypoints[i-1]->lat, fp->waypoints[i-1]->lon);
948 //cout << "h2 triggered, d0 now = " << d0 << '\n';
951 //cout << "THIS LEG NOW ACTIVE!\n";
953 _fromWaypoint = *fp->waypoints[i-1];
954 _activeWaypoint = *fp->waypoints[i];
961 void DCLGPS::OrientateToActiveFlightPlan() {
962 OrientateToFlightPlan(_activeFP);
965 /***************************************/
967 // Utility function - create a flightplan from a list of waypoint ids and types
968 void DCLGPS::CreateFlightPlan(GPSFlightPlan* fp, vector<string> ids, vector<GPSWpType> wps) {
969 if(fp == NULL) fp = new GPSFlightPlan;
971 if(!fp->waypoints.empty()) {
972 for(i=0; i<fp->waypoints.size(); ++i) {
973 delete fp->waypoints[i];
975 fp->waypoints.clear();
977 if(ids.size() != wps.size()) {
978 cout << "ID and Waypoint types list size mismatch in GPS::CreateFlightPlan - no flightplan created!\n";
981 for(i=0; i<ids.size(); ++i) {
985 GPSWaypoint* wp = new GPSWaypoint;
989 ap = FindFirstAptById(ids[i], multi, true);
994 wp->lat = ap->getLatitude() * SG_DEGREES_TO_RADIANS;
995 wp->lon = ap->getLongitude() * SG_DEGREES_TO_RADIANS;
997 fp->waypoints.push_back(wp);
1001 np = FindFirstVorById(ids[i], multi, true);
1006 wp->lat = np->get_lat() * SG_DEGREES_TO_RADIANS;
1007 wp->lon = np->get_lon() * SG_DEGREES_TO_RADIANS;
1009 fp->waypoints.push_back(wp);
1013 np = FindFirstNDBById(ids[i], multi, true);
1018 wp->lat = np->get_lat() * SG_DEGREES_TO_RADIANS;
1019 wp->lon = np->get_lon() * SG_DEGREES_TO_RADIANS;
1021 fp->waypoints.push_back(wp);
1037 /***************************************/
1039 class DCLGPSFilter : public FGPositioned::Filter
1042 virtual bool pass(const FGPositioned* aPos) const {
1043 switch (aPos->type()) {
1044 case FGPositioned::AIRPORT:
1045 // how about heliports and seaports?
1046 case FGPositioned::NDB:
1047 case FGPositioned::VOR:
1048 case FGPositioned::WAYPOINT:
1049 case FGPositioned::FIX:
1051 default: return false; // reject all other types
1057 GPSWaypoint* DCLGPS::FindFirstById(const string& id) const
1059 DCLGPSFilter filter;
1060 FGPositionedRef result = FGPositioned::findNextWithPartialId(NULL, id, &filter);
1061 return GPSWaypoint::createFromPositioned(result);
1064 GPSWaypoint* DCLGPS::FindFirstByExactId(const string& id) const
1066 SGGeod pos(SGGeod::fromRad(_lon, _lat));
1067 FGPositionedRef result = FGPositioned::findClosestWithIdent(id, pos);
1068 return GPSWaypoint::createFromPositioned(result);
1071 // TODO - add the ASCII / alphabetical stuff from the Atlas version
1072 FGPositioned* DCLGPS::FindTypedFirstById(const string& id, FGPositioned::Type ty, bool &multi, bool exact)
1075 FGPositioned::TypeFilter filter(ty);
1078 FGPositioned::List matches =
1079 FGPositioned::findAllWithIdentSortedByRange(id, SGGeod::fromRad(_lon, _lat), &filter);
1080 multi = (matches.size() > 1);
1081 return matches.empty() ? NULL : matches.front().ptr();
1084 return FGPositioned::findNextWithPartialId(NULL, id, &filter);
1087 FGNavRecord* DCLGPS::FindFirstVorById(const string& id, bool &multi, bool exact)
1089 return dynamic_cast<FGNavRecord*>(FindTypedFirstById(id, FGPositioned::VOR, multi, exact));
1092 FGNavRecord* DCLGPS::FindFirstNDBById(const string& id, bool &multi, bool exact)
1094 return dynamic_cast<FGNavRecord*>(FindTypedFirstById(id, FGPositioned::NDB, multi, exact));
1097 const FGFix* DCLGPS::FindFirstIntById(const string& id, bool &multi, bool exact)
1099 return dynamic_cast<FGFix*>(FindTypedFirstById(id, FGPositioned::FIX, multi, exact));
1102 const FGAirport* DCLGPS::FindFirstAptById(const string& id, bool &multi, bool exact)
1104 return dynamic_cast<FGAirport*>(FindTypedFirstById(id, FGPositioned::AIRPORT, multi, exact));
1107 FGNavRecord* DCLGPS::FindClosestVor(double lat_rad, double lon_rad) {
1108 FGPositioned::TypeFilter filter(FGPositioned::VOR);
1109 double cutoff = 1000; // nautical miles
1110 FGPositionedRef v = FGPositioned::findClosest(SGGeod::fromRad(lon_rad, lat_rad), cutoff, &filter);
1115 return dynamic_cast<FGNavRecord*>(v.ptr());
1118 //----------------------------------------------------------------------------------------------------------
1120 // Takes lat and lon in RADIANS!!!!!!!
1121 double DCLGPS::GetMagHeadingFromTo(double latA, double lonA, double latB, double lonB) {
1122 double h = GetGreatCircleCourse(latA, lonA, latB, lonB);
1123 h *= SG_RADIANS_TO_DEGREES;
1124 // TODO - use the real altitude below instead of 0.0!
1125 //cout << "MagVar = " << sgGetMagVar(_gpsLon, _gpsLat, 0.0, _time->getJD()) * SG_RADIANS_TO_DEGREES << '\n';
1126 h -= sgGetMagVar(_gpsLon, _gpsLat, 0.0, _time->getJD()) * SG_RADIANS_TO_DEGREES;
1127 while(h >= 360.0) h -= 360.0;
1128 while(h < 0.0) h += 360.0;
1132 // ---------------- Great Circle formulae from "The Aviation Formulary" -------------
1133 // Note that all of these assume that the world is spherical.
1135 double Rad2Nm(double theta) {
1136 return(((180.0*60.0)/SG_PI)*theta);
1139 double Nm2Rad(double d) {
1140 return((SG_PI/(180.0*60.0))*d);
1145 The great circle distance d between two points with coordinates {lat1,lon1} and {lat2,lon2} is given by:
1147 d=acos(sin(lat1)*sin(lat2)+cos(lat1)*cos(lat2)*cos(lon1-lon2))
1149 A mathematically equivalent formula, which is less subject to rounding error for short distances is:
1151 d=2*asin(sqrt((sin((lat1-lat2)/2))^2 +
1152 cos(lat1)*cos(lat2)*(sin((lon1-lon2)/2))^2))
1156 // Returns distance in nm, takes lat & lon in RADIANS
1157 double DCLGPS::GetGreatCircleDistance(double lat1, double lon1, double lat2, double lon2) const {
1158 double d = 2.0 * asin(sqrt(((sin((lat1-lat2)/2.0))*(sin((lat1-lat2)/2.0))) +
1159 cos(lat1)*cos(lat2)*(sin((lon1-lon2)/2.0))*(sin((lon1-lon2)/2.0))));
1163 // fmod dosen't do what we want :-(
1164 static double mod(double d1, double d2) {
1165 return(d1 - d2*floor(d1/d2));
1168 // Returns great circle course from point 1 to point 2
1169 // Input and output in RADIANS.
1170 double DCLGPS::GetGreatCircleCourse (double lat1, double lon1, double lat2, double lon2) const {
1173 // Special case the poles
1174 if(cos(lat1) < SG_EPSILON) {
1176 // Starting from North Pole
1179 // Starting from South Pole
1183 // Urgh - the formula below is for negative lon +ve !!!???
1184 double d = GetGreatCircleDistance(lat1, lon1, lat2, lon2);
1185 cout << "d = " << d;
1187 //cout << ", d_theta = " << d;
1188 //cout << ", and d = " << Rad2Nm(d) << ' ';
1189 if(sin(lon2 - lon1) < 0) {
1191 h = acos((sin(lat2)-sin(lat1)*cos(d))/(sin(d)*cos(lat1)));
1194 h = 2.0 * SG_PI - acos((sin(lat2)-sin(lat1)*cos(d))/(sin(d)*cos(lat1)));
1197 cout << h * SG_RADIANS_TO_DEGREES << '\n';
1200 return( mod(atan2(sin(lon2-lon1)*cos(lat2),
1201 cos(lat1)*sin(lat2)-sin(lat1)*cos(lat2)*cos(lon2-lon1)),
1205 // Return a position on a radial from wp1 given distance d (nm) and magnetic heading h (degrees)
1206 // Note that d should be less that 1/4 Earth diameter!
1207 GPSWaypoint DCLGPS::GetPositionOnMagRadial(const GPSWaypoint& wp1, double d, double h) {
1208 h += sgGetMagVar(wp1.lon, wp1.lat, 0.0, _time->getJD()) * SG_RADIANS_TO_DEGREES;
1209 return(GetPositionOnRadial(wp1, d, h));
1212 // Return a position on a radial from wp1 given distance d (nm) and TRUE heading h (degrees)
1213 // Note that d should be less that 1/4 Earth diameter!
1214 GPSWaypoint DCLGPS::GetPositionOnRadial(const GPSWaypoint& wp1, double d, double h) {
1215 while(h < 0.0) h += 360.0;
1216 while(h > 360.0) h -= 360.0;
1218 h *= SG_DEGREES_TO_RADIANS;
1219 d *= (SG_PI / (180.0 * 60.0));
1221 double lat=asin(sin(wp1.lat)*cos(d)+cos(wp1.lat)*sin(d)*cos(h));
1224 lon=wp1.lon; // endpoint a pole
1226 lon=mod(wp1.lon+asin(sin(h)*sin(d)/cos(lat))+SG_PI,2*SG_PI)-SG_PI;
1232 wp.type = GPS_WP_VIRT;
1236 // Returns cross-track deviation in Nm.
1237 double DCLGPS::CalcCrossTrackDeviation() const {
1238 return(CalcCrossTrackDeviation(_fromWaypoint, _activeWaypoint));
1241 // Returns cross-track deviation of the current position between two arbitary waypoints in nm.
1242 double DCLGPS::CalcCrossTrackDeviation(const GPSWaypoint& wp1, const GPSWaypoint& wp2) const {
1243 //if(wp1 == NULL || wp2 == NULL) return(0.0);
1244 if(wp1.id.empty() || wp2.id.empty()) return(0.0);
1245 double xtd = asin(sin(Nm2Rad(GetGreatCircleDistance(wp1.lat, wp1.lon, _gpsLat, _gpsLon)))
1246 * sin(GetGreatCircleCourse(wp1.lat, wp1.lon, _gpsLat, _gpsLon) - GetGreatCircleCourse(wp1.lat, wp1.lon, wp2.lat, wp2.lon)));
1247 return(Rad2Nm(xtd));