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/airport.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;
210 _messageStack.clear();
214 _approachLoaded = false;
215 _approachArm = false;
216 _approachReallyArmed = false;
217 _approachActive = false;
218 _approachFP = new GPSFlightPlan;
222 delete _approachFP; // Don't need to delete the waypoints inside since they point to
223 // the waypoints in the approach database.
224 // TODO - may need to delete the approach database!!
227 void DCLGPS::draw(osg::State& state) {
228 _instrument->Draw(state);
231 void DCLGPS::init() {
233 // Not sure if this should be here, but OK for now.
234 CreateDefaultFlightPlans();
239 void DCLGPS::bind() {
240 _tiedProperties.setRoot(fgGetNode("/instrumentation/gps", true));
241 _tiedProperties.Tie("waypoint-alert", this, &DCLGPS::GetWaypointAlert);
242 _tiedProperties.Tie("leg-mode", this, &DCLGPS::GetLegMode);
243 _tiedProperties.Tie("obs-mode", this, &DCLGPS::GetOBSMode);
244 _tiedProperties.Tie("approach-arm", this, &DCLGPS::GetApproachArm);
245 _tiedProperties.Tie("approach-active", this, &DCLGPS::GetApproachActive);
246 _tiedProperties.Tie("cdi-deflection", this, &DCLGPS::GetCDIDeflection);
247 _tiedProperties.Tie("to-flag", this, &DCLGPS::GetToFlag);
250 void DCLGPS::unbind() {
251 _tiedProperties.Untie();
254 void DCLGPS::update(double dt) {
255 //cout << "update called!\n";
257 _lon = _lon_node->getDoubleValue() * SG_DEGREES_TO_RADIANS;
258 _lat = _lat_node->getDoubleValue() * SG_DEGREES_TO_RADIANS;
259 _alt = _alt_node->getDoubleValue();
260 _groundSpeed_kts = _grnd_speed_node->getDoubleValue();
261 _groundSpeed_ms = _groundSpeed_kts * 0.5144444444;
262 _track = _true_track_node->getDoubleValue();
263 _magTrackDeg = _mag_track_node->getDoubleValue();
264 // Note - we can depriciate _gpsLat and _gpsLon if we implement error handling in FG
265 // gps code and not our own.
268 // Check for abnormal position slew
269 if(GetGreatCircleDistance(_gpsLat, _gpsLon, _checkLat, _checkLon) > 1.0) {
270 OrientateToActiveFlightPlan();
275 // TODO - check for unit power before running this.
276 if(!_powerOnTimerSet) {
280 // Check if an alarm timer has expired
282 if(_alarmTime.hr() == atoi(fgGetString("/instrumentation/clock/indicated-hour"))
283 && _alarmTime.min() == atoi(fgGetString("/instrumentation/clock/indicated-min"))) {
284 _messageStack.push_back("*Timer Expired");
290 if(_groundSpeed_kts > 30.0) {
292 string th = fgGetString("/instrumentation/clock/indicated-hour");
293 string tm = fgGetString("/instrumentation/clock/indicated-min");
294 if(th.size() == 1) th = "0" + th;
295 if(tm.size() == 1) tm = "0" + tm;
296 _departureTimeString = th + tm;
299 // TODO - check - is this prone to drift error over time?
300 // Should we difference the departure and current times?
301 // What about when the user resets the time of day from the menu?
305 // FIXME - currently all the below assumes leg mode and no DTO or OBS cancelled.
306 if(_activeFP->IsEmpty()) {
307 // Not sure if we need to reset these each update or only when fp altered
308 _activeWaypoint.id.clear();
310 } else if(_activeFP->waypoints.size() == 1) {
311 _activeWaypoint.id.clear();
314 if(_activeWaypoint.id.empty() || _fromWaypoint.id.empty()) {
315 //cout << "Error, in leg mode with flightplan of 2 or more waypoints, but either active or from wp is NULL!\n";
316 OrientateToActiveFlightPlan();
320 if(_approachLoaded) {
321 if(!_approachReallyArmed && !_approachActive) {
322 // arm if within 30nm of airport.
323 // TODO - let user cancel approach arm using external GPS-APR switch
325 const FGAirport* ap = FindFirstAptById(_approachID, multi, true);
327 double d = GetGreatCircleDistance(_gpsLat, _gpsLon, ap->getLatitude() * SG_DEGREES_TO_RADIANS, ap->getLongitude() * SG_DEGREES_TO_RADIANS);
330 _approachReallyArmed = true;
331 _messageStack.push_back("*Press ALT To Set Baro");
332 // Not sure what we do if the user has already set CDI to 0.3 nm?
333 _targetCdiScaleIndex = 1;
334 if(_currentCdiScaleIndex == 1) {
336 } else if(_currentCdiScaleIndex == 0) {
337 _sourceCdiScaleIndex = 0;
338 _cdiScaleTransition = true;
339 _cdiTransitionTime = 30.0;
340 _currentCdiScale = _cdiScales[_currentCdiScaleIndex];
345 // Check for approach active - we can only activate approach if it is really armed.
346 if(_activeWaypoint.appType == GPS_FAF) {
347 //cout << "Active waypoint is FAF, id is " << _activeWaypoint.id << '\n';
348 if(GetGreatCircleDistance(_gpsLat, _gpsLon, _activeWaypoint.lat, _activeWaypoint.lon) <= 2.0 && !_obsMode) {
349 // Assume heading is OK for now
350 _approachArm = false; // TODO - check - maybe arm is left on when actv comes on?
351 _approachReallyArmed = false;
352 _approachActive = true;
353 _targetCdiScaleIndex = 2;
354 if(_currentCdiScaleIndex == 2) {
356 } else if(_currentCdiScaleIndex == 1) {
357 _sourceCdiScaleIndex = 1;
358 _cdiScaleTransition = true;
359 _cdiTransitionTime = 30.0; // TODO - compress it if time to FAF < 30sec
360 _currentCdiScale = _cdiScales[_currentCdiScaleIndex];
362 // Abort going active?
363 _approachActive = false;
370 // CDI scale transition stuff
371 if(_cdiScaleTransition) {
372 if(fabs(_currentCdiScale - _cdiScales[_targetCdiScaleIndex]) < 0.001) {
373 _currentCdiScale = _cdiScales[_targetCdiScaleIndex];
374 _currentCdiScaleIndex = _targetCdiScaleIndex;
375 _cdiScaleTransition = false;
377 double scaleDiff = (_targetCdiScaleIndex > _sourceCdiScaleIndex
378 ? _cdiScales[_sourceCdiScaleIndex] - _cdiScales[_targetCdiScaleIndex]
379 : _cdiScales[_targetCdiScaleIndex] - _cdiScales[_sourceCdiScaleIndex]);
380 //cout << "ScaleDiff = " << scaleDiff << '\n';
381 if(_targetCdiScaleIndex > _sourceCdiScaleIndex) {
382 // Scaling down eg. 5nm -> 1nm
383 _currentCdiScale -= (scaleDiff * dt / _cdiTransitionTime);
384 if(_currentCdiScale < _cdiScales[_targetCdiScaleIndex]) {
385 _currentCdiScale = _cdiScales[_targetCdiScaleIndex];
386 _currentCdiScaleIndex = _targetCdiScaleIndex;
387 _cdiScaleTransition = false;
390 _currentCdiScale += (scaleDiff * dt / _cdiTransitionTime);
391 if(_currentCdiScale > _cdiScales[_targetCdiScaleIndex]) {
392 _currentCdiScale = _cdiScales[_targetCdiScaleIndex];
393 _currentCdiScaleIndex = _targetCdiScaleIndex;
394 _cdiScaleTransition = false;
397 //cout << "_currentCdiScale = " << _currentCdiScale << '\n';
400 _currentCdiScale = _cdiScales[_currentCdiScaleIndex];
404 // Urgh - I've been setting the heading bug based on DTK,
405 // bug I think it should be based on heading re. active waypoint
406 // based on what the sim does after the final waypoint is passed.
407 // (DTK remains the same, but if track is held == DTK heading bug
408 // reverses to from once wp is passed).
410 if(_fromWaypoint != NULL) {
411 // TODO - how do we handle the change of track with distance over long legs?
412 _dtkTrue = GetGreatCircleCourse(_fromWaypoint->lat, _fromWaypoint->lon, _activeWaypoint->lat, _activeWaypoint->lon) * SG_RADIANS_TO_DEGREES;
413 _dtkMag = GetMagHeadingFromTo(_fromWaypoint->lat, _fromWaypoint->lon, _activeWaypoint->lat, _activeWaypoint->lon);
414 // Don't change the heading bug if speed is too low otherwise it flickers to/from at rest
415 if(_groundSpeed_ms > 5) {
416 //cout << "track = " << _track << ", dtk = " << _dtkTrue << '\n';
417 double courseDev = _track - _dtkTrue;
418 //cout << "courseDev = " << courseDev << ", normalized = ";
419 SG_NORMALIZE_RANGE(courseDev, -180.0, 180.0);
420 //cout << courseDev << '\n';
421 _headingBugTo = (fabs(courseDev) > 90.0 ? false : true);
426 // TODO - in DTO operation the position of initiation of DTO defines the "from waypoint".
429 if(!_activeWaypoint.id.empty()) {
430 double hdgTrue = GetGreatCircleCourse(_gpsLat, _gpsLon, _activeWaypoint.lat, _activeWaypoint.lon) * SG_RADIANS_TO_DEGREES;
431 if(_groundSpeed_ms > 5) {
432 //cout << "track = " << _track << ", hdgTrue = " << hdgTrue << '\n';
433 double courseDev = _track - hdgTrue;
434 //cout << "courseDev = " << courseDev << ", normalized = ";
435 SG_NORMALIZE_RANGE(courseDev, -180.0, 180.0);
436 //cout << courseDev << '\n';
437 _headingBugTo = (fabs(courseDev) > 90.0 ? false : true);
439 if(!_fromWaypoint.id.empty()) {
440 _dtkTrue = GetGreatCircleCourse(_fromWaypoint.lat, _fromWaypoint.lon, _activeWaypoint.lat, _activeWaypoint.lon) * SG_RADIANS_TO_DEGREES;
441 _dtkMag = GetMagHeadingFromTo(_fromWaypoint.lat, _fromWaypoint.lon, _activeWaypoint.lat, _activeWaypoint.lon);
448 _dist2Act = GetGreatCircleDistance(_gpsLat, _gpsLon, _activeWaypoint.lat, _activeWaypoint.lon) * SG_NM_TO_METER;
449 if(_groundSpeed_ms > 10.0) {
450 _eta = _dist2Act / _groundSpeed_ms;
451 if(_eta <= 36) { // TODO - this is slightly different if turn anticipation is enabled.
453 _waypointAlert = true; // TODO - not if the from flag is set.
457 // Check if we should sequence to next leg.
458 // Perhaps this should be done on distance instead, but 60s time (about 1 - 2 nm) seems reasonable for now.
459 //double reverseHeading = GetGreatCircleCourse(_activeWaypoint->lat, _activeWaypoint->lon, _fromWaypoint->lat, _fromWaypoint->lon);
460 // Hack - let's cheat and do it on heading bug for now. TODO - that stops us 'cutting the corner'
461 // when we happen to approach the inside turn of a waypoint - we should probably sequence at the midpoint
462 // of the heading difference between legs in this instance.
463 int idx = GetActiveWaypointIndex();
464 bool finalLeg = (idx == (int)(_activeFP->waypoints.size()) - 1 ? true : false);
465 bool finalDto = (_dto && idx == -1); // Dto operation to a waypoint not in the flightplan - we don't sequence in this instance
468 // Do nothing - not sure if Dto should switch off when arriving at the final waypoint of a flightplan
469 } else if(finalDto) {
471 } else if(_activeWaypoint.appType == GPS_MAP) {
472 // Don't sequence beyond the missed approach point
473 //cout << "ACTIVE WAYPOINT is MAP - not sequencing!!!!!\n";
475 //cout << "Sequencing...\n";
476 _fromWaypoint = _activeWaypoint;
477 _activeWaypoint = *_activeFP->waypoints[idx + 1];
479 // Course alteration message format is dependent on whether we are slaved HSI/CDI indicator or not.
481 if(fgGetBool("/instrumentation/nav[0]/slaved-to-gps")) {
482 // TODO - avoid the hardwiring on nav[0]
483 s = "Adj Nav Crs to ";
485 string s = "GPS Course is ";
487 double d = GetMagHeadingFromTo(_fromWaypoint.lat, _fromWaypoint.lon, _activeWaypoint.lat, _activeWaypoint.lon);
488 while(d < 0.0) d += 360.0;
489 while(d >= 360.0) d -= 360.0;
491 snprintf(buf, 4, "%03i", (int)(d + 0.5));
493 _messageStack.push_back(s);
495 _waypointAlert = false;
503 // First attempt at a sensible cross-track correction calculation
504 // Uh? - I think this is implemented further down the file!
505 if(_fromWaypoint != NULL) {
508 _crosstrackDist = 0.0;
515 Expand a SIAP ident to the full procedure name (as shown on the approach chart).
516 NOTE: Some of this is inferred from data, some is from documentation.
518 Example expansions from ARINC 424-18 [and the airport they're taken from]:
519 "R10LY" <--> "RNAV (GPS) Y RWY 10 L" [KBOI]
520 "R10-Z" <--> "RNAV (GPS) Z RWY 10" [KHTO]
521 "S25" <--> "VOR or GPS RWY 25" [KHHR]
522 "P20" <--> "GPS RWY 20" [KDAN]
523 "NDB-B" <--> "NDB or GPS-B" [KDAW]
524 "NDBC" <--> "NDB or GPS-C" [KEMT]
525 "VDMA" <--> "VOR/DME or GPS-A" [KDAW]
526 "VDM-A" <--> "VOR/DME or GPS-A" [KEAG]
527 "VDMB" <--> "VOR/DME or GPS-B" [KDKX]
528 "VORA" <--> "VOR or GPS-A" [KEMT]
530 It seems that there are 2 basic types of expansions; those that include
531 the runway and those that don't. Of those that don't, it seems that 2
532 different positions within the string to encode the identifying letter
533 are used, i.e. with a dash and without.
535 string DCLGPS::ExpandSIAPIdent(const string& ident) {
537 bool has_rwy = false;
540 case 'N': name = "NDB or GPS"; has_rwy = false; break;
541 case 'P': name = "GPS"; has_rwy = true; break;
542 case 'R': name = "RNAV (GPS)"; has_rwy = true; break;
543 case 'S': name = "VOR or GPS"; has_rwy = true; break;
545 if(ident[1] == 'D') name = "VOR/DME or GPS";
546 else name = "VOR or GPS";
549 default: // TODO output a log message
554 // Add the identifying letter if present
555 if(ident.size() == 5) {
562 name += ident.substr(1, 2);
564 // Add a left/right/centre indication if present.
565 if(ident.size() > 3) {
566 if((ident[3] != '-') && (ident[3] != ' ')) { // Early versions of the spec allowed a blank instead of a dash so check for both
572 // Add the identifying letter, which I *think* should always be present, but seems to be inconsistent as to whether a dash is used.
573 if(ident.size() == 5) {
576 } else if(ident.size() == 4) {
588 Load instrument approaches from an ARINC 424 file.
589 Tested on ARINC 424-18.
590 Known / current best guess at the format:
591 Col 1: Always 'S'. If it isn't, ditch it.
592 Col 2-4: "Customer area" code, eg "USA", "CAN". I think that CAN is used for Alaska.
593 Col 5: Section code. Used in conjunction with sub-section code. Definitions are with sub-section code.
595 Col 7-10: ICAO (or FAA) airport ident. Left justified if < 4 chars.
596 Col 11-12: Based on ICAO geographical region.
597 Col 13: Sub-section code. Used in conjunction with section code.
598 "HD/E/F" => Helicopter record.
599 "HS" => Helicopter minimum safe altitude.
600 "PA" => Airport record.
601 "PF" => Approach segment.
602 "PG" => Runway record.
603 "PP" => Path point record. ???
604 "PS" => MSA record (minimum safe altitude).
606 ------ The following is for "PF", approach segment -------
608 Col 14-19: SIAP ident for this approach (left justified). This is a standardised abbreviated approach name.
609 e.g. "R10LZ" expands to "RNAV (GPS) Z RWY 10 L". See the comment block for ExpandSIAPIdent for full details.
610 Col 20: Route type. This is tricky - I don't have full documentation and am having to guess a bit.
611 'A' => Arrival route? This seems to be used to encode arrival routes from the IAF to the approach proper.
612 Note that the final fix of the arrival route is duplicated in the approach proper.
613 'D' => VOR/DME or GPS
615 'P' => GPS (ARINC 424-18), GPS and RNAV (GPS) (ARINC 424-15 and before).
616 'R' => RNAV (GPS) (ARINC 424-18).
618 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.
620 Col 27-29: Sequence number - position within the route segment. Rule: 10-20-30 etc.
621 Col 30-34: Fix identifer. The ident of the waypoint.
622 Col 35-36: ICAO geographical region code. I think we can ignore this for now.
623 Col 37: Section code - ??? I don't know what this means
624 Col 38 Subsection code - ??? ditto - no idea!
625 Col 40: Waypoint type.
626 'A' => Airport as waypoint
627 'E' => Essential waypoint (e.g. change of heading at this waypoint, etc).
628 'G' => Runway or helipad as waypoint
629 'H' => Heliport as waypoint
630 'N' => NDB as waypoint
631 'P' => Phantom waypoint (not sure if this is used in rev 18?)
632 'V' => VOR as waypoint
633 Col 41: Waypoint type.
634 'B' => Flyover, approach transition, or final approach.
635 'E' => end of route segment (transition waypoint). (Actually "End of terminal procedure route type" in the docs).
636 'N' => ??? I've also seen 'N' in this column, but don't know what it indicates.
638 Col 43: Waypoint type. May also be blank when none of the below.
639 'A' => Initial approach fix (IAF)
640 'F' => Final approach fix
642 'I' => Final approach course fix
643 'M' => Missed approach point
644 '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!
645 ??? Possibly procedure turn?
646 'C' => ??? This is also present in the data, but missing from the docs. Is at airport 00R.
647 Col 107-111 MSA center fix. We can ignore this.
649 void DCLGPS::LoadApproachData() {
653 const GPSWaypoint* cwp;
656 SGPath path = globals->get_fg_root();
657 path.append("Navaids/rnav.dat");
658 fin.open(path.c_str(), ios::in);
660 //cout << "Unable to open input file " << path.c_str() << '\n';
663 //cout << "Opened " << path.c_str() << " for reading\n";
668 string apt_ident; // This gets set to the ICAO code of the current airport being processed.
669 string iap_ident; // The abbreviated name of the current approach being processed.
670 string wp_ident; // The ident of the waypoint of the current line
671 string last_apt_ident;
672 string last_iap_ident;
673 string last_wp_ident;
674 // There is no need to save the full name - it can be generated on the fly from the abbreviated name as and when needed.
675 bool apt_in_progress = false; // Set true whilst loading all the approaches for a given airport.
676 bool iap_in_progress = false; // Set true whilst loading a given approach.
677 bool iap_error = false; // Set true if there is an error loading a given approach.
678 bool route_in_progress = false; // Set true when we are loading a "route" segment of the approach.
679 int last_sequence_number = 0; // Position within the route, rule (rev 18): 10, 20, 30 etc.
681 char last_route_type = 0;
683 char waypoint_fix_type; // This is the waypoint type from col 43, i.e. the type of fix. May be blank.
688 unsigned int nLoaded = 0;
689 unsigned int nErrors = 0;
691 //for(i=0; i<64; ++i) {
693 fin.getline(tmp, 256);
694 //s = Fake_rnav_dat[i];
696 if(s.size() < 132) continue;
697 if(s[0] == 'S') { // Valid line
698 string country_code = s.substr(1, 3);
699 if(country_code == "USA") { // For now we'll stick to US procedures in case there are unknown gotchas with others
700 if(s[4] == 'P') { // Includes approaches.
701 if(s[12] == 'A') { // Airport record
702 apt_ident = s.substr(6, 4);
703 // Trim any whitespace from the ident. The ident is left justified,
704 // so any space will be at the end.
705 if(apt_ident[3] == ' ') apt_ident = apt_ident.substr(0, 3);
706 // I think that all idents are either 3 or 4 chars - could check this though!
707 if(!apt_in_progress) {
708 last_apt_ident = apt_ident;
711 if(last_apt_ident != apt_ident) {
712 if(iap_in_progress) {
714 //cout << "ERROR: Unable to load approach " << iap->_ident << " at " << iap->_aptIdent << '\n';
717 _np_iap[iap->_aptIdent].push_back(iap);
718 //cout << "** Loaded " << iap->_aptIdent << "\t" << iap->_ident << '\n';
721 iap_in_progress = false;
724 last_apt_ident = apt_ident;
727 } else if(s[12] == 'F') { // Approach segment
728 if(apt_in_progress) {
729 iap_ident = s.substr(13, 6);
730 // Trim any whitespace from the RH end.
732 if(iap_ident[5-j] == ' ') {
733 iap_ident = iap_ident.substr(0, 5-j);
735 // It's important to break here, since earlier versions of ARINC 424 allowed spaces in the ident.
739 if(iap_in_progress) {
740 if(iap_ident != last_iap_ident) {
741 // This is a new approach - store the last one and trigger
742 // starting afresh by setting the in progress flag to false.
744 //cout << "ERROR: Unable to load approach " << iap->_ident << " at " << iap->_aptIdent << '\n';
747 _np_iap[iap->_aptIdent].push_back(iap);
748 //cout << "Loaded " << iap->_aptIdent << "\t" << iap->_ident << '\n';
751 iap_in_progress = false;
754 if(!iap_in_progress) {
756 iap->_aptIdent = apt_ident;
757 iap->_ident = iap_ident;
758 iap->_name = ExpandSIAPIdent(iap_ident); // I suspect that it's probably better to just store idents, and to expand the names as needed.
759 // Note, we haven't set iap->_rwyStr yet.
760 last_iap_ident = iap_ident;
761 iap_in_progress = true;
767 sequence_number = atoi(s.substr(26,3).c_str());
768 wp_ident = s.substr(29, 5);
769 waypoint_fix_type = s[42];
770 // Trim any whitespace from the RH end
772 if(wp_ident[4-j] == ' ') {
773 wp_ident = wp_ident.substr(0, 4-j);
779 // Ignore lines with no waypoint ID for now - these are normally part of the
780 // missed approach procedure, and we don't use them in the KLN89.
781 if(!wp_ident.empty()) {
782 // Make a local copy of the waypoint for now, since we're not yet sure if we'll be using it
785 bool wp_error = false;
786 if(w.id.substr(0, 2) == "RW" && waypoint_fix_type == 'M') {
787 // Assume that this is a missed-approach point based on the runway number, which appears to be standard for most approaches.
788 // Note: Currently fgFindAirportID returns NULL on error, but getRunwayByIdent throws an exception.
789 const FGAirport* apt = fgFindAirportID(iap->_aptIdent);
793 rwystr = w.id.substr(2, 2);
794 // TODO - sanity check the rwystr at this point to ensure we have a double digit number
795 if(w.id.size() > 4) {
796 if(w.id[4] == 'L' || w.id[4] == 'C' || w.id[4] == 'R') {
800 FGRunway* rwy = apt->getRunwayByIdent(rwystr);
801 w.lat = rwy->begin().getLatitudeRad();
802 w.lon = rwy->begin().getLongitudeRad();
803 } catch(const sg_exception&) {
804 SG_LOG(SG_INSTR, SG_WARN, "Unable to find runway " << w.id.substr(2, 2) << " at airport " << iap->_aptIdent);
805 //cout << "Unable to find runway " << w.id.substr(2, 2) << " at airport " << iap->_aptIdent << " ( w.id = " << w.id << ", rwystr = " << rwystr << " )\n";
812 cwp = FindFirstByExactId(w.id);
819 switch(waypoint_fix_type) {
820 case 'A': w.appType = GPS_IAF; break;
821 case 'F': w.appType = GPS_FAF; break;
822 case 'H': w.appType = GPS_MAHP; break;
823 case 'I': w.appType = GPS_IAP; break;
824 case 'M': w.appType = GPS_MAP; break;
825 case ' ': w.appType = GPS_APP_NONE; break;
826 //default: cout << "Unknown waypoint_fix_type: \'" << waypoint_fix_type << "\' [" << apt_ident << ", " << iap_ident << "]\n";
830 //cout << "Unable to find waypoint " << w.id << " [" << apt_ident << ", " << iap_ident << "]\n";
835 if(route_in_progress) {
836 if(sequence_number > last_sequence_number) {
837 // TODO - add a check for runway numbers
838 // Check for the waypoint ID being the same as the previous line.
839 // This is often the case for the missed approach holding point.
840 if(wp_ident == last_wp_ident) {
841 if(waypoint_fix_type == 'H') {
842 if(!iap->_IAP.empty()) {
843 if(iap->_IAP[iap->_IAP.size() - 1]->appType == GPS_APP_NONE) {
844 iap->_IAP[iap->_IAP.size() - 1]->appType = GPS_MAHP;
846 //cout << "Waypoint is MAHP and another type! " << w.id << " [" << apt_ident << ", " << iap_ident << "]\n";
851 // Create a new waypoint on the heap, copy the local copy into it, and push it onto the approach.
852 wp = new GPSWaypoint;
854 if(route_type == 'A') {
855 fp->waypoints.push_back(wp);
857 iap->_IAP.push_back(wp);
860 } else if(sequence_number == last_sequence_number) {
861 // This seems to happen once per final approach route - one of the waypoints
862 // is duplicated with the same sequence number - I'm not sure what information
863 // the second line give yet so ignore it for now.
864 // TODO - figure this out!
866 // Finalise the current route and start a new one
868 // Finalise the current route
869 if(last_route_type == 'A') {
870 // Push the flightplan onto the approach
871 iap->_approachRoutes.push_back(fp);
873 // All the waypoints get pushed individually - don't need to do it.
876 // There are basically 2 possibilities here - either it's one of the arrival transitions,
877 // or it's the core final approach course.
878 wp = new GPSWaypoint;
880 if(route_type == 'A') { // It's one of the arrival transition(s)
881 fp = new GPSFlightPlan;
882 fp->waypoints.push_back(wp);
884 iap->_IAP.push_back(wp);
886 route_in_progress = true;
889 // Start a new route.
890 // There are basically 2 possibilities here - either it's one of the arrival transitions,
891 // or it's the core final approach course.
892 wp = new GPSWaypoint;
894 if(route_type == 'A') { // It's one of the arrival transition(s)
895 fp = new GPSFlightPlan;
896 fp->waypoints.push_back(wp);
898 iap->_IAP.push_back(wp);
900 route_in_progress = true;
902 last_route_type = route_type;
903 last_wp_ident = wp_ident;
904 last_sequence_number = sequence_number;
908 // ERROR - no airport record read.
912 // Check and finalise any approaches in progress
913 // TODO - sanity check that the approach has all the required elements
914 if(iap_in_progress) {
915 // This is a new approach - store the last one and trigger
916 // starting afresh by setting the in progress flag to false.
918 //cout << "ERROR: Unable to load approach " << iap->_ident << " at " << iap->_aptIdent << '\n';
921 _np_iap[iap->_aptIdent].push_back(iap);
922 //cout << "* Loaded " << iap->_aptIdent << "\t" << iap->_ident << '\n';
925 iap_in_progress = false;
932 // If we get to the end of the file, load any approach that is still in progress
933 // TODO - sanity check that the approach has all the required elements
934 if(iap_in_progress) {
936 //cout << "ERROR: Unable to load approach " << iap->_ident << " at " << iap->_aptIdent << '\n';
939 _np_iap[iap->_aptIdent].push_back(iap);
940 //cout << "*** Loaded " << iap->_aptIdent << "\t" << iap->_ident << '\n';
945 //cout << "Done loading approach database\n";
946 //cout << "Loaded: " << nLoaded << '\n';
947 //cout << "Failed: " << nErrors << '\n';
952 GPSWaypoint* DCLGPS::GetActiveWaypoint() {
953 return &_activeWaypoint;
957 float DCLGPS::GetDistToActiveWaypoint() {
961 // I don't yet fully understand all the gotchas about where to source time from.
962 // This function sets the initial timer before the clock exports properties
963 // and the one below uses the clock to be consistent with the rest of the code.
964 // It might change soonish...
965 void DCLGPS::SetPowerOnTimer() {
966 struct tm *t = globals->get_time_params()->getGmt();
967 _powerOnTime.set_hr(t->tm_hour);
968 _powerOnTime.set_min(t->tm_min);
969 _powerOnTimerSet = true;
972 void DCLGPS::ResetPowerOnTimer() {
973 _powerOnTime.set_hr(atoi(fgGetString("/instrumentation/clock/indicated-hour")));
974 _powerOnTime.set_min(atoi(fgGetString("/instrumentation/clock/indicated-min")));
975 _powerOnTimerSet = true;
978 double DCLGPS::GetCDIDeflection() const {
979 double xtd = CalcCrossTrackDeviation(); //nm
980 return((xtd / _currentCdiScale) * 5.0 * 2.5 * -1.0);
983 void DCLGPS::DtoInitiate(const string& s) {
984 const GPSWaypoint* wp = FindFirstByExactId(s);
986 // TODO - Currently we start DTO operation unconditionally, regardless of which mode we are in.
987 // In fact, the following rules apply:
988 // In LEG mode, start DTO as we currently do.
989 // In OBS mode, set the active waypoint to the requested waypoint, and then:
990 // If the KLN89 is not connected to an external HSI or CDI, set the OBS course to go direct to the waypoint.
991 // If the KLN89 *is* connected to an external HSI or CDI, it cannot set the course itself, and will display
992 // a scratchpad message with the course to set manually on the HSI/CDI.
993 // In both OBS cases, leave _dto false, since we don't need the virtual waypoint created.
995 _activeWaypoint = *wp;
996 _fromWaypoint.lat = _gpsLat;
997 _fromWaypoint.lon = _gpsLon;
998 _fromWaypoint.type = GPS_WP_VIRT;
999 _fromWaypoint.id = "_DTOWP_";
1002 // TODO - Should bring up the user waypoint page.
1007 void DCLGPS::DtoCancel() {
1009 // i.e. don't bother reorientating if we're just cancelling a DTO button press
1010 // without having previously initiated DTO.
1011 OrientateToActiveFlightPlan();
1016 void DCLGPS::ToggleOBSMode() {
1017 _obsMode = !_obsMode;
1019 // We need to set the OBS heading. The rules for this are:
1021 // If the KLN89 is connected to an external HSI or CDI, then the OBS heading must be set
1022 // to the OBS radial on the external indicator, and cannot be changed in the KLN89.
1024 // If the KLN89 is not connected to an external indicator, then:
1025 // If there is an active waypoint, the OBS heading is set such that the
1026 // deviation indicator remains at the same deviation (i.e. set to DTK,
1027 // although there may be some small difference).
1029 // If there is not an active waypoint, I am not sure what value should be
1032 if(fgGetBool("/instrumentation/nav/slaved-to-gps")) {
1033 _obsHeading = static_cast<int>(fgGetDouble("/instrumentation/nav/radials/selected-deg") + 0.5);
1034 } else if(!_activeWaypoint.id.empty()) {
1035 // NOTE: I am not sure if this is completely correct. There are sometimes small differences
1036 // between DTK and default OBS heading in the simulator (~ 1 or 2 degrees). It might also
1037 // be that OBS heading should be stored as float and only displayed as int, in order to maintain
1038 // the initial bar deviation?
1039 _obsHeading = static_cast<int>(_dtkMag + 0.5);
1040 //cout << "_dtkMag = " << _dtkMag << ", _dtkTrue = " << _dtkTrue << ", _obsHeading = " << _obsHeading << '\n';
1042 // TODO - what should we really do here?
1046 // Valid OBS heading values are 0 -> 359 degrees inclusive (from kln89 simulator).
1047 if(_obsHeading > 359) _obsHeading -= 360;
1048 if(_obsHeading < 0) _obsHeading += 360;
1050 // TODO - the _fromWaypoint location will change as the OBS heading changes.
1051 // Might need to store the OBS initiation position somewhere in case it is needed again.
1052 SetOBSFromWaypoint();
1056 // Set the _fromWaypoint position based on the active waypoint and OBS radial.
1057 void DCLGPS::SetOBSFromWaypoint() {
1058 if(!_obsMode) return;
1059 if(_activeWaypoint.id.empty()) return;
1061 // TODO - base the 180 deg correction on the to/from flag.
1062 _fromWaypoint = GetPositionOnMagRadial(_activeWaypoint, 10, _obsHeading + 180.0);
1063 _fromWaypoint.type = GPS_WP_VIRT;
1064 _fromWaypoint.id = "_OBSWP_";
1067 void DCLGPS::CDIFSDIncrease() {
1068 if(_currentCdiScaleIndex == 0) {
1069 _currentCdiScaleIndex = _cdiScales.size() - 1;
1071 _currentCdiScaleIndex--;
1075 void DCLGPS::CDIFSDDecrease() {
1076 _currentCdiScaleIndex++;
1077 if(_currentCdiScaleIndex == _cdiScales.size()) {
1078 _currentCdiScaleIndex = 0;
1082 void DCLGPS::DrawChar(char c, int field, int px, int py, bool bold) {
1085 void DCLGPS::DrawText(const string& s, int field, int px, int py, bool bold) {
1088 void DCLGPS::CreateDefaultFlightPlans() {}
1090 // Get the time to the active waypoint in seconds.
1091 // Returns -1 if groundspeed < 30 kts
1092 double DCLGPS::GetTimeToActiveWaypoint() {
1093 if(_groundSpeed_kts < 30.0) {
1100 // Get the time to the final waypoint in seconds.
1101 // Returns -1 if groundspeed < 30 kts
1102 double DCLGPS::GetETE() {
1103 if(_groundSpeed_kts < 30.0) {
1106 // TODO - handle OBS / DTO operation appropriately
1107 if(_activeFP->waypoints.empty()) {
1110 return(GetTimeToWaypoint(_activeFP->waypoints[_activeFP->waypoints.size() - 1]->id));
1115 // Get the time to a given waypoint (spec'd by ID) in seconds.
1116 // returns -1 if groundspeed is less than 30kts.
1117 // If the waypoint is an unreached part of the active flight plan the time will be via each leg.
1118 // otherwise it will be a direct-to time.
1119 double DCLGPS::GetTimeToWaypoint(const string& id) {
1120 if(_groundSpeed_kts < 30.0) {
1125 int n1 = GetActiveWaypointIndex();
1126 int n2 = GetWaypointIndex(id);
1129 for(unsigned int i=n1+1; i<_activeFP->waypoints.size(); ++i) {
1130 GPSWaypoint* wp1 = _activeFP->waypoints[i-1];
1131 GPSWaypoint* wp2 = _activeFP->waypoints[i];
1132 double distm = GetGreatCircleDistance(wp1->lat, wp1->lon, wp2->lat, wp2->lon) * SG_NM_TO_METER;
1133 eta += (distm / _groundSpeed_ms);
1136 } else if(id == _activeWaypoint.id) {
1139 const GPSWaypoint* wp = FindFirstByExactId(id);
1140 if(wp == NULL) return(-1.0);
1141 double distm = GetGreatCircleDistance(_gpsLat, _gpsLon, wp->lat, wp->lon);
1143 return(distm / _groundSpeed_ms);
1145 return(-1.0); // Hopefully we never get here!
1148 // Returns magnetic great-circle heading
1149 // TODO - document units.
1150 float DCLGPS::GetHeadingToActiveWaypoint() {
1151 if(_activeWaypoint.id.empty()) {
1154 double h = GetMagHeadingFromTo(_gpsLat, _gpsLon, _activeWaypoint.lat, _activeWaypoint.lon);
1155 while(h <= 0.0) h += 360.0;
1156 while(h > 360.0) h -= 360.0;
1161 // Returns magnetic great-circle heading
1162 // TODO - what units?
1163 float DCLGPS::GetHeadingFromActiveWaypoint() {
1164 if(_activeWaypoint.id.empty()) {
1167 double h = GetMagHeadingFromTo(_activeWaypoint.lat, _activeWaypoint.lon, _gpsLat, _gpsLon);
1168 while(h <= 0.0) h += 360.0;
1169 while(h > 360.0) h -= 360.0;
1174 void DCLGPS::ClearFlightPlan(int n) {
1175 for(unsigned int i=0; i<_flightPlans[n]->waypoints.size(); ++i) {
1176 delete _flightPlans[n]->waypoints[i];
1178 _flightPlans[n]->waypoints.clear();
1181 void DCLGPS::ClearFlightPlan(GPSFlightPlan* fp) {
1182 for(unsigned int i=0; i<fp->waypoints.size(); ++i) {
1183 delete fp->waypoints[i];
1185 fp->waypoints.clear();
1188 int DCLGPS::GetActiveWaypointIndex() {
1189 for(unsigned int i=0; i<_flightPlans[0]->waypoints.size(); ++i) {
1190 if(_flightPlans[0]->waypoints[i]->id == _activeWaypoint.id) return((int)i);
1195 int DCLGPS::GetWaypointIndex(const string& id) {
1196 for(unsigned int i=0; i<_flightPlans[0]->waypoints.size(); ++i) {
1197 if(_flightPlans[0]->waypoints[i]->id == id) return((int)i);
1202 void DCLGPS::OrientateToFlightPlan(GPSFlightPlan* fp) {
1203 //cout << "Orientating...\n";
1204 //cout << "_lat = " << _lat << ", _lon = " << _lon << ", _gpsLat = " << _gpsLat << ", gpsLon = " << _gpsLon << '\n';
1206 _activeWaypoint.id.clear();
1209 _navFlagged = false;
1210 if(fp->waypoints.size() == 1) {
1211 // TODO - may need to flag nav here if not dto or obs, or possibly handle it somewhere else.
1212 _activeWaypoint = *fp->waypoints[0];
1213 _fromWaypoint.id.clear();
1215 // FIXME FIXME FIXME
1216 _fromWaypoint = *fp->waypoints[0];
1217 _activeWaypoint = *fp->waypoints[1];
1218 double dmin = 1000000; // nm!!
1219 // For now we will simply start on the leg closest to our current position.
1220 // It's possible that more fancy algorithms may take either heading or track
1221 // into account when setting inital leg - I'm not sure.
1222 // This method should handle most cases perfectly OK though.
1223 for(unsigned int i = 1; i < fp->waypoints.size(); ++i) {
1224 //cout << "Pass " << i << ", dmin = " << dmin << ", leg is " << fp->waypoints[i-1]->id << " to " << fp->waypoints[i]->id << '\n';
1225 // First get the cross track correction.
1226 double d0 = fabs(CalcCrossTrackDeviation(*fp->waypoints[i-1], *fp->waypoints[i]));
1227 // That is the shortest distance away we could be though - check for
1228 // longer distances if we are 'off the end' of the leg.
1229 double ht1 = GetGreatCircleCourse(fp->waypoints[i-1]->lat, fp->waypoints[i-1]->lon,
1230 fp->waypoints[i]->lat, fp->waypoints[i]->lon)
1231 * SG_RADIANS_TO_DEGREES;
1232 // not simply the reverse of the above due to great circle navigation.
1233 double ht2 = GetGreatCircleCourse(fp->waypoints[i]->lat, fp->waypoints[i]->lon,
1234 fp->waypoints[i-1]->lat, fp->waypoints[i-1]->lon)
1235 * SG_RADIANS_TO_DEGREES;
1236 double hw1 = GetGreatCircleCourse(_gpsLat, _gpsLon,
1237 fp->waypoints[i]->lat, fp->waypoints[i]->lon)
1238 * SG_RADIANS_TO_DEGREES;
1239 double hw2 = GetGreatCircleCourse(_gpsLat, _gpsLon,
1240 fp->waypoints[i-1]->lat, fp->waypoints[i-1]->lon)
1241 * SG_RADIANS_TO_DEGREES;
1242 double h1 = ht1 - hw1;
1243 double h2 = ht2 - hw2;
1244 //cout << "d0, h1, h2 = " << d0 << ", " << h1 << ", " << h2 << '\n';
1245 //cout << "Normalizing...\n";
1246 SG_NORMALIZE_RANGE(h1, -180.0, 180.0);
1247 SG_NORMALIZE_RANGE(h2, -180.0, 180.0);
1248 //cout << "d0, h1, h2 = " << d0 << ", " << h1 << ", " << h2 << '\n';
1249 if(fabs(h1) > 90.0) {
1250 // We are past the end of the to waypoint
1251 double d = GetGreatCircleDistance(_gpsLat, _gpsLon, fp->waypoints[i]->lat, fp->waypoints[i]->lon);
1253 //cout << "h1 triggered, d0 now = " << d0 << '\n';
1254 } else if(fabs(h2) > 90.0) {
1255 // We are past the end (not yet at!) the from waypoint
1256 double d = GetGreatCircleDistance(_gpsLat, _gpsLon, fp->waypoints[i-1]->lat, fp->waypoints[i-1]->lon);
1258 //cout << "h2 triggered, d0 now = " << d0 << '\n';
1261 //cout << "THIS LEG NOW ACTIVE!\n";
1263 _fromWaypoint = *fp->waypoints[i-1];
1264 _activeWaypoint = *fp->waypoints[i];
1271 void DCLGPS::OrientateToActiveFlightPlan() {
1272 OrientateToFlightPlan(_activeFP);
1275 /***************************************/
1277 // Utility function - create a flightplan from a list of waypoint ids and types
1278 void DCLGPS::CreateFlightPlan(GPSFlightPlan* fp, vector<string> ids, vector<GPSWpType> wps) {
1279 if(fp == NULL) fp = new GPSFlightPlan;
1281 if(!fp->waypoints.empty()) {
1282 for(i=0; i<fp->waypoints.size(); ++i) {
1283 delete fp->waypoints[i];
1285 fp->waypoints.clear();
1287 if(ids.size() != wps.size()) {
1288 cout << "ID and Waypoint types list size mismatch in GPS::CreateFlightPlan - no flightplan created!\n";
1291 for(i=0; i<ids.size(); ++i) {
1293 const FGAirport* ap;
1295 GPSWaypoint* wp = new GPSWaypoint;
1299 ap = FindFirstAptById(ids[i], multi, true);
1304 wp->lat = ap->getLatitude() * SG_DEGREES_TO_RADIANS;
1305 wp->lon = ap->getLongitude() * SG_DEGREES_TO_RADIANS;
1307 fp->waypoints.push_back(wp);
1311 np = FindFirstVorById(ids[i], multi, true);
1316 wp->lat = np->get_lat() * SG_DEGREES_TO_RADIANS;
1317 wp->lon = np->get_lon() * SG_DEGREES_TO_RADIANS;
1319 fp->waypoints.push_back(wp);
1323 np = FindFirstNDBById(ids[i], multi, true);
1328 wp->lat = np->get_lat() * SG_DEGREES_TO_RADIANS;
1329 wp->lon = np->get_lon() * SG_DEGREES_TO_RADIANS;
1331 fp->waypoints.push_back(wp);
1347 /***************************************/
1349 class DCLGPSFilter : public FGPositioned::Filter
1352 virtual bool pass(FGPositioned* aPos) const
1354 switch (aPos->type()) {
1355 case FGPositioned::AIRPORT:
1356 // how about heliports and seaports?
1357 case FGPositioned::NDB:
1358 case FGPositioned::VOR:
1359 case FGPositioned::WAYPOINT:
1360 case FGPositioned::FIX:
1362 default: return false; // reject all other types
1368 GPSWaypoint* DCLGPS::FindFirstById(const string& id) const
1370 DCLGPSFilter filter;
1371 FGPositionedList matches = FGPositioned::findAllWithIdent(id, &filter, false);
1372 if (matches.empty()) {
1376 FGPositioned::sortByRange(matches, SGGeod::fromRad(_lon, _lat));
1377 return GPSWaypoint::createFromPositioned(matches.front());
1380 GPSWaypoint* DCLGPS::FindFirstByExactId(const string& id) const
1382 SGGeod pos(SGGeod::fromRad(_lon, _lat));
1383 FGPositionedRef result = FGPositioned::findClosestWithIdent(id, pos);
1384 return GPSWaypoint::createFromPositioned(result);
1387 // TODO - add the ASCII / alphabetical stuff from the Atlas version
1388 FGPositioned* DCLGPS::FindTypedFirstById(const string& id, FGPositioned::Type ty, bool &multi, bool exact)
1391 FGPositioned::TypeFilter filter(ty);
1393 FGPositionedList matches =
1394 FGPositioned::findAllWithIdent(id, &filter, exact);
1395 if (matches.empty()) {
1399 FGPositioned::sortByRange(matches, SGGeod::fromRad(_lon, _lat));
1400 return matches.front();
1403 FGNavRecord* DCLGPS::FindFirstVorById(const string& id, bool &multi, bool exact)
1405 return dynamic_cast<FGNavRecord*>(FindTypedFirstById(id, FGPositioned::VOR, multi, exact));
1408 FGNavRecord* DCLGPS::FindFirstNDBById(const string& id, bool &multi, bool exact)
1410 return dynamic_cast<FGNavRecord*>(FindTypedFirstById(id, FGPositioned::NDB, multi, exact));
1413 const FGFix* DCLGPS::FindFirstIntById(const string& id, bool &multi, bool exact)
1415 return dynamic_cast<FGFix*>(FindTypedFirstById(id, FGPositioned::FIX, multi, exact));
1418 const FGAirport* DCLGPS::FindFirstAptById(const string& id, bool &multi, bool exact)
1420 return dynamic_cast<FGAirport*>(FindTypedFirstById(id, FGPositioned::AIRPORT, multi, exact));
1423 FGNavRecord* DCLGPS::FindClosestVor(double lat_rad, double lon_rad) {
1424 FGPositioned::TypeFilter filter(FGPositioned::VOR);
1425 double cutoff = 1000; // nautical miles
1426 FGPositionedRef v = FGPositioned::findClosest(SGGeod::fromRad(lon_rad, lat_rad), cutoff, &filter);
1431 return dynamic_cast<FGNavRecord*>(v.ptr());
1434 //----------------------------------------------------------------------------------------------------------
1436 // Takes lat and lon in RADIANS!!!!!!!
1437 double DCLGPS::GetMagHeadingFromTo(double latA, double lonA, double latB, double lonB) {
1438 double h = GetGreatCircleCourse(latA, lonA, latB, lonB);
1439 h *= SG_RADIANS_TO_DEGREES;
1440 // TODO - use the real altitude below instead of 0.0!
1441 //cout << "MagVar = " << sgGetMagVar(_gpsLon, _gpsLat, 0.0, _time->getJD()) * SG_RADIANS_TO_DEGREES << '\n';
1442 double jd = globals->get_time_params()->getJD();
1443 h -= sgGetMagVar(_gpsLon, _gpsLat, 0.0, jd) * SG_RADIANS_TO_DEGREES;
1444 while(h >= 360.0) h -= 360.0;
1445 while(h < 0.0) h += 360.0;
1449 // ---------------- Great Circle formulae from "The Aviation Formulary" -------------
1450 // Note that all of these assume that the world is spherical.
1452 double Rad2Nm(double theta) {
1453 return(((180.0*60.0)/SG_PI)*theta);
1456 double Nm2Rad(double d) {
1457 return((SG_PI/(180.0*60.0))*d);
1462 The great circle distance d between two points with coordinates {lat1,lon1} and {lat2,lon2} is given by:
1464 d=acos(sin(lat1)*sin(lat2)+cos(lat1)*cos(lat2)*cos(lon1-lon2))
1466 A mathematically equivalent formula, which is less subject to rounding error for short distances is:
1468 d=2*asin(sqrt((sin((lat1-lat2)/2))^2 +
1469 cos(lat1)*cos(lat2)*(sin((lon1-lon2)/2))^2))
1473 // Returns distance in nm, takes lat & lon in RADIANS
1474 double DCLGPS::GetGreatCircleDistance(double lat1, double lon1, double lat2, double lon2) const {
1475 double d = 2.0 * asin(sqrt(((sin((lat1-lat2)/2.0))*(sin((lat1-lat2)/2.0))) +
1476 cos(lat1)*cos(lat2)*(sin((lon1-lon2)/2.0))*(sin((lon1-lon2)/2.0))));
1480 // fmod dosen't do what we want :-(
1481 static double mod(double d1, double d2) {
1482 return(d1 - d2*floor(d1/d2));
1485 // Returns great circle course from point 1 to point 2
1486 // Input and output in RADIANS.
1487 double DCLGPS::GetGreatCircleCourse (double lat1, double lon1, double lat2, double lon2) const {
1490 // Special case the poles
1491 if(cos(lat1) < SG_EPSILON) {
1493 // Starting from North Pole
1496 // Starting from South Pole
1500 // Urgh - the formula below is for negative lon +ve !!!???
1501 double d = GetGreatCircleDistance(lat1, lon1, lat2, lon2);
1502 cout << "d = " << d;
1504 //cout << ", d_theta = " << d;
1505 //cout << ", and d = " << Rad2Nm(d) << ' ';
1506 if(sin(lon2 - lon1) < 0) {
1508 h = acos((sin(lat2)-sin(lat1)*cos(d))/(sin(d)*cos(lat1)));
1511 h = 2.0 * SG_PI - acos((sin(lat2)-sin(lat1)*cos(d))/(sin(d)*cos(lat1)));
1514 cout << h * SG_RADIANS_TO_DEGREES << '\n';
1517 return( mod(atan2(sin(lon2-lon1)*cos(lat2),
1518 cos(lat1)*sin(lat2)-sin(lat1)*cos(lat2)*cos(lon2-lon1)),
1522 // Return a position on a radial from wp1 given distance d (nm) and magnetic heading h (degrees)
1523 // Note that d should be less that 1/4 Earth diameter!
1524 GPSWaypoint DCLGPS::GetPositionOnMagRadial(const GPSWaypoint& wp1, double d, double h) {
1525 double jd = globals->get_time_params()->getJD();
1526 h += sgGetMagVar(wp1.lon, wp1.lat, 0.0, jd) * SG_RADIANS_TO_DEGREES;
1527 return(GetPositionOnRadial(wp1, d, h));
1530 // Return a position on a radial from wp1 given distance d (nm) and TRUE heading h (degrees)
1531 // Note that d should be less that 1/4 Earth diameter!
1532 GPSWaypoint DCLGPS::GetPositionOnRadial(const GPSWaypoint& wp1, double d, double h) {
1533 while(h < 0.0) h += 360.0;
1534 while(h > 360.0) h -= 360.0;
1536 h *= SG_DEGREES_TO_RADIANS;
1537 d *= (SG_PI / (180.0 * 60.0));
1539 double lat=asin(sin(wp1.lat)*cos(d)+cos(wp1.lat)*sin(d)*cos(h));
1542 lon=wp1.lon; // endpoint a pole
1544 lon=mod(wp1.lon+asin(sin(h)*sin(d)/cos(lat))+SG_PI,2*SG_PI)-SG_PI;
1550 wp.type = GPS_WP_VIRT;
1554 // Returns cross-track deviation in Nm.
1555 double DCLGPS::CalcCrossTrackDeviation() const {
1556 return(CalcCrossTrackDeviation(_fromWaypoint, _activeWaypoint));
1559 // Returns cross-track deviation of the current position between two arbitary waypoints in nm.
1560 double DCLGPS::CalcCrossTrackDeviation(const GPSWaypoint& wp1, const GPSWaypoint& wp2) const {
1561 //if(wp1 == NULL || wp2 == NULL) return(0.0);
1562 if(wp1.id.empty() || wp2.id.empty()) return(0.0);
1563 double xtd = asin(sin(Nm2Rad(GetGreatCircleDistance(wp1.lat, wp1.lon, _gpsLat, _gpsLon)))
1564 * sin(GetGreatCircleCourse(wp1.lat, wp1.lon, _gpsLat, _gpsLon) - GetGreatCircleCourse(wp1.lat, wp1.lon, wp2.lat, wp2.lon)));
1565 return(Rad2Nm(xtd));