src/Autopilot/newauto.hxx

   1 // newauto.hxx -- autopilot defines and prototypes (very alpha)
   2 //
   3 // Started April 1998  Copyright (C) 1998
   4 //
   5 // Contributions by Jeff Goeke-Smith <jgoeke@voyager.net>
   6 //                  Norman Vine <nhv@cape.com>
   7 //                  Curtis Olson <curt@flightgear.org>
   8 //
   9 // This program is free software; you can redistribute it and/or
  10 // modify it under the terms of the GNU General Public License as
  11 // published by the Free Software Foundation; either version 2 of the
  12 // License, or (at your option) any later version.
  13 //
  14 // This program is distributed in the hope that it will be useful, but
  15 // WITHOUT ANY WARRANTY; without even the implied warranty of
  16 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  17 // General Public License for more details.
  18 //
  19 // You should have received a copy of the GNU General Public License
  20 // along with this program; if not, write to the Free Software
  21 // Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  22 //
  23 // $Id$
  24
  25
  26 #ifndef _NEWAUTO_HXX
  27 #define _NEWAUTO_HXX
  28
  29
  30 #include <simgear/misc/props.hxx>
  31 #include <simgear/route/waypoint.hxx>
  32
  33 #include <Main/fgfs.hxx>
  34 #include <Main/fg_props.hxx>
  35
  36 // Structures
  37 class FGAutopilot : public FGSubsystem
  38 {
  39
  40 public:
  41
  42     enum fgAutoHeadingMode {
  43         FG_DG_HEADING_LOCK = 0,   // follow bug on directional gryo
  44         FG_TC_HEADING_LOCK = 1,   // keep turn coordinator zero'd
  45         FG_TRUE_HEADING_LOCK = 2, // lock to true heading (i.e. a perfect world)
  46         FG_HEADING_NAV1 = 3,      // follow nav1 radial
  47         FG_HEADING_NAV2 = 4,      // follow nav2 radial
  48         FG_HEADING_WAYPOINT = 5   // track next waypoint
  49     };
  50
  51     enum fgAutoAltitudeMode {
  52         FG_ALTITUDE_LOCK = 0,     // lock to a specific altitude
  53         FG_ALTITUDE_TERRAIN = 1,  // try to maintain a specific AGL
  54         FG_ALTITUDE_GS1 = 2,      // follow glide slope 1
  55         FG_ALTITUDE_GS2 = 3,      // follow glide slope 2
  56         FG_ALTITUDE_ARM = 4       // ascend to selected altitude
  57     };
  58
  59
  60 private:
  61
  62     bool heading_hold;          // the current state of the heading hold
  63     bool altitude_hold;         // the current state of the altitude hold
  64     bool auto_throttle;         // the current state of the auto throttle
  65
  66     fgAutoHeadingMode heading_mode;
  67     fgAutoAltitudeMode altitude_mode;
  68
  69     SGWayPoint waypoint;        // the waypoint the AP should steer to.
  70
  71     // double TargetLatitude;   // the latitude the AP should steer to.
  72     // double TargetLongitude;  // the longitude the AP should steer to.
  73     double TargetDistance;      // the distance to Target.
  74     double DGTargetHeading;     // the apparent DG heading to steer towards.
  75     double TargetHeading;       // the true heading the AP should steer to.
  76     double TargetAltitude;      // altitude to hold
  77     double TargetAGL;           // the terrain separation
  78
  79     double TargetSpeed;         // speed to shoot for
  80     double alt_error_accum;     // altitude error accumulator
  81     double climb_error_accum;   // climb error accumulator (for GS)
  82     double speed_error_accum;   // speed error accumulator
  83
  84     double TargetSlope;         // the glide slope hold value
  85
  86     double MaxRoll ;            // the max the plane can roll for the turn
  87     double RollOut;             // when the plane should roll out
  88                                 // measured from Heading
  89     double MaxAileron;          // how far to move the aleroin from center
  90     double RollOutSmooth;       // deg to use for smoothing Aileron Control
  91     double MaxElevator;         // the maximum elevator allowed
  92     double SlopeSmooth;         // smoothing angle for elevator
  93
  94     // following for testing disengagement of autopilot upon pilot
  95     // interaction with controls
  96     double old_aileron;
  97     double old_elevator;
  98     double old_elevator_trim;
  99     double old_rudder;
 100
 101     // manual controls override beyond this value
 102     double disengage_threshold;
 103
 104     // For future cross track error adjust
 105     double old_lat;
 106     double old_lon;
 107
 108     // keeping these locally to save work inside main loop
 109     char TargetLatitudeStr[64];
 110     char TargetLongitudeStr[64];
 111     char TargetLatLonStr[64];
 112     char TargetWP1Str[64];
 113     char TargetWP2Str[64];
 114     char TargetWP3Str[64];
 115     char TargetHeadingStr[64];
 116     char TargetAltitudeStr[64];
 117
 118     // property nodes
 119     SGPropertyNode *latitude_node;
 120     SGPropertyNode *longitude_node;
 121     SGPropertyNode *altitude_node;
 122     SGPropertyNode *altitude_agl_node;
 123     SGPropertyNode *vertical_speed_node;
 124     SGPropertyNode *heading_node;
 125     SGPropertyNode *roll_node;
 126     SGPropertyNode *pitch_node;
 127
 128     SGPropertyNode *min_climb;           // minimum climb speed
 129     SGPropertyNode *best_climb;          // best climb speed
 130     SGPropertyNode *elevator_adj_factor; // factor to optimize altitude hold adjustments
 131     SGPropertyNode *integral_contrib;    // amount of contribution of the integral
 132                                 // component of the pid
 133     SGPropertyNode *zero_pitch_throttle; // amount of throttle at which the aircraft does not pitch up
 134     SGPropertyNode *zero_pitch_trim_full_throttle; // amount of trim required to level at full throttle
 135     SGPropertyNode *TargetClimbRate;    // target climb rate
 136     SGPropertyNode *TargetDescentRate;  // target decent rate
 137     SGPropertyNode *current_throttle; // current throttle (engine 0)
 138
 139 public:
 140
 141     // constructor
 142     FGAutopilot();
 143
 144     // destructor
 145     ~FGAutopilot();
 146
 147 \f
 148     ////////////////////////////////////////////////////////////////////
 149     // Implementation of FGSubsystem.
 150     ////////////////////////////////////////////////////////////////////
 151
 152     void init ();
 153     void bind ();
 154     void unbind ();
 155     void update (double dt);
 156
 157     // Reset the autopilot system
 158     void reset(void);
 159
 160     void AltitudeSet( double new_altitude );
 161     void AltitudeAdjust( double inc );
 162     void HeadingAdjust( double inc );
 163     void AutoThrottleAdjust( double inc );
 164
 165     void HeadingSet( double value );
 166
 167     inline bool get_HeadingEnabled() const { return heading_hold; }
 168     inline void set_HeadingEnabled( bool value ) { heading_hold = value; }
 169     inline fgAutoHeadingMode get_HeadingMode() const { return heading_mode; }
 170     void set_HeadingMode( fgAutoHeadingMode mode );
 171
 172     inline bool get_AltitudeEnabled() const { return altitude_hold; }
 173     inline void set_AltitudeEnabled( bool value ) { altitude_hold = value; }
 174     inline fgAutoAltitudeMode get_AltitudeMode() const { return altitude_mode;}
 175     void set_AltitudeMode( fgAutoAltitudeMode mode );
 176
 177     inline bool get_AutoThrottleEnabled() const { return auto_throttle; }
 178     void set_AutoThrottleEnabled( bool value );
 179
 180     /* inline void set_WayPoint( const double lon, const double lat,
 181                               const double alt, const string s ) {
 182         waypoint = SGWayPoint( lon, lat, alt, SGWayPoint::WGS84, "Current WP" );
 183     } */
 184     inline double get_TargetLatitude() const {
 185         return waypoint.get_target_lat();
 186     }
 187     inline double get_TargetLongitude() const {
 188         return waypoint.get_target_lon();
 189     }
 190     inline void set_old_lat( double val ) { old_lat = val; }
 191     inline void set_old_lon( double val ) { old_lon = val; }
 192     inline double get_TargetHeading() const { return TargetHeading; }
 193     inline void set_TargetHeading( double val ) { TargetHeading = val; }
 194     inline double get_DGTargetHeading() const { return DGTargetHeading; }
 195     inline void set_DGTargetHeading( double val ) { DGTargetHeading = val; }
 196     inline double get_TargetDistance() const { return TargetDistance; }
 197     inline void set_TargetDistance( double val ) { TargetDistance = val; }
 198     inline double get_TargetAltitude() const { return TargetAltitude; }
 199     inline void set_TargetAltitude( double val ) { TargetAltitude = val; }
 200     inline double get_TargetAGL() const { return TargetAGL; }
 201     inline void set_TargetAGL( double val ) { TargetAGL = val; }
 202     inline double get_TargetClimbRate() const {
 203         return TargetClimbRate->getFloatValue();
 204     }
 205     inline void set_TargetClimbRate( double val ) {
 206         fgSetFloat( "/autopilot/config/target-climb-rate-fpm",  val);
 207     }
 208
 209     inline char *get_TargetLatitudeStr() { return TargetLatitudeStr; }
 210     inline char *get_TargetLongitudeStr() { return TargetLongitudeStr; }
 211     inline char *get_TargetWP1Str() { return TargetWP1Str; }
 212     inline char *get_TargetWP2Str() { return TargetWP2Str; }
 213     inline char *get_TargetWP3Str() { return TargetWP3Str; }
 214     inline char *get_TargetHeadingStr() { return TargetHeadingStr; }
 215     inline char *get_TargetAltitudeStr() { return TargetAltitudeStr; }
 216     inline char *get_TargetLatLonStr() { return TargetLatLonStr; }
 217
 218     // utility functions
 219     void MakeTargetLatLonStr( double lat, double lon );
 220     void MakeTargetAltitudeStr( double altitude );
 221     void MakeTargetHeadingStr( double bearing );
 222     void MakeTargetWPStr( double distance );
 223     void update_old_control_values();
 224
 225     // accessors
 226     inline double get_MaxRoll() const { return MaxRoll; }
 227     inline void set_MaxRoll( double val ) { MaxRoll = val; }
 228     inline double get_RollOut() const { return RollOut; }
 229     inline void set_RollOut( double val ) { RollOut = val; }
 230
 231     inline double get_MaxAileron() const { return MaxAileron; }
 232     inline void set_MaxAileron( double val ) { MaxAileron = val; }
 233     inline double get_RollOutSmooth() const { return RollOutSmooth; }
 234     inline void set_RollOutSmooth( double val ) { RollOutSmooth = val; }
 235
 236 private:
 237
 238     bool getAPAltitudeLock () const;
 239     void setAPAltitudeLock (bool lock);
 240     double getAPAltitude () const;
 241     void setAPAltitude (double altitude);
 242     bool getAPGSLock () const;
 243     void setAPGSLock (bool lock);
 244     bool getAPTerrainLock () const;
 245     void setAPTerrainLock (bool lock);
 246     double getAPClimb () const;
 247     void setAPClimb (double rate);
 248     bool getAPHeadingLock () const;
 249     void setAPHeadingLock (bool lock);
 250     double getAPHeadingBug () const;
 251     void setAPHeadingBug (double heading);
 252     bool getAPWingLeveler () const;
 253     void setAPWingLeveler (bool lock);
 254     bool getAPNAV1Lock () const;
 255     void setAPNAV1Lock (bool lock);
 256     bool getAPAutoThrottleLock () const;
 257     void setAPAutoThrottleLock (bool lock);
 258     double getAPRudderControl () const;
 259     void setAPRudderControl (double value);
 260     double getAPElevatorControl () const;
 261     void setAPElevatorControl (double value);
 262     double getAPThrottleControl () const;
 263     void setAPThrottleControl (double value);
 264
 265 };
 266
 267
 268 #define DEFAULT_AP_HEADING_LOCK FGAutopilot::FG_DG_HEADING_LOCK
 269 // #define DEFAULT_AP_HEADING_LOCK FGAutopilot::FG_TRUE_HEADING_LOCK
 270 #define DEFAULT_AP_ALTITUDE_LOCK FGAutopilot::FG_ALTITUDE_LOCK
 271
 272
 273 /**
 274  * static functions for autopilot properties
 275  */
 276
 277 #endif // _NEWAUTO_HXX