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>
45 GPSWaypoint::GPSWaypoint() {
46 appType = GPS_APP_NONE;
49 GPSWaypoint::GPSWaypoint(const std::string& aIdent, float aLat, float aLon, GPSWpType aType) :
58 GPSWaypoint::~GPSWaypoint() {}
60 string GPSWaypoint::GetAprId() {
61 if(appType == GPS_IAF) return(id + 'i');
62 else if(appType == GPS_FAF) return(id + 'f');
63 else if(appType == GPS_MAP) return(id + 'm');
64 else if(appType == GPS_MAHP) return(id + 'h');
69 GPSWpTypeFromFGPosType(FGPositioned::Type aType)
72 case FGPositioned::AIRPORT:
73 case FGPositioned::SEAPORT:
74 case FGPositioned::HELIPORT:
77 case FGPositioned::VOR:
80 case FGPositioned::NDB:
83 case FGPositioned::WAYPOINT:
86 case FGPositioned::FIX:
94 GPSWaypoint* GPSWaypoint::createFromPositioned(const FGPositioned* aPos)
97 return NULL; // happens if find returns no match
100 return new GPSWaypoint(aPos->ident(),
101 aPos->latitude() * SG_DEGREES_TO_RADIANS,
102 aPos->longitude() * SG_DEGREES_TO_RADIANS,
103 GPSWpTypeFromFGPosType(aPos->type())
107 ostream& operator << (ostream& os, GPSAppWpType type) {
109 case(GPS_IAF): return(os << "IAF");
110 case(GPS_IAP): return(os << "IAP");
111 case(GPS_FAF): return(os << "FAF");
112 case(GPS_MAP): return(os << "MAP");
113 case(GPS_MAHP): return(os << "MAHP");
114 case(GPS_HDR): return(os << "HEADER");
115 case(GPS_FENCE): return(os << "FENCE");
116 case(GPS_APP_NONE): return(os << "NONE");
118 return(os << "ERROR - Unknown switch in GPSAppWpType operator << ");
130 FGNPIAP::~FGNPIAP() {
133 ClockTime::ClockTime() {
138 ClockTime::ClockTime(int hr, int min) {
139 while(hr < 0) { hr += 24; }
141 while(min < 0) { min += 60; }
142 while(min > 60) { min -= 60; }
146 ClockTime::~ClockTime() {
149 // ------------------------------------------------------------------------------------- //
151 DCLGPS::DCLGPS(RenderArea2D* instrument) {
152 _instrument = instrument;
156 _lon_node = fgGetNode("/instrumentation/gps/indicated-longitude-deg", true);
157 _lat_node = fgGetNode("/instrumentation/gps/indicated-latitude-deg", true);
158 _alt_node = fgGetNode("/instrumentation/gps/indicated-altitude-ft", true);
159 _grnd_speed_node = fgGetNode("/instrumentation/gps/indicated-ground-speed-kt", true);
160 _true_track_node = fgGetNode("/instrumentation/gps/indicated-track-true-deg", true);
161 _mag_track_node = fgGetNode("/instrumentation/gps/indicated-track-magnetic-deg", true);
163 // Use FG's position values at construction in case FG's gps has not run first update yet.
164 _lon = fgGetDouble("/position/longitude-deg") * SG_DEGREES_TO_RADIANS;
165 _lat = fgGetDouble("/position/latitude-deg") * SG_DEGREES_TO_RADIANS;
166 _alt = fgGetDouble("/position/altitude-ft");
167 // Note - we can depriciate _gpsLat and _gpsLon if we implement error handling in FG
168 // gps code and not our own.
173 _groundSpeed_ms = 0.0;
174 _groundSpeed_kts = 0.0;
178 // Sensible defaults. These can be overriden by derived classes if desired.
180 _cdiScales.push_back(5.0);
181 _cdiScales.push_back(1.0);
182 _cdiScales.push_back(0.3);
183 _currentCdiScaleIndex = 0;
184 _targetCdiScaleIndex = 0;
185 _sourceCdiScaleIndex = 0;
186 _cdiScaleTransition = false;
187 _currentCdiScale = 5.0;
191 _activeWaypoint.id.clear();
193 _crosstrackDist = 0.0;
194 _headingBugTo = true;
196 _waypointAlert = false;
198 _departureTimeString = "----";
200 _powerOnTime.set_hr(0);
201 _powerOnTime.set_min(0);
202 _powerOnTimerSet = false;
205 // Configuration Initialisation
206 // Should this be in kln89.cxx ?
207 _turnAnticipationEnabled = false;
211 _messageStack.clear();
215 _approachLoaded = false;
216 _approachArm = false;
217 _approachReallyArmed = false;
218 _approachActive = false;
219 _approachFP = new GPSFlightPlan;
224 delete _approachFP; // Don't need to delete the waypoints inside since they point to
225 // the waypoints in the approach database.
226 // TODO - may need to delete the approach database!!
229 void DCLGPS::draw(osg::State& state) {
230 _instrument->Draw(state);
233 void DCLGPS::init() {
235 // Not sure if this should be here, but OK for now.
236 CreateDefaultFlightPlans();
241 void DCLGPS::bind() {
242 _tiedProperties.setRoot(fgGetNode("/instrumentation/gps", true));
243 _tiedProperties.Tie("waypoint-alert", this, &DCLGPS::GetWaypointAlert);
244 _tiedProperties.Tie("leg-mode", this, &DCLGPS::GetLegMode);
245 _tiedProperties.Tie("obs-mode", this, &DCLGPS::GetOBSMode);
246 _tiedProperties.Tie("approach-arm", this, &DCLGPS::GetApproachArm);
247 _tiedProperties.Tie("approach-active", this, &DCLGPS::GetApproachActive);
248 _tiedProperties.Tie("cdi-deflection", this, &DCLGPS::GetCDIDeflection);
249 _tiedProperties.Tie("to-flag", this, &DCLGPS::GetToFlag);
252 void DCLGPS::unbind() {
253 _tiedProperties.Untie();
256 void DCLGPS::update(double dt) {
257 //cout << "update called!\n";
259 _lon = _lon_node->getDoubleValue() * SG_DEGREES_TO_RADIANS;
260 _lat = _lat_node->getDoubleValue() * SG_DEGREES_TO_RADIANS;
261 _alt = _alt_node->getDoubleValue();
262 _groundSpeed_kts = _grnd_speed_node->getDoubleValue();
263 _groundSpeed_ms = _groundSpeed_kts * 0.5144444444;
264 _track = _true_track_node->getDoubleValue();
265 _magTrackDeg = _mag_track_node->getDoubleValue();
266 // Note - we can depriciate _gpsLat and _gpsLon if we implement error handling in FG
267 // gps code and not our own.
270 // Check for abnormal position slew
271 if(GetGreatCircleDistance(_gpsLat, _gpsLon, _checkLat, _checkLon) > 1.0) {
272 OrientateToActiveFlightPlan();
277 // TODO - check for unit power before running this.
278 if(!_powerOnTimerSet) {
282 // Check if an alarm timer has expired
284 if(_alarmTime.hr() == atoi(fgGetString("/instrumentation/clock/indicated-hour"))
285 && _alarmTime.min() == atoi(fgGetString("/instrumentation/clock/indicated-min"))) {
286 _messageStack.push_back("*Timer Expired");
292 if(_groundSpeed_kts > 30.0) {
294 string th = fgGetString("/instrumentation/clock/indicated-hour");
295 string tm = fgGetString("/instrumentation/clock/indicated-min");
296 if(th.size() == 1) th = "0" + th;
297 if(tm.size() == 1) tm = "0" + tm;
298 _departureTimeString = th + tm;
301 // TODO - check - is this prone to drift error over time?
302 // Should we difference the departure and current times?
303 // What about when the user resets the time of day from the menu?
307 _time->update(_gpsLon * SG_DEGREES_TO_RADIANS, _gpsLat * SG_DEGREES_TO_RADIANS, 0, 0);
308 // FIXME - currently all the below assumes leg mode and no DTO or OBS cancelled.
309 if(_activeFP->IsEmpty()) {
310 // Not sure if we need to reset these each update or only when fp altered
311 _activeWaypoint.id.clear();
313 } else if(_activeFP->waypoints.size() == 1) {
314 _activeWaypoint.id.clear();
317 if(_activeWaypoint.id.empty() || _fromWaypoint.id.empty()) {
318 //cout << "Error, in leg mode with flightplan of 2 or more waypoints, but either active or from wp is NULL!\n";
319 OrientateToActiveFlightPlan();
323 if(_approachLoaded) {
324 if(!_approachReallyArmed && !_approachActive) {
325 // arm if within 30nm of airport.
326 // TODO - let user cancel approach arm using external GPS-APR switch
328 const FGAirport* ap = FindFirstAptById(_approachID, multi, true);
330 double d = GetGreatCircleDistance(_gpsLat, _gpsLon, ap->getLatitude() * SG_DEGREES_TO_RADIANS, ap->getLongitude() * SG_DEGREES_TO_RADIANS);
333 _approachReallyArmed = true;
334 _messageStack.push_back("*Press ALT To Set Baro");
335 // Not sure what we do if the user has already set CDI to 0.3 nm?
336 _targetCdiScaleIndex = 1;
337 if(_currentCdiScaleIndex == 1) {
339 } else if(_currentCdiScaleIndex == 0) {
340 _sourceCdiScaleIndex = 0;
341 _cdiScaleTransition = true;
342 _cdiTransitionTime = 30.0;
343 _currentCdiScale = _cdiScales[_currentCdiScaleIndex];
348 // Check for approach active - we can only activate approach if it is really armed.
349 if(_activeWaypoint.appType == GPS_FAF) {
350 //cout << "Active waypoint is FAF, id is " << _activeWaypoint.id << '\n';
351 if(GetGreatCircleDistance(_gpsLat, _gpsLon, _activeWaypoint.lat, _activeWaypoint.lon) <= 2.0 && !_obsMode) {
352 // Assume heading is OK for now
353 _approachArm = false; // TODO - check - maybe arm is left on when actv comes on?
354 _approachReallyArmed = false;
355 _approachActive = true;
356 _targetCdiScaleIndex = 2;
357 if(_currentCdiScaleIndex == 2) {
359 } else if(_currentCdiScaleIndex == 1) {
360 _sourceCdiScaleIndex = 1;
361 _cdiScaleTransition = true;
362 _cdiTransitionTime = 30.0; // TODO - compress it if time to FAF < 30sec
363 _currentCdiScale = _cdiScales[_currentCdiScaleIndex];
365 // Abort going active?
366 _approachActive = false;
373 // CDI scale transition stuff
374 if(_cdiScaleTransition) {
375 if(fabs(_currentCdiScale - _cdiScales[_targetCdiScaleIndex]) < 0.001) {
376 _currentCdiScale = _cdiScales[_targetCdiScaleIndex];
377 _currentCdiScaleIndex = _targetCdiScaleIndex;
378 _cdiScaleTransition = false;
380 double scaleDiff = (_targetCdiScaleIndex > _sourceCdiScaleIndex
381 ? _cdiScales[_sourceCdiScaleIndex] - _cdiScales[_targetCdiScaleIndex]
382 : _cdiScales[_targetCdiScaleIndex] - _cdiScales[_sourceCdiScaleIndex]);
383 //cout << "ScaleDiff = " << scaleDiff << '\n';
384 if(_targetCdiScaleIndex > _sourceCdiScaleIndex) {
385 // Scaling down eg. 5nm -> 1nm
386 _currentCdiScale -= (scaleDiff * dt / _cdiTransitionTime);
387 if(_currentCdiScale < _cdiScales[_targetCdiScaleIndex]) {
388 _currentCdiScale = _cdiScales[_targetCdiScaleIndex];
389 _currentCdiScaleIndex = _targetCdiScaleIndex;
390 _cdiScaleTransition = false;
393 _currentCdiScale += (scaleDiff * dt / _cdiTransitionTime);
394 if(_currentCdiScale > _cdiScales[_targetCdiScaleIndex]) {
395 _currentCdiScale = _cdiScales[_targetCdiScaleIndex];
396 _currentCdiScaleIndex = _targetCdiScaleIndex;
397 _cdiScaleTransition = false;
400 //cout << "_currentCdiScale = " << _currentCdiScale << '\n';
403 _currentCdiScale = _cdiScales[_currentCdiScaleIndex];
407 // Urgh - I've been setting the heading bug based on DTK,
408 // bug I think it should be based on heading re. active waypoint
409 // based on what the sim does after the final waypoint is passed.
410 // (DTK remains the same, but if track is held == DTK heading bug
411 // reverses to from once wp is passed).
413 if(_fromWaypoint != NULL) {
414 // TODO - how do we handle the change of track with distance over long legs?
415 _dtkTrue = GetGreatCircleCourse(_fromWaypoint->lat, _fromWaypoint->lon, _activeWaypoint->lat, _activeWaypoint->lon) * SG_RADIANS_TO_DEGREES;
416 _dtkMag = GetMagHeadingFromTo(_fromWaypoint->lat, _fromWaypoint->lon, _activeWaypoint->lat, _activeWaypoint->lon);
417 // Don't change the heading bug if speed is too low otherwise it flickers to/from at rest
418 if(_groundSpeed_ms > 5) {
419 //cout << "track = " << _track << ", dtk = " << _dtkTrue << '\n';
420 double courseDev = _track - _dtkTrue;
421 //cout << "courseDev = " << courseDev << ", normalized = ";
422 SG_NORMALIZE_RANGE(courseDev, -180.0, 180.0);
423 //cout << courseDev << '\n';
424 _headingBugTo = (fabs(courseDev) > 90.0 ? false : true);
429 // TODO - in DTO operation the position of initiation of DTO defines the "from waypoint".
432 if(!_activeWaypoint.id.empty()) {
433 double hdgTrue = GetGreatCircleCourse(_gpsLat, _gpsLon, _activeWaypoint.lat, _activeWaypoint.lon) * SG_RADIANS_TO_DEGREES;
434 if(_groundSpeed_ms > 5) {
435 //cout << "track = " << _track << ", hdgTrue = " << hdgTrue << '\n';
436 double courseDev = _track - hdgTrue;
437 //cout << "courseDev = " << courseDev << ", normalized = ";
438 SG_NORMALIZE_RANGE(courseDev, -180.0, 180.0);
439 //cout << courseDev << '\n';
440 _headingBugTo = (fabs(courseDev) > 90.0 ? false : true);
442 if(!_fromWaypoint.id.empty()) {
443 _dtkTrue = GetGreatCircleCourse(_fromWaypoint.lat, _fromWaypoint.lon, _activeWaypoint.lat, _activeWaypoint.lon) * SG_RADIANS_TO_DEGREES;
444 _dtkMag = GetMagHeadingFromTo(_fromWaypoint.lat, _fromWaypoint.lon, _activeWaypoint.lat, _activeWaypoint.lon);
451 _dist2Act = GetGreatCircleDistance(_gpsLat, _gpsLon, _activeWaypoint.lat, _activeWaypoint.lon) * SG_NM_TO_METER;
452 if(_groundSpeed_ms > 10.0) {
453 _eta = _dist2Act / _groundSpeed_ms;
454 if(_eta <= 36) { // TODO - this is slightly different if turn anticipation is enabled.
456 _waypointAlert = true; // TODO - not if the from flag is set.
460 // Check if we should sequence to next leg.
461 // Perhaps this should be done on distance instead, but 60s time (about 1 - 2 nm) seems reasonable for now.
462 //double reverseHeading = GetGreatCircleCourse(_activeWaypoint->lat, _activeWaypoint->lon, _fromWaypoint->lat, _fromWaypoint->lon);
463 // Hack - let's cheat and do it on heading bug for now. TODO - that stops us 'cutting the corner'
464 // when we happen to approach the inside turn of a waypoint - we should probably sequence at the midpoint
465 // of the heading difference between legs in this instance.
466 int idx = GetActiveWaypointIndex();
467 bool finalLeg = (idx == (int)(_activeFP->waypoints.size()) - 1 ? true : false);
468 bool finalDto = (_dto && idx == -1); // Dto operation to a waypoint not in the flightplan - we don't sequence in this instance
471 // Do nothing - not sure if Dto should switch off when arriving at the final waypoint of a flightplan
472 } else if(finalDto) {
474 } else if(_activeWaypoint.appType == GPS_MAP) {
475 // Don't sequence beyond the missed approach point
476 //cout << "ACTIVE WAYPOINT is MAP - not sequencing!!!!!\n";
478 //cout << "Sequencing...\n";
479 _fromWaypoint = _activeWaypoint;
480 _activeWaypoint = *_activeFP->waypoints[idx + 1];
482 // Course alteration message format is dependent on whether we are slaved HSI/CDI indicator or not.
484 if(fgGetBool("/instrumentation/nav[0]/slaved-to-gps")) {
485 // TODO - avoid the hardwiring on nav[0]
486 s = "Adj Nav Crs to ";
488 string s = "GPS Course is ";
490 double d = GetMagHeadingFromTo(_fromWaypoint.lat, _fromWaypoint.lon, _activeWaypoint.lat, _activeWaypoint.lon);
491 while(d < 0.0) d += 360.0;
492 while(d >= 360.0) d -= 360.0;
494 snprintf(buf, 4, "%03i", (int)(d + 0.5));
496 _messageStack.push_back(s);
498 _waypointAlert = false;
506 // First attempt at a sensible cross-track correction calculation
507 // Uh? - I think this is implemented further down the file!
508 if(_fromWaypoint != NULL) {
511 _crosstrackDist = 0.0;
518 Expand a SIAP ident to the full procedure name (as shown on the approach chart).
519 NOTE: Some of this is inferred from data, some is from documentation.
521 Example expansions from ARINC 424-18 [and the airport they're taken from]:
522 "R10LY" <--> "RNAV (GPS) Y RWY 10 L" [KBOI]
523 "R10-Z" <--> "RNAV (GPS) Z RWY 10" [KHTO]
524 "S25" <--> "VOR or GPS RWY 25" [KHHR]
525 "P20" <--> "GPS RWY 20" [KDAN]
526 "NDB-B" <--> "NDB or GPS-B" [KDAW]
527 "NDBC" <--> "NDB or GPS-C" [KEMT]
528 "VDMA" <--> "VOR/DME or GPS-A" [KDAW]
529 "VDM-A" <--> "VOR/DME or GPS-A" [KEAG]
530 "VDMB" <--> "VOR/DME or GPS-B" [KDKX]
531 "VORA" <--> "VOR or GPS-A" [KEMT]
533 It seems that there are 2 basic types of expansions; those that include
534 the runway and those that don't. Of those that don't, it seems that 2
535 different positions within the string to encode the identifying letter
536 are used, i.e. with a dash and without.
538 string DCLGPS::ExpandSIAPIdent(const string& ident) {
540 bool has_rwy = false;
543 case 'N': name = "NDB or GPS"; has_rwy = false; break;
544 case 'P': name = "GPS"; has_rwy = true; break;
545 case 'R': name = "RNAV (GPS)"; has_rwy = true; break;
546 case 'S': name = "VOR or GPS"; has_rwy = true; break;
548 if(ident[1] == 'D') name = "VOR/DME or GPS";
549 else name = "VOR or GPS";
552 default: // TODO output a log message
557 // Add the identifying letter if present
558 if(ident.size() == 5) {
565 name += ident.substr(1, 2);
567 // Add a left/right/centre indication if present.
568 if(ident.size() > 3) {
569 if((ident[3] != '-') && (ident[3] != ' ')) { // Early versions of the spec allowed a blank instead of a dash so check for both
575 // Add the identifying letter, which I *think* should always be present, but seems to be inconsistent as to whether a dash is used.
576 if(ident.size() == 5) {
579 } else if(ident.size() == 4) {
591 Load instrument approaches from an ARINC 424 file.
592 Tested on ARINC 424-18.
593 Known / current best guess at the format:
594 Col 1: Always 'S'. If it isn't, ditch it.
595 Col 2-4: "Customer area" code, eg "USA", "CAN". I think that CAN is used for Alaska.
596 Col 5: Section code. Used in conjunction with sub-section code. Definitions are with sub-section code.
598 Col 7-10: ICAO (or FAA) airport ident. Left justified if < 4 chars.
599 Col 11-12: Based on ICAO geographical region.
600 Col 13: Sub-section code. Used in conjunction with section code.
601 "HD/E/F" => Helicopter record.
602 "HS" => Helicopter minimum safe altitude.
603 "PA" => Airport record.
604 "PF" => Approach segment.
605 "PG" => Runway record.
606 "PP" => Path point record. ???
607 "PS" => MSA record (minimum safe altitude).
609 ------ The following is for "PF", approach segment -------
611 Col 14-19: SIAP ident for this approach (left justified). This is a standardised abbreviated approach name.
612 e.g. "R10LZ" expands to "RNAV (GPS) Z RWY 10 L". See the comment block for ExpandSIAPIdent for full details.
613 Col 20: Route type. This is tricky - I don't have full documentation and am having to guess a bit.
614 'A' => Arrival route? This seems to be used to encode arrival routes from the IAF to the approach proper.
615 Note that the final fix of the arrival route is duplicated in the approach proper.
616 'D' => VOR/DME or GPS
618 'P' => GPS (ARINC 424-18), GPS and RNAV (GPS) (ARINC 424-15 and before).
619 'R' => RNAV (GPS) (ARINC 424-18).
621 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.
623 Col 27-29: Sequence number - position within the route segment. Rule: 10-20-30 etc.
624 Col 30-34: Fix identifer. The ident of the waypoint.
625 Col 35-36: ICAO geographical region code. I think we can ignore this for now.
626 Col 37: Section code - ??? I don't know what this means
627 Col 38 Subsection code - ??? ditto - no idea!
628 Col 40: Waypoint type.
629 'A' => Airport as waypoint
630 'E' => Essential waypoint (e.g. change of heading at this waypoint, etc).
631 'G' => Runway or helipad as waypoint
632 'H' => Heliport as waypoint
633 'N' => NDB as waypoint
634 'P' => Phantom waypoint (not sure if this is used in rev 18?)
635 'V' => VOR as waypoint
636 Col 41: Waypoint type.
637 'B' => Flyover, approach transition, or final approach.
638 'E' => end of route segment (transition waypoint). (Actually "End of terminal procedure route type" in the docs).
639 'N' => ??? I've also seen 'N' in this column, but don't know what it indicates.
641 Col 43: Waypoint type. May also be blank when none of the below.
642 'A' => Initial approach fix (IAF)
643 'F' => Final approach fix
645 'I' => Final approach course fix
646 'M' => Missed approach point
647 '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!
648 ??? Possibly procedure turn?
649 'C' => ??? This is also present in the data, but missing from the docs. Is at airport 00R.
650 Col 107-111 MSA center fix. We can ignore this.
652 void DCLGPS::LoadApproachData() {
656 const GPSWaypoint* cwp;
659 SGPath path = globals->get_fg_root();
660 path.append("Navaids/rnav.dat");
661 fin.open(path.c_str(), ios::in);
663 //cout << "Unable to open input file " << path.c_str() << '\n';
666 //cout << "Opened " << path.c_str() << " for reading\n";
671 string apt_ident; // This gets set to the ICAO code of the current airport being processed.
672 string iap_ident; // The abbreviated name of the current approach being processed.
673 string wp_ident; // The ident of the waypoint of the current line
674 string last_apt_ident;
675 string last_iap_ident;
676 string last_wp_ident;
677 // There is no need to save the full name - it can be generated on the fly from the abbreviated name as and when needed.
678 bool apt_in_progress = false; // Set true whilst loading all the approaches for a given airport.
679 bool iap_in_progress = false; // Set true whilst loading a given approach.
680 bool iap_error = false; // Set true if there is an error loading a given approach.
681 bool route_in_progress = false; // Set true when we are loading a "route" segment of the approach.
682 int last_sequence_number = 0; // Position within the route, rule (rev 18): 10, 20, 30 etc.
684 char last_route_type = 0;
686 char waypoint_fix_type; // This is the waypoint type from col 43, i.e. the type of fix. May be blank.
691 unsigned int nLoaded = 0;
692 unsigned int nErrors = 0;
694 //for(i=0; i<64; ++i) {
696 fin.getline(tmp, 256);
697 //s = Fake_rnav_dat[i];
699 if(s.size() < 132) continue;
700 if(s[0] == 'S') { // Valid line
701 string country_code = s.substr(1, 3);
702 if(country_code == "USA") { // For now we'll stick to US procedures in case there are unknown gotchas with others
703 if(s[4] == 'P') { // Includes approaches.
704 if(s[12] == 'A') { // Airport record
705 apt_ident = s.substr(6, 4);
706 // Trim any whitespace from the ident. The ident is left justified,
707 // so any space will be at the end.
708 if(apt_ident[3] == ' ') apt_ident = apt_ident.substr(0, 3);
709 // I think that all idents are either 3 or 4 chars - could check this though!
710 if(!apt_in_progress) {
711 last_apt_ident = apt_ident;
714 if(last_apt_ident != apt_ident) {
715 if(iap_in_progress) {
717 //cout << "ERROR: Unable to load approach " << iap->_ident << " at " << iap->_aptIdent << '\n';
720 _np_iap[iap->_aptIdent].push_back(iap);
721 //cout << "** Loaded " << iap->_aptIdent << "\t" << iap->_ident << '\n';
724 iap_in_progress = false;
727 last_apt_ident = apt_ident;
730 } else if(s[12] == 'F') { // Approach segment
731 if(apt_in_progress) {
732 iap_ident = s.substr(13, 6);
733 // Trim any whitespace from the RH end.
735 if(iap_ident[5-j] == ' ') {
736 iap_ident = iap_ident.substr(0, 5-j);
738 // It's important to break here, since earlier versions of ARINC 424 allowed spaces in the ident.
742 if(iap_in_progress) {
743 if(iap_ident != last_iap_ident) {
744 // This is a new approach - store the last one and trigger
745 // starting afresh by setting the in progress flag to false.
747 //cout << "ERROR: Unable to load approach " << iap->_ident << " at " << iap->_aptIdent << '\n';
750 _np_iap[iap->_aptIdent].push_back(iap);
751 //cout << "Loaded " << iap->_aptIdent << "\t" << iap->_ident << '\n';
754 iap_in_progress = false;
757 if(!iap_in_progress) {
759 iap->_aptIdent = apt_ident;
760 iap->_ident = iap_ident;
761 iap->_name = ExpandSIAPIdent(iap_ident); // I suspect that it's probably better to just store idents, and to expand the names as needed.
762 // Note, we haven't set iap->_rwyStr yet.
763 last_iap_ident = iap_ident;
764 iap_in_progress = true;
770 sequence_number = atoi(s.substr(26,3).c_str());
771 wp_ident = s.substr(29, 5);
772 waypoint_fix_type = s[42];
773 // Trim any whitespace from the RH end
775 if(wp_ident[4-j] == ' ') {
776 wp_ident = wp_ident.substr(0, 4-j);
782 // Ignore lines with no waypoint ID for now - these are normally part of the
783 // missed approach procedure, and we don't use them in the KLN89.
784 if(!wp_ident.empty()) {
785 // Make a local copy of the waypoint for now, since we're not yet sure if we'll be using it
788 bool wp_error = false;
789 if(w.id.substr(0, 2) == "RW" && waypoint_fix_type == 'M') {
790 // Assume that this is a missed-approach point based on the runway number, which appears to be standard for most approaches.
791 // Note: Currently fgFindAirportID returns NULL on error, but getRunwayByIdent throws an exception.
792 const FGAirport* apt = fgFindAirportID(iap->_aptIdent);
796 rwystr = w.id.substr(2, 2);
797 // TODO - sanity check the rwystr at this point to ensure we have a double digit number
798 if(w.id.size() > 4) {
799 if(w.id[4] == 'L' || w.id[4] == 'C' || w.id[4] == 'R') {
803 FGRunway* rwy = apt->getRunwayByIdent(rwystr);
804 w.lat = rwy->begin().getLatitudeRad();
805 w.lon = rwy->begin().getLongitudeRad();
806 } catch(const sg_exception&) {
807 SG_LOG(SG_INSTR, SG_WARN, "Unable to find runway " << w.id.substr(2, 2) << " at airport " << iap->_aptIdent);
808 //cout << "Unable to find runway " << w.id.substr(2, 2) << " at airport " << iap->_aptIdent << " ( w.id = " << w.id << ", rwystr = " << rwystr << " )\n";
815 cwp = FindFirstByExactId(w.id);
822 switch(waypoint_fix_type) {
823 case 'A': w.appType = GPS_IAF; break;
824 case 'F': w.appType = GPS_FAF; break;
825 case 'H': w.appType = GPS_MAHP; break;
826 case 'I': w.appType = GPS_IAP; break;
827 case 'M': w.appType = GPS_MAP; break;
828 case ' ': w.appType = GPS_APP_NONE; break;
829 //default: cout << "Unknown waypoint_fix_type: \'" << waypoint_fix_type << "\' [" << apt_ident << ", " << iap_ident << "]\n";
833 //cout << "Unable to find waypoint " << w.id << " [" << apt_ident << ", " << iap_ident << "]\n";
838 if(route_in_progress) {
839 if(sequence_number > last_sequence_number) {
840 // TODO - add a check for runway numbers
841 // Check for the waypoint ID being the same as the previous line.
842 // This is often the case for the missed approach holding point.
843 if(wp_ident == last_wp_ident) {
844 if(waypoint_fix_type == 'H') {
845 if(!iap->_IAP.empty()) {
846 if(iap->_IAP[iap->_IAP.size() - 1]->appType == GPS_APP_NONE) {
847 iap->_IAP[iap->_IAP.size() - 1]->appType = GPS_MAHP;
849 //cout << "Waypoint is MAHP and another type! " << w.id << " [" << apt_ident << ", " << iap_ident << "]\n";
854 // Create a new waypoint on the heap, copy the local copy into it, and push it onto the approach.
855 wp = new GPSWaypoint;
857 if(route_type == 'A') {
858 fp->waypoints.push_back(wp);
860 iap->_IAP.push_back(wp);
863 } else if(sequence_number == last_sequence_number) {
864 // This seems to happen once per final approach route - one of the waypoints
865 // is duplicated with the same sequence number - I'm not sure what information
866 // the second line give yet so ignore it for now.
867 // TODO - figure this out!
869 // Finalise the current route and start a new one
871 // Finalise the current route
872 if(last_route_type == 'A') {
873 // Push the flightplan onto the approach
874 iap->_approachRoutes.push_back(fp);
876 // All the waypoints get pushed individually - don't need to do it.
879 // There are basically 2 possibilities here - either it's one of the arrival transitions,
880 // or it's the core final approach course.
881 wp = new GPSWaypoint;
883 if(route_type == 'A') { // It's one of the arrival transition(s)
884 fp = new GPSFlightPlan;
885 fp->waypoints.push_back(wp);
887 iap->_IAP.push_back(wp);
889 route_in_progress = true;
892 // Start a new route.
893 // There are basically 2 possibilities here - either it's one of the arrival transitions,
894 // or it's the core final approach course.
895 wp = new GPSWaypoint;
897 if(route_type == 'A') { // It's one of the arrival transition(s)
898 fp = new GPSFlightPlan;
899 fp->waypoints.push_back(wp);
901 iap->_IAP.push_back(wp);
903 route_in_progress = true;
905 last_route_type = route_type;
906 last_wp_ident = wp_ident;
907 last_sequence_number = sequence_number;
911 // ERROR - no airport record read.
915 // Check and finalise any approaches in progress
916 // TODO - sanity check that the approach has all the required elements
917 if(iap_in_progress) {
918 // This is a new approach - store the last one and trigger
919 // starting afresh by setting the in progress flag to false.
921 //cout << "ERROR: Unable to load approach " << iap->_ident << " at " << iap->_aptIdent << '\n';
924 _np_iap[iap->_aptIdent].push_back(iap);
925 //cout << "* Loaded " << iap->_aptIdent << "\t" << iap->_ident << '\n';
928 iap_in_progress = false;
935 // If we get to the end of the file, load any approach that is still in progress
936 // TODO - sanity check that the approach has all the required elements
937 if(iap_in_progress) {
939 //cout << "ERROR: Unable to load approach " << iap->_ident << " at " << iap->_aptIdent << '\n';
942 _np_iap[iap->_aptIdent].push_back(iap);
943 //cout << "*** Loaded " << iap->_aptIdent << "\t" << iap->_ident << '\n';
948 //cout << "Done loading approach database\n";
949 //cout << "Loaded: " << nLoaded << '\n';
950 //cout << "Failed: " << nErrors << '\n';
955 GPSWaypoint* DCLGPS::GetActiveWaypoint() {
956 return &_activeWaypoint;
960 float DCLGPS::GetDistToActiveWaypoint() {
964 // I don't yet fully understand all the gotchas about where to source time from.
965 // This function sets the initial timer before the clock exports properties
966 // and the one below uses the clock to be consistent with the rest of the code.
967 // It might change soonish...
968 void DCLGPS::SetPowerOnTimer() {
969 struct tm *t = globals->get_time_params()->getGmt();
970 _powerOnTime.set_hr(t->tm_hour);
971 _powerOnTime.set_min(t->tm_min);
972 _powerOnTimerSet = true;
975 void DCLGPS::ResetPowerOnTimer() {
976 _powerOnTime.set_hr(atoi(fgGetString("/instrumentation/clock/indicated-hour")));
977 _powerOnTime.set_min(atoi(fgGetString("/instrumentation/clock/indicated-min")));
978 _powerOnTimerSet = true;
981 double DCLGPS::GetCDIDeflection() const {
982 double xtd = CalcCrossTrackDeviation(); //nm
983 return((xtd / _currentCdiScale) * 5.0 * 2.5 * -1.0);
986 void DCLGPS::DtoInitiate(const string& s) {
987 const GPSWaypoint* wp = FindFirstByExactId(s);
989 // TODO - Currently we start DTO operation unconditionally, regardless of which mode we are in.
990 // In fact, the following rules apply:
991 // In LEG mode, start DTO as we currently do.
992 // In OBS mode, set the active waypoint to the requested waypoint, and then:
993 // If the KLN89 is not connected to an external HSI or CDI, set the OBS course to go direct to the waypoint.
994 // If the KLN89 *is* connected to an external HSI or CDI, it cannot set the course itself, and will display
995 // a scratchpad message with the course to set manually on the HSI/CDI.
996 // In both OBS cases, leave _dto false, since we don't need the virtual waypoint created.
998 _activeWaypoint = *wp;
999 _fromWaypoint.lat = _gpsLat;
1000 _fromWaypoint.lon = _gpsLon;
1001 _fromWaypoint.type = GPS_WP_VIRT;
1002 _fromWaypoint.id = "_DTOWP_";
1005 // TODO - Should bring up the user waypoint page.
1010 void DCLGPS::DtoCancel() {
1012 // i.e. don't bother reorientating if we're just cancelling a DTO button press
1013 // without having previously initiated DTO.
1014 OrientateToActiveFlightPlan();
1019 void DCLGPS::ToggleOBSMode() {
1020 _obsMode = !_obsMode;
1022 // We need to set the OBS heading. The rules for this are:
1024 // If the KLN89 is connected to an external HSI or CDI, then the OBS heading must be set
1025 // to the OBS radial on the external indicator, and cannot be changed in the KLN89.
1027 // If the KLN89 is not connected to an external indicator, then:
1028 // If there is an active waypoint, the OBS heading is set such that the
1029 // deviation indicator remains at the same deviation (i.e. set to DTK,
1030 // although there may be some small difference).
1032 // If there is not an active waypoint, I am not sure what value should be
1035 if(fgGetBool("/instrumentation/nav/slaved-to-gps")) {
1036 _obsHeading = static_cast<int>(fgGetDouble("/instrumentation/nav/radials/selected-deg") + 0.5);
1037 } else if(!_activeWaypoint.id.empty()) {
1038 // NOTE: I am not sure if this is completely correct. There are sometimes small differences
1039 // between DTK and default OBS heading in the simulator (~ 1 or 2 degrees). It might also
1040 // be that OBS heading should be stored as float and only displayed as int, in order to maintain
1041 // the initial bar deviation?
1042 _obsHeading = static_cast<int>(_dtkMag + 0.5);
1043 //cout << "_dtkMag = " << _dtkMag << ", _dtkTrue = " << _dtkTrue << ", _obsHeading = " << _obsHeading << '\n';
1045 // TODO - what should we really do here?
1049 // Valid OBS heading values are 0 -> 359 degrees inclusive (from kln89 simulator).
1050 if(_obsHeading > 359) _obsHeading -= 360;
1051 if(_obsHeading < 0) _obsHeading += 360;
1053 // TODO - the _fromWaypoint location will change as the OBS heading changes.
1054 // Might need to store the OBS initiation position somewhere in case it is needed again.
1055 SetOBSFromWaypoint();
1059 // Set the _fromWaypoint position based on the active waypoint and OBS radial.
1060 void DCLGPS::SetOBSFromWaypoint() {
1061 if(!_obsMode) return;
1062 if(_activeWaypoint.id.empty()) return;
1064 // TODO - base the 180 deg correction on the to/from flag.
1065 _fromWaypoint = GetPositionOnMagRadial(_activeWaypoint, 10, _obsHeading + 180.0);
1066 _fromWaypoint.type = GPS_WP_VIRT;
1067 _fromWaypoint.id = "_OBSWP_";
1070 void DCLGPS::CDIFSDIncrease() {
1071 if(_currentCdiScaleIndex == 0) {
1072 _currentCdiScaleIndex = _cdiScales.size() - 1;
1074 _currentCdiScaleIndex--;
1078 void DCLGPS::CDIFSDDecrease() {
1079 _currentCdiScaleIndex++;
1080 if(_currentCdiScaleIndex == _cdiScales.size()) {
1081 _currentCdiScaleIndex = 0;
1085 void DCLGPS::DrawChar(char c, int field, int px, int py, bool bold) {
1088 void DCLGPS::DrawText(const string& s, int field, int px, int py, bool bold) {
1091 void DCLGPS::CreateDefaultFlightPlans() {}
1093 // Get the time to the active waypoint in seconds.
1094 // Returns -1 if groundspeed < 30 kts
1095 double DCLGPS::GetTimeToActiveWaypoint() {
1096 if(_groundSpeed_kts < 30.0) {
1103 // Get the time to the final waypoint in seconds.
1104 // Returns -1 if groundspeed < 30 kts
1105 double DCLGPS::GetETE() {
1106 if(_groundSpeed_kts < 30.0) {
1109 // TODO - handle OBS / DTO operation appropriately
1110 if(_activeFP->waypoints.empty()) {
1113 return(GetTimeToWaypoint(_activeFP->waypoints[_activeFP->waypoints.size() - 1]->id));
1118 // Get the time to a given waypoint (spec'd by ID) in seconds.
1119 // returns -1 if groundspeed is less than 30kts.
1120 // If the waypoint is an unreached part of the active flight plan the time will be via each leg.
1121 // otherwise it will be a direct-to time.
1122 double DCLGPS::GetTimeToWaypoint(const string& id) {
1123 if(_groundSpeed_kts < 30.0) {
1128 int n1 = GetActiveWaypointIndex();
1129 int n2 = GetWaypointIndex(id);
1132 for(unsigned int i=n1+1; i<_activeFP->waypoints.size(); ++i) {
1133 GPSWaypoint* wp1 = _activeFP->waypoints[i-1];
1134 GPSWaypoint* wp2 = _activeFP->waypoints[i];
1135 double distm = GetGreatCircleDistance(wp1->lat, wp1->lon, wp2->lat, wp2->lon) * SG_NM_TO_METER;
1136 eta += (distm / _groundSpeed_ms);
1139 } else if(id == _activeWaypoint.id) {
1142 const GPSWaypoint* wp = FindFirstByExactId(id);
1143 if(wp == NULL) return(-1.0);
1144 double distm = GetGreatCircleDistance(_gpsLat, _gpsLon, wp->lat, wp->lon);
1146 return(distm / _groundSpeed_ms);
1148 return(-1.0); // Hopefully we never get here!
1151 // Returns magnetic great-circle heading
1152 // TODO - document units.
1153 float DCLGPS::GetHeadingToActiveWaypoint() {
1154 if(_activeWaypoint.id.empty()) {
1157 double h = GetMagHeadingFromTo(_gpsLat, _gpsLon, _activeWaypoint.lat, _activeWaypoint.lon);
1158 while(h <= 0.0) h += 360.0;
1159 while(h > 360.0) h -= 360.0;
1164 // Returns magnetic great-circle heading
1165 // TODO - what units?
1166 float DCLGPS::GetHeadingFromActiveWaypoint() {
1167 if(_activeWaypoint.id.empty()) {
1170 double h = GetMagHeadingFromTo(_activeWaypoint.lat, _activeWaypoint.lon, _gpsLat, _gpsLon);
1171 while(h <= 0.0) h += 360.0;
1172 while(h > 360.0) h -= 360.0;
1177 void DCLGPS::ClearFlightPlan(int n) {
1178 for(unsigned int i=0; i<_flightPlans[n]->waypoints.size(); ++i) {
1179 delete _flightPlans[n]->waypoints[i];
1181 _flightPlans[n]->waypoints.clear();
1184 void DCLGPS::ClearFlightPlan(GPSFlightPlan* fp) {
1185 for(unsigned int i=0; i<fp->waypoints.size(); ++i) {
1186 delete fp->waypoints[i];
1188 fp->waypoints.clear();
1191 int DCLGPS::GetActiveWaypointIndex() {
1192 for(unsigned int i=0; i<_flightPlans[0]->waypoints.size(); ++i) {
1193 if(_flightPlans[0]->waypoints[i]->id == _activeWaypoint.id) return((int)i);
1198 int DCLGPS::GetWaypointIndex(const string& id) {
1199 for(unsigned int i=0; i<_flightPlans[0]->waypoints.size(); ++i) {
1200 if(_flightPlans[0]->waypoints[i]->id == id) return((int)i);
1205 void DCLGPS::OrientateToFlightPlan(GPSFlightPlan* fp) {
1206 //cout << "Orientating...\n";
1207 //cout << "_lat = " << _lat << ", _lon = " << _lon << ", _gpsLat = " << _gpsLat << ", gpsLon = " << _gpsLon << '\n';
1209 _activeWaypoint.id.clear();
1212 _navFlagged = false;
1213 if(fp->waypoints.size() == 1) {
1214 // TODO - may need to flag nav here if not dto or obs, or possibly handle it somewhere else.
1215 _activeWaypoint = *fp->waypoints[0];
1216 _fromWaypoint.id.clear();
1218 // FIXME FIXME FIXME
1219 _fromWaypoint = *fp->waypoints[0];
1220 _activeWaypoint = *fp->waypoints[1];
1221 double dmin = 1000000; // nm!!
1222 // For now we will simply start on the leg closest to our current position.
1223 // It's possible that more fancy algorithms may take either heading or track
1224 // into account when setting inital leg - I'm not sure.
1225 // This method should handle most cases perfectly OK though.
1226 for(unsigned int i = 1; i < fp->waypoints.size(); ++i) {
1227 //cout << "Pass " << i << ", dmin = " << dmin << ", leg is " << fp->waypoints[i-1]->id << " to " << fp->waypoints[i]->id << '\n';
1228 // First get the cross track correction.
1229 double d0 = fabs(CalcCrossTrackDeviation(*fp->waypoints[i-1], *fp->waypoints[i]));
1230 // That is the shortest distance away we could be though - check for
1231 // longer distances if we are 'off the end' of the leg.
1232 double ht1 = GetGreatCircleCourse(fp->waypoints[i-1]->lat, fp->waypoints[i-1]->lon,
1233 fp->waypoints[i]->lat, fp->waypoints[i]->lon)
1234 * SG_RADIANS_TO_DEGREES;
1235 // not simply the reverse of the above due to great circle navigation.
1236 double ht2 = GetGreatCircleCourse(fp->waypoints[i]->lat, fp->waypoints[i]->lon,
1237 fp->waypoints[i-1]->lat, fp->waypoints[i-1]->lon)
1238 * SG_RADIANS_TO_DEGREES;
1239 double hw1 = GetGreatCircleCourse(_gpsLat, _gpsLon,
1240 fp->waypoints[i]->lat, fp->waypoints[i]->lon)
1241 * SG_RADIANS_TO_DEGREES;
1242 double hw2 = GetGreatCircleCourse(_gpsLat, _gpsLon,
1243 fp->waypoints[i-1]->lat, fp->waypoints[i-1]->lon)
1244 * SG_RADIANS_TO_DEGREES;
1245 double h1 = ht1 - hw1;
1246 double h2 = ht2 - hw2;
1247 //cout << "d0, h1, h2 = " << d0 << ", " << h1 << ", " << h2 << '\n';
1248 //cout << "Normalizing...\n";
1249 SG_NORMALIZE_RANGE(h1, -180.0, 180.0);
1250 SG_NORMALIZE_RANGE(h2, -180.0, 180.0);
1251 //cout << "d0, h1, h2 = " << d0 << ", " << h1 << ", " << h2 << '\n';
1252 if(fabs(h1) > 90.0) {
1253 // We are past the end of the to waypoint
1254 double d = GetGreatCircleDistance(_gpsLat, _gpsLon, fp->waypoints[i]->lat, fp->waypoints[i]->lon);
1256 //cout << "h1 triggered, d0 now = " << d0 << '\n';
1257 } else if(fabs(h2) > 90.0) {
1258 // We are past the end (not yet at!) the from waypoint
1259 double d = GetGreatCircleDistance(_gpsLat, _gpsLon, fp->waypoints[i-1]->lat, fp->waypoints[i-1]->lon);
1261 //cout << "h2 triggered, d0 now = " << d0 << '\n';
1264 //cout << "THIS LEG NOW ACTIVE!\n";
1266 _fromWaypoint = *fp->waypoints[i-1];
1267 _activeWaypoint = *fp->waypoints[i];
1274 void DCLGPS::OrientateToActiveFlightPlan() {
1275 OrientateToFlightPlan(_activeFP);
1278 /***************************************/
1280 // Utility function - create a flightplan from a list of waypoint ids and types
1281 void DCLGPS::CreateFlightPlan(GPSFlightPlan* fp, vector<string> ids, vector<GPSWpType> wps) {
1282 if(fp == NULL) fp = new GPSFlightPlan;
1284 if(!fp->waypoints.empty()) {
1285 for(i=0; i<fp->waypoints.size(); ++i) {
1286 delete fp->waypoints[i];
1288 fp->waypoints.clear();
1290 if(ids.size() != wps.size()) {
1291 cout << "ID and Waypoint types list size mismatch in GPS::CreateFlightPlan - no flightplan created!\n";
1294 for(i=0; i<ids.size(); ++i) {
1296 const FGAirport* ap;
1298 GPSWaypoint* wp = new GPSWaypoint;
1302 ap = FindFirstAptById(ids[i], multi, true);
1307 wp->lat = ap->getLatitude() * SG_DEGREES_TO_RADIANS;
1308 wp->lon = ap->getLongitude() * SG_DEGREES_TO_RADIANS;
1310 fp->waypoints.push_back(wp);
1314 np = FindFirstVorById(ids[i], multi, true);
1319 wp->lat = np->get_lat() * SG_DEGREES_TO_RADIANS;
1320 wp->lon = np->get_lon() * SG_DEGREES_TO_RADIANS;
1322 fp->waypoints.push_back(wp);
1326 np = FindFirstNDBById(ids[i], multi, true);
1331 wp->lat = np->get_lat() * SG_DEGREES_TO_RADIANS;
1332 wp->lon = np->get_lon() * SG_DEGREES_TO_RADIANS;
1334 fp->waypoints.push_back(wp);
1350 /***************************************/
1352 class DCLGPSFilter : public FGPositioned::Filter
1355 virtual bool pass(const FGPositioned* aPos) const {
1356 switch (aPos->type()) {
1357 case FGPositioned::AIRPORT:
1358 // how about heliports and seaports?
1359 case FGPositioned::NDB:
1360 case FGPositioned::VOR:
1361 case FGPositioned::WAYPOINT:
1362 case FGPositioned::FIX:
1364 default: return false; // reject all other types
1370 GPSWaypoint* DCLGPS::FindFirstById(const string& id) const
1372 DCLGPSFilter filter;
1373 FGPositionedRef result = FGPositioned::findNextWithPartialId(NULL, id, &filter);
1374 return GPSWaypoint::createFromPositioned(result);
1377 GPSWaypoint* DCLGPS::FindFirstByExactId(const string& id) const
1379 SGGeod pos(SGGeod::fromRad(_lon, _lat));
1380 FGPositionedRef result = FGPositioned::findClosestWithIdent(id, pos);
1381 return GPSWaypoint::createFromPositioned(result);
1384 // TODO - add the ASCII / alphabetical stuff from the Atlas version
1385 FGPositioned* DCLGPS::FindTypedFirstById(const string& id, FGPositioned::Type ty, bool &multi, bool exact)
1388 FGPositioned::TypeFilter filter(ty);
1391 FGPositioned::List matches =
1392 FGPositioned::findAllWithIdent(id, &filter);
1393 FGPositioned::sortByRange(matches, SGGeod::fromRad(_lon, _lat));
1394 multi = (matches.size() > 1);
1395 return matches.empty() ? NULL : matches.front().ptr();
1398 return FGPositioned::findNextWithPartialId(NULL, id, &filter);
1401 FGNavRecord* DCLGPS::FindFirstVorById(const string& id, bool &multi, bool exact)
1403 return dynamic_cast<FGNavRecord*>(FindTypedFirstById(id, FGPositioned::VOR, multi, exact));
1406 FGNavRecord* DCLGPS::FindFirstNDBById(const string& id, bool &multi, bool exact)
1408 return dynamic_cast<FGNavRecord*>(FindTypedFirstById(id, FGPositioned::NDB, multi, exact));
1411 const FGFix* DCLGPS::FindFirstIntById(const string& id, bool &multi, bool exact)
1413 return dynamic_cast<FGFix*>(FindTypedFirstById(id, FGPositioned::FIX, multi, exact));
1416 const FGAirport* DCLGPS::FindFirstAptById(const string& id, bool &multi, bool exact)
1418 return dynamic_cast<FGAirport*>(FindTypedFirstById(id, FGPositioned::AIRPORT, multi, exact));
1421 FGNavRecord* DCLGPS::FindClosestVor(double lat_rad, double lon_rad) {
1422 FGPositioned::TypeFilter filter(FGPositioned::VOR);
1423 double cutoff = 1000; // nautical miles
1424 FGPositionedRef v = FGPositioned::findClosest(SGGeod::fromRad(lon_rad, lat_rad), cutoff, &filter);
1429 return dynamic_cast<FGNavRecord*>(v.ptr());
1432 //----------------------------------------------------------------------------------------------------------
1434 // Takes lat and lon in RADIANS!!!!!!!
1435 double DCLGPS::GetMagHeadingFromTo(double latA, double lonA, double latB, double lonB) {
1436 double h = GetGreatCircleCourse(latA, lonA, latB, lonB);
1437 h *= SG_RADIANS_TO_DEGREES;
1438 // TODO - use the real altitude below instead of 0.0!
1439 //cout << "MagVar = " << sgGetMagVar(_gpsLon, _gpsLat, 0.0, _time->getJD()) * SG_RADIANS_TO_DEGREES << '\n';
1440 h -= sgGetMagVar(_gpsLon, _gpsLat, 0.0, _time->getJD()) * SG_RADIANS_TO_DEGREES;
1441 while(h >= 360.0) h -= 360.0;
1442 while(h < 0.0) h += 360.0;
1446 // ---------------- Great Circle formulae from "The Aviation Formulary" -------------
1447 // Note that all of these assume that the world is spherical.
1449 double Rad2Nm(double theta) {
1450 return(((180.0*60.0)/SG_PI)*theta);
1453 double Nm2Rad(double d) {
1454 return((SG_PI/(180.0*60.0))*d);
1459 The great circle distance d between two points with coordinates {lat1,lon1} and {lat2,lon2} is given by:
1461 d=acos(sin(lat1)*sin(lat2)+cos(lat1)*cos(lat2)*cos(lon1-lon2))
1463 A mathematically equivalent formula, which is less subject to rounding error for short distances is:
1465 d=2*asin(sqrt((sin((lat1-lat2)/2))^2 +
1466 cos(lat1)*cos(lat2)*(sin((lon1-lon2)/2))^2))
1470 // Returns distance in nm, takes lat & lon in RADIANS
1471 double DCLGPS::GetGreatCircleDistance(double lat1, double lon1, double lat2, double lon2) const {
1472 double d = 2.0 * asin(sqrt(((sin((lat1-lat2)/2.0))*(sin((lat1-lat2)/2.0))) +
1473 cos(lat1)*cos(lat2)*(sin((lon1-lon2)/2.0))*(sin((lon1-lon2)/2.0))));
1477 // fmod dosen't do what we want :-(
1478 static double mod(double d1, double d2) {
1479 return(d1 - d2*floor(d1/d2));
1482 // Returns great circle course from point 1 to point 2
1483 // Input and output in RADIANS.
1484 double DCLGPS::GetGreatCircleCourse (double lat1, double lon1, double lat2, double lon2) const {
1487 // Special case the poles
1488 if(cos(lat1) < SG_EPSILON) {
1490 // Starting from North Pole
1493 // Starting from South Pole
1497 // Urgh - the formula below is for negative lon +ve !!!???
1498 double d = GetGreatCircleDistance(lat1, lon1, lat2, lon2);
1499 cout << "d = " << d;
1501 //cout << ", d_theta = " << d;
1502 //cout << ", and d = " << Rad2Nm(d) << ' ';
1503 if(sin(lon2 - lon1) < 0) {
1505 h = acos((sin(lat2)-sin(lat1)*cos(d))/(sin(d)*cos(lat1)));
1508 h = 2.0 * SG_PI - acos((sin(lat2)-sin(lat1)*cos(d))/(sin(d)*cos(lat1)));
1511 cout << h * SG_RADIANS_TO_DEGREES << '\n';
1514 return( mod(atan2(sin(lon2-lon1)*cos(lat2),
1515 cos(lat1)*sin(lat2)-sin(lat1)*cos(lat2)*cos(lon2-lon1)),
1519 // Return a position on a radial from wp1 given distance d (nm) and magnetic heading h (degrees)
1520 // Note that d should be less that 1/4 Earth diameter!
1521 GPSWaypoint DCLGPS::GetPositionOnMagRadial(const GPSWaypoint& wp1, double d, double h) {
1522 h += sgGetMagVar(wp1.lon, wp1.lat, 0.0, _time->getJD()) * SG_RADIANS_TO_DEGREES;
1523 return(GetPositionOnRadial(wp1, d, h));
1526 // Return a position on a radial from wp1 given distance d (nm) and TRUE heading h (degrees)
1527 // Note that d should be less that 1/4 Earth diameter!
1528 GPSWaypoint DCLGPS::GetPositionOnRadial(const GPSWaypoint& wp1, double d, double h) {
1529 while(h < 0.0) h += 360.0;
1530 while(h > 360.0) h -= 360.0;
1532 h *= SG_DEGREES_TO_RADIANS;
1533 d *= (SG_PI / (180.0 * 60.0));
1535 double lat=asin(sin(wp1.lat)*cos(d)+cos(wp1.lat)*sin(d)*cos(h));
1538 lon=wp1.lon; // endpoint a pole
1540 lon=mod(wp1.lon+asin(sin(h)*sin(d)/cos(lat))+SG_PI,2*SG_PI)-SG_PI;
1546 wp.type = GPS_WP_VIRT;
1550 // Returns cross-track deviation in Nm.
1551 double DCLGPS::CalcCrossTrackDeviation() const {
1552 return(CalcCrossTrackDeviation(_fromWaypoint, _activeWaypoint));
1555 // Returns cross-track deviation of the current position between two arbitary waypoints in nm.
1556 double DCLGPS::CalcCrossTrackDeviation(const GPSWaypoint& wp1, const GPSWaypoint& wp2) const {
1557 //if(wp1 == NULL || wp2 == NULL) return(0.0);
1558 if(wp1.id.empty() || wp2.id.empty()) return(0.0);
1559 double xtd = asin(sin(Nm2Rad(GetGreatCircleDistance(wp1.lat, wp1.lon, _gpsLat, _gpsLon)))
1560 * sin(GetGreatCircleCourse(wp1.lat, wp1.lon, _gpsLat, _gpsLon) - GetGreatCircleCourse(wp1.lat, wp1.lon, wp2.lat, wp2.lon)));
1561 return(Rad2Nm(xtd));