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.
5 //
6 // Written by David Luff, started 2005.
7 //
8 // Copyright (C) 2005 - David C Luff: daveluff --AT-- ntlworld --D0T-- com
9 //
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.
14 //
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.
19 //
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.
23 //
24 // $Id$
25
26 #include "dclgps.hxx"
27
28 #include <simgear/sg_inlines.h>
29 #include <simgear/misc/sg_path.hxx>
30 #include <simgear/io/iostreams/sgstream.hxx>
31 #include <simgear/timing/sg_time.hxx>
32 #include <simgear/magvar/magvar.hxx>
33 #include <simgear/structure/exception.hxx>
34
35 #include <Main/fg_props.hxx>
36 #include <Navaids/fix.hxx>
37 #include <Navaids/navrecord.hxx>
38 #include <Airports/airport.hxx>
39 #include <Airports/runways.hxx>
40
41 #include <fstream>
42 #include <iostream>
43 #include <cstdlib>
44
45 using namespace std;
46
GPSWaypoint()47 GPSWaypoint::GPSWaypoint() {
48 appType = GPS_APP_NONE;
49 }
50
GPSWaypoint(const std::string & aIdent,float aLat,float aLon,GPSWpType aType)51 GPSWaypoint::GPSWaypoint(const std::string& aIdent, float aLat, float aLon, GPSWpType aType) :
52 id(aIdent),
53 lat(aLat),
54 lon(aLon),
55 type(aType),
56 appType(GPS_APP_NONE)
57 {
58 }
59
~GPSWaypoint()60 GPSWaypoint::~GPSWaypoint() {}
61
GetAprId()62 string GPSWaypoint::GetAprId() {
63 if(appType == GPS_IAF) return(id + 'i');
64 else if(appType == GPS_FAF) return(id + 'f');
65 else if(appType == GPS_MAP) return(id + 'm');
66 else if(appType == GPS_MAHP) return(id + 'h');
67 else return(id);
68 }
69
70 static GPSWpType
GPSWpTypeFromFGPosType(FGPositioned::Type aType)71 GPSWpTypeFromFGPosType(FGPositioned::Type aType)
72 {
73 switch (aType) {
74 case FGPositioned::AIRPORT:
75 case FGPositioned::SEAPORT:
76 case FGPositioned::HELIPORT:
77 return GPS_WP_APT;
78
79 case FGPositioned::VOR:
80 return GPS_WP_VOR;
81
82 case FGPositioned::NDB:
83 return GPS_WP_NDB;
84
85 case FGPositioned::WAYPOINT:
86 return GPS_WP_USR;
87
88 case FGPositioned::FIX:
89 return GPS_WP_INT;
90
91 default:
92 return GPS_WP_USR;
93 }
94 }
95
createFromPositioned(const FGPositioned * aPos)96 GPSWaypoint* GPSWaypoint::createFromPositioned(const FGPositioned* aPos)
97 {
98 if (!aPos) {
99 return NULL; // happens if find returns no match
100 }
101
102 return new GPSWaypoint(aPos->ident(),
103 aPos->latitude() * SG_DEGREES_TO_RADIANS,
104 aPos->longitude() * SG_DEGREES_TO_RADIANS,
105 GPSWpTypeFromFGPosType(aPos->type())
106 );
107 }
108
operator <<(ostream & os,GPSAppWpType type)109 ostream& operator << (ostream& os, GPSAppWpType type) {
110 switch(type) {
111 case(GPS_IAF): return(os << "IAF");
112 case(GPS_IAP): return(os << "IAP");
113 case(GPS_FAF): return(os << "FAF");
114 case(GPS_MAP): return(os << "MAP");
115 case(GPS_MAHP): return(os << "MAHP");
116 case(GPS_HDR): return(os << "HEADER");
117 case(GPS_FENCE): return(os << "FENCE");
118 case(GPS_APP_NONE): return(os << "NONE");
119 }
120 return(os << "ERROR - Unknown switch in GPSAppWpType operator << ");
121 }
122
FGIAP()123 FGIAP::FGIAP() {
124 }
125
~FGIAP()126 FGIAP::~FGIAP() {
127 }
128
FGNPIAP()129 FGNPIAP::FGNPIAP() {
130 }
131
~FGNPIAP()132 FGNPIAP::~FGNPIAP() {
133 }
134
ClockTime()135 ClockTime::ClockTime() {
136 _hr = 0;
137 _min = 0;
138 }
139
ClockTime(int hr,int min)140 ClockTime::ClockTime(int hr, int min) {
141 while(hr < 0) { hr += 24; }
142 _hr = hr % 24;
143 while(min < 0) { min += 60; }
144 while(min > 60) { min -= 60; }
145 _min = min;
146 }
147
~ClockTime()148 ClockTime::~ClockTime() {
149 }
150
151 // ------------------------------------------------------------------------------------- //
152
DCLGPS(RenderArea2D * instrument)153 DCLGPS::DCLGPS(RenderArea2D* instrument) {
154 _instrument = instrument;
155 _nFields = 1;
156 _maxFields = 2;
157
158 _lon_node = fgGetNode("/instrumentation/gps/indicated-longitude-deg", true);
159 _lat_node = fgGetNode("/instrumentation/gps/indicated-latitude-deg", true);
160 _alt_node = fgGetNode("/instrumentation/gps/indicated-altitude-ft", true);
161 _grnd_speed_node = fgGetNode("/instrumentation/gps/indicated-ground-speed-kt", true);
162 _true_track_node = fgGetNode("/instrumentation/gps/indicated-track-true-deg", true);
163 _mag_track_node = fgGetNode("/instrumentation/gps/indicated-track-magnetic-deg", true);
164
165 // Use FG's position values at construction in case FG's gps has not run first update yet.
166 _lon = fgGetDouble("/position/longitude-deg") * SG_DEGREES_TO_RADIANS;
167 _lat = fgGetDouble("/position/latitude-deg") * SG_DEGREES_TO_RADIANS;
168 _alt = fgGetDouble("/position/altitude-ft");
169 // Note - we can depriciate _gpsLat and _gpsLon if we implement error handling in FG
170 // gps code and not our own.
171 _gpsLon = _lon;
172 _gpsLat = _lat;
173 _checkLon = _gpsLon;
174 _checkLat = _gpsLat;
175 _groundSpeed_ms = 0.0;
176 _groundSpeed_kts = 0.0;
177 _track = 0.0;
178 _magTrackDeg = 0.0;
179
180 // Sensible defaults. These can be overriden by derived classes if desired.
181 _cdiScales.clear();
182 _cdiScales.push_back(5.0);
183 _cdiScales.push_back(1.0);
184 _cdiScales.push_back(0.3);
185 _currentCdiScaleIndex = 0;
186 _targetCdiScaleIndex = 0;
187 _sourceCdiScaleIndex = 0;
188 _cdiScaleTransition = false;
189 _currentCdiScale = 5.0;
190
191 _cleanUpPage = -1;
192
193 _activeWaypoint.id.clear();
194 _dist2Act = 0.0;
195 _crosstrackDist = 0.0;
196 _headingBugTo = true;
197 _navFlagged = true;
198 _waypointAlert = false;
199 _departed = false;
200 _departureTimeString = "----";
201 _elapsedTime = 0.0;
202 _powerOnTime.set_hr(0);
203 _powerOnTime.set_min(0);
204 _powerOnTimerSet = false;
205 _alarmSet = false;
206
207 // Configuration Initialisation
208 // Should this be in kln89.cxx ?
209 _turnAnticipationEnabled = false;
210
211 _messageStack.clear();
212
213 _dto = false;
214
215 _approachLoaded = false;
216 _approachArm = false;
217 _approachReallyArmed = false;
218 _approachActive = false;
219 _approachFP = new GPSFlightPlan;
220 }
221
~DCLGPS()222 DCLGPS::~DCLGPS() {
223 delete _approachFP; // Don't need to delete the waypoints inside since they point to
224 // the waypoints in the approach database.
225 // TODO - may need to delete the approach database!!
226 }
227
draw(osg::State & state)228 void DCLGPS::draw(osg::State& state) {
229 _instrument->Draw(state);
230 }
231
init()232 void DCLGPS::init() {
233
234 // Not sure if this should be here, but OK for now.
235 CreateDefaultFlightPlans();
236
237 LoadApproachData();
238 }
239
bind()240 void DCLGPS::bind() {
241 _tiedProperties.setRoot(fgGetNode("/instrumentation/gps", true));
242 _tiedProperties.Tie("waypoint-alert", this, &DCLGPS::GetWaypointAlert);
243 _tiedProperties.Tie("leg-mode", this, &DCLGPS::GetLegMode);
244 _tiedProperties.Tie("obs-mode", this, &DCLGPS::GetOBSMode);
245 _tiedProperties.Tie("approach-arm", this, &DCLGPS::GetApproachArm);
246 _tiedProperties.Tie("approach-active", this, &DCLGPS::GetApproachActive);
247 _tiedProperties.Tie("cdi-deflection", this, &DCLGPS::GetCDIDeflection);
248 _tiedProperties.Tie("to-flag", this, &DCLGPS::GetToFlag);
249 }
250
unbind()251 void DCLGPS::unbind() {
252 _tiedProperties.Untie();
253 }
254
update(double dt)255 void DCLGPS::update(double dt) {
256 //cout << "update called!\n";
257
258 _lon = _lon_node->getDoubleValue() * SG_DEGREES_TO_RADIANS;
259 _lat = _lat_node->getDoubleValue() * SG_DEGREES_TO_RADIANS;
260 _alt = _alt_node->getDoubleValue();
261 _groundSpeed_kts = _grnd_speed_node->getDoubleValue();
262 _groundSpeed_ms = _groundSpeed_kts * 0.5144444444;
263 _track = _true_track_node->getDoubleValue();
264 _magTrackDeg = _mag_track_node->getDoubleValue();
265 // Note - we can depriciate _gpsLat and _gpsLon if we implement error handling in FG
266 // gps code and not our own.
267 _gpsLon = _lon;
268 _gpsLat = _lat;
269 // Check for abnormal position slew
270 if(GetGreatCircleDistance(_gpsLat, _gpsLon, _checkLat, _checkLon) > 1.0) {
271 OrientateToActiveFlightPlan();
272 }
273 _checkLon = _gpsLon;
274 _checkLat = _gpsLat;
275
276 // TODO - check for unit power before running this.
277 if(!_powerOnTimerSet) {
278 SetPowerOnTimer();
279 }
280
281 // Check if an alarm timer has expired
282 if(_alarmSet) {
283 if(_alarmTime.hr() == atoi(fgGetString("/instrumentation/clock/indicated-hour"))
284 && _alarmTime.min() == atoi(fgGetString("/instrumentation/clock/indicated-min"))) {
285 _messageStack.push_back("*Timer Expired");
286 _alarmSet = false;
287 }
288 }
289
290 if(!_departed) {
291 if(_groundSpeed_kts > 30.0) {
292 _departed = true;
293 string th = fgGetString("/instrumentation/clock/indicated-hour");
294 string tm = fgGetString("/instrumentation/clock/indicated-min");
295 if(th.size() == 1) th = "0" + th;
296 if(tm.size() == 1) tm = "0" + tm;
297 _departureTimeString = th + tm;
298 }
299 } else {
300 // TODO - check - is this prone to drift error over time?
301 // Should we difference the departure and current times?
302 // What about when the user resets the time of day from the menu?
303 _elapsedTime += dt;
304 }
305
306 // FIXME - currently all the below assumes leg mode and no DTO or OBS cancelled.
307 if(_activeFP->IsEmpty()) {
308 // Not sure if we need to reset these each update or only when fp altered
309 _activeWaypoint.id.clear();
310 _navFlagged = true;
311 } else if(_activeFP->waypoints.size() == 1) {
312 _activeWaypoint.id.clear();
313 } else {
314 _navFlagged = false;
315 if(_activeWaypoint.id.empty() || _fromWaypoint.id.empty()) {
316 //cout << "Error, in leg mode with flightplan of 2 or more waypoints, but either active or from wp is NULL!\n";
317 OrientateToActiveFlightPlan();
318 }
319
320 // Approach stuff
321 if(_approachLoaded) {
322 if(!_approachReallyArmed && !_approachActive) {
323 // arm if within 30nm of airport.
324 // TODO - let user cancel approach arm using external GPS-APR switch
325 bool multi;
326 const FGAirport* ap = FindFirstAptById(_approachID, multi, true);
327 if(ap != NULL) {
328 double d = GetGreatCircleDistance(_gpsLat, _gpsLon, ap->getLatitude() * SG_DEGREES_TO_RADIANS, ap->getLongitude() * SG_DEGREES_TO_RADIANS);
329 if(d <= 30) {
330 _approachArm = true;
331 _approachReallyArmed = true;
332 _messageStack.push_back("*Press ALT To Set Baro");
333 // Not sure what we do if the user has already set CDI to 0.3 nm?
334 _targetCdiScaleIndex = 1;
335 if(_currentCdiScaleIndex == 1) {
336 // No problem!
337 } else if(_currentCdiScaleIndex == 0) {
338 _sourceCdiScaleIndex = 0;
339 _cdiScaleTransition = true;
340 _cdiTransitionTime = 30.0;
341 _currentCdiScale = _cdiScales[_currentCdiScaleIndex];
342 }
343 }
344 }
345 } else {
346 // Check for approach active - we can only activate approach if it is really armed.
347 if(_activeWaypoint.appType == GPS_FAF) {
348 //cout << "Active waypoint is FAF, id is " << _activeWaypoint.id << '\n';
349 if(GetGreatCircleDistance(_gpsLat, _gpsLon, _activeWaypoint.lat, _activeWaypoint.lon) <= 2.0 && !_obsMode) {
350 // Assume heading is OK for now
351 _approachArm = false; // TODO - check - maybe arm is left on when actv comes on?
352 _approachReallyArmed = false;
353 _approachActive = true;
354 _targetCdiScaleIndex = 2;
355 if(_currentCdiScaleIndex == 2) {
356 // No problem!
357 } else if(_currentCdiScaleIndex == 1) {
358 _sourceCdiScaleIndex = 1;
359 _cdiScaleTransition = true;
360 _cdiTransitionTime = 30.0; // TODO - compress it if time to FAF < 30sec
361 _currentCdiScale = _cdiScales[_currentCdiScaleIndex];
362 } else {
363 // Abort going active?
364 _approachActive = false;
365 }
366 }
367 }
368 }
369 }
370
371 // CDI scale transition stuff
372 if(_cdiScaleTransition) {
373 if(fabs(_currentCdiScale - _cdiScales[_targetCdiScaleIndex]) < 0.001) {
374 _currentCdiScale = _cdiScales[_targetCdiScaleIndex];
375 _currentCdiScaleIndex = _targetCdiScaleIndex;
376 _cdiScaleTransition = false;
377 } else {
378 double scaleDiff = (_targetCdiScaleIndex > _sourceCdiScaleIndex
379 ? _cdiScales[_sourceCdiScaleIndex] - _cdiScales[_targetCdiScaleIndex]
380 : _cdiScales[_targetCdiScaleIndex] - _cdiScales[_sourceCdiScaleIndex]);
381 //cout << "ScaleDiff = " << scaleDiff << '\n';
382 if(_targetCdiScaleIndex > _sourceCdiScaleIndex) {
383 // Scaling down eg. 5nm -> 1nm
384 _currentCdiScale -= (scaleDiff * dt / _cdiTransitionTime);
385 if(_currentCdiScale < _cdiScales[_targetCdiScaleIndex]) {
386 _currentCdiScale = _cdiScales[_targetCdiScaleIndex];
387 _currentCdiScaleIndex = _targetCdiScaleIndex;
388 _cdiScaleTransition = false;
389 }
390 } else {
391 _currentCdiScale += (scaleDiff * dt / _cdiTransitionTime);
392 if(_currentCdiScale > _cdiScales[_targetCdiScaleIndex]) {
393 _currentCdiScale = _cdiScales[_targetCdiScaleIndex];
394 _currentCdiScaleIndex = _targetCdiScaleIndex;
395 _cdiScaleTransition = false;
396 }
397 }
398 //cout << "_currentCdiScale = " << _currentCdiScale << '\n';
399 }
400 } else {
401 _currentCdiScale = _cdiScales[_currentCdiScaleIndex];
402 }
403
404
405 // Urgh - I've been setting the heading bug based on DTK,
406 // bug I think it should be based on heading re. active waypoint
407 // based on what the sim does after the final waypoint is passed.
408 // (DTK remains the same, but if track is held == DTK heading bug
409 // reverses to from once wp is passed).
410 /*
411 if(_fromWaypoint != NULL) {
412 // TODO - how do we handle the change of track with distance over long legs?
413 _dtkTrue = GetGreatCircleCourse(_fromWaypoint->lat, _fromWaypoint->lon, _activeWaypoint->lat, _activeWaypoint->lon) * SG_RADIANS_TO_DEGREES;
414 _dtkMag = GetMagHeadingFromTo(_fromWaypoint->lat, _fromWaypoint->lon, _activeWaypoint->lat, _activeWaypoint->lon);
415 // Don't change the heading bug if speed is too low otherwise it flickers to/from at rest
416 if(_groundSpeed_ms > 5) {
417 //cout << "track = " << _track << ", dtk = " << _dtkTrue << '\n';
418 double courseDev = _track - _dtkTrue;
419 //cout << "courseDev = " << courseDev << ", normalized = ";
420 SG_NORMALIZE_RANGE(courseDev, -180.0, 180.0);
421 //cout << courseDev << '\n';
422 _headingBugTo = (fabs(courseDev) > 90.0 ? false : true);
423 }
424 } else {
425 _dtkTrue = 0.0;
426 _dtkMag = 0.0;
427 // TODO - in DTO operation the position of initiation of DTO defines the "from waypoint".
428 }
429 */
430 if(!_activeWaypoint.id.empty()) {
431 double hdgTrue = GetGreatCircleCourse(_gpsLat, _gpsLon, _activeWaypoint.lat, _activeWaypoint.lon) * SG_RADIANS_TO_DEGREES;
432 if(_groundSpeed_ms > 5) {
433 //cout << "track = " << _track << ", hdgTrue = " << hdgTrue << '\n';
434 double courseDev = _track - hdgTrue;
435 //cout << "courseDev = " << courseDev << ", normalized = ";
436 SG_NORMALIZE_RANGE(courseDev, -180.0, 180.0);
437 //cout << courseDev << '\n';
438 _headingBugTo = (fabs(courseDev) > 90.0 ? false : true);
439 }
440 if(!_fromWaypoint.id.empty()) {
441 _dtkTrue = GetGreatCircleCourse(_fromWaypoint.lat, _fromWaypoint.lon, _activeWaypoint.lat, _activeWaypoint.lon) * SG_RADIANS_TO_DEGREES;
442 _dtkMag = GetMagHeadingFromTo(_fromWaypoint.lat, _fromWaypoint.lon, _activeWaypoint.lat, _activeWaypoint.lon);
443 } else {
444 _dtkTrue = 0.0;
445 _dtkMag = 0.0;
446 }
447 }
448
449 _dist2Act = GetGreatCircleDistance(_gpsLat, _gpsLon, _activeWaypoint.lat, _activeWaypoint.lon) * SG_NM_TO_METER;
450 if(_groundSpeed_ms > 10.0) {
451 _eta = _dist2Act / _groundSpeed_ms;
452 if(_eta <= 36) { // TODO - this is slightly different if turn anticipation is enabled.
453 if(_headingBugTo) {
454 _waypointAlert = true; // TODO - not if the from flag is set.
455 }
456 }
457 if(_eta < 60) {
458 // Check if we should sequence to next leg.
459 // Perhaps this should be done on distance instead, but 60s time (about 1 - 2 nm) seems reasonable for now.
460 //double reverseHeading = GetGreatCircleCourse(_activeWaypoint->lat, _activeWaypoint->lon, _fromWaypoint->lat, _fromWaypoint->lon);
461 // Hack - let's cheat and do it on heading bug for now. TODO - that stops us 'cutting the corner'
462 // when we happen to approach the inside turn of a waypoint - we should probably sequence at the midpoint
463 // of the heading difference between legs in this instance.
464 int idx = GetActiveWaypointIndex();
465 bool finalLeg = (idx == (int)(_activeFP->waypoints.size()) - 1 ? true : false);
466 bool finalDto = (_dto && idx == -1); // Dto operation to a waypoint not in the flightplan - we don't sequence in this instance
467 if(!_headingBugTo) {
468 if(finalLeg) {
469 // Do nothing - not sure if Dto should switch off when arriving at the final waypoint of a flightplan
470 } else if(finalDto) {
471 // Do nothing
472 } else if(_activeWaypoint.appType == GPS_MAP) {
473 // Don't sequence beyond the missed approach point
474 //cout << "ACTIVE WAYPOINT is MAP - not sequencing!!!!!\n";
475 } else {
476 //cout << "Sequencing...\n";
477 _fromWaypoint = _activeWaypoint;
478 _activeWaypoint = *_activeFP->waypoints[idx + 1];
479 _dto = false;
480 // Course alteration message format is dependent on whether we are slaved HSI/CDI indicator or not.
481 string s;
482 if(fgGetBool("/instrumentation/nav[0]/slaved-to-gps")) {
483 // TODO - avoid the hardwiring on nav[0]
484 s = "Adj Nav Crs to ";
485 } else {
486 s = "GPS Course is ";
487 }
488 double d = GetMagHeadingFromTo(_fromWaypoint.lat, _fromWaypoint.lon, _activeWaypoint.lat, _activeWaypoint.lon);
489 while(d < 0.0) d += 360.0;
490 while(d >= 360.0) d -= 360.0;
491 char buf[4];
492 snprintf(buf, 4, "%03i", (int)(d + 0.5));
493 s += buf;
494 _messageStack.push_back(s);
495 }
496 _waypointAlert = false;
497 }
498 }
499 } else {
500 _eta = 0.0;
501 }
502
503 /*
504 // First attempt at a sensible cross-track correction calculation
505 // Uh? - I think this is implemented further down the file!
506 if(_fromWaypoint != NULL) {
507
508 } else {
509 _crosstrackDist = 0.0;
510 }
511 */
512 }
513 }
514
515 /*
516 Expand a SIAP ident to the full procedure name (as shown on the approach chart).
517 NOTE: Some of this is inferred from data, some is from documentation.
518
519 Example expansions from ARINC 424-18 [and the airport they're taken from]:
520 "R10LY" <--> "RNAV (GPS) Y RWY 10 L" [KBOI]
521 "R10-Z" <--> "RNAV (GPS) Z RWY 10" [KHTO]
522 "S25" <--> "VOR or GPS RWY 25" [KHHR]
523 "P20" <--> "GPS RWY 20" [KDAN]
524 "NDB-B" <--> "NDB or GPS-B" [KDAW]
525 "NDBC" <--> "NDB or GPS-C" [KEMT]
526 "VDMA" <--> "VOR/DME or GPS-A" [KDAW]
527 "VDM-A" <--> "VOR/DME or GPS-A" [KEAG]
528 "VDMB" <--> "VOR/DME or GPS-B" [KDKX]
529 "VORA" <--> "VOR or GPS-A" [KEMT]
530
531 It seems that there are 2 basic types of expansions; those that include
532 the runway and those that don't. Of those that don't, it seems that 2
533 different positions within the string to encode the identifying letter
534 are used, i.e. with a dash and without.
535 */
ExpandSIAPIdent(const string & ident)536 string DCLGPS::ExpandSIAPIdent(const string& ident) {
537 string name;
538 bool has_rwy = false;
539
540 switch(ident[0]) {
541 case 'N': name = "NDB or GPS"; has_rwy = false; break;
542 case 'P': name = "GPS"; has_rwy = true; break;
543 case 'R': name = "RNAV (GPS)"; has_rwy = true; break;
544 case 'S': name = "VOR or GPS"; has_rwy = true; break;
545 case 'V':
546 if(ident[1] == 'D') name = "VOR/DME or GPS";
547 else name = "VOR or GPS";
548 has_rwy = false;
549 break;
550 default: // TODO output a log message
551 break;
552 }
553
554 if(has_rwy) {
555 // Add the identifying letter if present
556 if(ident.size() == 5) {
557 name += ' ';
558 name += ident[4];
559 }
560
561 // Add the runway
562 name += " RWY ";
563 name += ident.substr(1, 2);
564
565 // Add a left/right/centre indication if present.
566 if(ident.size() > 3) {
567 if((ident[3] != '-') && (ident[3] != ' ')) { // Early versions of the spec allowed a blank instead of a dash so check for both
568 name += ' ';
569 name += ident[3];
570 }
571 }
572 } else {
573 // Add the identifying letter, which I *think* should always be present, but seems to be inconsistent as to whether a dash is used.
574 if(ident.size() == 5) {
575 name += '-';
576 name += ident[4];
577 } else if(ident.size() == 4) {
578 name += '-';
579 name += ident[3];
580 } else {
581 // No suffix letter
582 }
583 }
584
585 return(name);
586 }
587
588 /*
589 Load instrument approaches from an ARINC 424 file.
590 Tested on ARINC 424-18.
591 Known / current best guess at the format:
592 Col 1: Always 'S'. If it isn't, ditch it.
593 Col 2-4: "Customer area" code, eg "USA", "CAN". I think that CAN is used for Alaska.
594 Col 5: Section code. Used in conjunction with sub-section code. Definitions are with sub-section code.
595 Col 6: Always blank.
596 Col 7-10: ICAO (or FAA) airport ident. Left justified if < 4 chars.
597 Col 11-12: Based on ICAO geographical region.
598 Col 13: Sub-section code. Used in conjunction with section code.
599 "HD/E/F" => Helicopter record.
600 "HS" => Helicopter minimum safe altitude.
601 "PA" => Airport record.
602 "PF" => Approach segment.
603 "PG" => Runway record.
604 "PP" => Path point record. ???
605 "PS" => MSA record (minimum safe altitude).
606
607 ------ The following is for "PF", approach segment -------
608
609 Col 14-19: SIAP ident for this approach (left justified). This is a standardised abbreviated approach name.
610 e.g. "R10LZ" expands to "RNAV (GPS) Z RWY 10 L". See the comment block for ExpandSIAPIdent for full details.
611 Col 20: Route type. This is tricky - I don't have full documentation and am having to guess a bit.
612 'A' => Arrival route? This seems to be used to encode arrival routes from the IAF to the approach proper.
613 Note that the final fix of the arrival route is duplicated in the approach proper.
614 'D' => VOR/DME or GPS
615 'N' => NDB or GPS
616 'P' => GPS (ARINC 424-18), GPS and RNAV (GPS) (ARINC 424-15 and before).
617 'R' => RNAV (GPS) (ARINC 424-18).
618 'S' => VOR or GPS
619 Col 21-25: Transition identifier. AFAICT, this is the ident of the IAF for this initial approach route, and left blank for the final approach course. See col 30-34 for the actual fix ident.
620 Col 26: BLANK
621 Col 27-29: Sequence number - position within the route segment. Rule: 10-20-30 etc.
622 Col 30-34: Fix identifer. The ident of the waypoint.
623 Col 35-36: ICAO geographical region code. I think we can ignore this for now.
624 Col 37: Section code - ??? I don't know what this means
625 Col 38 Subsection code - ??? ditto - no idea!
626 Col 40: Waypoint type.
627 'A' => Airport as waypoint
628 'E' => Essential waypoint (e.g. change of heading at this waypoint, etc).
629 'G' => Runway or helipad as waypoint
630 'H' => Heliport as waypoint
631 'N' => NDB as waypoint
632 'P' => Phantom waypoint (not sure if this is used in rev 18?)
633 'V' => VOR as waypoint
634 Col 41: Waypoint type.
635 'B' => Flyover, approach transition, or final approach.
636 'E' => end of route segment (transition waypoint). (Actually "End of terminal procedure route type" in the docs).
637 'N' => ??? I've also seen 'N' in this column, but don't know what it indicates.
638 'Y' => Flyover.
639 Col 43: Waypoint type. May also be blank when none of the below.
640 'A' => Initial approach fix (IAF)
641 'F' => Final approach fix
642 'H' => Holding fix
643 'I' => Final approach course fix
644 'M' => Missed approach point
645 '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!
646 ??? Possibly procedure turn?
647 'C' => ??? This is also present in the data, but missing from the docs. Is at airport 00R.
648 Col 107-111 MSA center fix. We can ignore this.
649 */
LoadApproachData()650 void DCLGPS::LoadApproachData() {
651 FGNPIAP* iap = NULL;
652 GPSWaypoint* wp;
653 GPSFlightPlan* fp;
654 const GPSWaypoint* cwp;
655
656 sg_ifstream fin;
657 SGPath path = globals->get_fg_root();
658 path.append("Navaids/rnav.dat");
659 fin.open(path, ios::in);
660 if(!fin) {
661 //cout << "Unable to open input file " << path.c_str() << '\n';
662 return;
663 } else {
664 //cout << "Opened " << path.c_str() << " for reading\n";
665 }
666 char tmp[256];
667 string s;
668
669 string apt_ident; // This gets set to the ICAO code of the current airport being processed.
670 string iap_ident; // The abbreviated name of the current approach being processed.
671 string wp_ident; // The ident of the waypoint of the current line
672 string last_apt_ident;
673 string last_iap_ident;
674 string last_wp_ident;
675 // There is no need to save the full name - it can be generated on the fly from the abbreviated name as and when needed.
676 bool apt_in_progress = false; // Set true whilst loading all the approaches for a given airport.
677 bool iap_in_progress = false; // Set true whilst loading a given approach.
678 bool iap_error = false; // Set true if there is an error loading a given approach.
679 bool route_in_progress = false; // Set true when we are loading a "route" segment of the approach.
680 int last_sequence_number = 0; // Position within the route, rule (rev 18): 10, 20, 30 etc.
681 int sequence_number;
682 char last_route_type = 0;
683 char route_type;
684 char waypoint_fix_type; // This is the waypoint type from col 43, i.e. the type of fix. May be blank.
685
686 int j;
687
688 // Debugging info
689 unsigned int nLoaded = 0;
690 unsigned int nErrors = 0;
691
692 //for(i=0; i<64; ++i) {
693 while(!fin.eof()) {
694 fin.getline(tmp, 256);
695 //s = Fake_rnav_dat[i];
696 s = tmp;
697 if(s.size() < 132) continue;
698 if(s[0] == 'S') { // Valid line
699 string country_code = s.substr(1, 3);
700 if(country_code == "USA") { // For now we'll stick to US procedures in case there are unknown gotchas with others
701 if(s[4] == 'P') { // Includes approaches.
702 if(s[12] == 'A') { // Airport record
703 apt_ident = s.substr(6, 4);
704 // Trim any whitespace from the ident. The ident is left justified,
705 // so any space will be at the end.
706 if(apt_ident[3] == ' ') apt_ident = apt_ident.substr(0, 3);
707 // I think that all idents are either 3 or 4 chars - could check this though!
708 if(!apt_in_progress) {
709 last_apt_ident = apt_ident;
710 apt_in_progress = 1;
711 } else {
712 if(last_apt_ident != apt_ident) {
713 if(iap_in_progress) {
714 if(iap_error) {
715 //cout << "ERROR: Unable to load approach " << iap->_ident << " at " << iap->_aptIdent << '\n';
716 nErrors++;
717 } else {
718 _np_iap[iap->_aptIdent].push_back(iap);
719 //cout << "** Loaded " << iap->_aptIdent << "\t" << iap->_ident << '\n';
720 nLoaded++;
721 }
722 iap_in_progress = false;
723 }
724 }
725 last_apt_ident = apt_ident;
726 }
727 iap_in_progress = 0;
728 } else if(s[12] == 'F') { // Approach segment
729 if(apt_in_progress) {
730 iap_ident = s.substr(13, 6);
731 // Trim any whitespace from the RH end.
732 for(j=0; j<6; ++j) {
733 if(iap_ident[5-j] == ' ') {
734 iap_ident = iap_ident.substr(0, 5-j);
735 } else {
736 // It's important to break here, since earlier versions of ARINC 424 allowed spaces in the ident.
737 break;
738 }
739 }
740 if(iap_in_progress) {
741 if(iap_ident != last_iap_ident) {
742 // This is a new approach - store the last one and trigger
743 // starting afresh by setting the in progress flag to false.
744 if(iap_error) {
745 //cout << "ERROR: Unable to load approach " << iap->_ident << " at " << iap->_aptIdent << '\n';
746 nErrors++;
747 } else {
748 _np_iap[iap->_aptIdent].push_back(iap);
749 //cout << "Loaded " << iap->_aptIdent << "\t" << iap->_ident << '\n';
750 nLoaded++;
751 }
752 iap_in_progress = false;
753 }
754 }
755 if(!iap_in_progress) {
756 iap = new FGNPIAP;
757 iap->_aptIdent = apt_ident;
758 iap->_ident = iap_ident;
759 iap->_name = ExpandSIAPIdent(iap_ident); // I suspect that it's probably better to just store idents, and to expand the names as needed.
760 // Note, we haven't set iap->_rwyStr yet.
761 last_iap_ident = iap_ident;
762 iap_in_progress = true;
763 iap_error = false;
764 }
765
766 // Route type
767 route_type = s[19];
768 sequence_number = atoi(s.substr(26,3).c_str());
769 wp_ident = s.substr(29, 5);
770 waypoint_fix_type = s[42];
771 // Trim any whitespace from the RH end
772 for(j=0; j<5; ++j) {
773 if(wp_ident[4-j] == ' ') {
774 wp_ident = wp_ident.substr(0, 4-j);
775 } else {
776 break;
777 }
778 }
779
780 // Ignore lines with no waypoint ID for now - these are normally part of the
781 // missed approach procedure, and we don't use them in the KLN89.
782 if(!wp_ident.empty()) {
783 // Make a local copy of the waypoint for now, since we're not yet sure if we'll be using it
784 GPSWaypoint w;
785 w.id = wp_ident;
786 bool wp_error = false;
787 if(w.id.substr(0, 2) == "RW" && waypoint_fix_type == 'M') {
788 // Assume that this is a missed-approach point based on the runway number, which appears to be standard for most approaches.
789 // Note: Currently fgFindAirportID returns NULL on error, but getRunwayByIdent throws an exception.
790 const FGAirport* apt = fgFindAirportID(iap->_aptIdent);
791 if(apt) {
792 string rwystr;
793 try {
794 rwystr = w.id.substr(2, 2);
795 // TODO - sanity check the rwystr at this point to ensure we have a double digit number
796 if(w.id.size() > 4) {
797 if(w.id[4] == 'L' || w.id[4] == 'C' || w.id[4] == 'R') {
798 rwystr += w.id[4];
799 }
800 }
801 FGRunway* rwy = apt->getRunwayByIdent(rwystr);
802 w.lat = rwy->begin().getLatitudeRad();
803 w.lon = rwy->begin().getLongitudeRad();
804 } catch(const sg_exception&) {
805 SG_LOG(SG_INSTR, SG_WARN, "Unable to find runway " << w.id.substr(2, 2) << " at airport " << iap->_aptIdent);
806 //cout << "Unable to find runway " << w.id.substr(2, 2) << " at airport " << iap->_aptIdent << " ( w.id = " << w.id << ", rwystr = " << rwystr << " )\n";
807 wp_error = true;
808 }
809 } else {
810 wp_error = true;
811 }
812 } else {
813 cwp = FindFirstByExactId(w.id);
814 if(cwp) {
815 w = *cwp;
816 } else {
817 wp_error = true;
818 }
819 }
820 switch(waypoint_fix_type) {
821 case 'A': w.appType = GPS_IAF; break;
822 case 'F': w.appType = GPS_FAF; break;
823 case 'H': w.appType = GPS_MAHP; break;
824 case 'I': w.appType = GPS_IAP; break;
825 case 'M': w.appType = GPS_MAP; break;
826 case ' ': w.appType = GPS_APP_NONE; break;
827 //default: cout << "Unknown waypoint_fix_type: \'" << waypoint_fix_type << "\' [" << apt_ident << ", " << iap_ident << "]\n";
828 }
829
830 if(wp_error) {
831 //cout << "Unable to find waypoint " << w.id << " [" << apt_ident << ", " << iap_ident << "]\n";
832 iap_error = true;
833 }
834
835 if(!wp_error) {
836 if(route_in_progress) {
837 if(sequence_number > last_sequence_number) {
838 // TODO - add a check for runway numbers
839 // Check for the waypoint ID being the same as the previous line.
840 // This is often the case for the missed approach holding point.
841 if(wp_ident == last_wp_ident) {
842 if(waypoint_fix_type == 'H') {
843 if(!iap->_IAP.empty()) {
844 if(iap->_IAP[iap->_IAP.size() - 1]->appType == GPS_APP_NONE) {
845 iap->_IAP[iap->_IAP.size() - 1]->appType = GPS_MAHP;
846 } else {
847 //cout << "Waypoint is MAHP and another type! " << w.id << " [" << apt_ident << ", " << iap_ident << "]\n";
848 }
849 }
850 }
851 } else {
852 // Create a new waypoint on the heap, copy the local copy into it, and push it onto the approach.
853 wp = new GPSWaypoint;
854 *wp = w;
855 if(route_type == 'A') {
856 fp->waypoints.push_back(wp);
857 } else {
858 iap->_IAP.push_back(wp);
859 }
860 }
861 } else if(sequence_number == last_sequence_number) {
862 // This seems to happen once per final approach route - one of the waypoints
863 // is duplicated with the same sequence number - I'm not sure what information
864 // the second line give yet so ignore it for now.
865 // TODO - figure this out!
866 } else {
867 // Finalise the current route and start a new one
868 //
869 // Finalise the current route
870 if(last_route_type == 'A') {
871 // Push the flightplan onto the approach
872 iap->_approachRoutes.push_back(fp);
873 } else {
874 // All the waypoints get pushed individually - don't need to do it.
875 }
876 // Start a new one
877 // There are basically 2 possibilities here - either it's one of the arrival transitions,
878 // or it's the core final approach course.
879 wp = new GPSWaypoint;
880 *wp = w;
881 if(route_type == 'A') { // It's one of the arrival transition(s)
882 fp = new GPSFlightPlan;
883 fp->waypoints.push_back(wp);
884 } else {
885 iap->_IAP.push_back(wp);
886 }
887 route_in_progress = true;
888 }
889 } else {
890 // Start a new route.
891 // There are basically 2 possibilities here - either it's one of the arrival transitions,
892 // or it's the core final approach course.
893 wp = new GPSWaypoint;
894 *wp = w;
895 if(route_type == 'A') { // It's one of the arrival transition(s)
896 fp = new GPSFlightPlan;
897 fp->waypoints.push_back(wp);
898 } else {
899 iap->_IAP.push_back(wp);
900 }
901 route_in_progress = true;
902 }
903 last_route_type = route_type;
904 last_wp_ident = wp_ident;
905 last_sequence_number = sequence_number;
906 }
907 }
908 } else {
909 // ERROR - no airport record read.
910 }
911 }
912 } else {
913 // Check and finalise any approaches in progress
914 // TODO - sanity check that the approach has all the required elements
915 if(iap_in_progress) {
916 // This is a new approach - store the last one and trigger
917 // starting afresh by setting the in progress flag to false.
918 if(iap_error) {
919 //cout << "ERROR: Unable to load approach " << iap->_ident << " at " << iap->_aptIdent << '\n';
920 nErrors++;
921 } else {
922 _np_iap[iap->_aptIdent].push_back(iap);
923 //cout << "* Loaded " << iap->_aptIdent << "\t" << iap->_ident << '\n';
924 nLoaded++;
925 }
926 iap_in_progress = false;
927 }
928 }
929 }
930 }
931 }
932
933 // If we get to the end of the file, load any approach that is still in progress
934 // TODO - sanity check that the approach has all the required elements
935 if(iap_in_progress) {
936 if(iap_error) {
937 //cout << "ERROR: Unable to load approach " << iap->_ident << " at " << iap->_aptIdent << '\n';
938 nErrors++;
939 } else {
940 _np_iap[iap->_aptIdent].push_back(iap);
941 //cout << "*** Loaded " << iap->_aptIdent << "\t" << iap->_ident << '\n';
942 nLoaded++;
943 }
944 }
945
946 //cout << "Done loading approach database\n";
947 //cout << "Loaded: " << nLoaded << '\n';
948 //cout << "Failed: " << nErrors << '\n';
949
950 fin.close();
951 }
952
GetActiveWaypoint()953 GPSWaypoint* DCLGPS::GetActiveWaypoint() {
954 return &_activeWaypoint;
955 }
956
957 // Returns meters
GetDistToActiveWaypoint()958 float DCLGPS::GetDistToActiveWaypoint() {
959 return _dist2Act;
960 }
961
962 // I don't yet fully understand all the gotchas about where to source time from.
963 // This function sets the initial timer before the clock exports properties
964 // and the one below uses the clock to be consistent with the rest of the code.
965 // It might change soonish...
SetPowerOnTimer()966 void DCLGPS::SetPowerOnTimer() {
967 struct tm *t = globals->get_time_params()->getGmt();
968 _powerOnTime.set_hr(t->tm_hour);
969 _powerOnTime.set_min(t->tm_min);
970 _powerOnTimerSet = true;
971 }
972
ResetPowerOnTimer()973 void DCLGPS::ResetPowerOnTimer() {
974 _powerOnTime.set_hr(atoi(fgGetString("/instrumentation/clock/indicated-hour")));
975 _powerOnTime.set_min(atoi(fgGetString("/instrumentation/clock/indicated-min")));
976 _powerOnTimerSet = true;
977 }
978
GetCDIDeflection() const979 double DCLGPS::GetCDIDeflection() const {
980 double xtd = CalcCrossTrackDeviation(); //nm
981 return((xtd / _currentCdiScale) * 5.0 * 2.5 * -1.0);
982 }
983
DtoInitiate(const string & s)984 void DCLGPS::DtoInitiate(const string& s) {
985 const GPSWaypoint* wp = FindFirstByExactId(s);
986 if(wp) {
987 // TODO - Currently we start DTO operation unconditionally, regardless of which mode we are in.
988 // In fact, the following rules apply:
989 // In LEG mode, start DTO as we currently do.
990 // In OBS mode, set the active waypoint to the requested waypoint, and then:
991 // If the KLN89 is not connected to an external HSI or CDI, set the OBS course to go direct to the waypoint.
992 // If the KLN89 *is* connected to an external HSI or CDI, it cannot set the course itself, and will display
993 // a scratchpad message with the course to set manually on the HSI/CDI.
994 // In both OBS cases, leave _dto false, since we don't need the virtual waypoint created.
995 _dto = true;
996 _activeWaypoint = *wp;
997 _fromWaypoint.lat = _gpsLat;
998 _fromWaypoint.lon = _gpsLon;
999 _fromWaypoint.type = GPS_WP_VIRT;
1000 _fromWaypoint.id = "_DTOWP_";
1001 delete wp;
1002 } else {
1003 // TODO - Should bring up the user waypoint page.
1004 _dto = false;
1005 }
1006 }
1007
DtoCancel()1008 void DCLGPS::DtoCancel() {
1009 if(_dto) {
1010 // i.e. don't bother reorientating if we're just cancelling a DTO button press
1011 // without having previously initiated DTO.
1012 OrientateToActiveFlightPlan();
1013 }
1014 _dto = false;
1015 }
1016
ToggleOBSMode()1017 void DCLGPS::ToggleOBSMode() {
1018 _obsMode = !_obsMode;
1019 if(_obsMode) {
1020 // We need to set the OBS heading. The rules for this are:
1021 //
1022 // If the KLN89 is connected to an external HSI or CDI, then the OBS heading must be set
1023 // to the OBS radial on the external indicator, and cannot be changed in the KLN89.
1024 //
1025 // If the KLN89 is not connected to an external indicator, then:
1026 // If there is an active waypoint, the OBS heading is set such that the
1027 // deviation indicator remains at the same deviation (i.e. set to DTK,
1028 // although there may be some small difference).
1029 //
1030 // If there is not an active waypoint, I am not sure what value should be
1031 // set.
1032 //
1033 if(fgGetBool("/instrumentation/nav/slaved-to-gps")) {
1034 _obsHeading = static_cast<int>(fgGetDouble("/instrumentation/nav/radials/selected-deg") + 0.5);
1035 } else if(!_activeWaypoint.id.empty()) {
1036 // NOTE: I am not sure if this is completely correct. There are sometimes small differences
1037 // between DTK and default OBS heading in the simulator (~ 1 or 2 degrees). It might also
1038 // be that OBS heading should be stored as float and only displayed as int, in order to maintain
1039 // the initial bar deviation?
1040 _obsHeading = static_cast<int>(_dtkMag + 0.5);
1041 //cout << "_dtkMag = " << _dtkMag << ", _dtkTrue = " << _dtkTrue << ", _obsHeading = " << _obsHeading << '\n';
1042 } else {
1043 // TODO - what should we really do here?
1044 _obsHeading = 0;
1045 }
1046
1047 // Valid OBS heading values are 0 -> 359 degrees inclusive (from kln89 simulator).
1048 if(_obsHeading > 359) _obsHeading -= 360;
1049 if(_obsHeading < 0) _obsHeading += 360;
1050
1051 // TODO - the _fromWaypoint location will change as the OBS heading changes.
1052 // Might need to store the OBS initiation position somewhere in case it is needed again.
1053 SetOBSFromWaypoint();
1054 }
1055 }
1056
1057 // Set the _fromWaypoint position based on the active waypoint and OBS radial.
SetOBSFromWaypoint()1058 void DCLGPS::SetOBSFromWaypoint() {
1059 if(!_obsMode) return;
1060 if(_activeWaypoint.id.empty()) return;
1061
1062 // TODO - base the 180 deg correction on the to/from flag.
1063 _fromWaypoint = GetPositionOnMagRadial(_activeWaypoint, 10, _obsHeading + 180.0);
1064 _fromWaypoint.type = GPS_WP_VIRT;
1065 _fromWaypoint.id = "_OBSWP_";
1066 }
1067
CDIFSDIncrease()1068 void DCLGPS::CDIFSDIncrease() {
1069 if(_currentCdiScaleIndex == 0) {
1070 _currentCdiScaleIndex = _cdiScales.size() - 1;
1071 } else {
1072 _currentCdiScaleIndex--;
1073 }
1074 }
1075
CDIFSDDecrease()1076 void DCLGPS::CDIFSDDecrease() {
1077 _currentCdiScaleIndex++;
1078 if(_currentCdiScaleIndex == _cdiScales.size()) {
1079 _currentCdiScaleIndex = 0;
1080 }
1081 }
1082
DrawChar(char c,int field,int px,int py,bool bold)1083 void DCLGPS::DrawChar(char c, int field, int px, int py, bool bold) {
1084 }
1085
DrawText(const string & s,int field,int px,int py,bool bold)1086 void DCLGPS::DrawText(const string& s, int field, int px, int py, bool bold) {
1087 }
1088
CreateDefaultFlightPlans()1089 void DCLGPS::CreateDefaultFlightPlans() {}
1090
1091 // Get the time to the active waypoint in seconds.
1092 // Returns -1 if groundspeed < 30 kts
GetTimeToActiveWaypoint()1093 double DCLGPS::GetTimeToActiveWaypoint() {
1094 if(_groundSpeed_kts < 30.0) {
1095 return(-1.0);
1096 } else {
1097 return(_eta);
1098 }
1099 }
1100
1101 // Get the time to the final waypoint in seconds.
1102 // Returns -1 if groundspeed < 30 kts
GetETE()1103 double DCLGPS::GetETE() {
1104 if(_groundSpeed_kts < 30.0) {
1105 return(-1.0);
1106 } else {
1107 // TODO - handle OBS / DTO operation appropriately
1108 if(_activeFP->waypoints.empty()) {
1109 return(-1.0);
1110 } else {
1111 return(GetTimeToWaypoint(_activeFP->waypoints[_activeFP->waypoints.size() - 1]->id));
1112 }
1113 }
1114 }
1115
1116 // Get the time to a given waypoint (spec'd by ID) in seconds.
1117 // returns -1 if groundspeed is less than 30kts.
1118 // If the waypoint is an unreached part of the active flight plan the time will be via each leg.
1119 // otherwise it will be a direct-to time.
GetTimeToWaypoint(const string & id)1120 double DCLGPS::GetTimeToWaypoint(const string& id) {
1121 if(_groundSpeed_kts < 30.0) {
1122 return(-1.0);
1123 }
1124
1125 double eta = 0.0;
1126 int n1 = GetActiveWaypointIndex();
1127 int n2 = GetWaypointIndex(id);
1128 if(n2 > n1) {
1129 eta = _eta;
1130 for(unsigned int i=n1+1; i<_activeFP->waypoints.size(); ++i) {
1131 GPSWaypoint* wp1 = _activeFP->waypoints[i-1];
1132 GPSWaypoint* wp2 = _activeFP->waypoints[i];
1133 double distm = GetGreatCircleDistance(wp1->lat, wp1->lon, wp2->lat, wp2->lon) * SG_NM_TO_METER;
1134 eta += (distm / _groundSpeed_ms);
1135 }
1136 return(eta);
1137 } else if(id == _activeWaypoint.id) {
1138 return(_eta);
1139 } else {
1140 const GPSWaypoint* wp = FindFirstByExactId(id);
1141 if(wp == NULL) return(-1.0);
1142 double distm = GetGreatCircleDistance(_gpsLat, _gpsLon, wp->lat, wp->lon);
1143 delete wp;
1144 return(distm / _groundSpeed_ms);
1145 }
1146 return(-1.0); // Hopefully we never get here!
1147 }
1148
1149 // Returns magnetic great-circle heading
1150 // TODO - document units.
GetHeadingToActiveWaypoint()1151 float DCLGPS::GetHeadingToActiveWaypoint() {
1152 if(_activeWaypoint.id.empty()) {
1153 return(-888);
1154 } else {
1155 double h = GetMagHeadingFromTo(_gpsLat, _gpsLon, _activeWaypoint.lat, _activeWaypoint.lon);
1156 while(h <= 0.0) h += 360.0;
1157 while(h > 360.0) h -= 360.0;
1158 return((float)h);
1159 }
1160 }
1161
1162 // Returns magnetic great-circle heading
1163 // TODO - what units?
GetHeadingFromActiveWaypoint()1164 float DCLGPS::GetHeadingFromActiveWaypoint() {
1165 if(_activeWaypoint.id.empty()) {
1166 return(-888);
1167 } else {
1168 double h = GetMagHeadingFromTo(_activeWaypoint.lat, _activeWaypoint.lon, _gpsLat, _gpsLon);
1169 while(h <= 0.0) h += 360.0;
1170 while(h > 360.0) h -= 360.0;
1171 return(h);
1172 }
1173 }
1174
ClearFlightPlan(int n)1175 void DCLGPS::ClearFlightPlan(int n) {
1176 for(unsigned int i=0; i<_flightPlans[n]->waypoints.size(); ++i) {
1177 delete _flightPlans[n]->waypoints[i];
1178 }
1179 _flightPlans[n]->waypoints.clear();
1180 }
1181
ClearFlightPlan(GPSFlightPlan * fp)1182 void DCLGPS::ClearFlightPlan(GPSFlightPlan* fp) {
1183 for(unsigned int i=0; i<fp->waypoints.size(); ++i) {
1184 delete fp->waypoints[i];
1185 }
1186 fp->waypoints.clear();
1187 }
1188
GetActiveWaypointIndex()1189 int DCLGPS::GetActiveWaypointIndex() {
1190 for(unsigned int i=0; i<_flightPlans[0]->waypoints.size(); ++i) {
1191 if(_flightPlans[0]->waypoints[i]->id == _activeWaypoint.id) return((int)i);
1192 }
1193 return(-1);
1194 }
1195
GetWaypointIndex(const string & id)1196 int DCLGPS::GetWaypointIndex(const string& id) {
1197 for(unsigned int i=0; i<_flightPlans[0]->waypoints.size(); ++i) {
1198 if(_flightPlans[0]->waypoints[i]->id == id) return((int)i);
1199 }
1200 return(-1);
1201 }
1202
OrientateToFlightPlan(GPSFlightPlan * fp)1203 void DCLGPS::OrientateToFlightPlan(GPSFlightPlan* fp) {
1204 //cout << "Orientating...\n";
1205 //cout << "_lat = " << _lat << ", _lon = " << _lon << ", _gpsLat = " << _gpsLat << ", gpsLon = " << _gpsLon << '\n';
1206 if(fp->IsEmpty()) {
1207 _activeWaypoint.id.clear();
1208 _navFlagged = true;
1209 } else {
1210 _navFlagged = false;
1211 if(fp->waypoints.size() == 1) {
1212 // TODO - may need to flag nav here if not dto or obs, or possibly handle it somewhere else.
1213 _activeWaypoint = *fp->waypoints[0];
1214 _fromWaypoint.id.clear();
1215 } else {
1216 // FIXME FIXME FIXME
1217 _fromWaypoint = *fp->waypoints[0];
1218 _activeWaypoint = *fp->waypoints[1];
1219 double dmin = 1000000; // nm!!
1220 // For now we will simply start on the leg closest to our current position.
1221 // It's possible that more fancy algorithms may take either heading or track
1222 // into account when setting inital leg - I'm not sure.
1223 // This method should handle most cases perfectly OK though.
1224 for(unsigned int i = 1; i < fp->waypoints.size(); ++i) {
1225 //cout << "Pass " << i << ", dmin = " << dmin << ", leg is " << fp->waypoints[i-1]->id << " to " << fp->waypoints[i]->id << '\n';
1226 // First get the cross track correction.
1227 double d0 = fabs(CalcCrossTrackDeviation(*fp->waypoints[i-1], *fp->waypoints[i]));
1228 // That is the shortest distance away we could be though - check for
1229 // longer distances if we are 'off the end' of the leg.
1230 double ht1 = GetGreatCircleCourse(fp->waypoints[i-1]->lat, fp->waypoints[i-1]->lon,
1231 fp->waypoints[i]->lat, fp->waypoints[i]->lon)
1232 * SG_RADIANS_TO_DEGREES;
1233 // not simply the reverse of the above due to great circle navigation.
1234 double ht2 = GetGreatCircleCourse(fp->waypoints[i]->lat, fp->waypoints[i]->lon,
1235 fp->waypoints[i-1]->lat, fp->waypoints[i-1]->lon)
1236 * SG_RADIANS_TO_DEGREES;
1237 double hw1 = GetGreatCircleCourse(_gpsLat, _gpsLon,
1238 fp->waypoints[i]->lat, fp->waypoints[i]->lon)
1239 * SG_RADIANS_TO_DEGREES;
1240 double hw2 = GetGreatCircleCourse(_gpsLat, _gpsLon,
1241 fp->waypoints[i-1]->lat, fp->waypoints[i-1]->lon)
1242 * SG_RADIANS_TO_DEGREES;
1243 double h1 = ht1 - hw1;
1244 double h2 = ht2 - hw2;
1245 //cout << "d0, h1, h2 = " << d0 << ", " << h1 << ", " << h2 << '\n';
1246 //cout << "Normalizing...\n";
1247 SG_NORMALIZE_RANGE(h1, -180.0, 180.0);
1248 SG_NORMALIZE_RANGE(h2, -180.0, 180.0);
1249 //cout << "d0, h1, h2 = " << d0 << ", " << h1 << ", " << h2 << '\n';
1250 if(fabs(h1) > 90.0) {
1251 // We are past the end of the to waypoint
1252 double d = GetGreatCircleDistance(_gpsLat, _gpsLon, fp->waypoints[i]->lat, fp->waypoints[i]->lon);
1253 if(d > d0) d0 = d;
1254 //cout << "h1 triggered, d0 now = " << d0 << '\n';
1255 } else if(fabs(h2) > 90.0) {
1256 // We are past the end (not yet at!) the from waypoint
1257 double d = GetGreatCircleDistance(_gpsLat, _gpsLon, fp->waypoints[i-1]->lat, fp->waypoints[i-1]->lon);
1258 if(d > d0) d0 = d;
1259 //cout << "h2 triggered, d0 now = " << d0 << '\n';
1260 }
1261 if(d0 < dmin) {
1262 //cout << "THIS LEG NOW ACTIVE!\n";
1263 dmin = d0;
1264 _fromWaypoint = *fp->waypoints[i-1];
1265 _activeWaypoint = *fp->waypoints[i];
1266 }
1267 }
1268 }
1269 }
1270 }
1271
OrientateToActiveFlightPlan()1272 void DCLGPS::OrientateToActiveFlightPlan() {
1273 OrientateToFlightPlan(_activeFP);
1274 }
1275
1276 /***************************************/
1277
1278 // Utility function - create a flightplan from a list of waypoint ids and types
CreateFlightPlan(GPSFlightPlan * fp,vector<string> ids,vector<GPSWpType> wps)1279 void DCLGPS::CreateFlightPlan(GPSFlightPlan* fp, vector<string> ids, vector<GPSWpType> wps) {
1280 if(fp == NULL) fp = new GPSFlightPlan;
1281 unsigned int i;
1282 if(!fp->waypoints.empty()) {
1283 for(i=0; i<fp->waypoints.size(); ++i) {
1284 delete fp->waypoints[i];
1285 }
1286 fp->waypoints.clear();
1287 }
1288 if(ids.size() != wps.size()) {
1289 cout << "ID and Waypoint types list size mismatch in GPS::CreateFlightPlan - no flightplan created!\n";
1290 return;
1291 }
1292 for(i=0; i<ids.size(); ++i) {
1293 bool multi;
1294 const FGAirport* ap;
1295 FGNavRecord* np;
1296 GPSWaypoint* wp = new GPSWaypoint;
1297 wp->type = wps[i];
1298 switch(wp->type) {
1299 case GPS_WP_APT:
1300 ap = FindFirstAptById(ids[i], multi, true);
1301 if(ap == NULL) {
1302 // error
1303 delete wp;
1304 } else {
1305 wp->lat = ap->getLatitude() * SG_DEGREES_TO_RADIANS;
1306 wp->lon = ap->getLongitude() * SG_DEGREES_TO_RADIANS;
1307 wp->id = ids[i];
1308 fp->waypoints.push_back(wp);
1309 }
1310 break;
1311 case GPS_WP_VOR:
1312 np = FindFirstVorById(ids[i], multi, true);
1313 if(np == NULL) {
1314 // error
1315 delete wp;
1316 } else {
1317 wp->lat = np->get_lat() * SG_DEGREES_TO_RADIANS;
1318 wp->lon = np->get_lon() * SG_DEGREES_TO_RADIANS;
1319 wp->id = ids[i];
1320 fp->waypoints.push_back(wp);
1321 }
1322 break;
1323 case GPS_WP_NDB:
1324 np = FindFirstNDBById(ids[i], multi, true);
1325 if(np == NULL) {
1326 // error
1327 delete wp;
1328 } else {
1329 wp->lat = np->get_lat() * SG_DEGREES_TO_RADIANS;
1330 wp->lon = np->get_lon() * SG_DEGREES_TO_RADIANS;
1331 wp->id = ids[i];
1332 fp->waypoints.push_back(wp);
1333 }
1334 break;
1335 case GPS_WP_INT:
1336 // TODO TODO
1337 break;
1338 case GPS_WP_USR:
1339 // TODO
1340 break;
1341 case GPS_WP_VIRT:
1342 // TODO
1343 break;
1344 }
1345 }
1346 }
1347
1348 /***************************************/
1349
1350 class DCLGPSFilter : public FGPositioned::Filter
1351 {
1352 public:
pass(FGPositioned * aPos) const1353 virtual bool pass(FGPositioned* aPos) const
1354 {
1355 switch (aPos->type()) {
1356 case FGPositioned::AIRPORT:
1357 // how about heliports and seaports?
1358 case FGPositioned::NDB:
1359 case FGPositioned::VOR:
1360 case FGPositioned::WAYPOINT:
1361 case FGPositioned::FIX:
1362 break;
1363 default: return false; // reject all other types
1364 }
1365 return true;
1366 }
1367 };
1368
FindFirstById(const string & id) const1369 GPSWaypoint* DCLGPS::FindFirstById(const string& id) const
1370 {
1371 DCLGPSFilter filter;
1372 FGPositionedList matches = FGPositioned::findAllWithIdent(id, &filter, false);
1373 if (matches.empty()) {
1374 return NULL;
1375 }
1376
1377 FGPositioned::sortByRange(matches, SGGeod::fromRad(_lon, _lat));
1378 return GPSWaypoint::createFromPositioned(matches.front());
1379 }
1380
FindFirstByExactId(const string & id) const1381 GPSWaypoint* DCLGPS::FindFirstByExactId(const string& id) const
1382 {
1383 SGGeod pos(SGGeod::fromRad(_lon, _lat));
1384 FGPositionedRef result = FGPositioned::findClosestWithIdent(id, pos);
1385 return GPSWaypoint::createFromPositioned(result);
1386 }
1387
1388 // TODO - add the ASCII / alphabetical stuff from the Atlas version
FindTypedFirstById(const string & id,FGPositioned::Type ty,bool & multi,bool exact)1389 FGPositioned* DCLGPS::FindTypedFirstById(const string& id, FGPositioned::Type ty, bool &multi, bool exact)
1390 {
1391 multi = false;
1392 FGPositioned::TypeFilter filter(ty);
1393
1394 FGPositionedList matches =
1395 FGPositioned::findAllWithIdent(id, &filter, exact);
1396 if (matches.empty()) {
1397 return NULL;
1398 }
1399
1400 FGPositioned::sortByRange(matches, SGGeod::fromRad(_lon, _lat));
1401 return matches.front();
1402 }
1403
FindFirstVorById(const string & id,bool & multi,bool exact)1404 FGNavRecord* DCLGPS::FindFirstVorById(const string& id, bool &multi, bool exact)
1405 {
1406 return dynamic_cast<FGNavRecord*>(FindTypedFirstById(id, FGPositioned::VOR, multi, exact));
1407 }
1408
FindFirstNDBById(const string & id,bool & multi,bool exact)1409 FGNavRecord* DCLGPS::FindFirstNDBById(const string& id, bool &multi, bool exact)
1410 {
1411 return dynamic_cast<FGNavRecord*>(FindTypedFirstById(id, FGPositioned::NDB, multi, exact));
1412 }
1413
FindFirstIntById(const string & id,bool & multi,bool exact)1414 const FGFix* DCLGPS::FindFirstIntById(const string& id, bool &multi, bool exact)
1415 {
1416 return dynamic_cast<FGFix*>(FindTypedFirstById(id, FGPositioned::FIX, multi, exact));
1417 }
1418
FindFirstAptById(const string & id,bool & multi,bool exact)1419 const FGAirport* DCLGPS::FindFirstAptById(const string& id, bool &multi, bool exact)
1420 {
1421 return dynamic_cast<FGAirport*>(FindTypedFirstById(id, FGPositioned::AIRPORT, multi, exact));
1422 }
1423
FindClosestVor(double lat_rad,double lon_rad)1424 FGNavRecord* DCLGPS::FindClosestVor(double lat_rad, double lon_rad) {
1425 FGPositioned::TypeFilter filter(FGPositioned::VOR);
1426 double cutoff = 1000; // nautical miles
1427 FGPositionedRef v = FGPositioned::findClosest(SGGeod::fromRad(lon_rad, lat_rad), cutoff, &filter);
1428 if (!v) {
1429 return NULL;
1430 }
1431
1432 return dynamic_cast<FGNavRecord*>(v.ptr());
1433 }
1434
1435 //----------------------------------------------------------------------------------------------------------
1436
1437 // Takes lat and lon in RADIANS!!!!!!!
GetMagHeadingFromTo(double latA,double lonA,double latB,double lonB)1438 double DCLGPS::GetMagHeadingFromTo(double latA, double lonA, double latB, double lonB) {
1439 double h = GetGreatCircleCourse(latA, lonA, latB, lonB);
1440 h *= SG_RADIANS_TO_DEGREES;
1441 // TODO - use the real altitude below instead of 0.0!
1442 //cout << "MagVar = " << sgGetMagVar(_gpsLon, _gpsLat, 0.0, _time->getJD()) * SG_RADIANS_TO_DEGREES << '\n';
1443 double jd = globals->get_time_params()->getJD();
1444 h -= sgGetMagVar(_gpsLon, _gpsLat, 0.0, jd) * SG_RADIANS_TO_DEGREES;
1445 while(h >= 360.0) h -= 360.0;
1446 while(h < 0.0) h += 360.0;
1447 return(h);
1448 }
1449
1450 // ---------------- Great Circle formulae from "The Aviation Formulary" -------------
1451 // Note that all of these assume that the world is spherical.
1452
Rad2Nm(double theta)1453 double Rad2Nm(double theta) {
1454 return(((180.0*60.0)/SG_PI)*theta);
1455 }
1456
Nm2Rad(double d)1457 double Nm2Rad(double d) {
1458 return((SG_PI/(180.0*60.0))*d);
1459 }
1460
1461 /* QUOTE:
1462
1463 The great circle distance d between two points with coordinates {lat1,lon1} and {lat2,lon2} is given by:
1464
1465 d=acos(sin(lat1)*sin(lat2)+cos(lat1)*cos(lat2)*cos(lon1-lon2))
1466
1467 A mathematically equivalent formula, which is less subject to rounding error for short distances is:
1468
1469 d=2*asin(sqrt((sin((lat1-lat2)/2))^2 +
1470 cos(lat1)*cos(lat2)*(sin((lon1-lon2)/2))^2))
1471
1472 */
1473
1474 // Returns distance in nm, takes lat & lon in RADIANS
GetGreatCircleDistance(double lat1,double lon1,double lat2,double lon2) const1475 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))));
1478 return(Rad2Nm(d));
1479 }
1480
1481 // fmod dosen't do what we want :-(
mod(double d1,double d2)1482 static double mod(double d1, double d2) {
1483 return(d1 - d2*floor(d1/d2));
1484 }
1485
1486 // Returns great circle course from point 1 to point 2
1487 // Input and output in RADIANS.
GetGreatCircleCourse(double lat1,double lon1,double lat2,double lon2) const1488 double DCLGPS::GetGreatCircleCourse (double lat1, double lon1, double lat2, double lon2) const {
1489 //double h = 0.0;
1490 /*
1491 // Special case the poles
1492 if(cos(lat1) < SG_EPSILON) {
1493 if(lat1 > 0) {
1494 // Starting from North Pole
1495 h = SG_PI;
1496 } else {
1497 // Starting from South Pole
1498 h = 2.0 * SG_PI;
1499 }
1500 } else {
1501 // Urgh - the formula below is for negative lon +ve !!!???
1502 double d = GetGreatCircleDistance(lat1, lon1, lat2, lon2);
1503 cout << "d = " << d;
1504 d = Nm2Rad(d);
1505 //cout << ", d_theta = " << d;
1506 //cout << ", and d = " << Rad2Nm(d) << ' ';
1507 if(sin(lon2 - lon1) < 0) {
1508 cout << " A ";
1509 h = acos((sin(lat2)-sin(lat1)*cos(d))/(sin(d)*cos(lat1)));
1510 } else {
1511 cout << " B ";
1512 h = 2.0 * SG_PI - acos((sin(lat2)-sin(lat1)*cos(d))/(sin(d)*cos(lat1)));
1513 }
1514 }
1515 cout << h * SG_RADIANS_TO_DEGREES << '\n';
1516 */
1517
1518 return( mod(atan2(sin(lon2-lon1)*cos(lat2),
1519 cos(lat1)*sin(lat2)-sin(lat1)*cos(lat2)*cos(lon2-lon1)),
1520 2.0*SG_PI) );
1521 }
1522
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!
GetPositionOnMagRadial(const GPSWaypoint & wp1,double d,double h)1525 GPSWaypoint DCLGPS::GetPositionOnMagRadial(const GPSWaypoint& wp1, double d, double h) {
1526 double jd = globals->get_time_params()->getJD();
1527 h += sgGetMagVar(wp1.lon, wp1.lat, 0.0, jd) * SG_RADIANS_TO_DEGREES;
1528 return(GetPositionOnRadial(wp1, d, h));
1529 }
1530
1531 // Return a position on a radial from wp1 given distance d (nm) and TRUE heading h (degrees)
1532 // Note that d should be less that 1/4 Earth diameter!
GetPositionOnRadial(const GPSWaypoint & wp1,double d,double h)1533 GPSWaypoint DCLGPS::GetPositionOnRadial(const GPSWaypoint& wp1, double d, double h) {
1534 while(h < 0.0) h += 360.0;
1535 while(h > 360.0) h -= 360.0;
1536
1537 h *= SG_DEGREES_TO_RADIANS;
1538 d *= (SG_PI / (180.0 * 60.0));
1539
1540 double lat=asin(sin(wp1.lat)*cos(d)+cos(wp1.lat)*sin(d)*cos(h));
1541 double lon;
1542 if(cos(lat)==0) {
1543 lon=wp1.lon; // endpoint a pole
1544 } else {
1545 lon=mod(wp1.lon+asin(sin(h)*sin(d)/cos(lat))+SG_PI,2*SG_PI)-SG_PI;
1546 }
1547
1548 GPSWaypoint wp;
1549 wp.lat = lat;
1550 wp.lon = lon;
1551 wp.type = GPS_WP_VIRT;
1552 return(wp);
1553 }
1554
1555 // Returns cross-track deviation in Nm.
CalcCrossTrackDeviation() const1556 double DCLGPS::CalcCrossTrackDeviation() const {
1557 return(CalcCrossTrackDeviation(_fromWaypoint, _activeWaypoint));
1558 }
1559
1560 // Returns cross-track deviation of the current position between two arbitary waypoints in nm.
CalcCrossTrackDeviation(const GPSWaypoint & wp1,const GPSWaypoint & wp2) const1561 double DCLGPS::CalcCrossTrackDeviation(const GPSWaypoint& wp1, const GPSWaypoint& wp2) const {
1562 //if(wp1 == NULL || wp2 == NULL) return(0.0);
1563 if(wp1.id.empty() || wp2.id.empty()) return(0.0);
1564 double xtd = asin(sin(Nm2Rad(GetGreatCircleDistance(wp1.lat, wp1.lon, _gpsLat, _gpsLon)))
1565 * sin(GetGreatCircleCourse(wp1.lat, wp1.lon, _gpsLat, _gpsLon) - GetGreatCircleCourse(wp1.lat, wp1.lon, wp2.lat, wp2.lon)));
1566 return(Rad2Nm(xtd));
1567 }
1568
AlignedProjection()1569 AlignedProjection::AlignedProjection()
1570 {
1571 SGGeod g; // ctor initializes to zero
1572 Init( g, 0.0 );
1573 }
1574
AlignedProjection(const SGGeod & centre,double heading)1575 AlignedProjection::AlignedProjection(const SGGeod& centre, double heading)
1576 {
1577 Init( centre, heading );
1578 }
1579
~AlignedProjection()1580 AlignedProjection::~AlignedProjection() {
1581 }
1582
Init(const SGGeod & centre,double heading)1583 void AlignedProjection::Init(const SGGeod& centre, double heading) {
1584 _origin = centre;
1585 _theta = heading * SG_DEGREES_TO_RADIANS;
1586 _correction_factor = cos(_origin.getLatitudeRad());
1587 }
1588
ConvertToLocal(const SGGeod & pt)1589 SGVec3d AlignedProjection::ConvertToLocal(const SGGeod& pt) {
1590 // convert from lat/lon to orthogonal
1591 double delta_lat = pt.getLatitudeRad() - _origin.getLatitudeRad();
1592 double delta_lon = pt.getLongitudeRad() - _origin.getLongitudeRad();
1593 double y = sin(delta_lat) * SG_EQUATORIAL_RADIUS_M;
1594 double x = sin(delta_lon) * SG_EQUATORIAL_RADIUS_M * _correction_factor;
1595
1596 // Align
1597 if(_theta != 0.0) {
1598 double xbar = x;
1599 x = x*cos(_theta) - y*sin(_theta);
1600 y = (xbar*sin(_theta)) + (y*cos(_theta));
1601 }
1602
1603 return SGVec3d(x, y, pt.getElevationM());
1604 }
1605
ConvertFromLocal(const SGVec3d & pt)1606 SGGeod AlignedProjection::ConvertFromLocal(const SGVec3d& pt) {
1607 // de-align
1608 double thi = _theta * -1.0;
1609 double x = pt.x()*cos(thi) - pt.y()*sin(thi);
1610 double y = (pt.x()*sin(thi)) + (pt.y()*cos(thi));
1611
1612 // convert from orthogonal to lat/lon
1613 double delta_lat = asin(y / SG_EQUATORIAL_RADIUS_M);
1614 double delta_lon = asin(x / SG_EQUATORIAL_RADIUS_M) / _correction_factor;
1615
1616 return SGGeod::fromRadM(_origin.getLongitudeRad()+delta_lon, _origin.getLatitudeRad()+delta_lat, pt.z());
1617 }
1618