1 // dclgps.hxx - 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.
29 #include <Cockpit/render_area_2d.hxx>
36 #include <simgear/structure/subsystem_mgr.hxx>
37 #include <simgear/props/props.hxx>
38 #include <simgear/props/tiedpropertylist.hxx>
39 #include <Navaids/positioned.hxx>
49 // --------------------- Waypoint / Flightplan stuff -----------------------------
50 // This should be merged with other similar stuff in FG at some point.
52 // NOTE - ORDERING IS IMPORTANT HERE - it matches the Bendix-King page ordering!
59 GPS_WP_VIRT // Used for virtual waypoints, such as the start of DTO operation.
63 GPS_IAF, // Initial approach fix
64 GPS_IAP, // Waypoint on approach sequence that isn't any of the others.
65 GPS_FAF, // Final approach fix
66 GPS_MAP, // Missed approach point
67 GPS_MAHP, // Initial missed approach holding point.
68 GPS_HDR, // A virtual 'waypoint' to represent the approach header in the fpl page
69 GPS_FENCE, // A virtual 'waypoint' to represent the NO WPT SEQ fence.
70 GPS_APP_NONE // Not part of the approach sequence - the default.
73 std::ostream& operator << (std::ostream& os, GPSAppWpType type);
78 GPSWaypoint(const std::string& aIdent, float lat, float lon, GPSWpType aType);
80 static GPSWaypoint* createFromPositioned(const FGPositioned* aFix);
83 std::string GetAprId(); // Returns the id with i, f, m or h added if appropriate. (Initial approach fix, final approach fix, etc)
88 GPSAppWpType appType; // only used for waypoints that are part of an approach sequence
91 typedef std::vector < GPSWaypoint* > gps_waypoint_array;
92 typedef gps_waypoint_array::iterator gps_waypoint_array_iterator;
93 typedef std::map < std::string, gps_waypoint_array > gps_waypoint_map;
94 typedef gps_waypoint_map::iterator gps_waypoint_map_iterator;
95 typedef gps_waypoint_map::const_iterator gps_waypoint_map_const_iterator;
99 std::vector<GPSWaypoint*> waypoints;
100 inline bool IsEmpty() { return waypoints.empty(); }
103 // TODO - probably de-public the internals of the next 2 classes and add some methods!
104 // Instrument approach procedure base class
108 virtual ~FGIAP() = 0;
111 std::string _aptIdent; // The ident of the airport this approach is for
112 std::string _ident; // The approach ident.
113 std::string _name; // The full approach name.
114 std::string _rwyStr; // The string used to specify the rwy - eg "B" in this instance.
115 bool _precision; // True for precision approach, false for non-precision.
118 // Non-precision instrument approach procedure
119 class FGNPIAP : public FGIAP {
125 std::vector<GPSFlightPlan*> _approachRoutes; // The approach route(s) from the IAF(s) to the IF.
126 // NOTE: It is an assumption in the code that uses this that there is a unique IAF per approach route.
127 std::vector<GPSWaypoint*> _IAP; // The compulsory waypoints of the approach procedure (may duplicate one of the above).
128 // _IAP includes the FAF and MAF, and the missed approach waypoints.
131 typedef std::vector < FGIAP* > iap_list_type;
132 typedef std::map < std::string, iap_list_type > iap_map_type;
133 typedef iap_map_type::iterator iap_map_iterator;
135 // A class to encapsulate hr:min representation of time.
140 ClockTime(int hr, int min);
142 inline void set_hr(int hr) { _hr = hr; }
143 inline int hr() const { return(_hr); }
144 inline void set_min(int min) { _min = min; }
145 inline int min() const { return(_min); }
147 ClockTime operator+ (const ClockTime& t) {
148 int cumMin = _hr * 60 + _min + t.hr() * 60 + t.min();
149 ClockTime t2(cumMin / 60, cumMin % 60);
152 // Operator - has a max difference of 23:59,
153 // and assumes the day has wrapped if the second operand
154 // is larger that the first.
155 // eg. 2:59 - 3:00 = 23:59
156 ClockTime operator- (const ClockTime& t) {
157 int diff = (_hr * 60 + _min) - (t.hr() * 60 + t.min());
158 if(diff < 0) { diff += 24 * 60; }
159 ClockTime t2(diff / 60, diff % 60);
162 friend std::ostream& operator<< (std::ostream& out, const ClockTime& t);
169 // AlignedProjection - a class to project an area local to a runway onto an orthogonal co-ordinate system
170 // with the origin at the threshold and the runway aligned with the y axis.
171 class AlignedProjection {
175 AlignedProjection(const SGGeod& centre, double heading);
176 ~AlignedProjection();
178 void Init(const SGGeod& centre, double heading);
180 // Convert a lat/lon co-ordinate (degrees) to the local projection (meters)
181 SGVec3d ConvertToLocal(const SGGeod& pt);
183 // Convert a local projection co-ordinate (meters) to lat/lon (degrees)
184 SGGeod ConvertFromLocal(const SGVec3d& pt);
187 SGGeod _origin; // lat/lon of local area origin (the threshold)
188 double _theta; // the rotation angle for alignment in radians
189 double _correction_factor; // Reduction in surface distance per degree of longitude due to latitude. Saves having to do a cos() every call.
193 // ------------------------------------------------------------------------------
195 // TODO - merge generic GPS functions instead and split out KLN specific stuff.
196 class DCLGPS : public SGSubsystem {
199 DCLGPS(RenderArea2D* instrument);
200 virtual ~DCLGPS() = 0;
202 virtual void draw(osg::State& state);
206 virtual void unbind();
207 virtual void update(double dt);
209 // Expand a SIAP ident to the full procedure name.
210 std::string ExpandSIAPIdent(const std::string& ident);
212 // Render string s in display field field at position x, y
213 // WHERE POSITION IS IN CHARACTER UNITS!
215 virtual void DrawText(const std::string& s, int field, int px, int py, bool bold = false);
217 // Render a char at a given position as above
218 virtual void DrawChar(char c, int field, int px, int py, bool bold = false);
220 virtual void ToggleOBSMode();
222 // Set the number of fields
223 inline void SetNumFields(int n) { _nFields = (n > _maxFields ? _maxFields : (n < 1 ? 1 : n)); }
225 // It is expected that specific GPS units will override these functions.
226 // Increase the CDI full-scale deflection (ie. increase the nm per dot) one (GPS unit dependent) increment. Wraps if necessary (GPS unit dependent).
227 virtual void CDIFSDIncrease();
228 // Ditto for decrease the distance per dot
229 virtual void CDIFSDDecrease();
232 ////inline void SetOverlays(Overlays* overlays) { _overlays = overlays; }
234 virtual void CreateDefaultFlightPlans();
236 void SetOBSFromWaypoint();
238 GPSWaypoint* GetActiveWaypoint();
239 // Get the (zero-based) position of the active waypoint in the active flightplan
240 // Returns -1 if no active waypoint.
241 int GetActiveWaypointIndex();
242 // Ditto for an arbitrary waypoint id
243 int GetWaypointIndex(const std::string& id);
246 float GetDistToActiveWaypoint();
247 // Returns degrees (magnetic)
248 float GetHeadingToActiveWaypoint();
249 // Returns degrees (magnetic)
250 float GetHeadingFromActiveWaypoint();
251 // Get the time to the active waypoint in seconds.
252 // Returns -1 if groundspeed < 30 kts
253 double GetTimeToActiveWaypoint();
254 // Get the time to the final waypoint in seconds.
255 // Returns -1 if groundspeed < 30 kts
257 // Get the time to a given waypoint (spec'd by ID) in seconds.
258 // returns -1 if groundspeed is less than 30kts.
259 // If the waypoint is an unreached part of the active flight plan the time will be via each leg.
260 // otherwise it will be a direct-to time.
261 double GetTimeToWaypoint(const std::string& id);
263 // Return true if waypoint alerting is occuring
264 inline bool GetWaypointAlert() const { return(_waypointAlert); }
265 // Return true if in OBS mode
266 inline bool GetOBSMode() const { return(_obsMode); }
267 // Return true if in Leg mode
268 inline bool GetLegMode() const { return(!_obsMode); }
270 // Clear a flightplan
271 void ClearFlightPlan(int n);
272 void ClearFlightPlan(GPSFlightPlan* fp);
274 // Returns true if an approach is loaded/armed/active in the active flight plan
275 inline bool ApproachLoaded() const { return(_approachLoaded); }
276 inline bool GetApproachArm() const { return(_approachArm); }
277 inline bool GetApproachActive() const { return(_approachActive); }
278 double GetCDIDeflection() const;
279 inline bool GetToFlag() const { return(_headingBugTo); }
281 // Initiate Direct To operation to the supplied ID.
282 virtual void DtoInitiate(const std::string& id);
283 // Cancel Direct To operation
287 // Maximum number of display fields for this device
289 // Current number of on-screen fields
295 // Lower (y) border per field
296 int _yFieldBorder[4];
297 // Left (x) border per field
298 int _xFieldBorder[4];
299 // Field start in x dir (border is part of field since it is the normal char border - sometimes map mode etc draws in it)
301 // Field start in y dir (for completeness - KLN89 only has vertical divider.
304 // The number of pages on the cyclic knob control
305 unsigned int _nPages;
306 // The current page we're on (Not sure how this ties in with extra pages such as direct or nearest).
307 unsigned int _curPage;
310 RenderArea2D* _instrument;
312 // CDI full-scale deflection, specified either as an index into a vector of values (standard values) or as a double precision float (intermediate values).
313 // This will influence how an externally driven CDI will display as well as the NAV1 page.
314 // Hence the variables are located here, not in the nav page class.
315 std::vector<float> _cdiScales;
316 unsigned int _currentCdiScaleIndex;
317 bool _cdiScaleTransition; // Set true when the floating CDI value is used during transitions
318 double _currentCdiScale; // The floating value to use.
319 unsigned int _targetCdiScaleIndex; // The target indexed value to attain during a transition.
320 unsigned int _sourceCdiScaleIndex; // The source indexed value during a transition - so we know which way we're heading!
321 // Timers to handle the transitions - not sure if we need these.
322 double _apprArmTimer;
323 double _apprActvTimer;
324 double _cdiTransitionTime; // Time for transition to occur in - normally 30sec but may be quicker if time to FAF < 30sec?
327 // Data and lookup functions
331 void LoadApproachData();
333 // Find first of any type of waypoint by id. (TODO - Possibly we should return multiple waypoints here).
334 GPSWaypoint* FindFirstById(const std::string& id) const;
335 GPSWaypoint* FindFirstByExactId(const std::string& id) const;
337 FGNavRecord* FindFirstVorById(const std::string& id, bool &multi, bool exact = false);
338 FGNavRecord* FindFirstNDBById(const std::string& id, bool &multi, bool exact = false);
339 const FGAirport* FindFirstAptById(const std::string& id, bool &multi, bool exact = false);
340 const FGFix* FindFirstIntById(const std::string& id, bool &multi, bool exact = false);
341 // Find the closest VOR to a position in RADIANS.
342 FGNavRecord* FindClosestVor(double lat_rad, double lon_rad);
344 // helper to implement the above FindFirstXXX methods
345 FGPositioned* FindTypedFirstById(const std::string& id, FGPositioned::Type ty, bool &multi, bool exact);
347 // Position, orientation and velocity.
348 // These should be read from FG's built-in GPS logic if possible.
349 // Use the property node pointers below to do this.
350 SGPropertyNode_ptr _lon_node;
351 SGPropertyNode_ptr _lat_node;
352 SGPropertyNode_ptr _alt_node;
353 SGPropertyNode_ptr _grnd_speed_node;
354 SGPropertyNode_ptr _true_track_node;
355 SGPropertyNode_ptr _mag_track_node;
356 // Present position. (Radians)
358 // Present altitude (ft). (Yuk! but it saves converting ft->m->ft every update).
360 // Reported position as measured by GPS. For now this is the same
361 // as present position, but in the future we might want to model
362 // GPS lat and lon errors.
363 // Note - we can depriciate _gpsLat and _gpsLon if we implement error handling in FG
364 // gps code and not our own.
365 double _gpsLat, _gpsLon; //(Radians)
366 // Hack - it seems that the GPS gets initialised before FG's initial position is properly set.
367 // By checking for abnormal slew in the position we can force a re-initialisation of active flight
368 // plan leg and anything else that might be affected.
369 // TODO - sort FlightGear's initialisation order properly!!!
370 double _checkLat, _checkLon; // (Radians)
371 double _groundSpeed_ms; // filtered groundspeed (m/s)
372 double _groundSpeed_kts; // ditto in knots
373 double _track; // filtered true track (degrees)
374 double _magTrackDeg; // magnetic track in degrees calculated from true track above
376 // _navFlagged is set true when GPS navigation is either not possible or not logical.
377 // This includes not receiving adequate signals, and not having an active flightplan entered.
380 // Positional functions copied from ATCutils that might get replaced
381 // INPUT in RADIANS, returns DEGREES!
383 double GetMagHeadingFromTo(double latA, double lonA, double latB, double lonB);
385 //double GetHeadingFromTo(double latA, double lonA, double latB, double lonB);
387 // Given two positions (lat & lon in RADIANS), get the HORIZONTAL separation (in meters)
388 //double GetHorizontalSeparation(double lat1, double lon1, double lat2, double lon2);
390 // Proper great circle positional functions from The Aviation Formulary
391 // Returns distance in Nm, input in RADIANS.
392 double GetGreatCircleDistance(double lat1, double lon1, double lat2, double lon2) const;
394 // Input in RADIANS, output in DEGREES.
396 double GetGreatCircleCourse(double lat1, double lon1, double lat2, double lon2) const;
398 // Return a position on a radial from wp1 given distance d (nm) and magnetic heading h (degrees)
399 // Note that d should be less that 1/4 Earth diameter!
400 GPSWaypoint GetPositionOnMagRadial(const GPSWaypoint& wp1, double d, double h);
402 // Return a position on a radial from wp1 given distance d (nm) and TRUE heading h (degrees)
403 // Note that d should be less that 1/4 Earth diameter!
404 GPSWaypoint GetPositionOnRadial(const GPSWaypoint& wp1, double d, double h);
406 // Calculate the current cross-track deviation in nm.
407 // Returns zero if a sensible value cannot be calculated.
408 double CalcCrossTrackDeviation() const;
410 // Calculate the cross-track deviation between 2 arbitrary waypoints in nm.
411 // Returns zero if a sensible value cannot be calculated.
412 double CalcCrossTrackDeviation(const GPSWaypoint& wp1, const GPSWaypoint& wp2) const;
415 // GPS can have up to _maxFlightPlans flightplans stored, PLUS an active FP which may or my not be one of the stored ones.
416 // This is from KLN89, but is probably not far off the mark for most if not all GPS.
417 std::vector<GPSFlightPlan*> _flightPlans;
418 unsigned int _maxFlightPlans;
419 GPSFlightPlan* _activeFP;
421 // Modes of operation.
422 // This is currently somewhat Bendix-King specific, but probably applies fundamentally to other units as well
423 // Mode defaults to leg, but is OBS if _obsMode is true.
425 // _dto is set true for DTO operation
427 // In leg mode, we need to know if we are displaying a from and to waypoint, or just the to waypoint (eg. when OBS mode is cancelled).
429 // In OBS mode we need to know the set OBS heading
432 // Operational variables
433 GPSWaypoint _activeWaypoint;
434 GPSWaypoint _fromWaypoint;
436 float _crosstrackDist; // UNITS ??????????
437 double _eta; // ETA in SECONDS to active waypoint.
438 // Desired track for active leg, true and magnetic, in degrees
439 double _dtkTrue, _dtkMag;
440 bool _headingBugTo; // Set true when the heading bug is TO, false when FROM.
441 bool _waypointAlert; // Set true when waypoint alerting is happening. (This is a variable NOT a user-setting).
442 bool _departed; // Set when groundspeed first exceeds 30kts.
443 std::string _departureTimeString; // Ditto.
444 double _elapsedTime; // Elapsed time in seconds since departure
445 ClockTime _powerOnTime; // Time (hr:min) of unit power-up.
446 bool _powerOnTimerSet; // Indicates that we have set the above following power-up.
447 void SetPowerOnTimer();
449 void ResetPowerOnTimer();
450 // Set the alarm to go off at a given time.
451 inline void SetAlarm(int hr, int min) {
452 _alarmTime.set_hr(hr);
453 _alarmTime.set_min(min);
457 ClockTime _alarmTime;
460 // Configuration that affects flightplan operation
461 bool _turnAnticipationEnabled;
463 std::list<std::string> _messageStack;
465 virtual void CreateFlightPlan(GPSFlightPlan* fp, std::vector<std::string> ids, std::vector<GPSWpType> wps);
467 // Orientate the GPS unit to a flightplan - ie. figure out from current position
468 // and possibly orientation which leg of the FP we are on.
469 virtual void OrientateToFlightPlan(GPSFlightPlan* fp);
471 // Ditto for active fp. Probably all we need really!
472 virtual void OrientateToActiveFlightPlan();
474 int _cleanUpPage; // -1 => no cleanup required.
477 iap_map_type _np_iap; // Non-precision approaches
478 iap_map_type _pr_iap; // Precision approaches
479 bool _approachLoaded; // Set true when an approach is loaded in the active flightplan
480 bool _approachArm; // Set true when in approach-arm mode
481 bool _approachReallyArmed; // Apparently, approach-arm mode can be set from an external GPS-APR switch outside 30nm from airport,
482 // but the CDI scale change doesn't happen until 30nm from airport. Bizarre that it can be armed without
483 // the scale change, but it's in the manual...
484 bool _approachActive; // Set true when in approach-active mode
485 GPSFlightPlan* _approachFP; // Current approach - not necessarily loaded.
486 std::string _approachID; // ID of the airport we have an approach loaded for - bit of a hack that can hopefully be removed in future.
487 // More hackery since we aren't actually storing an approach class... Doh!
488 std::string _approachAbbrev;
489 std::string _approachRwyStr;
491 simgear::TiedPropertyList _tiedProperties;
494 #endif // _DCLGPS_HXX