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/misc/sg_path.hxx>
30 #include <simgear/timing/sg_time.hxx>
31 #include <simgear/magvar/magvar.hxx>
32 #include <simgear/structure/exception.hxx>
34 #include <Main/fg_props.hxx>
35 #include <Navaids/fix.hxx>
36 #include <Navaids/navrecord.hxx>
37 #include <Airports/simple.hxx>
38 #include <Airports/runways.hxx>
46 GPSWaypoint::GPSWaypoint() {
47 appType = GPS_APP_NONE;
50 GPSWaypoint::GPSWaypoint(const std::string& aIdent, float aLat, float aLon, GPSWpType aType) :
59 GPSWaypoint::~GPSWaypoint() {}
61 string GPSWaypoint::GetAprId() {
62 if(appType == GPS_IAF) return(id + 'i');
63 else if(appType == GPS_FAF) return(id + 'f');
64 else if(appType == GPS_MAP) return(id + 'm');
65 else if(appType == GPS_MAHP) return(id + 'h');
70 GPSWpTypeFromFGPosType(FGPositioned::Type aType)
73 case FGPositioned::AIRPORT:
74 case FGPositioned::SEAPORT:
75 case FGPositioned::HELIPORT:
78 case FGPositioned::VOR:
81 case FGPositioned::NDB:
84 case FGPositioned::WAYPOINT:
87 case FGPositioned::FIX:
95 GPSWaypoint* GPSWaypoint::createFromPositioned(const FGPositioned* aPos)
98 return NULL; // happens if find returns no match
101 return new GPSWaypoint(aPos->ident(),
102 aPos->latitude() * SG_DEGREES_TO_RADIANS,
103 aPos->longitude() * SG_DEGREES_TO_RADIANS,
104 GPSWpTypeFromFGPosType(aPos->type())
108 ostream& operator << (ostream& os, GPSAppWpType type) {
110 case(GPS_IAF): return(os << "IAF");
111 case(GPS_IAP): return(os << "IAP");
112 case(GPS_FAF): return(os << "FAF");
113 case(GPS_MAP): return(os << "MAP");
114 case(GPS_MAHP): return(os << "MAHP");
115 case(GPS_HDR): return(os << "HEADER");
116 case(GPS_FENCE): return(os << "FENCE");
117 case(GPS_APP_NONE): return(os << "NONE");
119 return(os << "ERROR - Unknown switch in GPSAppWpType operator << ");
131 FGNPIAP::~FGNPIAP() {
134 ClockTime::ClockTime() {
139 ClockTime::ClockTime(int hr, int min) {
140 while(hr < 0) { hr += 24; }
142 while(min < 0) { min += 60; }
143 while(min > 60) { min -= 60; }
147 ClockTime::~ClockTime() {
150 // ------------------------------------------------------------------------------------- //
152 DCLGPS::DCLGPS(RenderArea2D* instrument) {
153 _instrument = instrument;
157 _lon_node = fgGetNode("/instrumentation/gps/indicated-longitude-deg", true);
158 _lat_node = fgGetNode("/instrumentation/gps/indicated-latitude-deg", true);
159 _alt_node = fgGetNode("/instrumentation/gps/indicated-altitude-ft", true);
160 _grnd_speed_node = fgGetNode("/instrumentation/gps/indicated-ground-speed-kt", true);
161 _true_track_node = fgGetNode("/instrumentation/gps/indicated-track-true-deg", true);
162 _mag_track_node = fgGetNode("/instrumentation/gps/indicated-track-magnetic-deg", true);
164 // Use FG's position values at construction in case FG's gps has not run first update yet.
165 _lon = fgGetDouble("/position/longitude-deg") * SG_DEGREES_TO_RADIANS;
166 _lat = fgGetDouble("/position/latitude-deg") * SG_DEGREES_TO_RADIANS;
167 _alt = fgGetDouble("/position/altitude-ft");
168 // Note - we can depriciate _gpsLat and _gpsLon if we implement error handling in FG
169 // gps code and not our own.
174 _groundSpeed_ms = 0.0;
175 _groundSpeed_kts = 0.0;
179 // Sensible defaults. These can be overriden by derived classes if desired.
181 _cdiScales.push_back(5.0);
182 _cdiScales.push_back(1.0);
183 _cdiScales.push_back(0.3);
184 _currentCdiScaleIndex = 0;
185 _targetCdiScaleIndex = 0;
186 _sourceCdiScaleIndex = 0;
187 _cdiScaleTransition = false;
188 _currentCdiScale = 5.0;
192 _activeWaypoint.id.clear();
194 _crosstrackDist = 0.0;
195 _headingBugTo = true;
197 _waypointAlert = false;
199 _departureTimeString = "----";
201 _powerOnTime.set_hr(0);
202 _powerOnTime.set_min(0);
203 _powerOnTimerSet = false;
206 // Configuration Initialisation
207 // Should this be in kln89.cxx ?
208 _turnAnticipationEnabled = false;
212 _messageStack.clear();
216 _approachLoaded = false;
217 _approachArm = false;
218 _approachReallyArmed = false;
219 _approachActive = false;
220 _approachFP = new GPSFlightPlan;
225 delete _approachFP; // Don't need to delete the waypoints inside since they point to
226 // the waypoints in the approach database.
227 // TODO - may need to delete the approach database!!
230 void DCLGPS::draw(osg::State& state) {
231 _instrument->Draw(state);
234 void DCLGPS::init() {
236 // Not sure if this should be here, but OK for now.
237 CreateDefaultFlightPlans();
242 void DCLGPS::bind() {
243 _tiedProperties.setRoot(fgGetNode("/instrumentation/gps", true));
244 _tiedProperties.Tie("waypoint-alert", this, &DCLGPS::GetWaypointAlert);
245 _tiedProperties.Tie("leg-mode", this, &DCLGPS::GetLegMode);
246 _tiedProperties.Tie("obs-mode", this, &DCLGPS::GetOBSMode);
247 _tiedProperties.Tie("approach-arm", this, &DCLGPS::GetApproachArm);
248 _tiedProperties.Tie("approach-active", this, &DCLGPS::GetApproachActive);
249 _tiedProperties.Tie("cdi-deflection", this, &DCLGPS::GetCDIDeflection);
250 _tiedProperties.Tie("to-flag", this, &DCLGPS::GetToFlag);
253 void DCLGPS::unbind() {
254 _tiedProperties.Untie();
257 void DCLGPS::update(double dt) {
258 //cout << "update called!\n";
260 _lon = _lon_node->getDoubleValue() * SG_DEGREES_TO_RADIANS;
261 _lat = _lat_node->getDoubleValue() * SG_DEGREES_TO_RADIANS;
262 _alt = _alt_node->getDoubleValue();
263 _groundSpeed_kts = _grnd_speed_node->getDoubleValue();
264 _groundSpeed_ms = _groundSpeed_kts * 0.5144444444;
265 _track = _true_track_node->getDoubleValue();
266 _magTrackDeg = _mag_track_node->getDoubleValue();
267 // Note - we can depriciate _gpsLat and _gpsLon if we implement error handling in FG
268 // gps code and not our own.
271 // Check for abnormal position slew
272 if(GetGreatCircleDistance(_gpsLat, _gpsLon, _checkLat, _checkLon) > 1.0) {
273 OrientateToActiveFlightPlan();
278 // TODO - check for unit power before running this.
279 if(!_powerOnTimerSet) {
283 // Check if an alarm timer has expired
285 if(_alarmTime.hr() == atoi(fgGetString("/instrumentation/clock/indicated-hour"))
286 && _alarmTime.min() == atoi(fgGetString("/instrumentation/clock/indicated-min"))) {
287 _messageStack.push_back("*Timer Expired");
293 if(_groundSpeed_kts > 30.0) {
295 string th = fgGetString("/instrumentation/clock/indicated-hour");
296 string tm = fgGetString("/instrumentation/clock/indicated-min");
297 if(th.size() == 1) th = "0" + th;
298 if(tm.size() == 1) tm = "0" + tm;
299 _departureTimeString = th + tm;
302 // TODO - check - is this prone to drift error over time?
303 // Should we difference the departure and current times?
304 // What about when the user resets the time of day from the menu?
308 _time->update(_gpsLon * SG_DEGREES_TO_RADIANS, _gpsLat * SG_DEGREES_TO_RADIANS, 0, 0);
309 // FIXME - currently all the below assumes leg mode and no DTO or OBS cancelled.
310 if(_activeFP->IsEmpty()) {
311 // Not sure if we need to reset these each update or only when fp altered
312 _activeWaypoint.id.clear();
314 } else if(_activeFP->waypoints.size() == 1) {
315 _activeWaypoint.id.clear();
318 if(_activeWaypoint.id.empty() || _fromWaypoint.id.empty()) {
319 //cout << "Error, in leg mode with flightplan of 2 or more waypoints, but either active or from wp is NULL!\n";
320 OrientateToActiveFlightPlan();
324 if(_approachLoaded) {
325 if(!_approachReallyArmed && !_approachActive) {
326 // arm if within 30nm of airport.
327 // TODO - let user cancel approach arm using external GPS-APR switch
329 const FGAirport* ap = FindFirstAptById(_approachID, multi, true);
331 double d = GetGreatCircleDistance(_gpsLat, _gpsLon, ap->getLatitude() * SG_DEGREES_TO_RADIANS, ap->getLongitude() * SG_DEGREES_TO_RADIANS);
334 _approachReallyArmed = true;
335 _messageStack.push_back("*Press ALT To Set Baro");
336 // Not sure what we do if the user has already set CDI to 0.3 nm?
337 _targetCdiScaleIndex = 1;
338 if(_currentCdiScaleIndex == 1) {
340 } else if(_currentCdiScaleIndex == 0) {
341 _sourceCdiScaleIndex = 0;
342 _cdiScaleTransition = true;
343 _cdiTransitionTime = 30.0;
344 _currentCdiScale = _cdiScales[_currentCdiScaleIndex];
349 // Check for approach active - we can only activate approach if it is really armed.
350 if(_activeWaypoint.appType == GPS_FAF) {
351 //cout << "Active waypoint is FAF, id is " << _activeWaypoint.id << '\n';
352 if(GetGreatCircleDistance(_gpsLat, _gpsLon, _activeWaypoint.lat, _activeWaypoint.lon) <= 2.0 && !_obsMode) {
353 // Assume heading is OK for now
354 _approachArm = false; // TODO - check - maybe arm is left on when actv comes on?
355 _approachReallyArmed = false;
356 _approachActive = true;
357 _targetCdiScaleIndex = 2;
358 if(_currentCdiScaleIndex == 2) {
360 } else if(_currentCdiScaleIndex == 1) {
361 _sourceCdiScaleIndex = 1;
362 _cdiScaleTransition = true;
363 _cdiTransitionTime = 30.0; // TODO - compress it if time to FAF < 30sec
364 _currentCdiScale = _cdiScales[_currentCdiScaleIndex];
366 // Abort going active?
367 _approachActive = false;
374 // CDI scale transition stuff
375 if(_cdiScaleTransition) {
376 if(fabs(_currentCdiScale - _cdiScales[_targetCdiScaleIndex]) < 0.001) {
377 _currentCdiScale = _cdiScales[_targetCdiScaleIndex];
378 _currentCdiScaleIndex = _targetCdiScaleIndex;
379 _cdiScaleTransition = false;
381 double scaleDiff = (_targetCdiScaleIndex > _sourceCdiScaleIndex
382 ? _cdiScales[_sourceCdiScaleIndex] - _cdiScales[_targetCdiScaleIndex]
383 : _cdiScales[_targetCdiScaleIndex] - _cdiScales[_sourceCdiScaleIndex]);
384 //cout << "ScaleDiff = " << scaleDiff << '\n';
385 if(_targetCdiScaleIndex > _sourceCdiScaleIndex) {
386 // Scaling down eg. 5nm -> 1nm
387 _currentCdiScale -= (scaleDiff * dt / _cdiTransitionTime);
388 if(_currentCdiScale < _cdiScales[_targetCdiScaleIndex]) {
389 _currentCdiScale = _cdiScales[_targetCdiScaleIndex];
390 _currentCdiScaleIndex = _targetCdiScaleIndex;
391 _cdiScaleTransition = false;
394 _currentCdiScale += (scaleDiff * dt / _cdiTransitionTime);
395 if(_currentCdiScale > _cdiScales[_targetCdiScaleIndex]) {
396 _currentCdiScale = _cdiScales[_targetCdiScaleIndex];
397 _currentCdiScaleIndex = _targetCdiScaleIndex;
398 _cdiScaleTransition = false;
401 //cout << "_currentCdiScale = " << _currentCdiScale << '\n';
404 _currentCdiScale = _cdiScales[_currentCdiScaleIndex];
408 // Urgh - I've been setting the heading bug based on DTK,
409 // bug I think it should be based on heading re. active waypoint
410 // based on what the sim does after the final waypoint is passed.
411 // (DTK remains the same, but if track is held == DTK heading bug
412 // reverses to from once wp is passed).
414 if(_fromWaypoint != NULL) {
415 // TODO - how do we handle the change of track with distance over long legs?
416 _dtkTrue = GetGreatCircleCourse(_fromWaypoint->lat, _fromWaypoint->lon, _activeWaypoint->lat, _activeWaypoint->lon) * SG_RADIANS_TO_DEGREES;
417 _dtkMag = GetMagHeadingFromTo(_fromWaypoint->lat, _fromWaypoint->lon, _activeWaypoint->lat, _activeWaypoint->lon);
418 // Don't change the heading bug if speed is too low otherwise it flickers to/from at rest
419 if(_groundSpeed_ms > 5) {
420 //cout << "track = " << _track << ", dtk = " << _dtkTrue << '\n';
421 double courseDev = _track - _dtkTrue;
422 //cout << "courseDev = " << courseDev << ", normalized = ";
423 SG_NORMALIZE_RANGE(courseDev, -180.0, 180.0);
424 //cout << courseDev << '\n';
425 _headingBugTo = (fabs(courseDev) > 90.0 ? false : true);
430 // TODO - in DTO operation the position of initiation of DTO defines the "from waypoint".
433 if(!_activeWaypoint.id.empty()) {
434 double hdgTrue = GetGreatCircleCourse(_gpsLat, _gpsLon, _activeWaypoint.lat, _activeWaypoint.lon) * SG_RADIANS_TO_DEGREES;
435 if(_groundSpeed_ms > 5) {
436 //cout << "track = " << _track << ", hdgTrue = " << hdgTrue << '\n';
437 double courseDev = _track - hdgTrue;
438 //cout << "courseDev = " << courseDev << ", normalized = ";
439 SG_NORMALIZE_RANGE(courseDev, -180.0, 180.0);
440 //cout << courseDev << '\n';
441 _headingBugTo = (fabs(courseDev) > 90.0 ? false : true);
443 if(!_fromWaypoint.id.empty()) {
444 _dtkTrue = GetGreatCircleCourse(_fromWaypoint.lat, _fromWaypoint.lon, _activeWaypoint.lat, _activeWaypoint.lon) * SG_RADIANS_TO_DEGREES;
445 _dtkMag = GetMagHeadingFromTo(_fromWaypoint.lat, _fromWaypoint.lon, _activeWaypoint.lat, _activeWaypoint.lon);
452 _dist2Act = GetGreatCircleDistance(_gpsLat, _gpsLon, _activeWaypoint.lat, _activeWaypoint.lon) * SG_NM_TO_METER;
453 if(_groundSpeed_ms > 10.0) {
454 _eta = _dist2Act / _groundSpeed_ms;
455 if(_eta <= 36) { // TODO - this is slightly different if turn anticipation is enabled.
457 _waypointAlert = true; // TODO - not if the from flag is set.
461 // Check if we should sequence to next leg.
462 // Perhaps this should be done on distance instead, but 60s time (about 1 - 2 nm) seems reasonable for now.
463 //double reverseHeading = GetGreatCircleCourse(_activeWaypoint->lat, _activeWaypoint->lon, _fromWaypoint->lat, _fromWaypoint->lon);
464 // Hack - let's cheat and do it on heading bug for now. TODO - that stops us 'cutting the corner'
465 // when we happen to approach the inside turn of a waypoint - we should probably sequence at the midpoint
466 // of the heading difference between legs in this instance.
467 int idx = GetActiveWaypointIndex();
468 bool finalLeg = (idx == (int)(_activeFP->waypoints.size()) - 1 ? true : false);
469 bool finalDto = (_dto && idx == -1); // Dto operation to a waypoint not in the flightplan - we don't sequence in this instance
472 // Do nothing - not sure if Dto should switch off when arriving at the final waypoint of a flightplan
473 } else if(finalDto) {
475 } else if(_activeWaypoint.appType == GPS_MAP) {
476 // Don't sequence beyond the missed approach point
477 //cout << "ACTIVE WAYPOINT is MAP - not sequencing!!!!!\n";
479 //cout << "Sequencing...\n";
480 _fromWaypoint = _activeWaypoint;
481 _activeWaypoint = *_activeFP->waypoints[idx + 1];
483 // Course alteration message format is dependent on whether we are slaved HSI/CDI indicator or not.
485 if(fgGetBool("/instrumentation/nav[0]/slaved-to-gps")) {
486 // TODO - avoid the hardwiring on nav[0]
487 s = "Adj Nav Crs to ";
489 string s = "GPS Course is ";
491 double d = GetMagHeadingFromTo(_fromWaypoint.lat, _fromWaypoint.lon, _activeWaypoint.lat, _activeWaypoint.lon);
492 while(d < 0.0) d += 360.0;
493 while(d >= 360.0) d -= 360.0;
495 snprintf(buf, 4, "%03i", (int)(d + 0.5));
497 _messageStack.push_back(s);
499 _waypointAlert = false;
507 // First attempt at a sensible cross-track correction calculation
508 // Uh? - I think this is implemented further down the file!
509 if(_fromWaypoint != NULL) {
512 _crosstrackDist = 0.0;
519 Expand a SIAP ident to the full procedure name (as shown on the approach chart).
520 NOTE: Some of this is inferred from data, some is from documentation.
522 Example expansions from ARINC 424-18 [and the airport they're taken from]:
523 "R10LY" <--> "RNAV (GPS) Y RWY 10 L" [KBOI]
524 "R10-Z" <--> "RNAV (GPS) Z RWY 10" [KHTO]
525 "S25" <--> "VOR or GPS RWY 25" [KHHR]
526 "P20" <--> "GPS RWY 20" [KDAN]
527 "NDB-B" <--> "NDB or GPS-B" [KDAW]
528 "NDBC" <--> "NDB or GPS-C" [KEMT]
529 "VDMA" <--> "VOR/DME or GPS-A" [KDAW]
530 "VDM-A" <--> "VOR/DME or GPS-A" [KEAG]
531 "VDMB" <--> "VOR/DME or GPS-B" [KDKX]
532 "VORA" <--> "VOR or GPS-A" [KEMT]
534 It seems that there are 2 basic types of expansions; those that include
535 the runway and those that don't. Of those that don't, it seems that 2
536 different positions within the string to encode the identifying letter
537 are used, i.e. with a dash and without.
539 string DCLGPS::ExpandSIAPIdent(const string& ident) {
541 bool has_rwy = false;
544 case 'N': name = "NDB or GPS"; has_rwy = false; break;
545 case 'P': name = "GPS"; has_rwy = true; break;
546 case 'R': name = "RNAV (GPS)"; has_rwy = true; break;
547 case 'S': name = "VOR or GPS"; has_rwy = true; break;
549 if(ident[1] == 'D') name = "VOR/DME or GPS";
550 else name = "VOR or GPS";
553 default: // TODO output a log message
558 // Add the identifying letter if present
559 if(ident.size() == 5) {
566 name += ident.substr(1, 2);
568 // Add a left/right/centre indication if present.
569 if(ident.size() > 3) {
570 if((ident[3] != '-') && (ident[3] != ' ')) { // Early versions of the spec allowed a blank instead of a dash so check for both
576 // Add the identifying letter, which I *think* should always be present, but seems to be inconsistent as to whether a dash is used.
577 if(ident.size() == 5) {
580 } else if(ident.size() == 4) {
592 Load instrument approaches from an ARINC 424 file.
593 Tested on ARINC 424-18.
594 Known / current best guess at the format:
595 Col 1: Always 'S'. If it isn't, ditch it.
596 Col 2-4: "Customer area" code, eg "USA", "CAN". I think that CAN is used for Alaska.
597 Col 5: Section code. Used in conjunction with sub-section code. Definitions are with sub-section code.
599 Col 7-10: ICAO (or FAA) airport ident. Left justified if < 4 chars.
600 Col 11-12: Based on ICAO geographical region.
601 Col 13: Sub-section code. Used in conjunction with section code.
602 "HD/E/F" => Helicopter record.
603 "HS" => Helicopter minimum safe altitude.
604 "PA" => Airport record.
605 "PF" => Approach segment.
606 "PG" => Runway record.
607 "PP" => Path point record. ???
608 "PS" => MSA record (minimum safe altitude).
610 ------ The following is for "PF", approach segment -------
612 Col 14-19: SIAP ident for this approach (left justified). This is a standardised abbreviated approach name.
613 e.g. "R10LZ" expands to "RNAV (GPS) Z RWY 10 L". See the comment block for ExpandSIAPIdent for full details.
614 Col 20: Route type. This is tricky - I don't have full documentation and am having to guess a bit.
615 'A' => Arrival route? This seems to be used to encode arrival routes from the IAF to the approach proper.
616 Note that the final fix of the arrival route is duplicated in the approach proper.
617 'D' => VOR/DME or GPS
619 'P' => GPS (ARINC 424-18), GPS and RNAV (GPS) (ARINC 424-15 and before).
620 'R' => RNAV (GPS) (ARINC 424-18).
622 Col 21-25: Transition identifier. AFAICT, this is the ident of the IAF for this initial approach route, and left blank for the final approach course. See col 30-34 for the actual fix ident.
624 Col 27-29: Sequence number - position within the route segment. Rule: 10-20-30 etc.
625 Col 30-34: Fix identifer. The ident of the waypoint.
626 Col 35-36: ICAO geographical region code. I think we can ignore this for now.
627 Col 37: Section code - ??? I don't know what this means
628 Col 38 Subsection code - ??? ditto - no idea!
629 Col 40: Waypoint type.
630 'A' => Airport as waypoint
631 'E' => Essential waypoint (e.g. change of heading at this waypoint, etc).
632 'G' => Runway or helipad as waypoint
633 'H' => Heliport as waypoint
634 'N' => NDB as waypoint
635 'P' => Phantom waypoint (not sure if this is used in rev 18?)
636 'V' => VOR as waypoint
637 Col 41: Waypoint type.
638 'B' => Flyover, approach transition, or final approach.
639 'E' => end of route segment (transition waypoint). (Actually "End of terminal procedure route type" in the docs).
640 'N' => ??? I've also seen 'N' in this column, but don't know what it indicates.
642 Col 43: Waypoint type. May also be blank when none of the below.
643 'A' => Initial approach fix (IAF)
644 'F' => Final approach fix
646 'I' => Final approach course fix
647 'M' => Missed approach point
648 'P' => ??? This is present, but I don't know what this means and it wasn't in the FAA docs that I found the above in!
649 ??? Possibly procedure turn?
650 'C' => ??? This is also present in the data, but missing from the docs. Is at airport 00R.
651 Col 107-111 MSA center fix. We can ignore this.
653 void DCLGPS::LoadApproachData() {
657 const GPSWaypoint* cwp;
660 SGPath path = globals->get_fg_root();
661 path.append("Navaids/rnav.dat");
662 fin.open(path.c_str(), ios::in);
664 //cout << "Unable to open input file " << path.c_str() << '\n';
667 //cout << "Opened " << path.c_str() << " for reading\n";
672 string apt_ident; // This gets set to the ICAO code of the current airport being processed.
673 string iap_ident; // The abbreviated name of the current approach being processed.
674 string wp_ident; // The ident of the waypoint of the current line
675 string last_apt_ident;
676 string last_iap_ident;
677 string last_wp_ident;
678 // There is no need to save the full name - it can be generated on the fly from the abbreviated name as and when needed.
679 bool apt_in_progress = false; // Set true whilst loading all the approaches for a given airport.
680 bool iap_in_progress = false; // Set true whilst loading a given approach.
681 bool iap_error = false; // Set true if there is an error loading a given approach.
682 bool route_in_progress = false; // Set true when we are loading a "route" segment of the approach.
683 int last_sequence_number = 0; // Position within the route, rule (rev 18): 10, 20, 30 etc.
685 char last_route_type = 0;
687 char waypoint_fix_type; // This is the waypoint type from col 43, i.e. the type of fix. May be blank.
692 unsigned int nLoaded = 0;
693 unsigned int nErrors = 0;
695 //for(i=0; i<64; ++i) {
697 fin.getline(tmp, 256);
698 //s = Fake_rnav_dat[i];
700 if(s.size() < 132) continue;
701 if(s[0] == 'S') { // Valid line
702 string country_code = s.substr(1, 3);
703 if(country_code == "USA") { // For now we'll stick to US procedures in case there are unknown gotchas with others
704 if(s[4] == 'P') { // Includes approaches.
705 if(s[12] == 'A') { // Airport record
706 apt_ident = s.substr(6, 4);
707 // Trim any whitespace from the ident. The ident is left justified,
708 // so any space will be at the end.
709 if(apt_ident[3] == ' ') apt_ident = apt_ident.substr(0, 3);
710 // I think that all idents are either 3 or 4 chars - could check this though!
711 if(!apt_in_progress) {
712 last_apt_ident = apt_ident;
715 if(last_apt_ident != apt_ident) {
716 if(iap_in_progress) {
718 //cout << "ERROR: Unable to load approach " << iap->_ident << " at " << iap->_aptIdent << '\n';
721 _np_iap[iap->_aptIdent].push_back(iap);
722 //cout << "** Loaded " << iap->_aptIdent << "\t" << iap->_ident << '\n';
725 iap_in_progress = false;
728 last_apt_ident = apt_ident;
731 } else if(s[12] == 'F') { // Approach segment
732 if(apt_in_progress) {
733 iap_ident = s.substr(13, 6);
734 // Trim any whitespace from the RH end.
736 if(iap_ident[5-j] == ' ') {
737 iap_ident = iap_ident.substr(0, 5-j);
739 // It's important to break here, since earlier versions of ARINC 424 allowed spaces in the ident.
743 if(iap_in_progress) {
744 if(iap_ident != last_iap_ident) {
745 // This is a new approach - store the last one and trigger
746 // starting afresh by setting the in progress flag to false.
748 //cout << "ERROR: Unable to load approach " << iap->_ident << " at " << iap->_aptIdent << '\n';
751 _np_iap[iap->_aptIdent].push_back(iap);
752 //cout << "Loaded " << iap->_aptIdent << "\t" << iap->_ident << '\n';
755 iap_in_progress = false;
758 if(!iap_in_progress) {
760 iap->_aptIdent = apt_ident;
761 iap->_ident = iap_ident;
762 iap->_name = ExpandSIAPIdent(iap_ident); // I suspect that it's probably better to just store idents, and to expand the names as needed.
763 // Note, we haven't set iap->_rwyStr yet.
764 last_iap_ident = iap_ident;
765 iap_in_progress = true;
771 sequence_number = atoi(s.substr(26,3).c_str());
772 wp_ident = s.substr(29, 5);
773 waypoint_fix_type = s[42];
774 // Trim any whitespace from the RH end
776 if(wp_ident[4-j] == ' ') {
777 wp_ident = wp_ident.substr(0, 4-j);
783 // Ignore lines with no waypoint ID for now - these are normally part of the
784 // missed approach procedure, and we don't use them in the KLN89.
785 if(!wp_ident.empty()) {
786 // Make a local copy of the waypoint for now, since we're not yet sure if we'll be using it
789 bool wp_error = false;
790 if(w.id.substr(0, 2) == "RW" && waypoint_fix_type == 'M') {
791 // Assume that this is a missed-approach point based on the runway number, which appears to be standard for most approaches.
792 // Note: Currently fgFindAirportID returns NULL on error, but getRunwayByIdent throws an exception.
793 const FGAirport* apt = fgFindAirportID(iap->_aptIdent);
797 rwystr = w.id.substr(2, 2);
798 // TODO - sanity check the rwystr at this point to ensure we have a double digit number
799 if(w.id.size() > 4) {
800 if(w.id[4] == 'L' || w.id[4] == 'C' || w.id[4] == 'R') {
804 FGRunway* rwy = apt->getRunwayByIdent(rwystr);
805 w.lat = rwy->begin().getLatitudeRad();
806 w.lon = rwy->begin().getLongitudeRad();
807 } catch(const sg_exception&) {
808 SG_LOG(SG_INSTR, SG_WARN, "Unable to find runway " << w.id.substr(2, 2) << " at airport " << iap->_aptIdent);
809 //cout << "Unable to find runway " << w.id.substr(2, 2) << " at airport " << iap->_aptIdent << " ( w.id = " << w.id << ", rwystr = " << rwystr << " )\n";
816 cwp = FindFirstByExactId(w.id);
823 switch(waypoint_fix_type) {
824 case 'A': w.appType = GPS_IAF; break;
825 case 'F': w.appType = GPS_FAF; break;
826 case 'H': w.appType = GPS_MAHP; break;
827 case 'I': w.appType = GPS_IAP; break;
828 case 'M': w.appType = GPS_MAP; break;
829 case ' ': w.appType = GPS_APP_NONE; break;
830 //default: cout << "Unknown waypoint_fix_type: \'" << waypoint_fix_type << "\' [" << apt_ident << ", " << iap_ident << "]\n";
834 //cout << "Unable to find waypoint " << w.id << " [" << apt_ident << ", " << iap_ident << "]\n";
839 if(route_in_progress) {
840 if(sequence_number > last_sequence_number) {
841 // TODO - add a check for runway numbers
842 // Check for the waypoint ID being the same as the previous line.
843 // This is often the case for the missed approach holding point.
844 if(wp_ident == last_wp_ident) {
845 if(waypoint_fix_type == 'H') {
846 if(!iap->_IAP.empty()) {
847 if(iap->_IAP[iap->_IAP.size() - 1]->appType == GPS_APP_NONE) {
848 iap->_IAP[iap->_IAP.size() - 1]->appType = GPS_MAHP;
850 //cout << "Waypoint is MAHP and another type! " << w.id << " [" << apt_ident << ", " << iap_ident << "]\n";
855 // Create a new waypoint on the heap, copy the local copy into it, and push it onto the approach.
856 wp = new GPSWaypoint;
858 if(route_type == 'A') {
859 fp->waypoints.push_back(wp);
861 iap->_IAP.push_back(wp);
864 } else if(sequence_number == last_sequence_number) {
865 // This seems to happen once per final approach route - one of the waypoints
866 // is duplicated with the same sequence number - I'm not sure what information
867 // the second line give yet so ignore it for now.
868 // TODO - figure this out!
870 // Finalise the current route and start a new one
872 // Finalise the current route
873 if(last_route_type == 'A') {
874 // Push the flightplan onto the approach
875 iap->_approachRoutes.push_back(fp);
877 // All the waypoints get pushed individually - don't need to do it.
880 // There are basically 2 possibilities here - either it's one of the arrival transitions,
881 // or it's the core final approach course.
882 wp = new GPSWaypoint;
884 if(route_type == 'A') { // It's one of the arrival transition(s)
885 fp = new GPSFlightPlan;
886 fp->waypoints.push_back(wp);
888 iap->_IAP.push_back(wp);
890 route_in_progress = true;
893 // Start a new route.
894 // There are basically 2 possibilities here - either it's one of the arrival transitions,
895 // or it's the core final approach course.
896 wp = new GPSWaypoint;
898 if(route_type == 'A') { // It's one of the arrival transition(s)
899 fp = new GPSFlightPlan;
900 fp->waypoints.push_back(wp);
902 iap->_IAP.push_back(wp);
904 route_in_progress = true;
906 last_route_type = route_type;
907 last_wp_ident = wp_ident;
908 last_sequence_number = sequence_number;
912 // ERROR - no airport record read.
916 // Check and finalise any approaches in progress
917 // TODO - sanity check that the approach has all the required elements
918 if(iap_in_progress) {
919 // This is a new approach - store the last one and trigger
920 // starting afresh by setting the in progress flag to false.
922 //cout << "ERROR: Unable to load approach " << iap->_ident << " at " << iap->_aptIdent << '\n';
925 _np_iap[iap->_aptIdent].push_back(iap);
926 //cout << "* Loaded " << iap->_aptIdent << "\t" << iap->_ident << '\n';
929 iap_in_progress = false;
936 // If we get to the end of the file, load any approach that is still in progress
937 // TODO - sanity check that the approach has all the required elements
938 if(iap_in_progress) {
940 //cout << "ERROR: Unable to load approach " << iap->_ident << " at " << iap->_aptIdent << '\n';
943 _np_iap[iap->_aptIdent].push_back(iap);
944 //cout << "*** Loaded " << iap->_aptIdent << "\t" << iap->_ident << '\n';
949 //cout << "Done loading approach database\n";
950 //cout << "Loaded: " << nLoaded << '\n';
951 //cout << "Failed: " << nErrors << '\n';
956 GPSWaypoint* DCLGPS::GetActiveWaypoint() {
957 return &_activeWaypoint;
961 float DCLGPS::GetDistToActiveWaypoint() {
965 // I don't yet fully understand all the gotchas about where to source time from.
966 // This function sets the initial timer before the clock exports properties
967 // and the one below uses the clock to be consistent with the rest of the code.
968 // It might change soonish...
969 void DCLGPS::SetPowerOnTimer() {
970 struct tm *t = globals->get_time_params()->getGmt();
971 _powerOnTime.set_hr(t->tm_hour);
972 _powerOnTime.set_min(t->tm_min);
973 _powerOnTimerSet = true;
976 void DCLGPS::ResetPowerOnTimer() {
977 _powerOnTime.set_hr(atoi(fgGetString("/instrumentation/clock/indicated-hour")));
978 _powerOnTime.set_min(atoi(fgGetString("/instrumentation/clock/indicated-min")));
979 _powerOnTimerSet = true;
982 double DCLGPS::GetCDIDeflection() const {
983 double xtd = CalcCrossTrackDeviation(); //nm
984 return((xtd / _currentCdiScale) * 5.0 * 2.5 * -1.0);
987 void DCLGPS::DtoInitiate(const string& s) {
988 const GPSWaypoint* wp = FindFirstByExactId(s);
990 // TODO - Currently we start DTO operation unconditionally, regardless of which mode we are in.
991 // In fact, the following rules apply:
992 // In LEG mode, start DTO as we currently do.
993 // In OBS mode, set the active waypoint to the requested waypoint, and then:
994 // If the KLN89 is not connected to an external HSI or CDI, set the OBS course to go direct to the waypoint.
995 // If the KLN89 *is* connected to an external HSI or CDI, it cannot set the course itself, and will display
996 // a scratchpad message with the course to set manually on the HSI/CDI.
997 // In both OBS cases, leave _dto false, since we don't need the virtual waypoint created.
999 _activeWaypoint = *wp;
1000 _fromWaypoint.lat = _gpsLat;
1001 _fromWaypoint.lon = _gpsLon;
1002 _fromWaypoint.type = GPS_WP_VIRT;
1003 _fromWaypoint.id = "_DTOWP_";
1006 // TODO - Should bring up the user waypoint page.
1011 void DCLGPS::DtoCancel() {
1013 // i.e. don't bother reorientating if we're just cancelling a DTO button press
1014 // without having previously initiated DTO.
1015 OrientateToActiveFlightPlan();
1020 void DCLGPS::ToggleOBSMode() {
1021 _obsMode = !_obsMode;
1023 // We need to set the OBS heading. The rules for this are:
1025 // If the KLN89 is connected to an external HSI or CDI, then the OBS heading must be set
1026 // to the OBS radial on the external indicator, and cannot be changed in the KLN89.
1028 // If the KLN89 is not connected to an external indicator, then:
1029 // If there is an active waypoint, the OBS heading is set such that the
1030 // deviation indicator remains at the same deviation (i.e. set to DTK,
1031 // although there may be some small difference).
1033 // If there is not an active waypoint, I am not sure what value should be
1036 if(fgGetBool("/instrumentation/nav/slaved-to-gps")) {
1037 _obsHeading = static_cast<int>(fgGetDouble("/instrumentation/nav/radials/selected-deg") + 0.5);
1038 } else if(!_activeWaypoint.id.empty()) {
1039 // NOTE: I am not sure if this is completely correct. There are sometimes small differences
1040 // between DTK and default OBS heading in the simulator (~ 1 or 2 degrees). It might also
1041 // be that OBS heading should be stored as float and only displayed as int, in order to maintain
1042 // the initial bar deviation?
1043 _obsHeading = static_cast<int>(_dtkMag + 0.5);
1044 //cout << "_dtkMag = " << _dtkMag << ", _dtkTrue = " << _dtkTrue << ", _obsHeading = " << _obsHeading << '\n';
1046 // TODO - what should we really do here?
1050 // Valid OBS heading values are 0 -> 359 degrees inclusive (from kln89 simulator).
1051 if(_obsHeading > 359) _obsHeading -= 360;
1052 if(_obsHeading < 0) _obsHeading += 360;
1054 // TODO - the _fromWaypoint location will change as the OBS heading changes.
1055 // Might need to store the OBS initiation position somewhere in case it is needed again.
1056 SetOBSFromWaypoint();
1060 // Set the _fromWaypoint position based on the active waypoint and OBS radial.
1061 void DCLGPS::SetOBSFromWaypoint() {
1062 if(!_obsMode) return;
1063 if(_activeWaypoint.id.empty()) return;
1065 // TODO - base the 180 deg correction on the to/from flag.
1066 _fromWaypoint = GetPositionOnMagRadial(_activeWaypoint, 10, _obsHeading + 180.0);
1067 _fromWaypoint.type = GPS_WP_VIRT;
1068 _fromWaypoint.id = "_OBSWP_";
1071 void DCLGPS::CDIFSDIncrease() {
1072 if(_currentCdiScaleIndex == 0) {
1073 _currentCdiScaleIndex = _cdiScales.size() - 1;
1075 _currentCdiScaleIndex--;
1079 void DCLGPS::CDIFSDDecrease() {
1080 _currentCdiScaleIndex++;
1081 if(_currentCdiScaleIndex == _cdiScales.size()) {
1082 _currentCdiScaleIndex = 0;
1086 void DCLGPS::DrawChar(char c, int field, int px, int py, bool bold) {
1089 void DCLGPS::DrawText(const string& s, int field, int px, int py, bool bold) {
1092 void DCLGPS::CreateDefaultFlightPlans() {}
1094 // Get the time to the active waypoint in seconds.
1095 // Returns -1 if groundspeed < 30 kts
1096 double DCLGPS::GetTimeToActiveWaypoint() {
1097 if(_groundSpeed_kts < 30.0) {
1104 // Get the time to the final waypoint in seconds.
1105 // Returns -1 if groundspeed < 30 kts
1106 double DCLGPS::GetETE() {
1107 if(_groundSpeed_kts < 30.0) {
1110 // TODO - handle OBS / DTO operation appropriately
1111 if(_activeFP->waypoints.empty()) {
1114 return(GetTimeToWaypoint(_activeFP->waypoints[_activeFP->waypoints.size() - 1]->id));
1119 // Get the time to a given waypoint (spec'd by ID) in seconds.
1120 // returns -1 if groundspeed is less than 30kts.
1121 // If the waypoint is an unreached part of the active flight plan the time will be via each leg.
1122 // otherwise it will be a direct-to time.
1123 double DCLGPS::GetTimeToWaypoint(const string& id) {
1124 if(_groundSpeed_kts < 30.0) {
1129 int n1 = GetActiveWaypointIndex();
1130 int n2 = GetWaypointIndex(id);
1133 for(unsigned int i=n1+1; i<_activeFP->waypoints.size(); ++i) {
1134 GPSWaypoint* wp1 = _activeFP->waypoints[i-1];
1135 GPSWaypoint* wp2 = _activeFP->waypoints[i];
1136 double distm = GetGreatCircleDistance(wp1->lat, wp1->lon, wp2->lat, wp2->lon) * SG_NM_TO_METER;
1137 eta += (distm / _groundSpeed_ms);
1140 } else if(id == _activeWaypoint.id) {
1143 const GPSWaypoint* wp = FindFirstByExactId(id);
1144 if(wp == NULL) return(-1.0);
1145 double distm = GetGreatCircleDistance(_gpsLat, _gpsLon, wp->lat, wp->lon);
1147 return(distm / _groundSpeed_ms);
1149 return(-1.0); // Hopefully we never get here!
1152 // Returns magnetic great-circle heading
1153 // TODO - document units.
1154 float DCLGPS::GetHeadingToActiveWaypoint() {
1155 if(_activeWaypoint.id.empty()) {
1158 double h = GetMagHeadingFromTo(_gpsLat, _gpsLon, _activeWaypoint.lat, _activeWaypoint.lon);
1159 while(h <= 0.0) h += 360.0;
1160 while(h > 360.0) h -= 360.0;
1165 // Returns magnetic great-circle heading
1166 // TODO - what units?
1167 float DCLGPS::GetHeadingFromActiveWaypoint() {
1168 if(_activeWaypoint.id.empty()) {
1171 double h = GetMagHeadingFromTo(_activeWaypoint.lat, _activeWaypoint.lon, _gpsLat, _gpsLon);
1172 while(h <= 0.0) h += 360.0;
1173 while(h > 360.0) h -= 360.0;
1178 void DCLGPS::ClearFlightPlan(int n) {
1179 for(unsigned int i=0; i<_flightPlans[n]->waypoints.size(); ++i) {
1180 delete _flightPlans[n]->waypoints[i];
1182 _flightPlans[n]->waypoints.clear();
1185 void DCLGPS::ClearFlightPlan(GPSFlightPlan* fp) {
1186 for(unsigned int i=0; i<fp->waypoints.size(); ++i) {
1187 delete fp->waypoints[i];
1189 fp->waypoints.clear();
1192 int DCLGPS::GetActiveWaypointIndex() {
1193 for(unsigned int i=0; i<_flightPlans[0]->waypoints.size(); ++i) {
1194 if(_flightPlans[0]->waypoints[i]->id == _activeWaypoint.id) return((int)i);
1199 int DCLGPS::GetWaypointIndex(const string& id) {
1200 for(unsigned int i=0; i<_flightPlans[0]->waypoints.size(); ++i) {
1201 if(_flightPlans[0]->waypoints[i]->id == id) return((int)i);
1206 void DCLGPS::OrientateToFlightPlan(GPSFlightPlan* fp) {
1207 //cout << "Orientating...\n";
1208 //cout << "_lat = " << _lat << ", _lon = " << _lon << ", _gpsLat = " << _gpsLat << ", gpsLon = " << _gpsLon << '\n';
1210 _activeWaypoint.id.clear();
1213 _navFlagged = false;
1214 if(fp->waypoints.size() == 1) {
1215 // TODO - may need to flag nav here if not dto or obs, or possibly handle it somewhere else.
1216 _activeWaypoint = *fp->waypoints[0];
1217 _fromWaypoint.id.clear();
1219 // FIXME FIXME FIXME
1220 _fromWaypoint = *fp->waypoints[0];
1221 _activeWaypoint = *fp->waypoints[1];
1222 double dmin = 1000000; // nm!!
1223 // For now we will simply start on the leg closest to our current position.
1224 // It's possible that more fancy algorithms may take either heading or track
1225 // into account when setting inital leg - I'm not sure.
1226 // This method should handle most cases perfectly OK though.
1227 for(unsigned int i = 1; i < fp->waypoints.size(); ++i) {
1228 //cout << "Pass " << i << ", dmin = " << dmin << ", leg is " << fp->waypoints[i-1]->id << " to " << fp->waypoints[i]->id << '\n';
1229 // First get the cross track correction.
1230 double d0 = fabs(CalcCrossTrackDeviation(*fp->waypoints[i-1], *fp->waypoints[i]));
1231 // That is the shortest distance away we could be though - check for
1232 // longer distances if we are 'off the end' of the leg.
1233 double ht1 = GetGreatCircleCourse(fp->waypoints[i-1]->lat, fp->waypoints[i-1]->lon,
1234 fp->waypoints[i]->lat, fp->waypoints[i]->lon)
1235 * SG_RADIANS_TO_DEGREES;
1236 // not simply the reverse of the above due to great circle navigation.
1237 double ht2 = GetGreatCircleCourse(fp->waypoints[i]->lat, fp->waypoints[i]->lon,
1238 fp->waypoints[i-1]->lat, fp->waypoints[i-1]->lon)
1239 * SG_RADIANS_TO_DEGREES;
1240 double hw1 = GetGreatCircleCourse(_gpsLat, _gpsLon,
1241 fp->waypoints[i]->lat, fp->waypoints[i]->lon)
1242 * SG_RADIANS_TO_DEGREES;
1243 double hw2 = GetGreatCircleCourse(_gpsLat, _gpsLon,
1244 fp->waypoints[i-1]->lat, fp->waypoints[i-1]->lon)
1245 * SG_RADIANS_TO_DEGREES;
1246 double h1 = ht1 - hw1;
1247 double h2 = ht2 - hw2;
1248 //cout << "d0, h1, h2 = " << d0 << ", " << h1 << ", " << h2 << '\n';
1249 //cout << "Normalizing...\n";
1250 SG_NORMALIZE_RANGE(h1, -180.0, 180.0);
1251 SG_NORMALIZE_RANGE(h2, -180.0, 180.0);
1252 //cout << "d0, h1, h2 = " << d0 << ", " << h1 << ", " << h2 << '\n';
1253 if(fabs(h1) > 90.0) {
1254 // We are past the end of the to waypoint
1255 double d = GetGreatCircleDistance(_gpsLat, _gpsLon, fp->waypoints[i]->lat, fp->waypoints[i]->lon);
1257 //cout << "h1 triggered, d0 now = " << d0 << '\n';
1258 } else if(fabs(h2) > 90.0) {
1259 // We are past the end (not yet at!) the from waypoint
1260 double d = GetGreatCircleDistance(_gpsLat, _gpsLon, fp->waypoints[i-1]->lat, fp->waypoints[i-1]->lon);
1262 //cout << "h2 triggered, d0 now = " << d0 << '\n';
1265 //cout << "THIS LEG NOW ACTIVE!\n";
1267 _fromWaypoint = *fp->waypoints[i-1];
1268 _activeWaypoint = *fp->waypoints[i];
1275 void DCLGPS::OrientateToActiveFlightPlan() {
1276 OrientateToFlightPlan(_activeFP);
1279 /***************************************/
1281 // Utility function - create a flightplan from a list of waypoint ids and types
1282 void DCLGPS::CreateFlightPlan(GPSFlightPlan* fp, vector<string> ids, vector<GPSWpType> wps) {
1283 if(fp == NULL) fp = new GPSFlightPlan;
1285 if(!fp->waypoints.empty()) {
1286 for(i=0; i<fp->waypoints.size(); ++i) {
1287 delete fp->waypoints[i];
1289 fp->waypoints.clear();
1291 if(ids.size() != wps.size()) {
1292 cout << "ID and Waypoint types list size mismatch in GPS::CreateFlightPlan - no flightplan created!\n";
1295 for(i=0; i<ids.size(); ++i) {
1297 const FGAirport* ap;
1299 GPSWaypoint* wp = new GPSWaypoint;
1303 ap = FindFirstAptById(ids[i], multi, true);
1308 wp->lat = ap->getLatitude() * SG_DEGREES_TO_RADIANS;
1309 wp->lon = ap->getLongitude() * SG_DEGREES_TO_RADIANS;
1311 fp->waypoints.push_back(wp);
1315 np = FindFirstVorById(ids[i], multi, true);
1320 wp->lat = np->get_lat() * SG_DEGREES_TO_RADIANS;
1321 wp->lon = np->get_lon() * SG_DEGREES_TO_RADIANS;
1323 fp->waypoints.push_back(wp);
1327 np = FindFirstNDBById(ids[i], multi, true);
1332 wp->lat = np->get_lat() * SG_DEGREES_TO_RADIANS;
1333 wp->lon = np->get_lon() * SG_DEGREES_TO_RADIANS;
1335 fp->waypoints.push_back(wp);
1351 /***************************************/
1353 class DCLGPSFilter : public FGPositioned::Filter
1356 virtual bool pass(const FGPositioned* aPos) const {
1357 switch (aPos->type()) {
1358 case FGPositioned::AIRPORT:
1359 // how about heliports and seaports?
1360 case FGPositioned::NDB:
1361 case FGPositioned::VOR:
1362 case FGPositioned::WAYPOINT:
1363 case FGPositioned::FIX:
1365 default: return false; // reject all other types
1371 GPSWaypoint* DCLGPS::FindFirstById(const string& id) const
1373 DCLGPSFilter filter;
1374 FGPositionedRef result = FGPositioned::findNextWithPartialId(NULL, id, &filter);
1375 return GPSWaypoint::createFromPositioned(result);
1378 GPSWaypoint* DCLGPS::FindFirstByExactId(const string& id) const
1380 SGGeod pos(SGGeod::fromRad(_lon, _lat));
1381 FGPositionedRef result = FGPositioned::findClosestWithIdent(id, pos);
1382 return GPSWaypoint::createFromPositioned(result);
1385 // TODO - add the ASCII / alphabetical stuff from the Atlas version
1386 FGPositioned* DCLGPS::FindTypedFirstById(const string& id, FGPositioned::Type ty, bool &multi, bool exact)
1389 FGPositioned::TypeFilter filter(ty);
1392 FGPositioned::List matches =
1393 FGPositioned::findAllWithIdent(id, &filter);
1394 FGPositioned::sortByRange(matches, SGGeod::fromRad(_lon, _lat));
1395 multi = (matches.size() > 1);
1396 return matches.empty() ? NULL : matches.front().ptr();
1399 return FGPositioned::findNextWithPartialId(NULL, id, &filter);
1402 FGNavRecord* DCLGPS::FindFirstVorById(const string& id, bool &multi, bool exact)
1404 return dynamic_cast<FGNavRecord*>(FindTypedFirstById(id, FGPositioned::VOR, multi, exact));
1407 FGNavRecord* DCLGPS::FindFirstNDBById(const string& id, bool &multi, bool exact)
1409 return dynamic_cast<FGNavRecord*>(FindTypedFirstById(id, FGPositioned::NDB, multi, exact));
1412 const FGFix* DCLGPS::FindFirstIntById(const string& id, bool &multi, bool exact)
1414 return dynamic_cast<FGFix*>(FindTypedFirstById(id, FGPositioned::FIX, multi, exact));
1417 const FGAirport* DCLGPS::FindFirstAptById(const string& id, bool &multi, bool exact)
1419 return dynamic_cast<FGAirport*>(FindTypedFirstById(id, FGPositioned::AIRPORT, multi, exact));
1422 FGNavRecord* DCLGPS::FindClosestVor(double lat_rad, double lon_rad) {
1423 FGPositioned::TypeFilter filter(FGPositioned::VOR);
1424 double cutoff = 1000; // nautical miles
1425 FGPositionedRef v = FGPositioned::findClosest(SGGeod::fromRad(lon_rad, lat_rad), cutoff, &filter);
1430 return dynamic_cast<FGNavRecord*>(v.ptr());
1433 //----------------------------------------------------------------------------------------------------------
1435 // Takes lat and lon in RADIANS!!!!!!!
1436 double DCLGPS::GetMagHeadingFromTo(double latA, double lonA, double latB, double lonB) {
1437 double h = GetGreatCircleCourse(latA, lonA, latB, lonB);
1438 h *= SG_RADIANS_TO_DEGREES;
1439 // TODO - use the real altitude below instead of 0.0!
1440 //cout << "MagVar = " << sgGetMagVar(_gpsLon, _gpsLat, 0.0, _time->getJD()) * SG_RADIANS_TO_DEGREES << '\n';
1441 h -= sgGetMagVar(_gpsLon, _gpsLat, 0.0, _time->getJD()) * SG_RADIANS_TO_DEGREES;
1442 while(h >= 360.0) h -= 360.0;
1443 while(h < 0.0) h += 360.0;
1447 // ---------------- Great Circle formulae from "The Aviation Formulary" -------------
1448 // Note that all of these assume that the world is spherical.
1450 double Rad2Nm(double theta) {
1451 return(((180.0*60.0)/SG_PI)*theta);
1454 double Nm2Rad(double d) {
1455 return((SG_PI/(180.0*60.0))*d);
1460 The great circle distance d between two points with coordinates {lat1,lon1} and {lat2,lon2} is given by:
1462 d=acos(sin(lat1)*sin(lat2)+cos(lat1)*cos(lat2)*cos(lon1-lon2))
1464 A mathematically equivalent formula, which is less subject to rounding error for short distances is:
1466 d=2*asin(sqrt((sin((lat1-lat2)/2))^2 +
1467 cos(lat1)*cos(lat2)*(sin((lon1-lon2)/2))^2))
1471 // Returns distance in nm, takes lat & lon in RADIANS
1472 double DCLGPS::GetGreatCircleDistance(double lat1, double lon1, double lat2, double lon2) const {
1473 double d = 2.0 * asin(sqrt(((sin((lat1-lat2)/2.0))*(sin((lat1-lat2)/2.0))) +
1474 cos(lat1)*cos(lat2)*(sin((lon1-lon2)/2.0))*(sin((lon1-lon2)/2.0))));
1478 // fmod dosen't do what we want :-(
1479 static double mod(double d1, double d2) {
1480 return(d1 - d2*floor(d1/d2));
1483 // Returns great circle course from point 1 to point 2
1484 // Input and output in RADIANS.
1485 double DCLGPS::GetGreatCircleCourse (double lat1, double lon1, double lat2, double lon2) const {
1488 // Special case the poles
1489 if(cos(lat1) < SG_EPSILON) {
1491 // Starting from North Pole
1494 // Starting from South Pole
1498 // Urgh - the formula below is for negative lon +ve !!!???
1499 double d = GetGreatCircleDistance(lat1, lon1, lat2, lon2);
1500 cout << "d = " << d;
1502 //cout << ", d_theta = " << d;
1503 //cout << ", and d = " << Rad2Nm(d) << ' ';
1504 if(sin(lon2 - lon1) < 0) {
1506 h = acos((sin(lat2)-sin(lat1)*cos(d))/(sin(d)*cos(lat1)));
1509 h = 2.0 * SG_PI - acos((sin(lat2)-sin(lat1)*cos(d))/(sin(d)*cos(lat1)));
1512 cout << h * SG_RADIANS_TO_DEGREES << '\n';
1515 return( mod(atan2(sin(lon2-lon1)*cos(lat2),
1516 cos(lat1)*sin(lat2)-sin(lat1)*cos(lat2)*cos(lon2-lon1)),
1520 // Return a position on a radial from wp1 given distance d (nm) and magnetic heading h (degrees)
1521 // Note that d should be less that 1/4 Earth diameter!
1522 GPSWaypoint DCLGPS::GetPositionOnMagRadial(const GPSWaypoint& wp1, double d, double h) {
1523 h += sgGetMagVar(wp1.lon, wp1.lat, 0.0, _time->getJD()) * SG_RADIANS_TO_DEGREES;
1524 return(GetPositionOnRadial(wp1, d, h));
1527 // Return a position on a radial from wp1 given distance d (nm) and TRUE heading h (degrees)
1528 // Note that d should be less that 1/4 Earth diameter!
1529 GPSWaypoint DCLGPS::GetPositionOnRadial(const GPSWaypoint& wp1, double d, double h) {
1530 while(h < 0.0) h += 360.0;
1531 while(h > 360.0) h -= 360.0;
1533 h *= SG_DEGREES_TO_RADIANS;
1534 d *= (SG_PI / (180.0 * 60.0));
1536 double lat=asin(sin(wp1.lat)*cos(d)+cos(wp1.lat)*sin(d)*cos(h));
1539 lon=wp1.lon; // endpoint a pole
1541 lon=mod(wp1.lon+asin(sin(h)*sin(d)/cos(lat))+SG_PI,2*SG_PI)-SG_PI;
1547 wp.type = GPS_WP_VIRT;
1551 // Returns cross-track deviation in Nm.
1552 double DCLGPS::CalcCrossTrackDeviation() const {
1553 return(CalcCrossTrackDeviation(_fromWaypoint, _activeWaypoint));
1556 // Returns cross-track deviation of the current position between two arbitary waypoints in nm.
1557 double DCLGPS::CalcCrossTrackDeviation(const GPSWaypoint& wp1, const GPSWaypoint& wp2) const {
1558 //if(wp1 == NULL || wp2 == NULL) return(0.0);
1559 if(wp1.id.empty() || wp2.id.empty()) return(0.0);
1560 double xtd = asin(sin(Nm2Rad(GetGreatCircleDistance(wp1.lat, wp1.lon, _gpsLat, _gpsLon)))
1561 * sin(GetGreatCircleCourse(wp1.lat, wp1.lon, _gpsLat, _gpsLon) - GetGreatCircleCourse(wp1.lat, wp1.lon, wp2.lat, wp2.lon)));
1562 return(Rad2Nm(xtd));