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 // Units - lets default to US units - FG can set them to other units from config during startup if desired.
157 _altUnits = GPS_ALT_UNITS_FT;
158 _baroUnits = GPS_PRES_UNITS_IN;
159 _velUnits = GPS_VEL_UNITS_KT;
160 _distUnits = GPS_DIST_UNITS_NM;
162 _lon_node = fgGetNode("/instrumentation/gps/indicated-longitude-deg", true);
163 _lat_node = fgGetNode("/instrumentation/gps/indicated-latitude-deg", true);
164 _alt_node = fgGetNode("/instrumentation/gps/indicated-altitude-ft", true);
165 _grnd_speed_node = fgGetNode("/instrumentation/gps/indicated-ground-speed-kt", true);
166 _true_track_node = fgGetNode("/instrumentation/gps/indicated-track-true-deg", true);
167 _mag_track_node = fgGetNode("/instrumentation/gps/indicated-track-magnetic-deg", true);
169 // Use FG's position values at construction in case FG's gps has not run first update yet.
170 _lon = fgGetDouble("/position/longitude-deg") * SG_DEGREES_TO_RADIANS;
171 _lat = fgGetDouble("/position/latitude-deg") * SG_DEGREES_TO_RADIANS;
172 _alt = fgGetDouble("/position/altitude-ft");
173 // Note - we can depriciate _gpsLat and _gpsLon if we implement error handling in FG
174 // gps code and not our own.
179 _groundSpeed_ms = 0.0;
180 _groundSpeed_kts = 0.0;
184 // Sensible defaults. These can be overriden by derived classes if desired.
186 _cdiScales.push_back(5.0);
187 _cdiScales.push_back(1.0);
188 _cdiScales.push_back(0.3);
189 _currentCdiScaleIndex = 0;
190 _targetCdiScaleIndex = 0;
191 _sourceCdiScaleIndex = 0;
192 _cdiScaleTransition = false;
193 _currentCdiScale = 5.0;
197 _activeWaypoint.id.clear();
199 _crosstrackDist = 0.0;
200 _headingBugTo = true;
202 _waypointAlert = false;
204 _departureTimeString = "----";
206 _powerOnTime.set_hr(0);
207 _powerOnTime.set_min(0);
208 _powerOnTimerSet = false;
211 // Configuration Initialisation
212 // Should this be in kln89.cxx ?
213 _turnAnticipationEnabled = false;
217 _messageStack.clear();
221 _approachLoaded = false;
222 _approachArm = false;
223 _approachReallyArmed = false;
224 _approachActive = false;
225 _approachFP = new GPSFlightPlan;
230 delete _approachFP; // Don't need to delete the waypoints inside since they point to
231 // the waypoints in the approach database.
232 // TODO - may need to delete the approach database!!
235 void DCLGPS::draw(osg::State& state) {
236 _instrument->Draw(state);
239 void DCLGPS::init() {
241 // Not sure if this should be here, but OK for now.
242 CreateDefaultFlightPlans();
247 void DCLGPS::bind() {
248 fgTie("/instrumentation/gps/waypoint-alert", this, &DCLGPS::GetWaypointAlert);
249 fgTie("/instrumentation/gps/leg-mode", this, &DCLGPS::GetLegMode);
250 fgTie("/instrumentation/gps/obs-mode", this, &DCLGPS::GetOBSMode);
251 fgTie("/instrumentation/gps/approach-arm", this, &DCLGPS::GetApproachArm);
252 fgTie("/instrumentation/gps/approach-active", this, &DCLGPS::GetApproachActive);
253 fgTie("/instrumentation/gps/cdi-deflection", this, &DCLGPS::GetCDIDeflection);
254 fgTie("/instrumentation/gps/to-flag", this, &DCLGPS::GetToFlag);
257 void DCLGPS::unbind() {
258 fgUntie("/instrumentation/gps/waypoint-alert");
259 fgUntie("/instrumentation/gps/leg-mode");
260 fgUntie("/instrumentation/gps/obs-mode");
261 fgUntie("/instrumentation/gps/approach-arm");
262 fgUntie("/instrumentation/gps/approach-active");
263 fgUntie("/instrumentation/gps/cdi-deflection");
266 void DCLGPS::update(double dt) {
267 //cout << "update called!\n";
269 _lon = _lon_node->getDoubleValue() * SG_DEGREES_TO_RADIANS;
270 _lat = _lat_node->getDoubleValue() * SG_DEGREES_TO_RADIANS;
271 _alt = _alt_node->getDoubleValue();
272 _groundSpeed_kts = _grnd_speed_node->getDoubleValue();
273 _groundSpeed_ms = _groundSpeed_kts * 0.5144444444;
274 _track = _true_track_node->getDoubleValue();
275 _magTrackDeg = _mag_track_node->getDoubleValue();
276 // Note - we can depriciate _gpsLat and _gpsLon if we implement error handling in FG
277 // gps code and not our own.
280 // Check for abnormal position slew
281 if(GetGreatCircleDistance(_gpsLat, _gpsLon, _checkLat, _checkLon) > 1.0) {
282 OrientateToActiveFlightPlan();
287 // TODO - check for unit power before running this.
288 if(!_powerOnTimerSet) {
292 // Check if an alarm timer has expired
294 if(_alarmTime.hr() == atoi(fgGetString("/instrumentation/clock/indicated-hour"))
295 && _alarmTime.min() == atoi(fgGetString("/instrumentation/clock/indicated-min"))) {
296 _messageStack.push_back("*Timer Expired");
302 if(_groundSpeed_kts > 30.0) {
304 string th = fgGetString("/instrumentation/clock/indicated-hour");
305 string tm = fgGetString("/instrumentation/clock/indicated-min");
306 if(th.size() == 1) th = "0" + th;
307 if(tm.size() == 1) tm = "0" + tm;
308 _departureTimeString = th + tm;
311 // TODO - check - is this prone to drift error over time?
312 // Should we difference the departure and current times?
313 // What about when the user resets the time of day from the menu?
317 _time->update(_gpsLon * SG_DEGREES_TO_RADIANS, _gpsLat * SG_DEGREES_TO_RADIANS, 0, 0);
318 // FIXME - currently all the below assumes leg mode and no DTO or OBS cancelled.
319 if(_activeFP->IsEmpty()) {
320 // Not sure if we need to reset these each update or only when fp altered
321 _activeWaypoint.id.clear();
323 } else if(_activeFP->waypoints.size() == 1) {
324 _activeWaypoint.id.clear();
327 if(_activeWaypoint.id.empty() || _fromWaypoint.id.empty()) {
328 //cout << "Error, in leg mode with flightplan of 2 or more waypoints, but either active or from wp is NULL!\n";
329 OrientateToActiveFlightPlan();
333 if(_approachLoaded) {
334 if(!_approachReallyArmed && !_approachActive) {
335 // arm if within 30nm of airport.
336 // TODO - let user cancel approach arm using external GPS-APR switch
338 const FGAirport* ap = FindFirstAptById(_approachID, multi, true);
340 double d = GetGreatCircleDistance(_gpsLat, _gpsLon, ap->getLatitude() * SG_DEGREES_TO_RADIANS, ap->getLongitude() * SG_DEGREES_TO_RADIANS);
343 _approachReallyArmed = true;
344 _messageStack.push_back("*Press ALT To Set Baro");
345 // Not sure what we do if the user has already set CDI to 0.3 nm?
346 _targetCdiScaleIndex = 1;
347 if(_currentCdiScaleIndex == 1) {
349 } else if(_currentCdiScaleIndex == 0) {
350 _sourceCdiScaleIndex = 0;
351 _cdiScaleTransition = true;
352 _cdiTransitionTime = 30.0;
353 _currentCdiScale = _cdiScales[_currentCdiScaleIndex];
358 // Check for approach active - we can only activate approach if it is really armed.
359 if(_activeWaypoint.appType == GPS_FAF) {
360 //cout << "Active waypoint is FAF, id is " << _activeWaypoint.id << '\n';
361 if(GetGreatCircleDistance(_gpsLat, _gpsLon, _activeWaypoint.lat, _activeWaypoint.lon) <= 2.0 && !_obsMode) {
362 // Assume heading is OK for now
363 _approachArm = false; // TODO - check - maybe arm is left on when actv comes on?
364 _approachReallyArmed = false;
365 _approachActive = true;
366 _targetCdiScaleIndex = 2;
367 if(_currentCdiScaleIndex == 2) {
369 } else if(_currentCdiScaleIndex == 1) {
370 _sourceCdiScaleIndex = 1;
371 _cdiScaleTransition = true;
372 _cdiTransitionTime = 30.0; // TODO - compress it if time to FAF < 30sec
373 _currentCdiScale = _cdiScales[_currentCdiScaleIndex];
375 // Abort going active?
376 _approachActive = false;
383 // CDI scale transition stuff
384 if(_cdiScaleTransition) {
385 if(fabs(_currentCdiScale - _cdiScales[_targetCdiScaleIndex]) < 0.001) {
386 _currentCdiScale = _cdiScales[_targetCdiScaleIndex];
387 _currentCdiScaleIndex = _targetCdiScaleIndex;
388 _cdiScaleTransition = false;
390 double scaleDiff = (_targetCdiScaleIndex > _sourceCdiScaleIndex
391 ? _cdiScales[_sourceCdiScaleIndex] - _cdiScales[_targetCdiScaleIndex]
392 : _cdiScales[_targetCdiScaleIndex] - _cdiScales[_sourceCdiScaleIndex]);
393 //cout << "ScaleDiff = " << scaleDiff << '\n';
394 if(_targetCdiScaleIndex > _sourceCdiScaleIndex) {
395 // Scaling down eg. 5nm -> 1nm
396 _currentCdiScale -= (scaleDiff * dt / _cdiTransitionTime);
397 if(_currentCdiScale < _cdiScales[_targetCdiScaleIndex]) {
398 _currentCdiScale = _cdiScales[_targetCdiScaleIndex];
399 _currentCdiScaleIndex = _targetCdiScaleIndex;
400 _cdiScaleTransition = false;
403 _currentCdiScale += (scaleDiff * dt / _cdiTransitionTime);
404 if(_currentCdiScale > _cdiScales[_targetCdiScaleIndex]) {
405 _currentCdiScale = _cdiScales[_targetCdiScaleIndex];
406 _currentCdiScaleIndex = _targetCdiScaleIndex;
407 _cdiScaleTransition = false;
410 //cout << "_currentCdiScale = " << _currentCdiScale << '\n';
413 _currentCdiScale = _cdiScales[_currentCdiScaleIndex];
417 // Urgh - I've been setting the heading bug based on DTK,
418 // bug I think it should be based on heading re. active waypoint
419 // based on what the sim does after the final waypoint is passed.
420 // (DTK remains the same, but if track is held == DTK heading bug
421 // reverses to from once wp is passed).
423 if(_fromWaypoint != NULL) {
424 // TODO - how do we handle the change of track with distance over long legs?
425 _dtkTrue = GetGreatCircleCourse(_fromWaypoint->lat, _fromWaypoint->lon, _activeWaypoint->lat, _activeWaypoint->lon) * SG_RADIANS_TO_DEGREES;
426 _dtkMag = GetMagHeadingFromTo(_fromWaypoint->lat, _fromWaypoint->lon, _activeWaypoint->lat, _activeWaypoint->lon);
427 // Don't change the heading bug if speed is too low otherwise it flickers to/from at rest
428 if(_groundSpeed_ms > 5) {
429 //cout << "track = " << _track << ", dtk = " << _dtkTrue << '\n';
430 double courseDev = _track - _dtkTrue;
431 //cout << "courseDev = " << courseDev << ", normalized = ";
432 SG_NORMALIZE_RANGE(courseDev, -180.0, 180.0);
433 //cout << courseDev << '\n';
434 _headingBugTo = (fabs(courseDev) > 90.0 ? false : true);
439 // TODO - in DTO operation the position of initiation of DTO defines the "from waypoint".
442 if(!_activeWaypoint.id.empty()) {
443 double hdgTrue = GetGreatCircleCourse(_gpsLat, _gpsLon, _activeWaypoint.lat, _activeWaypoint.lon) * SG_RADIANS_TO_DEGREES;
444 if(_groundSpeed_ms > 5) {
445 //cout << "track = " << _track << ", hdgTrue = " << hdgTrue << '\n';
446 double courseDev = _track - hdgTrue;
447 //cout << "courseDev = " << courseDev << ", normalized = ";
448 SG_NORMALIZE_RANGE(courseDev, -180.0, 180.0);
449 //cout << courseDev << '\n';
450 _headingBugTo = (fabs(courseDev) > 90.0 ? false : true);
452 if(!_fromWaypoint.id.empty()) {
453 _dtkTrue = GetGreatCircleCourse(_fromWaypoint.lat, _fromWaypoint.lon, _activeWaypoint.lat, _activeWaypoint.lon) * SG_RADIANS_TO_DEGREES;
454 _dtkMag = GetMagHeadingFromTo(_fromWaypoint.lat, _fromWaypoint.lon, _activeWaypoint.lat, _activeWaypoint.lon);
461 _dist2Act = GetGreatCircleDistance(_gpsLat, _gpsLon, _activeWaypoint.lat, _activeWaypoint.lon) * SG_NM_TO_METER;
462 if(_groundSpeed_ms > 10.0) {
463 _eta = _dist2Act / _groundSpeed_ms;
464 if(_eta <= 36) { // TODO - this is slightly different if turn anticipation is enabled.
466 _waypointAlert = true; // TODO - not if the from flag is set.
470 // Check if we should sequence to next leg.
471 // Perhaps this should be done on distance instead, but 60s time (about 1 - 2 nm) seems reasonable for now.
472 //double reverseHeading = GetGreatCircleCourse(_activeWaypoint->lat, _activeWaypoint->lon, _fromWaypoint->lat, _fromWaypoint->lon);
473 // Hack - let's cheat and do it on heading bug for now. TODO - that stops us 'cutting the corner'
474 // when we happen to approach the inside turn of a waypoint - we should probably sequence at the midpoint
475 // of the heading difference between legs in this instance.
476 int idx = GetActiveWaypointIndex();
477 bool finalLeg = (idx == (int)(_activeFP->waypoints.size()) - 1 ? true : false);
478 bool finalDto = (_dto && idx == -1); // Dto operation to a waypoint not in the flightplan - we don't sequence in this instance
481 // Do nothing - not sure if Dto should switch off when arriving at the final waypoint of a flightplan
482 } else if(finalDto) {
484 } else if(_activeWaypoint.appType == GPS_MAP) {
485 // Don't sequence beyond the missed approach point
486 //cout << "ACTIVE WAYPOINT is MAP - not sequencing!!!!!\n";
488 //cout << "Sequencing...\n";
489 _fromWaypoint = _activeWaypoint;
490 _activeWaypoint = *_activeFP->waypoints[idx + 1];
492 // Course alteration message format is dependent on whether we are slaved HSI/CDI indicator or not.
494 if(fgGetBool("/instrumentation/nav[0]/slaved-to-gps")) {
495 // TODO - avoid the hardwiring on nav[0]
496 s = "Adj Nav Crs to ";
498 string s = "GPS Course is ";
500 double d = GetMagHeadingFromTo(_fromWaypoint.lat, _fromWaypoint.lon, _activeWaypoint.lat, _activeWaypoint.lon);
501 while(d < 0.0) d += 360.0;
502 while(d >= 360.0) d -= 360.0;
504 snprintf(buf, 4, "%03i", (int)(d + 0.5));
506 _messageStack.push_back(s);
508 _waypointAlert = false;
516 // First attempt at a sensible cross-track correction calculation
517 // Uh? - I think this is implemented further down the file!
518 if(_fromWaypoint != NULL) {
521 _crosstrackDist = 0.0;
528 Expand a SIAP ident to the full procedure name (as shown on the approach chart).
529 NOTE: Some of this is inferred from data, some is from documentation.
531 Example expansions from ARINC 424-18 [and the airport they're taken from]:
532 "R10LY" <--> "RNAV (GPS) Y RWY 10 L" [KBOI]
533 "R10-Z" <--> "RNAV (GPS) Z RWY 10" [KHTO]
534 "S25" <--> "VOR or GPS RWY 25" [KHHR]
535 "P20" <--> "GPS RWY 20" [KDAN]
536 "NDB-B" <--> "NDB or GPS-B" [KDAW]
537 "NDBC" <--> "NDB or GPS-C" [KEMT]
538 "VDMA" <--> "VOR/DME or GPS-A" [KDAW]
539 "VDM-A" <--> "VOR/DME or GPS-A" [KEAG]
540 "VDMB" <--> "VOR/DME or GPS-B" [KDKX]
541 "VORA" <--> "VOR or GPS-A" [KEMT]
543 It seems that there are 2 basic types of expansions; those that include
544 the runway and those that don't. Of those that don't, it seems that 2
545 different positions within the string to encode the identifying letter
546 are used, i.e. with a dash and without.
548 string DCLGPS::ExpandSIAPIdent(const string& ident) {
550 bool has_rwy = false;
553 case 'N': name = "NDB or GPS"; has_rwy = false; break;
554 case 'P': name = "GPS"; has_rwy = true; break;
555 case 'R': name = "RNAV (GPS)"; has_rwy = true; break;
556 case 'S': name = "VOR or GPS"; has_rwy = true; break;
558 if(ident[1] == 'D') name = "VOR/DME or GPS";
559 else name = "VOR or GPS";
562 default: // TODO output a log message
567 // Add the identifying letter if present
568 if(ident.size() == 5) {
575 name += ident.substr(1, 2);
577 // Add a left/right/centre indication if present.
578 if(ident.size() > 3) {
579 if((ident[3] != '-') && (ident[3] != ' ')) { // Early versions of the spec allowed a blank instead of a dash so check for both
585 // Add the identifying letter, which I *think* should always be present, but seems to be inconsistent as to whether a dash is used.
586 if(ident.size() == 5) {
589 } else if(ident.size() == 4) {
601 Load instrument approaches from an ARINC 424 file.
602 Tested on ARINC 424-18.
603 Known / current best guess at the format:
604 Col 1: Always 'S'. If it isn't, ditch it.
605 Col 2-4: "Customer area" code, eg "USA", "CAN". I think that CAN is used for Alaska.
606 Col 5: Section code. Used in conjunction with sub-section code. Definitions are with sub-section code.
608 Col 7-10: ICAO (or FAA) airport ident. Left justified if < 4 chars.
609 Col 11-12: Based on ICAO geographical region.
610 Col 13: Sub-section code. Used in conjunction with section code.
611 "HD/E/F" => Helicopter record.
612 "HS" => Helicopter minimum safe altitude.
613 "PA" => Airport record.
614 "PF" => Approach segment.
615 "PG" => Runway record.
616 "PP" => Path point record. ???
617 "PS" => MSA record (minimum safe altitude).
619 ------ The following is for "PF", approach segment -------
621 Col 14-19: SIAP ident for this approach (left justified). This is a standardised abbreviated approach name.
622 e.g. "R10LZ" expands to "RNAV (GPS) Z RWY 10 L". See the comment block for ExpandSIAPIdent for full details.
623 Col 20: Route type. This is tricky - I don't have full documentation and am having to guess a bit.
624 'A' => Arrival route? This seems to be used to encode arrival routes from the IAF to the approach proper.
625 Note that the final fix of the arrival route is duplicated in the approach proper.
626 'D' => VOR/DME or GPS
628 'P' => GPS (ARINC 424-18), GPS and RNAV (GPS) (ARINC 424-15 and before).
629 'R' => RNAV (GPS) (ARINC 424-18).
631 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.
633 Col 27-29: Sequence number - position within the route segment. Rule: 10-20-30 etc.
634 Col 30-34: Fix identifer. The ident of the waypoint.
635 Col 35-36: ICAO geographical region code. I think we can ignore this for now.
636 Col 37: Section code - ??? I don't know what this means
637 Col 38 Subsection code - ??? ditto - no idea!
638 Col 40: Waypoint type.
639 'A' => Airport as waypoint
640 'E' => Essential waypoint (e.g. change of heading at this waypoint, etc).
641 'G' => Runway or helipad as waypoint
642 'H' => Heliport as waypoint
643 'N' => NDB as waypoint
644 'P' => Phantom waypoint (not sure if this is used in rev 18?)
645 'V' => VOR as waypoint
646 Col 41: Waypoint type.
647 'B' => Flyover, approach transition, or final approach.
648 'E' => end of route segment (transition waypoint). (Actually "End of terminal procedure route type" in the docs).
649 'N' => ??? I've also seen 'N' in this column, but don't know what it indicates.
651 Col 43: Waypoint type. May also be blank when none of the below.
652 'A' => Initial approach fix (IAF)
653 'F' => Final approach fix
655 'I' => Final approach course fix
656 'M' => Missed approach point
657 '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!
658 ??? Possibly procedure turn?
659 'C' => ??? This is also present in the data, but missing from the docs. Is at airport 00R.
660 Col 107-111 MSA center fix. We can ignore this.
662 void DCLGPS::LoadApproachData() {
666 const GPSWaypoint* cwp;
669 SGPath path = globals->get_fg_root();
670 path.append("Navaids/rnav.dat");
671 fin.open(path.c_str(), ios::in);
673 //cout << "Unable to open input file " << path.c_str() << '\n';
676 //cout << "Opened " << path.c_str() << " for reading\n";
681 string apt_ident; // This gets set to the ICAO code of the current airport being processed.
682 string iap_ident; // The abbreviated name of the current approach being processed.
683 string wp_ident; // The ident of the waypoint of the current line
684 string last_apt_ident;
685 string last_iap_ident;
686 string last_wp_ident;
687 // There is no need to save the full name - it can be generated on the fly from the abbreviated name as and when needed.
688 bool apt_in_progress = false; // Set true whilst loading all the approaches for a given airport.
689 bool iap_in_progress = false; // Set true whilst loading a given approach.
690 bool iap_error = false; // Set true if there is an error loading a given approach.
691 bool route_in_progress = false; // Set true when we are loading a "route" segment of the approach.
692 int last_sequence_number = 0; // Position within the route, rule (rev 18): 10, 20, 30 etc.
694 char last_route_type = 0;
696 char waypoint_fix_type; // This is the waypoint type from col 43, i.e. the type of fix. May be blank.
701 unsigned int nLoaded = 0;
702 unsigned int nErrors = 0;
704 //for(i=0; i<64; ++i) {
706 fin.getline(tmp, 256);
707 //s = Fake_rnav_dat[i];
709 if(s.size() < 132) continue;
710 if(s[0] == 'S') { // Valid line
711 string country_code = s.substr(1, 3);
712 if(country_code == "USA") { // For now we'll stick to US procedures in case there are unknown gotchas with others
713 if(s[4] == 'P') { // Includes approaches.
714 if(s[12] == 'A') { // Airport record
715 apt_ident = s.substr(6, 4);
716 // Trim any whitespace from the ident. The ident is left justified,
717 // so any space will be at the end.
718 if(apt_ident[3] == ' ') apt_ident = apt_ident.substr(0, 3);
719 // I think that all idents are either 3 or 4 chars - could check this though!
720 if(!apt_in_progress) {
721 last_apt_ident = apt_ident;
724 if(last_apt_ident != apt_ident) {
725 if(iap_in_progress) {
727 //cout << "ERROR: Unable to load approach " << iap->_ident << " at " << iap->_aptIdent << '\n';
730 _np_iap[iap->_aptIdent].push_back(iap);
731 //cout << "** Loaded " << iap->_aptIdent << "\t" << iap->_ident << '\n';
734 iap_in_progress = false;
737 last_apt_ident = apt_ident;
740 } else if(s[12] == 'F') { // Approach segment
741 if(apt_in_progress) {
742 iap_ident = s.substr(13, 6);
743 // Trim any whitespace from the RH end.
745 if(iap_ident[5-j] == ' ') {
746 iap_ident = iap_ident.substr(0, 5-j);
748 // It's important to break here, since earlier versions of ARINC 424 allowed spaces in the ident.
752 if(iap_in_progress) {
753 if(iap_ident != last_iap_ident) {
754 // This is a new approach - store the last one and trigger
755 // starting afresh by setting the in progress flag to false.
757 //cout << "ERROR: Unable to load approach " << iap->_ident << " at " << iap->_aptIdent << '\n';
760 _np_iap[iap->_aptIdent].push_back(iap);
761 //cout << "Loaded " << iap->_aptIdent << "\t" << iap->_ident << '\n';
764 iap_in_progress = false;
767 if(!iap_in_progress) {
769 iap->_aptIdent = apt_ident;
770 iap->_ident = iap_ident;
771 iap->_name = ExpandSIAPIdent(iap_ident); // I suspect that it's probably better to just store idents, and to expand the names as needed.
772 // Note, we haven't set iap->_rwyStr yet.
773 last_iap_ident = iap_ident;
774 iap_in_progress = true;
780 sequence_number = atoi(s.substr(26,3).c_str());
781 wp_ident = s.substr(29, 5);
782 waypoint_fix_type = s[42];
783 // Trim any whitespace from the RH end
785 if(wp_ident[4-j] == ' ') {
786 wp_ident = wp_ident.substr(0, 4-j);
792 // Ignore lines with no waypoint ID for now - these are normally part of the
793 // missed approach procedure, and we don't use them in the KLN89.
794 if(!wp_ident.empty()) {
795 // Make a local copy of the waypoint for now, since we're not yet sure if we'll be using it
798 bool wp_error = false;
799 if(w.id.substr(0, 2) == "RW" && waypoint_fix_type == 'M') {
800 // Assume that this is a missed-approach point based on the runway number, which appears to be standard for most approaches.
801 // Note: Currently fgFindAirportID returns NULL on error, but getRunwayByIdent throws an exception.
802 const FGAirport* apt = fgFindAirportID(iap->_aptIdent);
806 rwystr = w.id.substr(2, 2);
807 // TODO - sanity check the rwystr at this point to ensure we have a double digit number
808 if(w.id.size() > 4) {
809 if(w.id[4] == 'L' || w.id[4] == 'C' || w.id[4] == 'R') {
813 FGRunway* rwy = apt->getRunwayByIdent(rwystr);
814 w.lat = rwy->begin().getLatitudeRad();
815 w.lon = rwy->begin().getLongitudeRad();
816 } catch(const sg_exception&) {
817 SG_LOG(SG_GENERAL, SG_WARN, "Unable to find runway " << w.id.substr(2, 2) << " at airport " << iap->_aptIdent);
818 //cout << "Unable to find runway " << w.id.substr(2, 2) << " at airport " << iap->_aptIdent << " ( w.id = " << w.id << ", rwystr = " << rwystr << " )\n";
825 cwp = FindFirstByExactId(w.id);
832 switch(waypoint_fix_type) {
833 case 'A': w.appType = GPS_IAF; break;
834 case 'F': w.appType = GPS_FAF; break;
835 case 'H': w.appType = GPS_MAHP; break;
836 case 'I': w.appType = GPS_IAP; break;
837 case 'M': w.appType = GPS_MAP; break;
838 case ' ': w.appType = GPS_APP_NONE; break;
839 //default: cout << "Unknown waypoint_fix_type: \'" << waypoint_fix_type << "\' [" << apt_ident << ", " << iap_ident << "]\n";
843 //cout << "Unable to find waypoint " << w.id << " [" << apt_ident << ", " << iap_ident << "]\n";
848 if(route_in_progress) {
849 if(sequence_number > last_sequence_number) {
850 // TODO - add a check for runway numbers
851 // Check for the waypoint ID being the same as the previous line.
852 // This is often the case for the missed approach holding point.
853 if(wp_ident == last_wp_ident) {
854 if(waypoint_fix_type == 'H') {
855 if(!iap->_IAP.empty()) {
856 if(iap->_IAP[iap->_IAP.size() - 1]->appType == GPS_APP_NONE) {
857 iap->_IAP[iap->_IAP.size() - 1]->appType = GPS_MAHP;
859 //cout << "Waypoint is MAHP and another type! " << w.id << " [" << apt_ident << ", " << iap_ident << "]\n";
864 // Create a new waypoint on the heap, copy the local copy into it, and push it onto the approach.
865 wp = new GPSWaypoint;
867 if(route_type == 'A') {
868 fp->waypoints.push_back(wp);
870 iap->_IAP.push_back(wp);
873 } else if(sequence_number == last_sequence_number) {
874 // This seems to happen once per final approach route - one of the waypoints
875 // is duplicated with the same sequence number - I'm not sure what information
876 // the second line give yet so ignore it for now.
877 // TODO - figure this out!
879 // Finalise the current route and start a new one
881 // Finalise the current route
882 if(last_route_type == 'A') {
883 // Push the flightplan onto the approach
884 iap->_approachRoutes.push_back(fp);
886 // All the waypoints get pushed individually - don't need to do it.
889 // There are basically 2 possibilities here - either it's one of the arrival transitions,
890 // or it's the core final approach course.
891 wp = new GPSWaypoint;
893 if(route_type == 'A') { // It's one of the arrival transition(s)
894 fp = new GPSFlightPlan;
895 fp->waypoints.push_back(wp);
897 iap->_IAP.push_back(wp);
899 route_in_progress = true;
902 // Start a new route.
903 // There are basically 2 possibilities here - either it's one of the arrival transitions,
904 // or it's the core final approach course.
905 wp = new GPSWaypoint;
907 if(route_type == 'A') { // It's one of the arrival transition(s)
908 fp = new GPSFlightPlan;
909 fp->waypoints.push_back(wp);
911 iap->_IAP.push_back(wp);
913 route_in_progress = true;
915 last_route_type = route_type;
916 last_wp_ident = wp_ident;
917 last_sequence_number = sequence_number;
921 // ERROR - no airport record read.
925 // Check and finalise any approaches in progress
926 // TODO - sanity check that the approach has all the required elements
927 if(iap_in_progress) {
928 // This is a new approach - store the last one and trigger
929 // starting afresh by setting the in progress flag to false.
931 //cout << "ERROR: Unable to load approach " << iap->_ident << " at " << iap->_aptIdent << '\n';
934 _np_iap[iap->_aptIdent].push_back(iap);
935 //cout << "* Loaded " << iap->_aptIdent << "\t" << iap->_ident << '\n';
938 iap_in_progress = false;
945 // If we get to the end of the file, load any approach that is still in progress
946 // TODO - sanity check that the approach has all the required elements
947 if(iap_in_progress) {
949 //cout << "ERROR: Unable to load approach " << iap->_ident << " at " << iap->_aptIdent << '\n';
952 _np_iap[iap->_aptIdent].push_back(iap);
953 //cout << "*** Loaded " << iap->_aptIdent << "\t" << iap->_ident << '\n';
958 //cout << "Done loading approach database\n";
959 //cout << "Loaded: " << nLoaded << '\n';
960 //cout << "Failed: " << nErrors << '\n';
965 GPSWaypoint* DCLGPS::GetActiveWaypoint() {
966 return &_activeWaypoint;
970 float DCLGPS::GetDistToActiveWaypoint() {
974 // I don't yet fully understand all the gotchas about where to source time from.
975 // This function sets the initial timer before the clock exports properties
976 // and the one below uses the clock to be consistent with the rest of the code.
977 // It might change soonish...
978 void DCLGPS::SetPowerOnTimer() {
979 struct tm *t = globals->get_time_params()->getGmt();
980 _powerOnTime.set_hr(t->tm_hour);
981 _powerOnTime.set_min(t->tm_min);
982 _powerOnTimerSet = true;
985 void DCLGPS::ResetPowerOnTimer() {
986 _powerOnTime.set_hr(atoi(fgGetString("/instrumentation/clock/indicated-hour")));
987 _powerOnTime.set_min(atoi(fgGetString("/instrumentation/clock/indicated-min")));
988 _powerOnTimerSet = true;
991 double DCLGPS::GetCDIDeflection() const {
992 double xtd = CalcCrossTrackDeviation(); //nm
993 return((xtd / _currentCdiScale) * 5.0 * 2.5 * -1.0);
996 void DCLGPS::DtoInitiate(const string& s) {
997 const GPSWaypoint* wp = FindFirstByExactId(s);
999 // TODO - Currently we start DTO operation unconditionally, regardless of which mode we are in.
1000 // In fact, the following rules apply:
1001 // In LEG mode, start DTO as we currently do.
1002 // In OBS mode, set the active waypoint to the requested waypoint, and then:
1003 // If the KLN89 is not connected to an external HSI or CDI, set the OBS course to go direct to the waypoint.
1004 // If the KLN89 *is* connected to an external HSI or CDI, it cannot set the course itself, and will display
1005 // a scratchpad message with the course to set manually on the HSI/CDI.
1006 // In both OBS cases, leave _dto false, since we don't need the virtual waypoint created.
1008 _activeWaypoint = *wp;
1009 _fromWaypoint.lat = _gpsLat;
1010 _fromWaypoint.lon = _gpsLon;
1011 _fromWaypoint.type = GPS_WP_VIRT;
1012 _fromWaypoint.id = "_DTOWP_";
1015 // TODO - Should bring up the user waypoint page.
1020 void DCLGPS::DtoCancel() {
1022 // i.e. don't bother reorientating if we're just cancelling a DTO button press
1023 // without having previously initiated DTO.
1024 OrientateToActiveFlightPlan();
1029 void DCLGPS::ToggleOBSMode() {
1030 _obsMode = !_obsMode;
1032 // We need to set the OBS heading. The rules for this are:
1034 // If the KLN89 is connected to an external HSI or CDI, then the OBS heading must be set
1035 // to the OBS radial on the external indicator, and cannot be changed in the KLN89.
1037 // If the KLN89 is not connected to an external indicator, then:
1038 // If there is an active waypoint, the OBS heading is set such that the
1039 // deviation indicator remains at the same deviation (i.e. set to DTK,
1040 // although there may be some small difference).
1042 // If there is not an active waypoint, I am not sure what value should be
1045 if(fgGetBool("/instrumentation/nav/slaved-to-gps")) {
1046 _obsHeading = static_cast<int>(fgGetDouble("/instrumentation/nav/radials/selected-deg") + 0.5);
1047 } else if(!_activeWaypoint.id.empty()) {
1048 // NOTE: I am not sure if this is completely correct. There are sometimes small differences
1049 // between DTK and default OBS heading in the simulator (~ 1 or 2 degrees). It might also
1050 // be that OBS heading should be stored as float and only displayed as int, in order to maintain
1051 // the initial bar deviation?
1052 _obsHeading = static_cast<int>(_dtkMag + 0.5);
1053 //cout << "_dtkMag = " << _dtkMag << ", _dtkTrue = " << _dtkTrue << ", _obsHeading = " << _obsHeading << '\n';
1055 // TODO - what should we really do here?
1059 // Valid OBS heading values are 0 -> 359 degrees inclusive (from kln89 simulator).
1060 if(_obsHeading > 359) _obsHeading -= 360;
1061 if(_obsHeading < 0) _obsHeading += 360;
1063 // TODO - the _fromWaypoint location will change as the OBS heading changes.
1064 // Might need to store the OBS initiation position somewhere in case it is needed again.
1065 SetOBSFromWaypoint();
1069 // Set the _fromWaypoint position based on the active waypoint and OBS radial.
1070 void DCLGPS::SetOBSFromWaypoint() {
1071 if(!_obsMode) return;
1072 if(_activeWaypoint.id.empty()) return;
1074 // TODO - base the 180 deg correction on the to/from flag.
1075 _fromWaypoint = GetPositionOnMagRadial(_activeWaypoint, 10, _obsHeading + 180.0);
1076 _fromWaypoint.type = GPS_WP_VIRT;
1077 _fromWaypoint.id = "_OBSWP_";
1080 void DCLGPS::CDIFSDIncrease() {
1081 if(_currentCdiScaleIndex == 0) {
1082 _currentCdiScaleIndex = _cdiScales.size() - 1;
1084 _currentCdiScaleIndex--;
1088 void DCLGPS::CDIFSDDecrease() {
1089 _currentCdiScaleIndex++;
1090 if(_currentCdiScaleIndex == _cdiScales.size()) {
1091 _currentCdiScaleIndex = 0;
1095 void DCLGPS::DrawChar(char c, int field, int px, int py, bool bold) {
1098 void DCLGPS::DrawText(const string& s, int field, int px, int py, bool bold) {
1101 void DCLGPS::SetBaroUnits(int n, bool wrap) {
1103 _baroUnits = (GPSPressureUnits)(wrap ? 3 : 1);
1105 _baroUnits = (GPSPressureUnits)(wrap ? 1 : 3);
1107 _baroUnits = (GPSPressureUnits)n;
1111 void DCLGPS::CreateDefaultFlightPlans() {}
1113 // Get the time to the active waypoint in seconds.
1114 // Returns -1 if groundspeed < 30 kts
1115 double DCLGPS::GetTimeToActiveWaypoint() {
1116 if(_groundSpeed_kts < 30.0) {
1123 // Get the time to the final waypoint in seconds.
1124 // Returns -1 if groundspeed < 30 kts
1125 double DCLGPS::GetETE() {
1126 if(_groundSpeed_kts < 30.0) {
1129 // TODO - handle OBS / DTO operation appropriately
1130 if(_activeFP->waypoints.empty()) {
1133 return(GetTimeToWaypoint(_activeFP->waypoints[_activeFP->waypoints.size() - 1]->id));
1138 // Get the time to a given waypoint (spec'd by ID) in seconds.
1139 // returns -1 if groundspeed is less than 30kts.
1140 // If the waypoint is an unreached part of the active flight plan the time will be via each leg.
1141 // otherwise it will be a direct-to time.
1142 double DCLGPS::GetTimeToWaypoint(const string& id) {
1143 if(_groundSpeed_kts < 30.0) {
1148 int n1 = GetActiveWaypointIndex();
1149 int n2 = GetWaypointIndex(id);
1152 for(unsigned int i=n1+1; i<_activeFP->waypoints.size(); ++i) {
1153 GPSWaypoint* wp1 = _activeFP->waypoints[i-1];
1154 GPSWaypoint* wp2 = _activeFP->waypoints[i];
1155 double distm = GetGreatCircleDistance(wp1->lat, wp1->lon, wp2->lat, wp2->lon) * SG_NM_TO_METER;
1156 eta += (distm / _groundSpeed_ms);
1159 } else if(id == _activeWaypoint.id) {
1162 const GPSWaypoint* wp = FindFirstByExactId(id);
1163 if(wp == NULL) return(-1.0);
1164 double distm = GetGreatCircleDistance(_gpsLat, _gpsLon, wp->lat, wp->lon);
1166 return(distm / _groundSpeed_ms);
1168 return(-1.0); // Hopefully we never get here!
1171 // Returns magnetic great-circle heading
1172 // TODO - document units.
1173 float DCLGPS::GetHeadingToActiveWaypoint() {
1174 if(_activeWaypoint.id.empty()) {
1177 double h = GetMagHeadingFromTo(_gpsLat, _gpsLon, _activeWaypoint.lat, _activeWaypoint.lon);
1178 while(h <= 0.0) h += 360.0;
1179 while(h > 360.0) h -= 360.0;
1184 // Returns magnetic great-circle heading
1185 // TODO - what units?
1186 float DCLGPS::GetHeadingFromActiveWaypoint() {
1187 if(_activeWaypoint.id.empty()) {
1190 double h = GetMagHeadingFromTo(_activeWaypoint.lat, _activeWaypoint.lon, _gpsLat, _gpsLon);
1191 while(h <= 0.0) h += 360.0;
1192 while(h > 360.0) h -= 360.0;
1197 void DCLGPS::ClearFlightPlan(int n) {
1198 for(unsigned int i=0; i<_flightPlans[n]->waypoints.size(); ++i) {
1199 delete _flightPlans[n]->waypoints[i];
1201 _flightPlans[n]->waypoints.clear();
1204 void DCLGPS::ClearFlightPlan(GPSFlightPlan* fp) {
1205 for(unsigned int i=0; i<fp->waypoints.size(); ++i) {
1206 delete fp->waypoints[i];
1208 fp->waypoints.clear();
1211 int DCLGPS::GetActiveWaypointIndex() {
1212 for(unsigned int i=0; i<_flightPlans[0]->waypoints.size(); ++i) {
1213 if(_flightPlans[0]->waypoints[i]->id == _activeWaypoint.id) return((int)i);
1218 int DCLGPS::GetWaypointIndex(const string& id) {
1219 for(unsigned int i=0; i<_flightPlans[0]->waypoints.size(); ++i) {
1220 if(_flightPlans[0]->waypoints[i]->id == id) return((int)i);
1225 void DCLGPS::OrientateToFlightPlan(GPSFlightPlan* fp) {
1226 //cout << "Orientating...\n";
1227 //cout << "_lat = " << _lat << ", _lon = " << _lon << ", _gpsLat = " << _gpsLat << ", gpsLon = " << _gpsLon << '\n';
1229 _activeWaypoint.id.clear();
1232 _navFlagged = false;
1233 if(fp->waypoints.size() == 1) {
1234 // TODO - may need to flag nav here if not dto or obs, or possibly handle it somewhere else.
1235 _activeWaypoint = *fp->waypoints[0];
1236 _fromWaypoint.id.clear();
1238 // FIXME FIXME FIXME
1239 _fromWaypoint = *fp->waypoints[0];
1240 _activeWaypoint = *fp->waypoints[1];
1241 double dmin = 1000000; // nm!!
1242 // For now we will simply start on the leg closest to our current position.
1243 // It's possible that more fancy algorithms may take either heading or track
1244 // into account when setting inital leg - I'm not sure.
1245 // This method should handle most cases perfectly OK though.
1246 for(unsigned int i = 1; i < fp->waypoints.size(); ++i) {
1247 //cout << "Pass " << i << ", dmin = " << dmin << ", leg is " << fp->waypoints[i-1]->id << " to " << fp->waypoints[i]->id << '\n';
1248 // First get the cross track correction.
1249 double d0 = fabs(CalcCrossTrackDeviation(*fp->waypoints[i-1], *fp->waypoints[i]));
1250 // That is the shortest distance away we could be though - check for
1251 // longer distances if we are 'off the end' of the leg.
1252 double ht1 = GetGreatCircleCourse(fp->waypoints[i-1]->lat, fp->waypoints[i-1]->lon,
1253 fp->waypoints[i]->lat, fp->waypoints[i]->lon)
1254 * SG_RADIANS_TO_DEGREES;
1255 // not simply the reverse of the above due to great circle navigation.
1256 double ht2 = GetGreatCircleCourse(fp->waypoints[i]->lat, fp->waypoints[i]->lon,
1257 fp->waypoints[i-1]->lat, fp->waypoints[i-1]->lon)
1258 * SG_RADIANS_TO_DEGREES;
1259 double hw1 = GetGreatCircleCourse(_gpsLat, _gpsLon,
1260 fp->waypoints[i]->lat, fp->waypoints[i]->lon)
1261 * SG_RADIANS_TO_DEGREES;
1262 double hw2 = GetGreatCircleCourse(_gpsLat, _gpsLon,
1263 fp->waypoints[i-1]->lat, fp->waypoints[i-1]->lon)
1264 * SG_RADIANS_TO_DEGREES;
1265 double h1 = ht1 - hw1;
1266 double h2 = ht2 - hw2;
1267 //cout << "d0, h1, h2 = " << d0 << ", " << h1 << ", " << h2 << '\n';
1268 //cout << "Normalizing...\n";
1269 SG_NORMALIZE_RANGE(h1, -180.0, 180.0);
1270 SG_NORMALIZE_RANGE(h2, -180.0, 180.0);
1271 //cout << "d0, h1, h2 = " << d0 << ", " << h1 << ", " << h2 << '\n';
1272 if(fabs(h1) > 90.0) {
1273 // We are past the end of the to waypoint
1274 double d = GetGreatCircleDistance(_gpsLat, _gpsLon, fp->waypoints[i]->lat, fp->waypoints[i]->lon);
1276 //cout << "h1 triggered, d0 now = " << d0 << '\n';
1277 } else if(fabs(h2) > 90.0) {
1278 // We are past the end (not yet at!) the from waypoint
1279 double d = GetGreatCircleDistance(_gpsLat, _gpsLon, fp->waypoints[i-1]->lat, fp->waypoints[i-1]->lon);
1281 //cout << "h2 triggered, d0 now = " << d0 << '\n';
1284 //cout << "THIS LEG NOW ACTIVE!\n";
1286 _fromWaypoint = *fp->waypoints[i-1];
1287 _activeWaypoint = *fp->waypoints[i];
1294 void DCLGPS::OrientateToActiveFlightPlan() {
1295 OrientateToFlightPlan(_activeFP);
1298 /***************************************/
1300 // Utility function - create a flightplan from a list of waypoint ids and types
1301 void DCLGPS::CreateFlightPlan(GPSFlightPlan* fp, vector<string> ids, vector<GPSWpType> wps) {
1302 if(fp == NULL) fp = new GPSFlightPlan;
1304 if(!fp->waypoints.empty()) {
1305 for(i=0; i<fp->waypoints.size(); ++i) {
1306 delete fp->waypoints[i];
1308 fp->waypoints.clear();
1310 if(ids.size() != wps.size()) {
1311 cout << "ID and Waypoint types list size mismatch in GPS::CreateFlightPlan - no flightplan created!\n";
1314 for(i=0; i<ids.size(); ++i) {
1316 const FGAirport* ap;
1318 GPSWaypoint* wp = new GPSWaypoint;
1322 ap = FindFirstAptById(ids[i], multi, true);
1327 wp->lat = ap->getLatitude() * SG_DEGREES_TO_RADIANS;
1328 wp->lon = ap->getLongitude() * SG_DEGREES_TO_RADIANS;
1330 fp->waypoints.push_back(wp);
1334 np = FindFirstVorById(ids[i], multi, true);
1339 wp->lat = np->get_lat() * SG_DEGREES_TO_RADIANS;
1340 wp->lon = np->get_lon() * SG_DEGREES_TO_RADIANS;
1342 fp->waypoints.push_back(wp);
1346 np = FindFirstNDBById(ids[i], multi, true);
1351 wp->lat = np->get_lat() * SG_DEGREES_TO_RADIANS;
1352 wp->lon = np->get_lon() * SG_DEGREES_TO_RADIANS;
1354 fp->waypoints.push_back(wp);
1370 /***************************************/
1372 class DCLGPSFilter : public FGPositioned::Filter
1375 virtual bool pass(const FGPositioned* aPos) const {
1376 switch (aPos->type()) {
1377 case FGPositioned::AIRPORT:
1378 // how about heliports and seaports?
1379 case FGPositioned::NDB:
1380 case FGPositioned::VOR:
1381 case FGPositioned::WAYPOINT:
1382 case FGPositioned::FIX:
1384 default: return false; // reject all other types
1390 GPSWaypoint* DCLGPS::FindFirstById(const string& id) const
1392 DCLGPSFilter filter;
1393 FGPositionedRef result = FGPositioned::findNextWithPartialId(NULL, id, &filter);
1394 return GPSWaypoint::createFromPositioned(result);
1397 GPSWaypoint* DCLGPS::FindFirstByExactId(const string& id) const
1399 SGGeod pos(SGGeod::fromRad(_lon, _lat));
1400 FGPositionedRef result = FGPositioned::findClosestWithIdent(id, pos);
1401 return GPSWaypoint::createFromPositioned(result);
1404 // TODO - add the ASCII / alphabetical stuff from the Atlas version
1405 FGPositioned* DCLGPS::FindTypedFirstById(const string& id, FGPositioned::Type ty, bool &multi, bool exact)
1408 FGPositioned::TypeFilter filter(ty);
1411 FGPositioned::List matches =
1412 FGPositioned::findAllWithIdent(id, &filter);
1413 FGPositioned::sortByRange(matches, SGGeod::fromRad(_lon, _lat));
1414 multi = (matches.size() > 1);
1415 return matches.empty() ? NULL : matches.front().ptr();
1418 return FGPositioned::findNextWithPartialId(NULL, id, &filter);
1421 FGNavRecord* DCLGPS::FindFirstVorById(const string& id, bool &multi, bool exact)
1423 return dynamic_cast<FGNavRecord*>(FindTypedFirstById(id, FGPositioned::VOR, multi, exact));
1426 FGNavRecord* DCLGPS::FindFirstNDBById(const string& id, bool &multi, bool exact)
1428 return dynamic_cast<FGNavRecord*>(FindTypedFirstById(id, FGPositioned::NDB, multi, exact));
1431 const FGFix* DCLGPS::FindFirstIntById(const string& id, bool &multi, bool exact)
1433 return dynamic_cast<FGFix*>(FindTypedFirstById(id, FGPositioned::FIX, multi, exact));
1436 const FGAirport* DCLGPS::FindFirstAptById(const string& id, bool &multi, bool exact)
1438 return dynamic_cast<FGAirport*>(FindTypedFirstById(id, FGPositioned::AIRPORT, multi, exact));
1441 FGNavRecord* DCLGPS::FindClosestVor(double lat_rad, double lon_rad) {
1442 FGPositioned::TypeFilter filter(FGPositioned::VOR);
1443 double cutoff = 1000; // nautical miles
1444 FGPositionedRef v = FGPositioned::findClosest(SGGeod::fromRad(lon_rad, lat_rad), cutoff, &filter);
1449 return dynamic_cast<FGNavRecord*>(v.ptr());
1452 //----------------------------------------------------------------------------------------------------------
1454 // Takes lat and lon in RADIANS!!!!!!!
1455 double DCLGPS::GetMagHeadingFromTo(double latA, double lonA, double latB, double lonB) {
1456 double h = GetGreatCircleCourse(latA, lonA, latB, lonB);
1457 h *= SG_RADIANS_TO_DEGREES;
1458 // TODO - use the real altitude below instead of 0.0!
1459 //cout << "MagVar = " << sgGetMagVar(_gpsLon, _gpsLat, 0.0, _time->getJD()) * SG_RADIANS_TO_DEGREES << '\n';
1460 h -= sgGetMagVar(_gpsLon, _gpsLat, 0.0, _time->getJD()) * SG_RADIANS_TO_DEGREES;
1461 while(h >= 360.0) h -= 360.0;
1462 while(h < 0.0) h += 360.0;
1466 // ---------------- Great Circle formulae from "The Aviation Formulary" -------------
1467 // Note that all of these assume that the world is spherical.
1469 double Rad2Nm(double theta) {
1470 return(((180.0*60.0)/SG_PI)*theta);
1473 double Nm2Rad(double d) {
1474 return((SG_PI/(180.0*60.0))*d);
1479 The great circle distance d between two points with coordinates {lat1,lon1} and {lat2,lon2} is given by:
1481 d=acos(sin(lat1)*sin(lat2)+cos(lat1)*cos(lat2)*cos(lon1-lon2))
1483 A mathematically equivalent formula, which is less subject to rounding error for short distances is:
1485 d=2*asin(sqrt((sin((lat1-lat2)/2))^2 +
1486 cos(lat1)*cos(lat2)*(sin((lon1-lon2)/2))^2))
1490 // Returns distance in nm, takes lat & lon in RADIANS
1491 double DCLGPS::GetGreatCircleDistance(double lat1, double lon1, double lat2, double lon2) const {
1492 double d = 2.0 * asin(sqrt(((sin((lat1-lat2)/2.0))*(sin((lat1-lat2)/2.0))) +
1493 cos(lat1)*cos(lat2)*(sin((lon1-lon2)/2.0))*(sin((lon1-lon2)/2.0))));
1497 // fmod dosen't do what we want :-(
1498 static double mod(double d1, double d2) {
1499 return(d1 - d2*floor(d1/d2));
1502 // Returns great circle course from point 1 to point 2
1503 // Input and output in RADIANS.
1504 double DCLGPS::GetGreatCircleCourse (double lat1, double lon1, double lat2, double lon2) const {
1507 // Special case the poles
1508 if(cos(lat1) < SG_EPSILON) {
1510 // Starting from North Pole
1513 // Starting from South Pole
1517 // Urgh - the formula below is for negative lon +ve !!!???
1518 double d = GetGreatCircleDistance(lat1, lon1, lat2, lon2);
1519 cout << "d = " << d;
1521 //cout << ", d_theta = " << d;
1522 //cout << ", and d = " << Rad2Nm(d) << ' ';
1523 if(sin(lon2 - lon1) < 0) {
1525 h = acos((sin(lat2)-sin(lat1)*cos(d))/(sin(d)*cos(lat1)));
1528 h = 2.0 * SG_PI - acos((sin(lat2)-sin(lat1)*cos(d))/(sin(d)*cos(lat1)));
1531 cout << h * SG_RADIANS_TO_DEGREES << '\n';
1534 return( mod(atan2(sin(lon2-lon1)*cos(lat2),
1535 cos(lat1)*sin(lat2)-sin(lat1)*cos(lat2)*cos(lon2-lon1)),
1539 // Return a position on a radial from wp1 given distance d (nm) and magnetic heading h (degrees)
1540 // Note that d should be less that 1/4 Earth diameter!
1541 GPSWaypoint DCLGPS::GetPositionOnMagRadial(const GPSWaypoint& wp1, double d, double h) {
1542 h += sgGetMagVar(wp1.lon, wp1.lat, 0.0, _time->getJD()) * SG_RADIANS_TO_DEGREES;
1543 return(GetPositionOnRadial(wp1, d, h));
1546 // Return a position on a radial from wp1 given distance d (nm) and TRUE heading h (degrees)
1547 // Note that d should be less that 1/4 Earth diameter!
1548 GPSWaypoint DCLGPS::GetPositionOnRadial(const GPSWaypoint& wp1, double d, double h) {
1549 while(h < 0.0) h += 360.0;
1550 while(h > 360.0) h -= 360.0;
1552 h *= SG_DEGREES_TO_RADIANS;
1553 d *= (SG_PI / (180.0 * 60.0));
1555 double lat=asin(sin(wp1.lat)*cos(d)+cos(wp1.lat)*sin(d)*cos(h));
1558 lon=wp1.lon; // endpoint a pole
1560 lon=mod(wp1.lon+asin(sin(h)*sin(d)/cos(lat))+SG_PI,2*SG_PI)-SG_PI;
1566 wp.type = GPS_WP_VIRT;
1570 // Returns cross-track deviation in Nm.
1571 double DCLGPS::CalcCrossTrackDeviation() const {
1572 return(CalcCrossTrackDeviation(_fromWaypoint, _activeWaypoint));
1575 // Returns cross-track deviation of the current position between two arbitary waypoints in nm.
1576 double DCLGPS::CalcCrossTrackDeviation(const GPSWaypoint& wp1, const GPSWaypoint& wp2) const {
1577 //if(wp1 == NULL || wp2 == NULL) return(0.0);
1578 if(wp1.id.empty() || wp2.id.empty()) return(0.0);
1579 double xtd = asin(sin(Nm2Rad(GetGreatCircleDistance(wp1.lat, wp1.lon, _gpsLat, _gpsLon)))
1580 * sin(GetGreatCircleCourse(wp1.lat, wp1.lon, _gpsLat, _gpsLon) - GetGreatCircleCourse(wp1.lat, wp1.lon, wp2.lat, wp2.lon)));
1581 return(Rad2Nm(xtd));