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 fgTie("/instrumentation/gps/waypoint-alert", this, &DCLGPS::GetWaypointAlert);
243 fgTie("/instrumentation/gps/leg-mode", this, &DCLGPS::GetLegMode);
244 fgTie("/instrumentation/gps/obs-mode", this, &DCLGPS::GetOBSMode);
245 fgTie("/instrumentation/gps/approach-arm", this, &DCLGPS::GetApproachArm);
246 fgTie("/instrumentation/gps/approach-active", this, &DCLGPS::GetApproachActive);
247 fgTie("/instrumentation/gps/cdi-deflection", this, &DCLGPS::GetCDIDeflection);
248 fgTie("/instrumentation/gps/to-flag", this, &DCLGPS::GetToFlag);
251 void DCLGPS::unbind() {
252 fgUntie("/instrumentation/gps/waypoint-alert");
253 fgUntie("/instrumentation/gps/leg-mode");
254 fgUntie("/instrumentation/gps/obs-mode");
255 fgUntie("/instrumentation/gps/approach-arm");
256 fgUntie("/instrumentation/gps/approach-active");
257 fgUntie("/instrumentation/gps/cdi-deflection");
260 void DCLGPS::update(double dt) {
261 //cout << "update called!\n";
263 _lon = _lon_node->getDoubleValue() * SG_DEGREES_TO_RADIANS;
264 _lat = _lat_node->getDoubleValue() * SG_DEGREES_TO_RADIANS;
265 _alt = _alt_node->getDoubleValue();
266 _groundSpeed_kts = _grnd_speed_node->getDoubleValue();
267 _groundSpeed_ms = _groundSpeed_kts * 0.5144444444;
268 _track = _true_track_node->getDoubleValue();
269 _magTrackDeg = _mag_track_node->getDoubleValue();
270 // Note - we can depriciate _gpsLat and _gpsLon if we implement error handling in FG
271 // gps code and not our own.
274 // Check for abnormal position slew
275 if(GetGreatCircleDistance(_gpsLat, _gpsLon, _checkLat, _checkLon) > 1.0) {
276 OrientateToActiveFlightPlan();
281 // TODO - check for unit power before running this.
282 if(!_powerOnTimerSet) {
286 // Check if an alarm timer has expired
288 if(_alarmTime.hr() == atoi(fgGetString("/instrumentation/clock/indicated-hour"))
289 && _alarmTime.min() == atoi(fgGetString("/instrumentation/clock/indicated-min"))) {
290 _messageStack.push_back("*Timer Expired");
296 if(_groundSpeed_kts > 30.0) {
298 string th = fgGetString("/instrumentation/clock/indicated-hour");
299 string tm = fgGetString("/instrumentation/clock/indicated-min");
300 if(th.size() == 1) th = "0" + th;
301 if(tm.size() == 1) tm = "0" + tm;
302 _departureTimeString = th + tm;
305 // TODO - check - is this prone to drift error over time?
306 // Should we difference the departure and current times?
307 // What about when the user resets the time of day from the menu?
311 _time->update(_gpsLon * SG_DEGREES_TO_RADIANS, _gpsLat * SG_DEGREES_TO_RADIANS, 0, 0);
312 // FIXME - currently all the below assumes leg mode and no DTO or OBS cancelled.
313 if(_activeFP->IsEmpty()) {
314 // Not sure if we need to reset these each update or only when fp altered
315 _activeWaypoint.id.clear();
317 } else if(_activeFP->waypoints.size() == 1) {
318 _activeWaypoint.id.clear();
321 if(_activeWaypoint.id.empty() || _fromWaypoint.id.empty()) {
322 //cout << "Error, in leg mode with flightplan of 2 or more waypoints, but either active or from wp is NULL!\n";
323 OrientateToActiveFlightPlan();
327 if(_approachLoaded) {
328 if(!_approachReallyArmed && !_approachActive) {
329 // arm if within 30nm of airport.
330 // TODO - let user cancel approach arm using external GPS-APR switch
332 const FGAirport* ap = FindFirstAptById(_approachID, multi, true);
334 double d = GetGreatCircleDistance(_gpsLat, _gpsLon, ap->getLatitude() * SG_DEGREES_TO_RADIANS, ap->getLongitude() * SG_DEGREES_TO_RADIANS);
337 _approachReallyArmed = true;
338 _messageStack.push_back("*Press ALT To Set Baro");
339 // Not sure what we do if the user has already set CDI to 0.3 nm?
340 _targetCdiScaleIndex = 1;
341 if(_currentCdiScaleIndex == 1) {
343 } else if(_currentCdiScaleIndex == 0) {
344 _sourceCdiScaleIndex = 0;
345 _cdiScaleTransition = true;
346 _cdiTransitionTime = 30.0;
347 _currentCdiScale = _cdiScales[_currentCdiScaleIndex];
352 // Check for approach active - we can only activate approach if it is really armed.
353 if(_activeWaypoint.appType == GPS_FAF) {
354 //cout << "Active waypoint is FAF, id is " << _activeWaypoint.id << '\n';
355 if(GetGreatCircleDistance(_gpsLat, _gpsLon, _activeWaypoint.lat, _activeWaypoint.lon) <= 2.0 && !_obsMode) {
356 // Assume heading is OK for now
357 _approachArm = false; // TODO - check - maybe arm is left on when actv comes on?
358 _approachReallyArmed = false;
359 _approachActive = true;
360 _targetCdiScaleIndex = 2;
361 if(_currentCdiScaleIndex == 2) {
363 } else if(_currentCdiScaleIndex == 1) {
364 _sourceCdiScaleIndex = 1;
365 _cdiScaleTransition = true;
366 _cdiTransitionTime = 30.0; // TODO - compress it if time to FAF < 30sec
367 _currentCdiScale = _cdiScales[_currentCdiScaleIndex];
369 // Abort going active?
370 _approachActive = false;
377 // CDI scale transition stuff
378 if(_cdiScaleTransition) {
379 if(fabs(_currentCdiScale - _cdiScales[_targetCdiScaleIndex]) < 0.001) {
380 _currentCdiScale = _cdiScales[_targetCdiScaleIndex];
381 _currentCdiScaleIndex = _targetCdiScaleIndex;
382 _cdiScaleTransition = false;
384 double scaleDiff = (_targetCdiScaleIndex > _sourceCdiScaleIndex
385 ? _cdiScales[_sourceCdiScaleIndex] - _cdiScales[_targetCdiScaleIndex]
386 : _cdiScales[_targetCdiScaleIndex] - _cdiScales[_sourceCdiScaleIndex]);
387 //cout << "ScaleDiff = " << scaleDiff << '\n';
388 if(_targetCdiScaleIndex > _sourceCdiScaleIndex) {
389 // Scaling down eg. 5nm -> 1nm
390 _currentCdiScale -= (scaleDiff * dt / _cdiTransitionTime);
391 if(_currentCdiScale < _cdiScales[_targetCdiScaleIndex]) {
392 _currentCdiScale = _cdiScales[_targetCdiScaleIndex];
393 _currentCdiScaleIndex = _targetCdiScaleIndex;
394 _cdiScaleTransition = false;
397 _currentCdiScale += (scaleDiff * dt / _cdiTransitionTime);
398 if(_currentCdiScale > _cdiScales[_targetCdiScaleIndex]) {
399 _currentCdiScale = _cdiScales[_targetCdiScaleIndex];
400 _currentCdiScaleIndex = _targetCdiScaleIndex;
401 _cdiScaleTransition = false;
404 //cout << "_currentCdiScale = " << _currentCdiScale << '\n';
407 _currentCdiScale = _cdiScales[_currentCdiScaleIndex];
411 // Urgh - I've been setting the heading bug based on DTK,
412 // bug I think it should be based on heading re. active waypoint
413 // based on what the sim does after the final waypoint is passed.
414 // (DTK remains the same, but if track is held == DTK heading bug
415 // reverses to from once wp is passed).
417 if(_fromWaypoint != NULL) {
418 // TODO - how do we handle the change of track with distance over long legs?
419 _dtkTrue = GetGreatCircleCourse(_fromWaypoint->lat, _fromWaypoint->lon, _activeWaypoint->lat, _activeWaypoint->lon) * SG_RADIANS_TO_DEGREES;
420 _dtkMag = GetMagHeadingFromTo(_fromWaypoint->lat, _fromWaypoint->lon, _activeWaypoint->lat, _activeWaypoint->lon);
421 // Don't change the heading bug if speed is too low otherwise it flickers to/from at rest
422 if(_groundSpeed_ms > 5) {
423 //cout << "track = " << _track << ", dtk = " << _dtkTrue << '\n';
424 double courseDev = _track - _dtkTrue;
425 //cout << "courseDev = " << courseDev << ", normalized = ";
426 SG_NORMALIZE_RANGE(courseDev, -180.0, 180.0);
427 //cout << courseDev << '\n';
428 _headingBugTo = (fabs(courseDev) > 90.0 ? false : true);
433 // TODO - in DTO operation the position of initiation of DTO defines the "from waypoint".
436 if(!_activeWaypoint.id.empty()) {
437 double hdgTrue = GetGreatCircleCourse(_gpsLat, _gpsLon, _activeWaypoint.lat, _activeWaypoint.lon) * SG_RADIANS_TO_DEGREES;
438 if(_groundSpeed_ms > 5) {
439 //cout << "track = " << _track << ", hdgTrue = " << hdgTrue << '\n';
440 double courseDev = _track - hdgTrue;
441 //cout << "courseDev = " << courseDev << ", normalized = ";
442 SG_NORMALIZE_RANGE(courseDev, -180.0, 180.0);
443 //cout << courseDev << '\n';
444 _headingBugTo = (fabs(courseDev) > 90.0 ? false : true);
446 if(!_fromWaypoint.id.empty()) {
447 _dtkTrue = GetGreatCircleCourse(_fromWaypoint.lat, _fromWaypoint.lon, _activeWaypoint.lat, _activeWaypoint.lon) * SG_RADIANS_TO_DEGREES;
448 _dtkMag = GetMagHeadingFromTo(_fromWaypoint.lat, _fromWaypoint.lon, _activeWaypoint.lat, _activeWaypoint.lon);
455 _dist2Act = GetGreatCircleDistance(_gpsLat, _gpsLon, _activeWaypoint.lat, _activeWaypoint.lon) * SG_NM_TO_METER;
456 if(_groundSpeed_ms > 10.0) {
457 _eta = _dist2Act / _groundSpeed_ms;
458 if(_eta <= 36) { // TODO - this is slightly different if turn anticipation is enabled.
460 _waypointAlert = true; // TODO - not if the from flag is set.
464 // Check if we should sequence to next leg.
465 // Perhaps this should be done on distance instead, but 60s time (about 1 - 2 nm) seems reasonable for now.
466 //double reverseHeading = GetGreatCircleCourse(_activeWaypoint->lat, _activeWaypoint->lon, _fromWaypoint->lat, _fromWaypoint->lon);
467 // Hack - let's cheat and do it on heading bug for now. TODO - that stops us 'cutting the corner'
468 // when we happen to approach the inside turn of a waypoint - we should probably sequence at the midpoint
469 // of the heading difference between legs in this instance.
470 int idx = GetActiveWaypointIndex();
471 bool finalLeg = (idx == (int)(_activeFP->waypoints.size()) - 1 ? true : false);
472 bool finalDto = (_dto && idx == -1); // Dto operation to a waypoint not in the flightplan - we don't sequence in this instance
475 // Do nothing - not sure if Dto should switch off when arriving at the final waypoint of a flightplan
476 } else if(finalDto) {
478 } else if(_activeWaypoint.appType == GPS_MAP) {
479 // Don't sequence beyond the missed approach point
480 //cout << "ACTIVE WAYPOINT is MAP - not sequencing!!!!!\n";
482 //cout << "Sequencing...\n";
483 _fromWaypoint = _activeWaypoint;
484 _activeWaypoint = *_activeFP->waypoints[idx + 1];
486 // Course alteration message format is dependent on whether we are slaved HSI/CDI indicator or not.
488 if(fgGetBool("/instrumentation/nav[0]/slaved-to-gps")) {
489 // TODO - avoid the hardwiring on nav[0]
490 s = "Adj Nav Crs to ";
492 string s = "GPS Course is ";
494 double d = GetMagHeadingFromTo(_fromWaypoint.lat, _fromWaypoint.lon, _activeWaypoint.lat, _activeWaypoint.lon);
495 while(d < 0.0) d += 360.0;
496 while(d >= 360.0) d -= 360.0;
498 snprintf(buf, 4, "%03i", (int)(d + 0.5));
500 _messageStack.push_back(s);
502 _waypointAlert = false;
510 // First attempt at a sensible cross-track correction calculation
511 // Uh? - I think this is implemented further down the file!
512 if(_fromWaypoint != NULL) {
515 _crosstrackDist = 0.0;
522 Expand a SIAP ident to the full procedure name (as shown on the approach chart).
523 NOTE: Some of this is inferred from data, some is from documentation.
525 Example expansions from ARINC 424-18 [and the airport they're taken from]:
526 "R10LY" <--> "RNAV (GPS) Y RWY 10 L" [KBOI]
527 "R10-Z" <--> "RNAV (GPS) Z RWY 10" [KHTO]
528 "S25" <--> "VOR or GPS RWY 25" [KHHR]
529 "P20" <--> "GPS RWY 20" [KDAN]
530 "NDB-B" <--> "NDB or GPS-B" [KDAW]
531 "NDBC" <--> "NDB or GPS-C" [KEMT]
532 "VDMA" <--> "VOR/DME or GPS-A" [KDAW]
533 "VDM-A" <--> "VOR/DME or GPS-A" [KEAG]
534 "VDMB" <--> "VOR/DME or GPS-B" [KDKX]
535 "VORA" <--> "VOR or GPS-A" [KEMT]
537 It seems that there are 2 basic types of expansions; those that include
538 the runway and those that don't. Of those that don't, it seems that 2
539 different positions within the string to encode the identifying letter
540 are used, i.e. with a dash and without.
542 string DCLGPS::ExpandSIAPIdent(const string& ident) {
544 bool has_rwy = false;
547 case 'N': name = "NDB or GPS"; has_rwy = false; break;
548 case 'P': name = "GPS"; has_rwy = true; break;
549 case 'R': name = "RNAV (GPS)"; has_rwy = true; break;
550 case 'S': name = "VOR or GPS"; has_rwy = true; break;
552 if(ident[1] == 'D') name = "VOR/DME or GPS";
553 else name = "VOR or GPS";
556 default: // TODO output a log message
561 // Add the identifying letter if present
562 if(ident.size() == 5) {
569 name += ident.substr(1, 2);
571 // Add a left/right/centre indication if present.
572 if(ident.size() > 3) {
573 if((ident[3] != '-') && (ident[3] != ' ')) { // Early versions of the spec allowed a blank instead of a dash so check for both
579 // Add the identifying letter, which I *think* should always be present, but seems to be inconsistent as to whether a dash is used.
580 if(ident.size() == 5) {
583 } else if(ident.size() == 4) {
595 Load instrument approaches from an ARINC 424 file.
596 Tested on ARINC 424-18.
597 Known / current best guess at the format:
598 Col 1: Always 'S'. If it isn't, ditch it.
599 Col 2-4: "Customer area" code, eg "USA", "CAN". I think that CAN is used for Alaska.
600 Col 5: Section code. Used in conjunction with sub-section code. Definitions are with sub-section code.
602 Col 7-10: ICAO (or FAA) airport ident. Left justified if < 4 chars.
603 Col 11-12: Based on ICAO geographical region.
604 Col 13: Sub-section code. Used in conjunction with section code.
605 "HD/E/F" => Helicopter record.
606 "HS" => Helicopter minimum safe altitude.
607 "PA" => Airport record.
608 "PF" => Approach segment.
609 "PG" => Runway record.
610 "PP" => Path point record. ???
611 "PS" => MSA record (minimum safe altitude).
613 ------ The following is for "PF", approach segment -------
615 Col 14-19: SIAP ident for this approach (left justified). This is a standardised abbreviated approach name.
616 e.g. "R10LZ" expands to "RNAV (GPS) Z RWY 10 L". See the comment block for ExpandSIAPIdent for full details.
617 Col 20: Route type. This is tricky - I don't have full documentation and am having to guess a bit.
618 'A' => Arrival route? This seems to be used to encode arrival routes from the IAF to the approach proper.
619 Note that the final fix of the arrival route is duplicated in the approach proper.
620 'D' => VOR/DME or GPS
622 'P' => GPS (ARINC 424-18), GPS and RNAV (GPS) (ARINC 424-15 and before).
623 'R' => RNAV (GPS) (ARINC 424-18).
625 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.
627 Col 27-29: Sequence number - position within the route segment. Rule: 10-20-30 etc.
628 Col 30-34: Fix identifer. The ident of the waypoint.
629 Col 35-36: ICAO geographical region code. I think we can ignore this for now.
630 Col 37: Section code - ??? I don't know what this means
631 Col 38 Subsection code - ??? ditto - no idea!
632 Col 40: Waypoint type.
633 'A' => Airport as waypoint
634 'E' => Essential waypoint (e.g. change of heading at this waypoint, etc).
635 'G' => Runway or helipad as waypoint
636 'H' => Heliport as waypoint
637 'N' => NDB as waypoint
638 'P' => Phantom waypoint (not sure if this is used in rev 18?)
639 'V' => VOR as waypoint
640 Col 41: Waypoint type.
641 'B' => Flyover, approach transition, or final approach.
642 'E' => end of route segment (transition waypoint). (Actually "End of terminal procedure route type" in the docs).
643 'N' => ??? I've also seen 'N' in this column, but don't know what it indicates.
645 Col 43: Waypoint type. May also be blank when none of the below.
646 'A' => Initial approach fix (IAF)
647 'F' => Final approach fix
649 'I' => Final approach course fix
650 'M' => Missed approach point
651 '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!
652 ??? Possibly procedure turn?
653 'C' => ??? This is also present in the data, but missing from the docs. Is at airport 00R.
654 Col 107-111 MSA center fix. We can ignore this.
656 void DCLGPS::LoadApproachData() {
660 const GPSWaypoint* cwp;
663 SGPath path = globals->get_fg_root();
664 path.append("Navaids/rnav.dat");
665 fin.open(path.c_str(), ios::in);
667 //cout << "Unable to open input file " << path.c_str() << '\n';
670 //cout << "Opened " << path.c_str() << " for reading\n";
675 string apt_ident; // This gets set to the ICAO code of the current airport being processed.
676 string iap_ident; // The abbreviated name of the current approach being processed.
677 string wp_ident; // The ident of the waypoint of the current line
678 string last_apt_ident;
679 string last_iap_ident;
680 string last_wp_ident;
681 // There is no need to save the full name - it can be generated on the fly from the abbreviated name as and when needed.
682 bool apt_in_progress = false; // Set true whilst loading all the approaches for a given airport.
683 bool iap_in_progress = false; // Set true whilst loading a given approach.
684 bool iap_error = false; // Set true if there is an error loading a given approach.
685 bool route_in_progress = false; // Set true when we are loading a "route" segment of the approach.
686 int last_sequence_number = 0; // Position within the route, rule (rev 18): 10, 20, 30 etc.
688 char last_route_type = 0;
690 char waypoint_fix_type; // This is the waypoint type from col 43, i.e. the type of fix. May be blank.
695 unsigned int nLoaded = 0;
696 unsigned int nErrors = 0;
698 //for(i=0; i<64; ++i) {
700 fin.getline(tmp, 256);
701 //s = Fake_rnav_dat[i];
703 if(s.size() < 132) continue;
704 if(s[0] == 'S') { // Valid line
705 string country_code = s.substr(1, 3);
706 if(country_code == "USA") { // For now we'll stick to US procedures in case there are unknown gotchas with others
707 if(s[4] == 'P') { // Includes approaches.
708 if(s[12] == 'A') { // Airport record
709 apt_ident = s.substr(6, 4);
710 // Trim any whitespace from the ident. The ident is left justified,
711 // so any space will be at the end.
712 if(apt_ident[3] == ' ') apt_ident = apt_ident.substr(0, 3);
713 // I think that all idents are either 3 or 4 chars - could check this though!
714 if(!apt_in_progress) {
715 last_apt_ident = apt_ident;
718 if(last_apt_ident != apt_ident) {
719 if(iap_in_progress) {
721 //cout << "ERROR: Unable to load approach " << iap->_ident << " at " << iap->_aptIdent << '\n';
724 _np_iap[iap->_aptIdent].push_back(iap);
725 //cout << "** Loaded " << iap->_aptIdent << "\t" << iap->_ident << '\n';
728 iap_in_progress = false;
731 last_apt_ident = apt_ident;
734 } else if(s[12] == 'F') { // Approach segment
735 if(apt_in_progress) {
736 iap_ident = s.substr(13, 6);
737 // Trim any whitespace from the RH end.
739 if(iap_ident[5-j] == ' ') {
740 iap_ident = iap_ident.substr(0, 5-j);
742 // It's important to break here, since earlier versions of ARINC 424 allowed spaces in the ident.
746 if(iap_in_progress) {
747 if(iap_ident != last_iap_ident) {
748 // This is a new approach - store the last one and trigger
749 // starting afresh by setting the in progress flag to false.
751 //cout << "ERROR: Unable to load approach " << iap->_ident << " at " << iap->_aptIdent << '\n';
754 _np_iap[iap->_aptIdent].push_back(iap);
755 //cout << "Loaded " << iap->_aptIdent << "\t" << iap->_ident << '\n';
758 iap_in_progress = false;
761 if(!iap_in_progress) {
763 iap->_aptIdent = apt_ident;
764 iap->_ident = iap_ident;
765 iap->_name = ExpandSIAPIdent(iap_ident); // I suspect that it's probably better to just store idents, and to expand the names as needed.
766 // Note, we haven't set iap->_rwyStr yet.
767 last_iap_ident = iap_ident;
768 iap_in_progress = true;
774 sequence_number = atoi(s.substr(26,3).c_str());
775 wp_ident = s.substr(29, 5);
776 waypoint_fix_type = s[42];
777 // Trim any whitespace from the RH end
779 if(wp_ident[4-j] == ' ') {
780 wp_ident = wp_ident.substr(0, 4-j);
786 // Ignore lines with no waypoint ID for now - these are normally part of the
787 // missed approach procedure, and we don't use them in the KLN89.
788 if(!wp_ident.empty()) {
789 // Make a local copy of the waypoint for now, since we're not yet sure if we'll be using it
792 bool wp_error = false;
793 if(w.id.substr(0, 2) == "RW" && waypoint_fix_type == 'M') {
794 // Assume that this is a missed-approach point based on the runway number, which appears to be standard for most approaches.
795 // Note: Currently fgFindAirportID returns NULL on error, but getRunwayByIdent throws an exception.
796 const FGAirport* apt = fgFindAirportID(iap->_aptIdent);
800 rwystr = w.id.substr(2, 2);
801 // TODO - sanity check the rwystr at this point to ensure we have a double digit number
802 if(w.id.size() > 4) {
803 if(w.id[4] == 'L' || w.id[4] == 'C' || w.id[4] == 'R') {
807 FGRunway* rwy = apt->getRunwayByIdent(rwystr);
808 w.lat = rwy->begin().getLatitudeRad();
809 w.lon = rwy->begin().getLongitudeRad();
810 } catch(const sg_exception&) {
811 SG_LOG(SG_GENERAL, SG_WARN, "Unable to find runway " << w.id.substr(2, 2) << " at airport " << iap->_aptIdent);
812 //cout << "Unable to find runway " << w.id.substr(2, 2) << " at airport " << iap->_aptIdent << " ( w.id = " << w.id << ", rwystr = " << rwystr << " )\n";
819 cwp = FindFirstByExactId(w.id);
826 switch(waypoint_fix_type) {
827 case 'A': w.appType = GPS_IAF; break;
828 case 'F': w.appType = GPS_FAF; break;
829 case 'H': w.appType = GPS_MAHP; break;
830 case 'I': w.appType = GPS_IAP; break;
831 case 'M': w.appType = GPS_MAP; break;
832 case ' ': w.appType = GPS_APP_NONE; break;
833 //default: cout << "Unknown waypoint_fix_type: \'" << waypoint_fix_type << "\' [" << apt_ident << ", " << iap_ident << "]\n";
837 //cout << "Unable to find waypoint " << w.id << " [" << apt_ident << ", " << iap_ident << "]\n";
842 if(route_in_progress) {
843 if(sequence_number > last_sequence_number) {
844 // TODO - add a check for runway numbers
845 // Check for the waypoint ID being the same as the previous line.
846 // This is often the case for the missed approach holding point.
847 if(wp_ident == last_wp_ident) {
848 if(waypoint_fix_type == 'H') {
849 if(!iap->_IAP.empty()) {
850 if(iap->_IAP[iap->_IAP.size() - 1]->appType == GPS_APP_NONE) {
851 iap->_IAP[iap->_IAP.size() - 1]->appType = GPS_MAHP;
853 //cout << "Waypoint is MAHP and another type! " << w.id << " [" << apt_ident << ", " << iap_ident << "]\n";
858 // Create a new waypoint on the heap, copy the local copy into it, and push it onto the approach.
859 wp = new GPSWaypoint;
861 if(route_type == 'A') {
862 fp->waypoints.push_back(wp);
864 iap->_IAP.push_back(wp);
867 } else if(sequence_number == last_sequence_number) {
868 // This seems to happen once per final approach route - one of the waypoints
869 // is duplicated with the same sequence number - I'm not sure what information
870 // the second line give yet so ignore it for now.
871 // TODO - figure this out!
873 // Finalise the current route and start a new one
875 // Finalise the current route
876 if(last_route_type == 'A') {
877 // Push the flightplan onto the approach
878 iap->_approachRoutes.push_back(fp);
880 // All the waypoints get pushed individually - don't need to do it.
883 // There are basically 2 possibilities here - either it's one of the arrival transitions,
884 // or it's the core final approach course.
885 wp = new GPSWaypoint;
887 if(route_type == 'A') { // It's one of the arrival transition(s)
888 fp = new GPSFlightPlan;
889 fp->waypoints.push_back(wp);
891 iap->_IAP.push_back(wp);
893 route_in_progress = true;
896 // Start a new route.
897 // There are basically 2 possibilities here - either it's one of the arrival transitions,
898 // or it's the core final approach course.
899 wp = new GPSWaypoint;
901 if(route_type == 'A') { // It's one of the arrival transition(s)
902 fp = new GPSFlightPlan;
903 fp->waypoints.push_back(wp);
905 iap->_IAP.push_back(wp);
907 route_in_progress = true;
909 last_route_type = route_type;
910 last_wp_ident = wp_ident;
911 last_sequence_number = sequence_number;
915 // ERROR - no airport record read.
919 // Check and finalise any approaches in progress
920 // TODO - sanity check that the approach has all the required elements
921 if(iap_in_progress) {
922 // This is a new approach - store the last one and trigger
923 // starting afresh by setting the in progress flag to false.
925 //cout << "ERROR: Unable to load approach " << iap->_ident << " at " << iap->_aptIdent << '\n';
928 _np_iap[iap->_aptIdent].push_back(iap);
929 //cout << "* Loaded " << iap->_aptIdent << "\t" << iap->_ident << '\n';
932 iap_in_progress = false;
939 // If we get to the end of the file, load any approach that is still in progress
940 // TODO - sanity check that the approach has all the required elements
941 if(iap_in_progress) {
943 //cout << "ERROR: Unable to load approach " << iap->_ident << " at " << iap->_aptIdent << '\n';
946 _np_iap[iap->_aptIdent].push_back(iap);
947 //cout << "*** Loaded " << iap->_aptIdent << "\t" << iap->_ident << '\n';
952 //cout << "Done loading approach database\n";
953 //cout << "Loaded: " << nLoaded << '\n';
954 //cout << "Failed: " << nErrors << '\n';
959 GPSWaypoint* DCLGPS::GetActiveWaypoint() {
960 return &_activeWaypoint;
964 float DCLGPS::GetDistToActiveWaypoint() {
968 // I don't yet fully understand all the gotchas about where to source time from.
969 // This function sets the initial timer before the clock exports properties
970 // and the one below uses the clock to be consistent with the rest of the code.
971 // It might change soonish...
972 void DCLGPS::SetPowerOnTimer() {
973 struct tm *t = globals->get_time_params()->getGmt();
974 _powerOnTime.set_hr(t->tm_hour);
975 _powerOnTime.set_min(t->tm_min);
976 _powerOnTimerSet = true;
979 void DCLGPS::ResetPowerOnTimer() {
980 _powerOnTime.set_hr(atoi(fgGetString("/instrumentation/clock/indicated-hour")));
981 _powerOnTime.set_min(atoi(fgGetString("/instrumentation/clock/indicated-min")));
982 _powerOnTimerSet = true;
985 double DCLGPS::GetCDIDeflection() const {
986 double xtd = CalcCrossTrackDeviation(); //nm
987 return((xtd / _currentCdiScale) * 5.0 * 2.5 * -1.0);
990 void DCLGPS::DtoInitiate(const string& s) {
991 const GPSWaypoint* wp = FindFirstByExactId(s);
993 // TODO - Currently we start DTO operation unconditionally, regardless of which mode we are in.
994 // In fact, the following rules apply:
995 // In LEG mode, start DTO as we currently do.
996 // In OBS mode, set the active waypoint to the requested waypoint, and then:
997 // If the KLN89 is not connected to an external HSI or CDI, set the OBS course to go direct to the waypoint.
998 // If the KLN89 *is* connected to an external HSI or CDI, it cannot set the course itself, and will display
999 // a scratchpad message with the course to set manually on the HSI/CDI.
1000 // In both OBS cases, leave _dto false, since we don't need the virtual waypoint created.
1002 _activeWaypoint = *wp;
1003 _fromWaypoint.lat = _gpsLat;
1004 _fromWaypoint.lon = _gpsLon;
1005 _fromWaypoint.type = GPS_WP_VIRT;
1006 _fromWaypoint.id = "_DTOWP_";
1009 // TODO - Should bring up the user waypoint page.
1014 void DCLGPS::DtoCancel() {
1016 // i.e. don't bother reorientating if we're just cancelling a DTO button press
1017 // without having previously initiated DTO.
1018 OrientateToActiveFlightPlan();
1023 void DCLGPS::ToggleOBSMode() {
1024 _obsMode = !_obsMode;
1026 // We need to set the OBS heading. The rules for this are:
1028 // If the KLN89 is connected to an external HSI or CDI, then the OBS heading must be set
1029 // to the OBS radial on the external indicator, and cannot be changed in the KLN89.
1031 // If the KLN89 is not connected to an external indicator, then:
1032 // If there is an active waypoint, the OBS heading is set such that the
1033 // deviation indicator remains at the same deviation (i.e. set to DTK,
1034 // although there may be some small difference).
1036 // If there is not an active waypoint, I am not sure what value should be
1039 if(fgGetBool("/instrumentation/nav/slaved-to-gps")) {
1040 _obsHeading = static_cast<int>(fgGetDouble("/instrumentation/nav/radials/selected-deg") + 0.5);
1041 } else if(!_activeWaypoint.id.empty()) {
1042 // NOTE: I am not sure if this is completely correct. There are sometimes small differences
1043 // between DTK and default OBS heading in the simulator (~ 1 or 2 degrees). It might also
1044 // be that OBS heading should be stored as float and only displayed as int, in order to maintain
1045 // the initial bar deviation?
1046 _obsHeading = static_cast<int>(_dtkMag + 0.5);
1047 //cout << "_dtkMag = " << _dtkMag << ", _dtkTrue = " << _dtkTrue << ", _obsHeading = " << _obsHeading << '\n';
1049 // TODO - what should we really do here?
1053 // Valid OBS heading values are 0 -> 359 degrees inclusive (from kln89 simulator).
1054 if(_obsHeading > 359) _obsHeading -= 360;
1055 if(_obsHeading < 0) _obsHeading += 360;
1057 // TODO - the _fromWaypoint location will change as the OBS heading changes.
1058 // Might need to store the OBS initiation position somewhere in case it is needed again.
1059 SetOBSFromWaypoint();
1063 // Set the _fromWaypoint position based on the active waypoint and OBS radial.
1064 void DCLGPS::SetOBSFromWaypoint() {
1065 if(!_obsMode) return;
1066 if(_activeWaypoint.id.empty()) return;
1068 // TODO - base the 180 deg correction on the to/from flag.
1069 _fromWaypoint = GetPositionOnMagRadial(_activeWaypoint, 10, _obsHeading + 180.0);
1070 _fromWaypoint.type = GPS_WP_VIRT;
1071 _fromWaypoint.id = "_OBSWP_";
1074 void DCLGPS::CDIFSDIncrease() {
1075 if(_currentCdiScaleIndex == 0) {
1076 _currentCdiScaleIndex = _cdiScales.size() - 1;
1078 _currentCdiScaleIndex--;
1082 void DCLGPS::CDIFSDDecrease() {
1083 _currentCdiScaleIndex++;
1084 if(_currentCdiScaleIndex == _cdiScales.size()) {
1085 _currentCdiScaleIndex = 0;
1089 void DCLGPS::DrawChar(char c, int field, int px, int py, bool bold) {
1092 void DCLGPS::DrawText(const string& s, int field, int px, int py, bool bold) {
1095 void DCLGPS::CreateDefaultFlightPlans() {}
1097 // Get the time to the active waypoint in seconds.
1098 // Returns -1 if groundspeed < 30 kts
1099 double DCLGPS::GetTimeToActiveWaypoint() {
1100 if(_groundSpeed_kts < 30.0) {
1107 // Get the time to the final waypoint in seconds.
1108 // Returns -1 if groundspeed < 30 kts
1109 double DCLGPS::GetETE() {
1110 if(_groundSpeed_kts < 30.0) {
1113 // TODO - handle OBS / DTO operation appropriately
1114 if(_activeFP->waypoints.empty()) {
1117 return(GetTimeToWaypoint(_activeFP->waypoints[_activeFP->waypoints.size() - 1]->id));
1122 // Get the time to a given waypoint (spec'd by ID) in seconds.
1123 // returns -1 if groundspeed is less than 30kts.
1124 // If the waypoint is an unreached part of the active flight plan the time will be via each leg.
1125 // otherwise it will be a direct-to time.
1126 double DCLGPS::GetTimeToWaypoint(const string& id) {
1127 if(_groundSpeed_kts < 30.0) {
1132 int n1 = GetActiveWaypointIndex();
1133 int n2 = GetWaypointIndex(id);
1136 for(unsigned int i=n1+1; i<_activeFP->waypoints.size(); ++i) {
1137 GPSWaypoint* wp1 = _activeFP->waypoints[i-1];
1138 GPSWaypoint* wp2 = _activeFP->waypoints[i];
1139 double distm = GetGreatCircleDistance(wp1->lat, wp1->lon, wp2->lat, wp2->lon) * SG_NM_TO_METER;
1140 eta += (distm / _groundSpeed_ms);
1143 } else if(id == _activeWaypoint.id) {
1146 const GPSWaypoint* wp = FindFirstByExactId(id);
1147 if(wp == NULL) return(-1.0);
1148 double distm = GetGreatCircleDistance(_gpsLat, _gpsLon, wp->lat, wp->lon);
1150 return(distm / _groundSpeed_ms);
1152 return(-1.0); // Hopefully we never get here!
1155 // Returns magnetic great-circle heading
1156 // TODO - document units.
1157 float DCLGPS::GetHeadingToActiveWaypoint() {
1158 if(_activeWaypoint.id.empty()) {
1161 double h = GetMagHeadingFromTo(_gpsLat, _gpsLon, _activeWaypoint.lat, _activeWaypoint.lon);
1162 while(h <= 0.0) h += 360.0;
1163 while(h > 360.0) h -= 360.0;
1168 // Returns magnetic great-circle heading
1169 // TODO - what units?
1170 float DCLGPS::GetHeadingFromActiveWaypoint() {
1171 if(_activeWaypoint.id.empty()) {
1174 double h = GetMagHeadingFromTo(_activeWaypoint.lat, _activeWaypoint.lon, _gpsLat, _gpsLon);
1175 while(h <= 0.0) h += 360.0;
1176 while(h > 360.0) h -= 360.0;
1181 void DCLGPS::ClearFlightPlan(int n) {
1182 for(unsigned int i=0; i<_flightPlans[n]->waypoints.size(); ++i) {
1183 delete _flightPlans[n]->waypoints[i];
1185 _flightPlans[n]->waypoints.clear();
1188 void DCLGPS::ClearFlightPlan(GPSFlightPlan* fp) {
1189 for(unsigned int i=0; i<fp->waypoints.size(); ++i) {
1190 delete fp->waypoints[i];
1192 fp->waypoints.clear();
1195 int DCLGPS::GetActiveWaypointIndex() {
1196 for(unsigned int i=0; i<_flightPlans[0]->waypoints.size(); ++i) {
1197 if(_flightPlans[0]->waypoints[i]->id == _activeWaypoint.id) return((int)i);
1202 int DCLGPS::GetWaypointIndex(const string& id) {
1203 for(unsigned int i=0; i<_flightPlans[0]->waypoints.size(); ++i) {
1204 if(_flightPlans[0]->waypoints[i]->id == id) return((int)i);
1209 void DCLGPS::OrientateToFlightPlan(GPSFlightPlan* fp) {
1210 //cout << "Orientating...\n";
1211 //cout << "_lat = " << _lat << ", _lon = " << _lon << ", _gpsLat = " << _gpsLat << ", gpsLon = " << _gpsLon << '\n';
1213 _activeWaypoint.id.clear();
1216 _navFlagged = false;
1217 if(fp->waypoints.size() == 1) {
1218 // TODO - may need to flag nav here if not dto or obs, or possibly handle it somewhere else.
1219 _activeWaypoint = *fp->waypoints[0];
1220 _fromWaypoint.id.clear();
1222 // FIXME FIXME FIXME
1223 _fromWaypoint = *fp->waypoints[0];
1224 _activeWaypoint = *fp->waypoints[1];
1225 double dmin = 1000000; // nm!!
1226 // For now we will simply start on the leg closest to our current position.
1227 // It's possible that more fancy algorithms may take either heading or track
1228 // into account when setting inital leg - I'm not sure.
1229 // This method should handle most cases perfectly OK though.
1230 for(unsigned int i = 1; i < fp->waypoints.size(); ++i) {
1231 //cout << "Pass " << i << ", dmin = " << dmin << ", leg is " << fp->waypoints[i-1]->id << " to " << fp->waypoints[i]->id << '\n';
1232 // First get the cross track correction.
1233 double d0 = fabs(CalcCrossTrackDeviation(*fp->waypoints[i-1], *fp->waypoints[i]));
1234 // That is the shortest distance away we could be though - check for
1235 // longer distances if we are 'off the end' of the leg.
1236 double ht1 = GetGreatCircleCourse(fp->waypoints[i-1]->lat, fp->waypoints[i-1]->lon,
1237 fp->waypoints[i]->lat, fp->waypoints[i]->lon)
1238 * SG_RADIANS_TO_DEGREES;
1239 // not simply the reverse of the above due to great circle navigation.
1240 double ht2 = GetGreatCircleCourse(fp->waypoints[i]->lat, fp->waypoints[i]->lon,
1241 fp->waypoints[i-1]->lat, fp->waypoints[i-1]->lon)
1242 * SG_RADIANS_TO_DEGREES;
1243 double hw1 = GetGreatCircleCourse(_gpsLat, _gpsLon,
1244 fp->waypoints[i]->lat, fp->waypoints[i]->lon)
1245 * SG_RADIANS_TO_DEGREES;
1246 double hw2 = GetGreatCircleCourse(_gpsLat, _gpsLon,
1247 fp->waypoints[i-1]->lat, fp->waypoints[i-1]->lon)
1248 * SG_RADIANS_TO_DEGREES;
1249 double h1 = ht1 - hw1;
1250 double h2 = ht2 - hw2;
1251 //cout << "d0, h1, h2 = " << d0 << ", " << h1 << ", " << h2 << '\n';
1252 //cout << "Normalizing...\n";
1253 SG_NORMALIZE_RANGE(h1, -180.0, 180.0);
1254 SG_NORMALIZE_RANGE(h2, -180.0, 180.0);
1255 //cout << "d0, h1, h2 = " << d0 << ", " << h1 << ", " << h2 << '\n';
1256 if(fabs(h1) > 90.0) {
1257 // We are past the end of the to waypoint
1258 double d = GetGreatCircleDistance(_gpsLat, _gpsLon, fp->waypoints[i]->lat, fp->waypoints[i]->lon);
1260 //cout << "h1 triggered, d0 now = " << d0 << '\n';
1261 } else if(fabs(h2) > 90.0) {
1262 // We are past the end (not yet at!) the from waypoint
1263 double d = GetGreatCircleDistance(_gpsLat, _gpsLon, fp->waypoints[i-1]->lat, fp->waypoints[i-1]->lon);
1265 //cout << "h2 triggered, d0 now = " << d0 << '\n';
1268 //cout << "THIS LEG NOW ACTIVE!\n";
1270 _fromWaypoint = *fp->waypoints[i-1];
1271 _activeWaypoint = *fp->waypoints[i];
1278 void DCLGPS::OrientateToActiveFlightPlan() {
1279 OrientateToFlightPlan(_activeFP);
1282 /***************************************/
1284 // Utility function - create a flightplan from a list of waypoint ids and types
1285 void DCLGPS::CreateFlightPlan(GPSFlightPlan* fp, vector<string> ids, vector<GPSWpType> wps) {
1286 if(fp == NULL) fp = new GPSFlightPlan;
1288 if(!fp->waypoints.empty()) {
1289 for(i=0; i<fp->waypoints.size(); ++i) {
1290 delete fp->waypoints[i];
1292 fp->waypoints.clear();
1294 if(ids.size() != wps.size()) {
1295 cout << "ID and Waypoint types list size mismatch in GPS::CreateFlightPlan - no flightplan created!\n";
1298 for(i=0; i<ids.size(); ++i) {
1300 const FGAirport* ap;
1302 GPSWaypoint* wp = new GPSWaypoint;
1306 ap = FindFirstAptById(ids[i], multi, true);
1311 wp->lat = ap->getLatitude() * SG_DEGREES_TO_RADIANS;
1312 wp->lon = ap->getLongitude() * SG_DEGREES_TO_RADIANS;
1314 fp->waypoints.push_back(wp);
1318 np = FindFirstVorById(ids[i], multi, true);
1323 wp->lat = np->get_lat() * SG_DEGREES_TO_RADIANS;
1324 wp->lon = np->get_lon() * SG_DEGREES_TO_RADIANS;
1326 fp->waypoints.push_back(wp);
1330 np = FindFirstNDBById(ids[i], multi, true);
1335 wp->lat = np->get_lat() * SG_DEGREES_TO_RADIANS;
1336 wp->lon = np->get_lon() * SG_DEGREES_TO_RADIANS;
1338 fp->waypoints.push_back(wp);
1354 /***************************************/
1356 class DCLGPSFilter : public FGPositioned::Filter
1359 virtual bool pass(const FGPositioned* aPos) const {
1360 switch (aPos->type()) {
1361 case FGPositioned::AIRPORT:
1362 // how about heliports and seaports?
1363 case FGPositioned::NDB:
1364 case FGPositioned::VOR:
1365 case FGPositioned::WAYPOINT:
1366 case FGPositioned::FIX:
1368 default: return false; // reject all other types
1374 GPSWaypoint* DCLGPS::FindFirstById(const string& id) const
1376 DCLGPSFilter filter;
1377 FGPositionedRef result = FGPositioned::findNextWithPartialId(NULL, id, &filter);
1378 return GPSWaypoint::createFromPositioned(result);
1381 GPSWaypoint* DCLGPS::FindFirstByExactId(const string& id) const
1383 SGGeod pos(SGGeod::fromRad(_lon, _lat));
1384 FGPositionedRef result = FGPositioned::findClosestWithIdent(id, pos);
1385 return GPSWaypoint::createFromPositioned(result);
1388 // TODO - add the ASCII / alphabetical stuff from the Atlas version
1389 FGPositioned* DCLGPS::FindTypedFirstById(const string& id, FGPositioned::Type ty, bool &multi, bool exact)
1392 FGPositioned::TypeFilter filter(ty);
1395 FGPositioned::List matches =
1396 FGPositioned::findAllWithIdent(id, &filter);
1397 FGPositioned::sortByRange(matches, SGGeod::fromRad(_lon, _lat));
1398 multi = (matches.size() > 1);
1399 return matches.empty() ? NULL : matches.front().ptr();
1402 return FGPositioned::findNextWithPartialId(NULL, id, &filter);
1405 FGNavRecord* DCLGPS::FindFirstVorById(const string& id, bool &multi, bool exact)
1407 return dynamic_cast<FGNavRecord*>(FindTypedFirstById(id, FGPositioned::VOR, multi, exact));
1410 FGNavRecord* DCLGPS::FindFirstNDBById(const string& id, bool &multi, bool exact)
1412 return dynamic_cast<FGNavRecord*>(FindTypedFirstById(id, FGPositioned::NDB, multi, exact));
1415 const FGFix* DCLGPS::FindFirstIntById(const string& id, bool &multi, bool exact)
1417 return dynamic_cast<FGFix*>(FindTypedFirstById(id, FGPositioned::FIX, multi, exact));
1420 const FGAirport* DCLGPS::FindFirstAptById(const string& id, bool &multi, bool exact)
1422 return dynamic_cast<FGAirport*>(FindTypedFirstById(id, FGPositioned::AIRPORT, multi, exact));
1425 FGNavRecord* DCLGPS::FindClosestVor(double lat_rad, double lon_rad) {
1426 FGPositioned::TypeFilter filter(FGPositioned::VOR);
1427 double cutoff = 1000; // nautical miles
1428 FGPositionedRef v = FGPositioned::findClosest(SGGeod::fromRad(lon_rad, lat_rad), cutoff, &filter);
1433 return dynamic_cast<FGNavRecord*>(v.ptr());
1436 //----------------------------------------------------------------------------------------------------------
1438 // Takes lat and lon in RADIANS!!!!!!!
1439 double DCLGPS::GetMagHeadingFromTo(double latA, double lonA, double latB, double lonB) {
1440 double h = GetGreatCircleCourse(latA, lonA, latB, lonB);
1441 h *= SG_RADIANS_TO_DEGREES;
1442 // TODO - use the real altitude below instead of 0.0!
1443 //cout << "MagVar = " << sgGetMagVar(_gpsLon, _gpsLat, 0.0, _time->getJD()) * SG_RADIANS_TO_DEGREES << '\n';
1444 h -= sgGetMagVar(_gpsLon, _gpsLat, 0.0, _time->getJD()) * SG_RADIANS_TO_DEGREES;
1445 while(h >= 360.0) h -= 360.0;
1446 while(h < 0.0) h += 360.0;
1450 // ---------------- Great Circle formulae from "The Aviation Formulary" -------------
1451 // Note that all of these assume that the world is spherical.
1453 double Rad2Nm(double theta) {
1454 return(((180.0*60.0)/SG_PI)*theta);
1457 double Nm2Rad(double d) {
1458 return((SG_PI/(180.0*60.0))*d);
1463 The great circle distance d between two points with coordinates {lat1,lon1} and {lat2,lon2} is given by:
1465 d=acos(sin(lat1)*sin(lat2)+cos(lat1)*cos(lat2)*cos(lon1-lon2))
1467 A mathematically equivalent formula, which is less subject to rounding error for short distances is:
1469 d=2*asin(sqrt((sin((lat1-lat2)/2))^2 +
1470 cos(lat1)*cos(lat2)*(sin((lon1-lon2)/2))^2))
1474 // Returns distance in nm, takes lat & lon in RADIANS
1475 double DCLGPS::GetGreatCircleDistance(double lat1, double lon1, double lat2, double lon2) const {
1476 double d = 2.0 * asin(sqrt(((sin((lat1-lat2)/2.0))*(sin((lat1-lat2)/2.0))) +
1477 cos(lat1)*cos(lat2)*(sin((lon1-lon2)/2.0))*(sin((lon1-lon2)/2.0))));
1481 // fmod dosen't do what we want :-(
1482 static double mod(double d1, double d2) {
1483 return(d1 - d2*floor(d1/d2));
1486 // Returns great circle course from point 1 to point 2
1487 // Input and output in RADIANS.
1488 double DCLGPS::GetGreatCircleCourse (double lat1, double lon1, double lat2, double lon2) const {
1491 // Special case the poles
1492 if(cos(lat1) < SG_EPSILON) {
1494 // Starting from North Pole
1497 // Starting from South Pole
1501 // Urgh - the formula below is for negative lon +ve !!!???
1502 double d = GetGreatCircleDistance(lat1, lon1, lat2, lon2);
1503 cout << "d = " << d;
1505 //cout << ", d_theta = " << d;
1506 //cout << ", and d = " << Rad2Nm(d) << ' ';
1507 if(sin(lon2 - lon1) < 0) {
1509 h = acos((sin(lat2)-sin(lat1)*cos(d))/(sin(d)*cos(lat1)));
1512 h = 2.0 * SG_PI - acos((sin(lat2)-sin(lat1)*cos(d))/(sin(d)*cos(lat1)));
1515 cout << h * SG_RADIANS_TO_DEGREES << '\n';
1518 return( mod(atan2(sin(lon2-lon1)*cos(lat2),
1519 cos(lat1)*sin(lat2)-sin(lat1)*cos(lat2)*cos(lon2-lon1)),
1523 // Return a position on a radial from wp1 given distance d (nm) and magnetic heading h (degrees)
1524 // Note that d should be less that 1/4 Earth diameter!
1525 GPSWaypoint DCLGPS::GetPositionOnMagRadial(const GPSWaypoint& wp1, double d, double h) {
1526 h += sgGetMagVar(wp1.lon, wp1.lat, 0.0, _time->getJD()) * SG_RADIANS_TO_DEGREES;
1527 return(GetPositionOnRadial(wp1, d, h));
1530 // Return a position on a radial from wp1 given distance d (nm) and TRUE heading h (degrees)
1531 // Note that d should be less that 1/4 Earth diameter!
1532 GPSWaypoint DCLGPS::GetPositionOnRadial(const GPSWaypoint& wp1, double d, double h) {
1533 while(h < 0.0) h += 360.0;
1534 while(h > 360.0) h -= 360.0;
1536 h *= SG_DEGREES_TO_RADIANS;
1537 d *= (SG_PI / (180.0 * 60.0));
1539 double lat=asin(sin(wp1.lat)*cos(d)+cos(wp1.lat)*sin(d)*cos(h));
1542 lon=wp1.lon; // endpoint a pole
1544 lon=mod(wp1.lon+asin(sin(h)*sin(d)/cos(lat))+SG_PI,2*SG_PI)-SG_PI;
1550 wp.type = GPS_WP_VIRT;
1554 // Returns cross-track deviation in Nm.
1555 double DCLGPS::CalcCrossTrackDeviation() const {
1556 return(CalcCrossTrackDeviation(_fromWaypoint, _activeWaypoint));
1559 // Returns cross-track deviation of the current position between two arbitary waypoints in nm.
1560 double DCLGPS::CalcCrossTrackDeviation(const GPSWaypoint& wp1, const GPSWaypoint& wp2) const {
1561 //if(wp1 == NULL || wp2 == NULL) return(0.0);
1562 if(wp1.id.empty() || wp2.id.empty()) return(0.0);
1563 double xtd = asin(sin(Nm2Rad(GetGreatCircleDistance(wp1.lat, wp1.lon, _gpsLat, _gpsLon)))
1564 * sin(GetGreatCircleCourse(wp1.lat, wp1.lon, _gpsLat, _gpsLon) - GetGreatCircleCourse(wp1.lat, wp1.lon, wp2.lat, wp2.lon)));
1565 return(Rad2Nm(xtd));