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. 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 #ifndef _DCLGPS_HXX 27 #define _DCLGPS_HXX 28 29 #include <Cockpit/render_area_2d.hxx> 30 31 #include <string> 32 #include <list> 33 #include <vector> 34 #include <map> 35 36 #include <simgear/structure/subsystem_mgr.hxx> 37 #include <simgear/props/props.hxx> 38 #include <simgear/props/tiedpropertylist.hxx> 39 #include <Navaids/positioned.hxx> 40 41 class SGTime; 42 class FGPositioned; 43 44 // XXX fix me 45 class FGNavRecord; 46 class FGAirport; 47 class FGFix; 48 49 // --------------------- Waypoint / Flightplan stuff ----------------------------- 50 // This should be merged with other similar stuff in FG at some point. 51 52 // NOTE - ORDERING IS IMPORTANT HERE - it matches the Bendix-King page ordering! 53 enum GPSWpType { 54 GPS_WP_APT = 0, 55 GPS_WP_VOR, 56 GPS_WP_NDB, 57 GPS_WP_INT, 58 GPS_WP_USR, 59 GPS_WP_VIRT // Used for virtual waypoints, such as the start of DTO operation. 60 }; 61 62 enum GPSAppWpType { 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. 71 }; 72 73 std::ostream& operator << (std::ostream& os, GPSAppWpType type); 74 75 struct GPSWaypoint { 76 GPSWaypoint(); 77 78 GPSWaypoint(const std::string& aIdent, float lat, float lon, GPSWpType aType); 79 80 static GPSWaypoint* createFromPositioned(const FGPositioned* aFix); 81 82 ~GPSWaypoint(); 83 std::string GetAprId(); // Returns the id with i, f, m or h added if appropriate. (Initial approach fix, final approach fix, etc) 84 std::string id; 85 float lat; // Radians 86 float lon; // Radians 87 GPSWpType type; 88 GPSAppWpType appType; // only used for waypoints that are part of an approach sequence 89 }; 90 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; 96 97 class GPSFlightPlan 98 { 99 public: 100 std::vector<GPSWaypoint*> waypoints; IsEmpty()101 inline bool IsEmpty() { return waypoints.empty(); } 102 }; 103 104 // TODO - probably de-public the internals of the next 2 classes and add some methods! 105 // Instrument approach procedure base class 106 class FGIAP 107 { 108 public: 109 FGIAP(); 110 virtual ~FGIAP() = 0; 111 112 //protected: 113 std::string _aptIdent; // The ident of the airport this approach is for 114 std::string _ident; // The approach ident. 115 std::string _name; // The full approach name. 116 std::string _rwyStr; // The string used to specify the rwy - eg "B" in this instance. 117 bool _precision; // True for precision approach, false for non-precision. 118 }; 119 120 // Non-precision instrument approach procedure 121 class FGNPIAP : public FGIAP 122 { 123 public: 124 FGNPIAP(); 125 ~FGNPIAP(); 126 127 //private: 128 public: 129 std::vector<GPSFlightPlan*> _approachRoutes; // The approach route(s) from the IAF(s) to the IF. 130 // NOTE: It is an assumption in the code that uses this that there is a unique IAF per approach route. 131 std::vector<GPSWaypoint*> _IAP; // The compulsory waypoints of the approach procedure (may duplicate one of the above). 132 // _IAP includes the FAF and MAF, and the missed approach waypoints. 133 }; 134 135 typedef std::vector < FGIAP* > iap_list_type; 136 typedef std::map < std::string, iap_list_type > iap_map_type; 137 typedef iap_map_type::iterator iap_map_iterator; 138 139 // A class to encapsulate hr:min representation of time. 140 class ClockTime 141 { 142 public: 143 ClockTime(); 144 ClockTime(int hr, int min); 145 ~ClockTime(); set_hr(int hr)146 inline void set_hr(int hr) { _hr = hr; } hr() const147 inline int hr() const { return(_hr); } set_min(int min)148 inline void set_min(int min) { _min = min; } min() const149 inline int min() const { return(_min); } 150 operator +(const ClockTime & t)151 ClockTime operator+ (const ClockTime& t) { 152 int cumMin = _hr * 60 + _min + t.hr() * 60 + t.min(); 153 ClockTime t2(cumMin / 60, cumMin % 60); 154 return(t2); 155 } 156 // Operator - has a max difference of 23:59, 157 // and assumes the day has wrapped if the second operand 158 // is larger that the first. 159 // eg. 2:59 - 3:00 = 23:59 operator -(const ClockTime & t)160 ClockTime operator- (const ClockTime& t) { 161 int diff = (_hr * 60 + _min) - (t.hr() * 60 + t.min()); 162 if(diff < 0) { diff += 24 * 60; } 163 ClockTime t2(diff / 60, diff % 60); 164 return(t2); 165 } 166 friend std::ostream& operator<< (std::ostream& out, const ClockTime& t); 167 168 private: 169 int _hr; 170 int _min; 171 }; 172 173 // AlignedProjection - a class to project an area local to a runway onto an orthogonal co-ordinate system 174 // with the origin at the threshold and the runway aligned with the y axis. 175 class AlignedProjection 176 { 177 public: 178 AlignedProjection(); 179 AlignedProjection(const SGGeod& centre, double heading); 180 ~AlignedProjection(); 181 182 void Init(const SGGeod& centre, double heading); 183 184 // Convert a lat/lon co-ordinate (degrees) to the local projection (meters) 185 SGVec3d ConvertToLocal(const SGGeod& pt); 186 187 // Convert a local projection co-ordinate (meters) to lat/lon (degrees) 188 SGGeod ConvertFromLocal(const SGVec3d& pt); 189 190 private: 191 SGGeod _origin; // lat/lon of local area origin (the threshold) 192 double _theta; // the rotation angle for alignment in radians 193 double _correction_factor; // Reduction in surface distance per degree of longitude due to latitude. Saves having to do a cos() every call. 194 }; 195 196 // ------------------------------------------------------------------------------ 197 198 // TODO - merge generic GPS functions instead and split out KLN specific stuff. 199 class DCLGPS : public SGSubsystem 200 { 201 public: 202 DCLGPS(RenderArea2D* instrument); 203 virtual ~DCLGPS() = 0; 204 205 // Subsystem API. 206 void bind() override; 207 void init() override; 208 void unbind() override; 209 void update(double dt) override; 210 211 virtual void draw(osg::State& state); 212 213 // Expand a SIAP ident to the full procedure name. 214 std::string ExpandSIAPIdent(const std::string& ident); 215 216 // Render string s in display field field at position x, y 217 // WHERE POSITION IS IN CHARACTER UNITS! 218 // zero y at bottom? 219 virtual void DrawText(const std::string& s, int field, int px, int py, bool bold = false); 220 221 // Render a char at a given position as above 222 virtual void DrawChar(char c, int field, int px, int py, bool bold = false); 223 224 virtual void ToggleOBSMode(); 225 226 // Set the number of fields SetNumFields(int n)227 inline void SetNumFields(int n) { _nFields = (n > _maxFields ? _maxFields : (n < 1 ? 1 : n)); } 228 229 // It is expected that specific GPS units will override these functions. 230 // Increase the CDI full-scale deflection (ie. increase the nm per dot) one (GPS unit dependent) increment. Wraps if necessary (GPS unit dependent). 231 virtual void CDIFSDIncrease(); 232 // Ditto for decrease the distance per dot 233 virtual void CDIFSDDecrease(); 234 235 // Host specifc 236 ////inline void SetOverlays(Overlays* overlays) { _overlays = overlays; } 237 238 virtual void CreateDefaultFlightPlans(); 239 240 void SetOBSFromWaypoint(); 241 242 GPSWaypoint* GetActiveWaypoint(); 243 // Get the (zero-based) position of the active waypoint in the active flightplan 244 // Returns -1 if no active waypoint. 245 int GetActiveWaypointIndex(); 246 // Ditto for an arbitrary waypoint id 247 int GetWaypointIndex(const std::string& id); 248 249 // Returns meters 250 float GetDistToActiveWaypoint(); 251 // Returns degrees (magnetic) 252 float GetHeadingToActiveWaypoint(); 253 // Returns degrees (magnetic) 254 float GetHeadingFromActiveWaypoint(); 255 // Get the time to the active waypoint in seconds. 256 // Returns -1 if groundspeed < 30 kts 257 double GetTimeToActiveWaypoint(); 258 // Get the time to the final waypoint in seconds. 259 // Returns -1 if groundspeed < 30 kts 260 double GetETE(); 261 // Get the time to a given waypoint (spec'd by ID) in seconds. 262 // returns -1 if groundspeed is less than 30kts. 263 // If the waypoint is an unreached part of the active flight plan the time will be via each leg. 264 // otherwise it will be a direct-to time. 265 double GetTimeToWaypoint(const std::string& id); 266 267 // Return true if waypoint alerting is occuring GetWaypointAlert() const268 inline bool GetWaypointAlert() const { return(_waypointAlert); } 269 // Return true if in OBS mode GetOBSMode() const270 inline bool GetOBSMode() const { return(_obsMode); } 271 // Return true if in Leg mode GetLegMode() const272 inline bool GetLegMode() const { return(!_obsMode); } 273 274 // Clear a flightplan 275 void ClearFlightPlan(int n); 276 void ClearFlightPlan(GPSFlightPlan* fp); 277 278 // Returns true if an approach is loaded/armed/active in the active flight plan ApproachLoaded() const279 inline bool ApproachLoaded() const { return(_approachLoaded); } GetApproachArm() const280 inline bool GetApproachArm() const { return(_approachArm); } GetApproachActive() const281 inline bool GetApproachActive() const { return(_approachActive); } 282 double GetCDIDeflection() const; GetToFlag() const283 inline bool GetToFlag() const { return(_headingBugTo); } 284 285 // Initiate Direct To operation to the supplied ID. 286 virtual void DtoInitiate(const std::string& id); 287 // Cancel Direct To operation 288 void DtoCancel(); 289 290 protected: 291 // Maximum number of display fields for this device 292 int _maxFields; 293 // Current number of on-screen fields 294 int _nFields; 295 // Full x border 296 int _xBorder; 297 // Full y border 298 int _yBorder; 299 // Lower (y) border per field 300 int _yFieldBorder[4]; 301 // Left (x) border per field 302 int _xFieldBorder[4]; 303 // Field start in x dir (border is part of field since it is the normal char border - sometimes map mode etc draws in it) 304 int _xFieldStart[4]; 305 // Field start in y dir (for completeness - KLN89 only has vertical divider. 306 int _yFieldStart[4]; 307 308 // The number of pages on the cyclic knob control 309 unsigned int _nPages; 310 // The current page we're on (Not sure how this ties in with extra pages such as direct or nearest). 311 unsigned int _curPage; 312 313 // 2D rendering area 314 RenderArea2D* _instrument; 315 316 // CDI full-scale deflection, specified either as an index into a vector of values (standard values) or as a double precision float (intermediate values). 317 // This will influence how an externally driven CDI will display as well as the NAV1 page. 318 // Hence the variables are located here, not in the nav page class. 319 std::vector<float> _cdiScales; 320 unsigned int _currentCdiScaleIndex; 321 bool _cdiScaleTransition; // Set true when the floating CDI value is used during transitions 322 double _currentCdiScale; // The floating value to use. 323 unsigned int _targetCdiScaleIndex; // The target indexed value to attain during a transition. 324 unsigned int _sourceCdiScaleIndex; // The source indexed value during a transition - so we know which way we're heading! 325 // Timers to handle the transitions - not sure if we need these. 326 double _apprArmTimer; 327 double _apprActvTimer; 328 double _cdiTransitionTime; // Time for transition to occur in - normally 30sec but may be quicker if time to FAF < 30sec? 329 // 330 331 // Data and lookup functions 332 333 protected: 334 void LoadApproachData(); 335 336 // Find first of any type of waypoint by id. (TODO - Possibly we should return multiple waypoints here). 337 GPSWaypoint* FindFirstById(const std::string& id) const; 338 GPSWaypoint* FindFirstByExactId(const std::string& id) const; 339 340 FGNavRecord* FindFirstVorById(const std::string& id, bool &multi, bool exact = false); 341 FGNavRecord* FindFirstNDBById(const std::string& id, bool &multi, bool exact = false); 342 const FGAirport* FindFirstAptById(const std::string& id, bool &multi, bool exact = false); 343 const FGFix* FindFirstIntById(const std::string& id, bool &multi, bool exact = false); 344 // Find the closest VOR to a position in RADIANS. 345 FGNavRecord* FindClosestVor(double lat_rad, double lon_rad); 346 347 // helper to implement the above FindFirstXXX methods 348 FGPositioned* FindTypedFirstById(const std::string& id, FGPositioned::Type ty, bool &multi, bool exact); 349 350 // Position, orientation and velocity. 351 // These should be read from FG's built-in GPS logic if possible. 352 // Use the property node pointers below to do this. 353 SGPropertyNode_ptr _lon_node; 354 SGPropertyNode_ptr _lat_node; 355 SGPropertyNode_ptr _alt_node; 356 SGPropertyNode_ptr _grnd_speed_node; 357 SGPropertyNode_ptr _true_track_node; 358 SGPropertyNode_ptr _mag_track_node; 359 // Present position. (Radians) 360 double _lat, _lon; 361 // Present altitude (ft). (Yuk! but it saves converting ft->m->ft every update). 362 double _alt; 363 // Reported position as measured by GPS. For now this is the same 364 // as present position, but in the future we might want to model 365 // GPS lat and lon errors. 366 // Note - we can depriciate _gpsLat and _gpsLon if we implement error handling in FG 367 // gps code and not our own. 368 double _gpsLat, _gpsLon; //(Radians) 369 // Hack - it seems that the GPS gets initialised before FG's initial position is properly set. 370 // By checking for abnormal slew in the position we can force a re-initialisation of active flight 371 // plan leg and anything else that might be affected. 372 // TODO - sort FlightGear's initialisation order properly!!! 373 double _checkLat, _checkLon; // (Radians) 374 double _groundSpeed_ms; // filtered groundspeed (m/s) 375 double _groundSpeed_kts; // ditto in knots 376 double _track; // filtered true track (degrees) 377 double _magTrackDeg; // magnetic track in degrees calculated from true track above 378 379 // _navFlagged is set true when GPS navigation is either not possible or not logical. 380 // This includes not receiving adequate signals, and not having an active flightplan entered. 381 bool _navFlagged; 382 383 // Positional functions copied from ATCutils that might get replaced 384 // INPUT in RADIANS, returns DEGREES! 385 // Magnetic 386 double GetMagHeadingFromTo(double latA, double lonA, double latB, double lonB); 387 // True 388 //double GetHeadingFromTo(double latA, double lonA, double latB, double lonB); 389 390 // Given two positions (lat & lon in RADIANS), get the HORIZONTAL separation (in meters) 391 //double GetHorizontalSeparation(double lat1, double lon1, double lat2, double lon2); 392 393 // Proper great circle positional functions from The Aviation Formulary 394 // Returns distance in Nm, input in RADIANS. 395 double GetGreatCircleDistance(double lat1, double lon1, double lat2, double lon2) const; 396 397 // Input in RADIANS, output in DEGREES. 398 // True 399 double GetGreatCircleCourse(double lat1, double lon1, double lat2, double lon2) const; 400 401 // Return a position on a radial from wp1 given distance d (nm) and magnetic heading h (degrees) 402 // Note that d should be less that 1/4 Earth diameter! 403 GPSWaypoint GetPositionOnMagRadial(const GPSWaypoint& wp1, double d, double h); 404 405 // Return a position on a radial from wp1 given distance d (nm) and TRUE heading h (degrees) 406 // Note that d should be less that 1/4 Earth diameter! 407 GPSWaypoint GetPositionOnRadial(const GPSWaypoint& wp1, double d, double h); 408 409 // Calculate the current cross-track deviation in nm. 410 // Returns zero if a sensible value cannot be calculated. 411 double CalcCrossTrackDeviation() const; 412 413 // Calculate the cross-track deviation between 2 arbitrary waypoints in nm. 414 // Returns zero if a sensible value cannot be calculated. 415 double CalcCrossTrackDeviation(const GPSWaypoint& wp1, const GPSWaypoint& wp2) const; 416 417 // Flightplans 418 // GPS can have up to _maxFlightPlans flightplans stored, PLUS an active FP which may or my not be one of the stored ones. 419 // This is from KLN89, but is probably not far off the mark for most if not all GPS. 420 std::vector<GPSFlightPlan*> _flightPlans; 421 unsigned int _maxFlightPlans; 422 GPSFlightPlan* _activeFP; 423 424 // Modes of operation. 425 // This is currently somewhat Bendix-King specific, but probably applies fundamentally to other units as well 426 // Mode defaults to leg, but is OBS if _obsMode is true. 427 bool _obsMode; 428 // _dto is set true for DTO operation 429 bool _dto; 430 // 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). 431 bool _fullLegMode; 432 // In OBS mode we need to know the set OBS heading 433 int _obsHeading; 434 435 // Operational variables 436 GPSWaypoint _activeWaypoint; 437 GPSWaypoint _fromWaypoint; 438 float _dist2Act; 439 float _crosstrackDist; // UNITS ?????????? 440 double _eta; // ETA in SECONDS to active waypoint. 441 // Desired track for active leg, true and magnetic, in degrees 442 double _dtkTrue, _dtkMag; 443 bool _headingBugTo; // Set true when the heading bug is TO, false when FROM. 444 bool _waypointAlert; // Set true when waypoint alerting is happening. (This is a variable NOT a user-setting). 445 bool _departed; // Set when groundspeed first exceeds 30kts. 446 std::string _departureTimeString; // Ditto. 447 double _elapsedTime; // Elapsed time in seconds since departure 448 ClockTime _powerOnTime; // Time (hr:min) of unit power-up. 449 bool _powerOnTimerSet; // Indicates that we have set the above following power-up. 450 void SetPowerOnTimer(); 451 452 public: 453 void ResetPowerOnTimer(); 454 // Set the alarm to go off at a given time. SetAlarm(int hr,int min)455 inline void SetAlarm(int hr, int min) { 456 _alarmTime.set_hr(hr); 457 _alarmTime.set_min(min); 458 _alarmSet = true; 459 } 460 461 protected: 462 ClockTime _alarmTime; 463 bool _alarmSet; 464 465 // Configuration that affects flightplan operation 466 bool _turnAnticipationEnabled; 467 468 std::list<std::string> _messageStack; 469 470 virtual void CreateFlightPlan(GPSFlightPlan* fp, std::vector<std::string> ids, std::vector<GPSWpType> wps); 471 472 // Orientate the GPS unit to a flightplan - ie. figure out from current position 473 // and possibly orientation which leg of the FP we are on. 474 virtual void OrientateToFlightPlan(GPSFlightPlan* fp); 475 476 // Ditto for active fp. Probably all we need really! 477 virtual void OrientateToActiveFlightPlan(); 478 479 int _cleanUpPage; // -1 => no cleanup required. 480 481 // IAP stuff 482 iap_map_type _np_iap; // Non-precision approaches 483 iap_map_type _pr_iap; // Precision approaches 484 bool _approachLoaded; // Set true when an approach is loaded in the active flightplan 485 bool _approachArm; // Set true when in approach-arm mode 486 bool _approachReallyArmed; // Apparently, approach-arm mode can be set from an external GPS-APR switch outside 30nm from airport, 487 // but the CDI scale change doesn't happen until 30nm from airport. Bizarre that it can be armed without 488 // the scale change, but it's in the manual... 489 bool _approachActive; // Set true when in approach-active mode 490 GPSFlightPlan* _approachFP; // Current approach - not necessarily loaded. 491 std::string _approachID; // ID of the airport we have an approach loaded for - bit of a hack that can hopefully be removed in future. 492 // More hackery since we aren't actually storing an approach class... Doh! 493 std::string _approachAbbrev; 494 std::string _approachRwyStr; 495 496 private: 497 simgear::TiedPropertyList _tiedProperties; 498 }; 499 500 #endif // _DCLGPS_HXX 501