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/timing/sg_time.hxx>
30 #include <simgear/magvar/magvar.hxx>
32 #include <Main/fg_props.hxx>
33 #include <Navaids/fix.hxx>
34 #include <Navaids/navrecord.hxx>
35 #include <Airports/simple.hxx>
36 #include <Airports/runways.hxx>
42 GPSWaypoint::GPSWaypoint() {
43 appType = GPS_APP_NONE;
46 GPSWaypoint::GPSWaypoint(const std::string& aIdent, float aLat, float aLon, GPSWpType aType) :
55 GPSWaypoint::~GPSWaypoint() {}
57 string GPSWaypoint::GetAprId() {
58 if(appType == GPS_IAF) return(id + 'i');
59 else if(appType == GPS_FAF) return(id + 'f');
60 else if(appType == GPS_MAP) return(id + 'm');
61 else if(appType == GPS_MAHP) return(id + 'h');
66 GPSWpTypeFromFGPosType(FGPositioned::Type aType)
69 case FGPositioned::AIRPORT:
70 case FGPositioned::SEAPORT:
71 case FGPositioned::HELIPORT:
74 case FGPositioned::VOR:
77 case FGPositioned::NDB:
80 case FGPositioned::WAYPOINT:
83 case FGPositioned::FIX:
91 GPSWaypoint* GPSWaypoint::createFromPositioned(const FGPositioned* aPos)
94 return NULL; // happens if find returns no match
97 return new GPSWaypoint(aPos->ident(),
98 aPos->latitude() * SG_DEGREES_TO_RADIANS,
99 aPos->longitude() * SG_DEGREES_TO_RADIANS,
100 GPSWpTypeFromFGPosType(aPos->type())
104 ostream& operator << (ostream& os, GPSAppWpType type) {
106 case(GPS_IAF): return(os << "IAF");
107 case(GPS_IAP): return(os << "IAP");
108 case(GPS_FAF): return(os << "FAF");
109 case(GPS_MAP): return(os << "MAP");
110 case(GPS_MAHP): return(os << "MAHP");
111 case(GPS_HDR): return(os << "HEADER");
112 case(GPS_FENCE): return(os << "FENCE");
113 case(GPS_APP_NONE): return(os << "NONE");
115 return(os << "ERROR - Unknown switch in GPSAppWpType operator << ");
127 FGNPIAP::~FGNPIAP() {
130 ClockTime::ClockTime() {
135 ClockTime::ClockTime(int hr, int min) {
136 while(hr < 0) { hr += 24; }
138 while(min < 0) { min += 60; }
139 while(min > 60) { min -= 60; }
143 ClockTime::~ClockTime() {
146 // ------------------------------------------------------------------------------------- //
148 DCLGPS::DCLGPS(RenderArea2D* instrument) {
149 _instrument = instrument;
153 // Units - lets default to US units - FG can set them to other units from config during startup if desired.
154 _altUnits = GPS_ALT_UNITS_FT;
155 _baroUnits = GPS_PRES_UNITS_IN;
156 _velUnits = GPS_VEL_UNITS_KT;
157 _distUnits = GPS_DIST_UNITS_NM;
159 _lon_node = fgGetNode("/instrumentation/gps/indicated-longitude-deg", true);
160 _lat_node = fgGetNode("/instrumentation/gps/indicated-latitude-deg", true);
161 _alt_node = fgGetNode("/instrumentation/gps/indicated-altitude-ft", true);
162 _grnd_speed_node = fgGetNode("/instrumentation/gps/indicated-ground-speed-kt", true);
163 _true_track_node = fgGetNode("/instrumentation/gps/indicated-track-true-deg", true);
164 _mag_track_node = fgGetNode("/instrumentation/gps/indicated-track-magnetic-deg", true);
166 // Use FG's position values at construction in case FG's gps has not run first update yet.
167 _lon = fgGetDouble("/position/longitude-deg") * SG_DEGREES_TO_RADIANS;
168 _lat = fgGetDouble("/position/latitude-deg") * SG_DEGREES_TO_RADIANS;
169 _alt = fgGetDouble("/position/altitude-ft");
170 // Note - we can depriciate _gpsLat and _gpsLon if we implement error handling in FG
171 // gps code and not our own.
176 _groundSpeed_ms = 0.0;
177 _groundSpeed_kts = 0.0;
181 // Sensible defaults. These can be overriden by derived classes if desired.
183 _cdiScales.push_back(5.0);
184 _cdiScales.push_back(1.0);
185 _cdiScales.push_back(0.3);
186 _currentCdiScaleIndex = 0;
187 _targetCdiScaleIndex = 0;
188 _sourceCdiScaleIndex = 0;
189 _cdiScaleTransition = false;
190 _currentCdiScale = 5.0;
194 _activeWaypoint.id.clear();
196 _crosstrackDist = 0.0;
197 _headingBugTo = true;
199 _waypointAlert = false;
201 _departureTimeString = "----";
203 _powerOnTime.set_hr(0);
204 _powerOnTime.set_min(0);
205 _powerOnTimerSet = false;
208 // Configuration Initialisation
209 // Should this be in kln89.cxx ?
210 _turnAnticipationEnabled = false;
211 _suaAlertEnabled = false;
212 _altAlertEnabled = false;
216 _messageStack.clear();
220 _approachLoaded = false;
221 _approachArm = false;
222 _approachReallyArmed = false;
223 _approachActive = false;
224 _approachFP = new GPSFlightPlan;
229 delete _approachFP; // Don't need to delete the waypoints inside since they point to
230 // the waypoints in the approach database.
231 // TODO - may need to delete the approach database!!
234 void DCLGPS::draw(osg::State& state) {
235 _instrument->draw(state);
238 void DCLGPS::init() {
240 // Not sure if this should be here, but OK for now.
241 CreateDefaultFlightPlans();
243 // Hack - hardwire some instrument approaches for development.
244 // These will shortly be replaced by a routine to read ARINC data from file instead.
248 const GPSWaypoint* cwp;
251 iap->_aptIdent = "KHAF";
252 iap->_ident = "R12-Y";
253 iap->_name = ExpandSIAPIdent(iap->_ident);
255 iap->_approachRoutes.clear();
259 wp = new GPSWaypoint;
261 // Nasty using the find any function here, but it saves converting data from FGFix etc.
262 cwp = FindFirstByExactId(wp->id);
265 wp->appType = GPS_IAF;
266 fp = new GPSFlightPlan;
267 fp->waypoints.push_back(wp);
269 //cout << "Unable to find waypoint " << wp->id << '\n';
272 wp = new GPSWaypoint;
274 cwp = FindFirstByExactId(wp->id);
277 wp->appType = GPS_IAP;
278 fp->waypoints.push_back(wp);
279 iap->_approachRoutes.push_back(fp);
280 iap->_IAP.push_back(wp);
282 //cout << "Unable to find waypoint " << wp->id << '\n';
285 wp = new GPSWaypoint;
287 cwp = FindFirstByExactId(wp->id);
290 wp->appType = GPS_FAF;
291 iap->_IAP.push_back(wp);
293 //cout << "Unable to find waypoint " << wp->id << '\n';
296 wp = new GPSWaypoint;
298 wp->appType = GPS_MAP;
299 if(wp->id.substr(0, 2) == "RW" && wp->appType == GPS_MAP) {
300 // Assume that this is a missed-approach point based on the runway number, which appears to be standard for most approaches.
301 const FGAirport* apt = fgFindAirportID(iap->_aptIdent);
303 // TODO - sanity check the waypoint ID to ensure we have a double digit number
304 FGRunway* rwy = apt->getRunwayByIdent(wp->id.substr(2, 2));
306 wp->lat = rwy->begin().getLatitudeRad();
307 wp->lon = rwy->begin().getLongitudeRad();
311 cwp = FindFirstByExactId(wp->id);
314 wp->appType = GPS_MAP;
316 //cout << "Unable to find waypoint " << wp->id << '\n';
319 iap->_IAP.push_back(wp);
321 wp = new GPSWaypoint;
323 cwp = FindFirstByExactId(wp->id);
326 wp->appType = GPS_MAHP;
327 iap->_MAP.push_back(wp);
329 //cout << "Unable to find waypoint " << wp->id << '\n';
332 _np_iap[iap->_aptIdent].push_back(iap);
335 void DCLGPS::bind() {
336 fgTie("/instrumentation/gps/waypoint-alert", this, &DCLGPS::GetWaypointAlert);
337 fgTie("/instrumentation/gps/leg-mode", this, &DCLGPS::GetLegMode);
338 fgTie("/instrumentation/gps/obs-mode", this, &DCLGPS::GetOBSMode);
339 fgTie("/instrumentation/gps/approach-arm", this, &DCLGPS::GetApproachArm);
340 fgTie("/instrumentation/gps/approach-active", this, &DCLGPS::GetApproachActive);
341 fgTie("/instrumentation/gps/cdi-deflection", this, &DCLGPS::GetCDIDeflection);
342 fgTie("/instrumentation/gps/to-flag", this, &DCLGPS::GetToFlag);
345 void DCLGPS::unbind() {
346 fgUntie("/instrumentation/gps/waypoint-alert");
347 fgUntie("/instrumentation/gps/leg-mode");
348 fgUntie("/instrumentation/gps/obs-mode");
349 fgUntie("/instrumentation/gps/approach-arm");
350 fgUntie("/instrumentation/gps/approach-active");
351 fgUntie("/instrumentation/gps/cdi-deflection");
354 void DCLGPS::update(double dt) {
355 //cout << "update called!\n";
357 _lon = _lon_node->getDoubleValue() * SG_DEGREES_TO_RADIANS;
358 _lat = _lat_node->getDoubleValue() * SG_DEGREES_TO_RADIANS;
359 _alt = _alt_node->getDoubleValue();
360 _groundSpeed_kts = _grnd_speed_node->getDoubleValue();
361 _groundSpeed_ms = _groundSpeed_kts * 0.5144444444;
362 _track = _true_track_node->getDoubleValue();
363 _magTrackDeg = _mag_track_node->getDoubleValue();
364 // Note - we can depriciate _gpsLat and _gpsLon if we implement error handling in FG
365 // gps code and not our own.
368 // Check for abnormal position slew
369 if(GetGreatCircleDistance(_gpsLat, _gpsLon, _checkLat, _checkLon) > 1.0) {
370 OrientateToActiveFlightPlan();
375 // TODO - check for unit power before running this.
376 if(!_powerOnTimerSet) {
380 // Check if an alarm timer has expired
382 if(_alarmTime.hr() == atoi(fgGetString("/instrumentation/clock/indicated-hour"))
383 && _alarmTime.min() == atoi(fgGetString("/instrumentation/clock/indicated-min"))) {
384 _messageStack.push_back("*Timer Expired");
390 if(_groundSpeed_kts > 30.0) {
392 string th = fgGetString("/instrumentation/clock/indicated-hour");
393 string tm = fgGetString("/instrumentation/clock/indicated-min");
394 if(th.size() == 1) th = "0" + th;
395 if(tm.size() == 1) tm = "0" + tm;
396 _departureTimeString = th + tm;
399 // TODO - check - is this prone to drift error over time?
400 // Should we difference the departure and current times?
401 // What about when the user resets the time of day from the menu?
405 _time->update(_gpsLon * SG_DEGREES_TO_RADIANS, _gpsLat * SG_DEGREES_TO_RADIANS, 0, 0);
406 // FIXME - currently all the below assumes leg mode and no DTO or OBS cancelled.
407 if(_activeFP->IsEmpty()) {
408 // Not sure if we need to reset these each update or only when fp altered
409 _activeWaypoint.id.clear();
411 } else if(_activeFP->waypoints.size() == 1) {
412 _activeWaypoint.id.clear();
415 if(_activeWaypoint.id.empty() || _fromWaypoint.id.empty()) {
416 //cout << "Error, in leg mode with flightplan of 2 or more waypoints, but either active or from wp is NULL!\n";
417 OrientateToActiveFlightPlan();
421 if(_approachLoaded) {
422 if(!_approachReallyArmed && !_approachActive) {
423 // arm if within 30nm of airport.
424 // TODO - let user cancel approach arm using external GPS-APR switch
426 const FGAirport* ap = FindFirstAptById(_approachID, multi, true);
428 double d = GetGreatCircleDistance(_gpsLat, _gpsLon, ap->getLatitude() * SG_DEGREES_TO_RADIANS, ap->getLongitude() * SG_DEGREES_TO_RADIANS);
431 _approachReallyArmed = true;
432 _messageStack.push_back("*Press ALT To Set Baro");
433 // Not sure what we do if the user has already set CDI to 0.3 nm?
434 _targetCdiScaleIndex = 1;
435 if(_currentCdiScaleIndex == 1) {
437 } else if(_currentCdiScaleIndex == 0) {
438 _sourceCdiScaleIndex = 0;
439 _cdiScaleTransition = true;
440 _cdiTransitionTime = 30.0;
441 _currentCdiScale = _cdiScales[_currentCdiScaleIndex];
446 // Check for approach active - we can only activate approach if it is really armed.
447 if(_activeWaypoint.appType == GPS_FAF) {
448 //cout << "Active waypoint is FAF, id is " << _activeWaypoint.id << '\n';
449 if(GetGreatCircleDistance(_gpsLat, _gpsLon, _activeWaypoint.lat, _activeWaypoint.lon) <= 2.0 && !_obsMode) {
450 // Assume heading is OK for now
451 _approachArm = false; // TODO - check - maybe arm is left on when actv comes on?
452 _approachReallyArmed = false;
453 _approachActive = true;
454 _targetCdiScaleIndex = 2;
455 if(_currentCdiScaleIndex == 2) {
457 } else if(_currentCdiScaleIndex == 1) {
458 _sourceCdiScaleIndex = 1;
459 _cdiScaleTransition = true;
460 _cdiTransitionTime = 30.0; // TODO - compress it if time to FAF < 30sec
461 _currentCdiScale = _cdiScales[_currentCdiScaleIndex];
463 // Abort going active?
464 _approachActive = false;
471 // CDI scale transition stuff
472 if(_cdiScaleTransition) {
473 if(fabs(_currentCdiScale - _cdiScales[_targetCdiScaleIndex]) < 0.001) {
474 _currentCdiScale = _cdiScales[_targetCdiScaleIndex];
475 _currentCdiScaleIndex = _targetCdiScaleIndex;
476 _cdiScaleTransition = false;
478 double scaleDiff = (_targetCdiScaleIndex > _sourceCdiScaleIndex
479 ? _cdiScales[_sourceCdiScaleIndex] - _cdiScales[_targetCdiScaleIndex]
480 : _cdiScales[_targetCdiScaleIndex] - _cdiScales[_sourceCdiScaleIndex]);
481 //cout << "ScaleDiff = " << scaleDiff << '\n';
482 if(_targetCdiScaleIndex > _sourceCdiScaleIndex) {
483 // Scaling down eg. 5nm -> 1nm
484 _currentCdiScale -= (scaleDiff * dt / _cdiTransitionTime);
485 if(_currentCdiScale < _cdiScales[_targetCdiScaleIndex]) {
486 _currentCdiScale = _cdiScales[_targetCdiScaleIndex];
487 _currentCdiScaleIndex = _targetCdiScaleIndex;
488 _cdiScaleTransition = false;
491 _currentCdiScale += (scaleDiff * dt / _cdiTransitionTime);
492 if(_currentCdiScale > _cdiScales[_targetCdiScaleIndex]) {
493 _currentCdiScale = _cdiScales[_targetCdiScaleIndex];
494 _currentCdiScaleIndex = _targetCdiScaleIndex;
495 _cdiScaleTransition = false;
498 //cout << "_currentCdiScale = " << _currentCdiScale << '\n';
501 _currentCdiScale = _cdiScales[_currentCdiScaleIndex];
505 // Urgh - I've been setting the heading bug based on DTK,
506 // bug I think it should be based on heading re. active waypoint
507 // based on what the sim does after the final waypoint is passed.
508 // (DTK remains the same, but if track is held == DTK heading bug
509 // reverses to from once wp is passed).
511 if(_fromWaypoint != NULL) {
512 // TODO - how do we handle the change of track with distance over long legs?
513 _dtkTrue = GetGreatCircleCourse(_fromWaypoint->lat, _fromWaypoint->lon, _activeWaypoint->lat, _activeWaypoint->lon) * SG_RADIANS_TO_DEGREES;
514 _dtkMag = GetMagHeadingFromTo(_fromWaypoint->lat, _fromWaypoint->lon, _activeWaypoint->lat, _activeWaypoint->lon);
515 // Don't change the heading bug if speed is too low otherwise it flickers to/from at rest
516 if(_groundSpeed_ms > 5) {
517 //cout << "track = " << _track << ", dtk = " << _dtkTrue << '\n';
518 double courseDev = _track - _dtkTrue;
519 //cout << "courseDev = " << courseDev << ", normalized = ";
520 SG_NORMALIZE_RANGE(courseDev, -180.0, 180.0);
521 //cout << courseDev << '\n';
522 _headingBugTo = (fabs(courseDev) > 90.0 ? false : true);
527 // TODO - in DTO operation the position of initiation of DTO defines the "from waypoint".
530 if(!_activeWaypoint.id.empty()) {
531 double hdgTrue = GetGreatCircleCourse(_gpsLat, _gpsLon, _activeWaypoint.lat, _activeWaypoint.lon) * SG_RADIANS_TO_DEGREES;
532 if(_groundSpeed_ms > 5) {
533 //cout << "track = " << _track << ", hdgTrue = " << hdgTrue << '\n';
534 double courseDev = _track - hdgTrue;
535 //cout << "courseDev = " << courseDev << ", normalized = ";
536 SG_NORMALIZE_RANGE(courseDev, -180.0, 180.0);
537 //cout << courseDev << '\n';
538 _headingBugTo = (fabs(courseDev) > 90.0 ? false : true);
540 if(!_fromWaypoint.id.empty()) {
541 _dtkTrue = GetGreatCircleCourse(_fromWaypoint.lat, _fromWaypoint.lon, _activeWaypoint.lat, _activeWaypoint.lon) * SG_RADIANS_TO_DEGREES;
542 _dtkMag = GetMagHeadingFromTo(_fromWaypoint.lat, _fromWaypoint.lon, _activeWaypoint.lat, _activeWaypoint.lon);
549 _dist2Act = GetGreatCircleDistance(_gpsLat, _gpsLon, _activeWaypoint.lat, _activeWaypoint.lon) * SG_NM_TO_METER;
550 if(_groundSpeed_ms > 10.0) {
551 _eta = _dist2Act / _groundSpeed_ms;
552 if(_eta <= 36) { // TODO - this is slightly different if turn anticipation is enabled.
554 _waypointAlert = true; // TODO - not if the from flag is set.
558 // Check if we should sequence to next leg.
559 // Perhaps this should be done on distance instead, but 60s time (about 1 - 2 nm) seems reasonable for now.
560 //double reverseHeading = GetGreatCircleCourse(_activeWaypoint->lat, _activeWaypoint->lon, _fromWaypoint->lat, _fromWaypoint->lon);
561 // Hack - let's cheat and do it on heading bug for now. TODO - that stops us 'cutting the corner'
562 // when we happen to approach the inside turn of a waypoint - we should probably sequence at the midpoint
563 // of the heading difference between legs in this instance.
564 int idx = GetActiveWaypointIndex();
565 bool finalLeg = (idx == (int)(_activeFP->waypoints.size()) - 1 ? true : false);
566 bool finalDto = (_dto && idx == -1); // Dto operation to a waypoint not in the flightplan - we don't sequence in this instance
569 // Do nothing - not sure if Dto should switch off when arriving at the final waypoint of a flightplan
570 } else if(finalDto) {
572 } else if(_activeWaypoint.appType == GPS_MAP) {
573 // Don't sequence beyond the missed approach point
574 //cout << "ACTIVE WAYPOINT is MAP - not sequencing!!!!!\n";
576 //cout << "Sequencing...\n";
577 _fromWaypoint = _activeWaypoint;
578 _activeWaypoint = *_activeFP->waypoints[idx + 1];
580 // TODO - course alteration message format is dependent on whether we are slaved HSI/CDI indicator or not.
581 // For now assume we are not.
583 if(fgGetBool("/instrumentation/nav[0]/slaved-to-gps")) {
584 // TODO - avoid the hardwiring on nav[0]
585 s = "Adj Nav Crs to ";
587 string s = "GPS Course is ";
589 double d = GetMagHeadingFromTo(_fromWaypoint.lat, _fromWaypoint.lon, _activeWaypoint.lat, _activeWaypoint.lon);
590 while(d < 0.0) d += 360.0;
591 while(d >= 360.0) d -= 360.0;
593 snprintf(buf, 4, "%03i", (int)(d + 0.5));
595 _messageStack.push_back(s);
597 _waypointAlert = false;
605 // First attempt at a sensible cross-track correction calculation
606 // Uh? - I think this is implemented further down the file!
607 if(_fromWaypoint != NULL) {
610 _crosstrackDist = 0.0;
617 Expand a SIAP ident to the full procedure name (as shown on the approach chart).
618 NOTE: Some of this is inferred from data, some is from documentation.
620 Example expansions from ARINC 424-18 [and the airport they're taken from]:
621 "R10LY" <--> "RNAV (GPS) Y RWY 10 L" [KBOI]
622 "R10-Z" <--> "RNAV (GPS) Z RWY 10" [KHTO]
623 "S25" <--> "VOR or GPS RWY 25" [KHHR]
624 "P20" <--> "GPS RWY 20" [KDAN]
625 "NDB-B" <--> "NDB or GPS-B" [KDAW]
626 "NDBC" <--> "NDB or GPS-C" [KEMT]
627 "VDMA" <--> "VOR/DME or GPS-A" [KDAW]
628 "VDM-A" <--> "VOR/DME or GPS-A" [KEAG]
629 "VDMB" <--> "VOR/DME or GPS-B" [KDKX]
630 "VORA" <--> "VOR or GPS-A" [KEMT]
632 It seems that there are 2 basic types of expansions; those that include
633 the runway and those that don't. Of those that don't, it seems that 2
634 different positions within the string to encode the identifying letter
635 are used, i.e. with a dash and without.
637 string DCLGPS::ExpandSIAPIdent(const string& ident) {
642 case 'N': name = "NDB or GPS"; has_rwy = false; break;
643 case 'P': name = "GPS"; has_rwy = true; break;
644 case 'R': name = "RNAV (GPS)"; has_rwy = true; break;
645 case 'S': name = "VOR or GPS"; has_rwy = true; break;
647 if(ident[1] == 'D') name = "VOR/DME or GPS";
648 else name = "VOR or GPS";
651 default: // TODO output a log message
656 // Add the identifying letter if present
657 if(ident.size() == 5) {
664 name += ident.substr(1, 2);
666 // Add a left/right/centre indication if present.
667 if(ident.size() > 3) {
668 if((ident[3] != '-') && (ident[3] != ' ')) { // Early versions of the spec allowed a blank instead of a dash so check for both
674 // Add the identifying letter, which I *think* should always be present, but seems to be inconsistent as to whether a dash is used.
675 if(ident.size() == 5) {
678 } else if(ident.size() == 4) {
689 GPSWaypoint* DCLGPS::GetActiveWaypoint() {
690 return &_activeWaypoint;
694 float DCLGPS::GetDistToActiveWaypoint() {
698 // I don't yet fully understand all the gotchas about where to source time from.
699 // This function sets the initial timer before the clock exports properties
700 // and the one below uses the clock to be consistent with the rest of the code.
701 // It might change soonish...
702 void DCLGPS::SetPowerOnTimer() {
703 struct tm *t = globals->get_time_params()->getGmt();
704 _powerOnTime.set_hr(t->tm_hour);
705 _powerOnTime.set_min(t->tm_min);
706 _powerOnTimerSet = true;
709 void DCLGPS::ResetPowerOnTimer() {
710 _powerOnTime.set_hr(atoi(fgGetString("/instrumentation/clock/indicated-hour")));
711 _powerOnTime.set_min(atoi(fgGetString("/instrumentation/clock/indicated-min")));
712 _powerOnTimerSet = true;
715 double DCLGPS::GetCDIDeflection() const {
716 double xtd = CalcCrossTrackDeviation(); //nm
717 return((xtd / _currentCdiScale) * 5.0 * 2.5 * -1.0);
720 void DCLGPS::DtoInitiate(const string& s) {
721 //cout << "DtoInitiate, s = " << s << '\n';
722 const GPSWaypoint* wp = FindFirstByExactId(s);
724 //cout << "Waypoint found, starting dto operation!\n";
726 _activeWaypoint = *wp;
727 _fromWaypoint.lat = _gpsLat;
728 _fromWaypoint.lon = _gpsLon;
729 _fromWaypoint.type = GPS_WP_VIRT;
730 _fromWaypoint.id = "DTOWP";
733 //cout << "Waypoint not found, ignoring dto request\n";
734 // Should bring up the user waypoint page, but we're not implementing that yet.
735 _dto = false; // TODO - implement this some day.
739 void DCLGPS::DtoCancel() {
741 // i.e. don't bother reorientating if we're just cancelling a DTO button press
742 // without having previously initiated DTO.
743 OrientateToActiveFlightPlan();
748 void DCLGPS::ToggleOBSMode() {
749 _obsMode = !_obsMode;
751 if(!_activeWaypoint.id.empty()) {
752 _obsHeading = static_cast<int>(_dtkMag);
754 // TODO - the _fromWaypoint location will change as the OBS heading changes.
755 // Might need to store the OBS initiation position somewhere in case it is needed again.
756 SetOBSFromWaypoint();
760 // Set the _fromWaypoint position based on the active waypoint and OBS radial.
761 void DCLGPS::SetOBSFromWaypoint() {
762 if(!_obsMode) return;
763 if(_activeWaypoint.id.empty()) return;
765 // TODO - base the 180 deg correction on the to/from flag.
766 _fromWaypoint = GetPositionOnMagRadial(_activeWaypoint, 10, _obsHeading + 180.0);
767 _fromWaypoint.id = "OBSWP";
770 void DCLGPS::CDIFSDIncrease() {
771 if(_currentCdiScaleIndex == 0) {
772 _currentCdiScaleIndex = _cdiScales.size() - 1;
774 _currentCdiScaleIndex--;
778 void DCLGPS::CDIFSDDecrease() {
779 _currentCdiScaleIndex++;
780 if(_currentCdiScaleIndex == _cdiScales.size()) {
781 _currentCdiScaleIndex = 0;
785 void DCLGPS::DrawChar(char c, int field, int px, int py, bool bold) {
788 void DCLGPS::DrawText(const string& s, int field, int px, int py, bool bold) {
791 void DCLGPS::SetBaroUnits(int n, bool wrap) {
793 _baroUnits = (GPSPressureUnits)(wrap ? 3 : 1);
795 _baroUnits = (GPSPressureUnits)(wrap ? 1 : 3);
797 _baroUnits = (GPSPressureUnits)n;
801 void DCLGPS::CreateDefaultFlightPlans() {}
803 // Get the time to the active waypoint in seconds.
804 // Returns -1 if groundspeed < 30 kts
805 double DCLGPS::GetTimeToActiveWaypoint() {
806 if(_groundSpeed_kts < 30.0) {
813 // Get the time to the final waypoint in seconds.
814 // Returns -1 if groundspeed < 30 kts
815 double DCLGPS::GetETE() {
816 if(_groundSpeed_kts < 30.0) {
819 // TODO - handle OBS / DTO operation appropriately
820 if(_activeFP->waypoints.empty()) {
823 return(GetTimeToWaypoint(_activeFP->waypoints[_activeFP->waypoints.size() - 1]->id));
828 // Get the time to a given waypoint (spec'd by ID) in seconds.
829 // returns -1 if groundspeed is less than 30kts.
830 // If the waypoint is an unreached part of the active flight plan the time will be via each leg.
831 // otherwise it will be a direct-to time.
832 double DCLGPS::GetTimeToWaypoint(const string& id) {
833 if(_groundSpeed_kts < 30.0) {
838 int n1 = GetActiveWaypointIndex();
839 int n2 = GetWaypointIndex(id);
842 for(unsigned int i=n1+1; i<_activeFP->waypoints.size(); ++i) {
843 GPSWaypoint* wp1 = _activeFP->waypoints[i-1];
844 GPSWaypoint* wp2 = _activeFP->waypoints[i];
845 double distm = GetGreatCircleDistance(wp1->lat, wp1->lon, wp2->lat, wp2->lon) * SG_NM_TO_METER;
846 eta += (distm / _groundSpeed_ms);
849 } else if(id == _activeWaypoint.id) {
852 const GPSWaypoint* wp = FindFirstByExactId(id);
853 if(wp == NULL) return(-1.0);
854 double distm = GetGreatCircleDistance(_gpsLat, _gpsLon, wp->lat, wp->lon);
856 return(distm / _groundSpeed_ms);
858 return(-1.0); // Hopefully we never get here!
861 // Returns magnetic great-circle heading
862 // TODO - document units.
863 float DCLGPS::GetHeadingToActiveWaypoint() {
864 if(_activeWaypoint.id.empty()) {
867 double h = GetMagHeadingFromTo(_gpsLat, _gpsLon, _activeWaypoint.lat, _activeWaypoint.lon);
868 while(h <= 0.0) h += 360.0;
869 while(h > 360.0) h -= 360.0;
874 // Returns magnetic great-circle heading
875 // TODO - what units?
876 float DCLGPS::GetHeadingFromActiveWaypoint() {
877 if(_activeWaypoint.id.empty()) {
880 double h = GetMagHeadingFromTo(_activeWaypoint.lat, _activeWaypoint.lon, _gpsLat, _gpsLon);
881 while(h <= 0.0) h += 360.0;
882 while(h > 360.0) h -= 360.0;
887 void DCLGPS::ClearFlightPlan(int n) {
888 for(unsigned int i=0; i<_flightPlans[n]->waypoints.size(); ++i) {
889 delete _flightPlans[n]->waypoints[i];
891 _flightPlans[n]->waypoints.clear();
894 void DCLGPS::ClearFlightPlan(GPSFlightPlan* fp) {
895 for(unsigned int i=0; i<fp->waypoints.size(); ++i) {
896 delete fp->waypoints[i];
898 fp->waypoints.clear();
901 int DCLGPS::GetActiveWaypointIndex() {
902 for(unsigned int i=0; i<_flightPlans[0]->waypoints.size(); ++i) {
903 if(_flightPlans[0]->waypoints[i]->id == _activeWaypoint.id) return((int)i);
908 int DCLGPS::GetWaypointIndex(const string& id) {
909 for(unsigned int i=0; i<_flightPlans[0]->waypoints.size(); ++i) {
910 if(_flightPlans[0]->waypoints[i]->id == id) return((int)i);
915 void DCLGPS::OrientateToFlightPlan(GPSFlightPlan* fp) {
916 //cout << "Orientating...\n";
917 //cout << "_lat = " << _lat << ", _lon = " << _lon << ", _gpsLat = " << _gpsLat << ", gpsLon = " << _gpsLon << '\n';
919 _activeWaypoint.id.clear();
923 if(fp->waypoints.size() == 1) {
924 // TODO - may need to flag nav here if not dto or obs, or possibly handle it somewhere else.
925 _activeWaypoint = *fp->waypoints[0];
926 _fromWaypoint.id.clear();
929 _fromWaypoint = *fp->waypoints[0];
930 _activeWaypoint = *fp->waypoints[1];
931 double dmin = 1000000; // nm!!
932 // For now we will simply start on the leg closest to our current position.
933 // It's possible that more fancy algorithms may take either heading or track
934 // into account when setting inital leg - I'm not sure.
935 // This method should handle most cases perfectly OK though.
936 for(unsigned int i = 1; i < fp->waypoints.size(); ++i) {
937 //cout << "Pass " << i << ", dmin = " << dmin << ", leg is " << fp->waypoints[i-1]->id << " to " << fp->waypoints[i]->id << '\n';
938 // First get the cross track correction.
939 double d0 = fabs(CalcCrossTrackDeviation(*fp->waypoints[i-1], *fp->waypoints[i]));
940 // That is the shortest distance away we could be though - check for
941 // longer distances if we are 'off the end' of the leg.
942 double ht1 = GetGreatCircleCourse(fp->waypoints[i-1]->lat, fp->waypoints[i-1]->lon,
943 fp->waypoints[i]->lat, fp->waypoints[i]->lon)
944 * SG_RADIANS_TO_DEGREES;
945 // not simply the reverse of the above due to great circle navigation.
946 double ht2 = GetGreatCircleCourse(fp->waypoints[i]->lat, fp->waypoints[i]->lon,
947 fp->waypoints[i-1]->lat, fp->waypoints[i-1]->lon)
948 * SG_RADIANS_TO_DEGREES;
949 double hw1 = GetGreatCircleCourse(_gpsLat, _gpsLon,
950 fp->waypoints[i]->lat, fp->waypoints[i]->lon)
951 * SG_RADIANS_TO_DEGREES;
952 double hw2 = GetGreatCircleCourse(_gpsLat, _gpsLon,
953 fp->waypoints[i-1]->lat, fp->waypoints[i-1]->lon)
954 * SG_RADIANS_TO_DEGREES;
955 double h1 = ht1 - hw1;
956 double h2 = ht2 - hw2;
957 //cout << "d0, h1, h2 = " << d0 << ", " << h1 << ", " << h2 << '\n';
958 //cout << "Normalizing...\n";
959 SG_NORMALIZE_RANGE(h1, -180.0, 180.0);
960 SG_NORMALIZE_RANGE(h2, -180.0, 180.0);
961 //cout << "d0, h1, h2 = " << d0 << ", " << h1 << ", " << h2 << '\n';
962 if(fabs(h1) > 90.0) {
963 // We are past the end of the to waypoint
964 double d = GetGreatCircleDistance(_gpsLat, _gpsLon, fp->waypoints[i]->lat, fp->waypoints[i]->lon);
966 //cout << "h1 triggered, d0 now = " << d0 << '\n';
967 } else if(fabs(h2) > 90.0) {
968 // We are past the end (not yet at!) the from waypoint
969 double d = GetGreatCircleDistance(_gpsLat, _gpsLon, fp->waypoints[i-1]->lat, fp->waypoints[i-1]->lon);
971 //cout << "h2 triggered, d0 now = " << d0 << '\n';
974 //cout << "THIS LEG NOW ACTIVE!\n";
976 _fromWaypoint = *fp->waypoints[i-1];
977 _activeWaypoint = *fp->waypoints[i];
984 void DCLGPS::OrientateToActiveFlightPlan() {
985 OrientateToFlightPlan(_activeFP);
988 /***************************************/
990 // Utility function - create a flightplan from a list of waypoint ids and types
991 void DCLGPS::CreateFlightPlan(GPSFlightPlan* fp, vector<string> ids, vector<GPSWpType> wps) {
992 if(fp == NULL) fp = new GPSFlightPlan;
994 if(!fp->waypoints.empty()) {
995 for(i=0; i<fp->waypoints.size(); ++i) {
996 delete fp->waypoints[i];
998 fp->waypoints.clear();
1000 if(ids.size() != wps.size()) {
1001 cout << "ID and Waypoint types list size mismatch in GPS::CreateFlightPlan - no flightplan created!\n";
1004 for(i=0; i<ids.size(); ++i) {
1006 const FGAirport* ap;
1008 GPSWaypoint* wp = new GPSWaypoint;
1012 ap = FindFirstAptById(ids[i], multi, true);
1017 wp->lat = ap->getLatitude() * SG_DEGREES_TO_RADIANS;
1018 wp->lon = ap->getLongitude() * SG_DEGREES_TO_RADIANS;
1020 fp->waypoints.push_back(wp);
1024 np = FindFirstVorById(ids[i], multi, true);
1029 wp->lat = np->get_lat() * SG_DEGREES_TO_RADIANS;
1030 wp->lon = np->get_lon() * SG_DEGREES_TO_RADIANS;
1032 fp->waypoints.push_back(wp);
1036 np = FindFirstNDBById(ids[i], multi, true);
1041 wp->lat = np->get_lat() * SG_DEGREES_TO_RADIANS;
1042 wp->lon = np->get_lon() * SG_DEGREES_TO_RADIANS;
1044 fp->waypoints.push_back(wp);
1060 /***************************************/
1062 class DCLGPSFilter : public FGPositioned::Filter
1065 virtual bool pass(const FGPositioned* aPos) const {
1066 switch (aPos->type()) {
1067 case FGPositioned::AIRPORT:
1068 // how about heliports and seaports?
1069 case FGPositioned::NDB:
1070 case FGPositioned::VOR:
1071 case FGPositioned::WAYPOINT:
1072 case FGPositioned::FIX:
1074 default: return false; // reject all other types
1080 GPSWaypoint* DCLGPS::FindFirstById(const string& id) const
1082 DCLGPSFilter filter;
1083 FGPositionedRef result = FGPositioned::findNextWithPartialId(NULL, id, &filter);
1084 return GPSWaypoint::createFromPositioned(result);
1087 GPSWaypoint* DCLGPS::FindFirstByExactId(const string& id) const
1089 SGGeod pos(SGGeod::fromRad(_lon, _lat));
1090 FGPositionedRef result = FGPositioned::findClosestWithIdent(id, pos);
1091 return GPSWaypoint::createFromPositioned(result);
1094 // TODO - add the ASCII / alphabetical stuff from the Atlas version
1095 FGPositioned* DCLGPS::FindTypedFirstById(const string& id, FGPositioned::Type ty, bool &multi, bool exact)
1098 FGPositioned::TypeFilter filter(ty);
1101 FGPositioned::List matches =
1102 FGPositioned::findAllWithIdentSortedByRange(id, SGGeod::fromRad(_lon, _lat), &filter);
1103 multi = (matches.size() > 1);
1104 return matches.empty() ? NULL : matches.front().ptr();
1107 return FGPositioned::findNextWithPartialId(NULL, id, &filter);
1110 FGNavRecord* DCLGPS::FindFirstVorById(const string& id, bool &multi, bool exact)
1112 return dynamic_cast<FGNavRecord*>(FindTypedFirstById(id, FGPositioned::VOR, multi, exact));
1115 FGNavRecord* DCLGPS::FindFirstNDBById(const string& id, bool &multi, bool exact)
1117 return dynamic_cast<FGNavRecord*>(FindTypedFirstById(id, FGPositioned::NDB, multi, exact));
1120 const FGFix* DCLGPS::FindFirstIntById(const string& id, bool &multi, bool exact)
1122 return dynamic_cast<FGFix*>(FindTypedFirstById(id, FGPositioned::FIX, multi, exact));
1125 const FGAirport* DCLGPS::FindFirstAptById(const string& id, bool &multi, bool exact)
1127 return dynamic_cast<FGAirport*>(FindTypedFirstById(id, FGPositioned::AIRPORT, multi, exact));
1130 FGNavRecord* DCLGPS::FindClosestVor(double lat_rad, double lon_rad) {
1131 FGPositioned::TypeFilter filter(FGPositioned::VOR);
1132 double cutoff = 1000; // nautical miles
1133 FGPositionedRef v = FGPositioned::findClosest(SGGeod::fromRad(lon_rad, lat_rad), cutoff, &filter);
1138 return dynamic_cast<FGNavRecord*>(v.ptr());
1141 //----------------------------------------------------------------------------------------------------------
1143 // Takes lat and lon in RADIANS!!!!!!!
1144 double DCLGPS::GetMagHeadingFromTo(double latA, double lonA, double latB, double lonB) {
1145 double h = GetGreatCircleCourse(latA, lonA, latB, lonB);
1146 h *= SG_RADIANS_TO_DEGREES;
1147 // TODO - use the real altitude below instead of 0.0!
1148 //cout << "MagVar = " << sgGetMagVar(_gpsLon, _gpsLat, 0.0, _time->getJD()) * SG_RADIANS_TO_DEGREES << '\n';
1149 h -= sgGetMagVar(_gpsLon, _gpsLat, 0.0, _time->getJD()) * SG_RADIANS_TO_DEGREES;
1150 while(h >= 360.0) h -= 360.0;
1151 while(h < 0.0) h += 360.0;
1155 // ---------------- Great Circle formulae from "The Aviation Formulary" -------------
1156 // Note that all of these assume that the world is spherical.
1158 double Rad2Nm(double theta) {
1159 return(((180.0*60.0)/SG_PI)*theta);
1162 double Nm2Rad(double d) {
1163 return((SG_PI/(180.0*60.0))*d);
1168 The great circle distance d between two points with coordinates {lat1,lon1} and {lat2,lon2} is given by:
1170 d=acos(sin(lat1)*sin(lat2)+cos(lat1)*cos(lat2)*cos(lon1-lon2))
1172 A mathematically equivalent formula, which is less subject to rounding error for short distances is:
1174 d=2*asin(sqrt((sin((lat1-lat2)/2))^2 +
1175 cos(lat1)*cos(lat2)*(sin((lon1-lon2)/2))^2))
1179 // Returns distance in nm, takes lat & lon in RADIANS
1180 double DCLGPS::GetGreatCircleDistance(double lat1, double lon1, double lat2, double lon2) const {
1181 double d = 2.0 * asin(sqrt(((sin((lat1-lat2)/2.0))*(sin((lat1-lat2)/2.0))) +
1182 cos(lat1)*cos(lat2)*(sin((lon1-lon2)/2.0))*(sin((lon1-lon2)/2.0))));
1186 // fmod dosen't do what we want :-(
1187 static double mod(double d1, double d2) {
1188 return(d1 - d2*floor(d1/d2));
1191 // Returns great circle course from point 1 to point 2
1192 // Input and output in RADIANS.
1193 double DCLGPS::GetGreatCircleCourse (double lat1, double lon1, double lat2, double lon2) const {
1196 // Special case the poles
1197 if(cos(lat1) < SG_EPSILON) {
1199 // Starting from North Pole
1202 // Starting from South Pole
1206 // Urgh - the formula below is for negative lon +ve !!!???
1207 double d = GetGreatCircleDistance(lat1, lon1, lat2, lon2);
1208 cout << "d = " << d;
1210 //cout << ", d_theta = " << d;
1211 //cout << ", and d = " << Rad2Nm(d) << ' ';
1212 if(sin(lon2 - lon1) < 0) {
1214 h = acos((sin(lat2)-sin(lat1)*cos(d))/(sin(d)*cos(lat1)));
1217 h = 2.0 * SG_PI - acos((sin(lat2)-sin(lat1)*cos(d))/(sin(d)*cos(lat1)));
1220 cout << h * SG_RADIANS_TO_DEGREES << '\n';
1223 return( mod(atan2(sin(lon2-lon1)*cos(lat2),
1224 cos(lat1)*sin(lat2)-sin(lat1)*cos(lat2)*cos(lon2-lon1)),
1228 // Return a position on a radial from wp1 given distance d (nm) and magnetic heading h (degrees)
1229 // Note that d should be less that 1/4 Earth diameter!
1230 GPSWaypoint DCLGPS::GetPositionOnMagRadial(const GPSWaypoint& wp1, double d, double h) {
1231 h += sgGetMagVar(wp1.lon, wp1.lat, 0.0, _time->getJD()) * SG_RADIANS_TO_DEGREES;
1232 return(GetPositionOnRadial(wp1, d, h));
1235 // Return a position on a radial from wp1 given distance d (nm) and TRUE heading h (degrees)
1236 // Note that d should be less that 1/4 Earth diameter!
1237 GPSWaypoint DCLGPS::GetPositionOnRadial(const GPSWaypoint& wp1, double d, double h) {
1238 while(h < 0.0) h += 360.0;
1239 while(h > 360.0) h -= 360.0;
1241 h *= SG_DEGREES_TO_RADIANS;
1242 d *= (SG_PI / (180.0 * 60.0));
1244 double lat=asin(sin(wp1.lat)*cos(d)+cos(wp1.lat)*sin(d)*cos(h));
1247 lon=wp1.lon; // endpoint a pole
1249 lon=mod(wp1.lon+asin(sin(h)*sin(d)/cos(lat))+SG_PI,2*SG_PI)-SG_PI;
1255 wp.type = GPS_WP_VIRT;
1259 // Returns cross-track deviation in Nm.
1260 double DCLGPS::CalcCrossTrackDeviation() const {
1261 return(CalcCrossTrackDeviation(_fromWaypoint, _activeWaypoint));
1264 // Returns cross-track deviation of the current position between two arbitary waypoints in nm.
1265 double DCLGPS::CalcCrossTrackDeviation(const GPSWaypoint& wp1, const GPSWaypoint& wp2) const {
1266 //if(wp1 == NULL || wp2 == NULL) return(0.0);
1267 if(wp1.id.empty() || wp2.id.empty()) return(0.0);
1268 double xtd = asin(sin(Nm2Rad(GetGreatCircleDistance(wp1.lat, wp1.lon, _gpsLat, _gpsLon)))
1269 * sin(GetGreatCircleCourse(wp1.lat, wp1.lon, _gpsLat, _gpsLon) - GetGreatCircleCourse(wp1.lat, wp1.lon, wp2.lat, wp2.lon)));
1270 return(Rad2Nm(xtd));