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 #error choke me
  27
  28 #ifndef _NEWAUTO_HXX
  29 #define _NEWAUTO_HXX
  30
  31
  32 #include <simgear/props/props.hxx>
  33 #include <simgear/structure/subsystem_mgr.hxx>
  34 #include <simgear/route/waypoint.hxx>
  35
  36 #include <Main/fg_props.hxx>
  37
  38 // Structures
  39 class FGAutopilot : public SGSubsystem
  40 {
  41
  42 public:
  43
  44     enum fgAutoHeadingMode {
  45         FG_DG_HEADING_LOCK = 0,   // follow bug on directional gryo (vacuum)
  46         FG_TC_HEADING_LOCK = 1,   // keep turn coordinator zero'd
  47         FG_TRUE_HEADING_LOCK = 2, // lock to true heading (i.e. a perfect world)
  48         FG_HEADING_NAV1 = 3,      // follow nav1 radial
  49         FG_HEADING_NAV2 = 4,      // follow nav2 radial
  50         FG_HEADING_WAYPOINT = 5   // track next waypoint
  51     };
  52
  53     enum fgAutoAltitudeMode {
  54         FG_ALTITUDE_LOCK = 0,     // lock to a specific altitude (indicated)
  55         FG_ALTITUDE_TERRAIN = 1,  // try to maintain a specific AGL
  56         FG_ALTITUDE_GS1 = 2,      // follow glide slope 1
  57         FG_ALTITUDE_GS2 = 3,      // follow glide slope 2
  58         FG_ALTITUDE_ARM = 4,      // ascend to selected altitude
  59         FG_TRUE_ALTITUDE_LOCK = 5, // lock to a specific true altitude
  60                                    // (i.e. a perfect world)
  61         FG_VERT_SPEED = 6  // ascend or descend at selected fpm
  62     };
  63
  64 private:
  65
  66     bool heading_hold;          // the current state of the heading hold
  67     bool altitude_hold;         // the current state of the altitude hold
  68     bool auto_throttle;         // the current state of the auto throttle
  69
  70     fgAutoHeadingMode heading_mode;
  71     fgAutoAltitudeMode altitude_mode;
  72
  73     SGWayPoint waypoint;        // the waypoint the AP should steer to.
  74
  75     // double TargetLatitude;   // the latitude the AP should steer to.
  76     // double TargetLongitude;  // the longitude the AP should steer to.
  77     double TargetDistance;      // the distance to Target.
  78     double DGTargetHeading;     // the apparent DG heading to steer towards.
  79     double TargetHeading;       // the true heading the AP should steer to.
  80     double TargetAltitude;      // altitude to hold
  81     double TargetAGL;           // the terrain separation
  82
  83     double TargetVertSpeed;         // vertical speed to shoot for
  84
  85     double TargetSpeed;         // speed to shoot for
  86     double alt_error_accum;     // altitude error accumulator
  87     double climb_error_accum;   // climb error accumulator (for GS)
  88     double speed_error_accum;   // speed error accumulator
  89
  90     double current_ap_throttle;  // current ap stored throttle setting used to set all engines
  91     double previous_speed;  // used to detect acceleration rate
  92
  93     double TargetSlope;         // the glide slope hold value
  94
  95     double MaxRoll ;            // the max the plane can roll for the turn
  96     double RollOut;             // when the plane should roll out
  97                                 // measured from Heading
  98     double MaxAileron;          // how far to move the aleroin from center
  99     double RollOutSmooth;       // deg to use for smoothing Aileron Control
 100     double MaxElevator;         // the maximum elevator allowed
 101     double SlopeSmooth;         // smoothing angle for elevator
 102
 103     // following for testing disengagement of autopilot upon pilot
 104     // interaction with controls
 105     double old_aileron;
 106     double old_elevator;
 107     double old_elevator_trim;
 108     double old_rudder;
 109
 110     // manual controls override beyond this value
 111     double disengage_threshold;
 112
 113     // For future cross track error adjust
 114     double old_lat;
 115     double old_lon;
 116
 117     // keeping these locally to save work inside main loop
 118     char TargetLatitudeStr[64];
 119     char TargetLongitudeStr[64];
 120     char TargetLatLonStr[64];
 121     char TargetWP1Str[64];
 122     char TargetWP2Str[64];
 123     char TargetWP3Str[64];
 124     char TargetHeadingStr[64];
 125     char TargetAltitudeStr[64];
 126
 127     // property nodes
 128     SGPropertyNode *latitude_node;
 129     SGPropertyNode *longitude_node;
 130     SGPropertyNode *altitude_node;
 131     SGPropertyNode *altitude_agl_node;
 132     SGPropertyNode *vertical_speed_node;
 133     SGPropertyNode *heading_node;
 134     SGPropertyNode *indicated_heading_node;
 135     SGPropertyNode *roll_node;
 136     SGPropertyNode *pitch_node;
 137     SGPropertyNode *airspeed_node;
 138
 139     SGPropertyNode *min_climb;           // minimum climb speed
 140     SGPropertyNode *best_climb;          // best climb speed
 141     SGPropertyNode *elevator_adj_factor; // factor to optimize altitude hold adjustments
 142     SGPropertyNode *integral_contrib;    // amount of contribution of the integral
 143                                 // component of the pid
 144     SGPropertyNode *zero_pitch_throttle; // amount of throttle at which the aircraft does not pitch up
 145     SGPropertyNode *zero_pitch_trim_full_throttle; // amount of trim required to level at full throttle
 146     SGPropertyNode *max_aileron_node; // maximum aileron setting range -1 ~ 1
 147     SGPropertyNode *max_roll_node; // maximum roll setting in degrees
 148     SGPropertyNode *roll_out_node; // start rollout offset from desired heading in degrees
 149     SGPropertyNode *roll_out_smooth_node; // rollout smoothing offset in degrees
 150     SGPropertyNode *throttle_adj_factor; // factor to optimize autothrottle adjustments
 151     SGPropertyNode *throttle_integral;    // amount of contribution of the integral
 152                                 // component of the pid
 153     SGPropertyNode *speed_change_node;    // anticipated speed change
 154
 155     SGPropertyNode *TargetClimbRate;    // target climb rate
 156     SGPropertyNode *TargetDescentRate;  // target decent rate
 157     SGPropertyNode *current_throttle; // current throttle (engine 0)
 158     SGPropertyNode *terrain_follow_factor; // modifies the climb rate to
 159                         // permit more control when using terrain following mode
 160
 161 public:
 162
 163     // constructor
 164     FGAutopilot();
 165
 166     // destructor
 167     ~FGAutopilot();
 168
 169 \f
 170     ////////////////////////////////////////////////////////////////////
 171     // Implementation of SGSubsystem.
 172     ////////////////////////////////////////////////////////////////////
 173
 174     void init ();
 175     void bind ();
 176     void unbind ();
 177     void update (double dt);
 178
 179     // Reset the autopilot system
 180     void reset(void);
 181
 182     void AltitudeSet( double new_altitude );
 183     void AltitudeAdjust( double inc );
 184     void HeadingAdjust( double inc );
 185     void AutoThrottleAdjust( double inc );
 186
 187     void HeadingSet( double value );
 188
 189     inline bool get_HeadingEnabled() const { return heading_hold; }
 190     inline void set_HeadingEnabled( bool value ) { heading_hold = value; }
 191     inline fgAutoHeadingMode get_HeadingMode() const { return heading_mode; }
 192     void set_HeadingMode( fgAutoHeadingMode mode );
 193
 194     inline bool get_AltitudeEnabled() const { return altitude_hold; }
 195     inline void set_AltitudeEnabled( bool value ) { altitude_hold = value; }
 196     inline fgAutoAltitudeMode get_AltitudeMode() const { return altitude_mode;}
 197     void set_AltitudeMode( fgAutoAltitudeMode mode );
 198
 199     inline bool get_AutoThrottleEnabled() const { return auto_throttle; }
 200     void set_AutoThrottleEnabled( bool value );
 201
 202     /* inline void set_WayPoint( const double lon, const double lat,
 203                               const double alt, const string s ) {
 204         waypoint = SGWayPoint( lon, lat, alt, SGWayPoint::WGS84, "Current WP" );
 205     } */
 206     inline double get_TargetLatitude() const {
 207         return waypoint.get_target_lat();
 208     }
 209     inline double get_TargetLongitude() const {
 210         return waypoint.get_target_lon();
 211     }
 212     inline void set_old_lat( double val ) { old_lat = val; }
 213     inline void set_old_lon( double val ) { old_lon = val; }
 214     inline double get_TargetHeading() const { return TargetHeading; }
 215     inline void set_TargetHeading( double val ) { TargetHeading = val; }
 216     inline double get_DGTargetHeading() const { return DGTargetHeading; }
 217     inline void set_DGTargetHeading( double val ) { DGTargetHeading = val; }
 218     inline double get_TargetDistance() const { return TargetDistance; }
 219     inline void set_TargetDistance( double val ) { TargetDistance = val; }
 220     inline double get_TargetAltitude() const { return TargetAltitude; }
 221     inline void set_TargetAltitude( double val ) { TargetAltitude = val; }
 222     inline double get_TargetSpeed() const { return TargetSpeed; }
 223     inline void set_TargetSpeed( double val ) { TargetSpeed = val; }
 224     inline double get_TargetAGL() const { return TargetAGL; }
 225     inline void set_TargetAGL( double val ) { TargetAGL = val; }
 226     inline double get_TargetClimbRate() const {
 227         return TargetClimbRate->getFloatValue();
 228     }
 229     inline void set_TargetClimbRate( double val ) {
 230         fgSetFloat( "/autopilot/config/target-climb-rate-fpm",  val);
 231     }
 232
 233     inline char *get_TargetLatitudeStr() { return TargetLatitudeStr; }
 234     inline char *get_TargetLongitudeStr() { return TargetLongitudeStr; }
 235     inline char *get_TargetWP1Str() { return TargetWP1Str; }
 236     inline char *get_TargetWP2Str() { return TargetWP2Str; }
 237     inline char *get_TargetWP3Str() { return TargetWP3Str; }
 238     inline char *get_TargetHeadingStr() { return TargetHeadingStr; }
 239     inline char *get_TargetAltitudeStr() { return TargetAltitudeStr; }
 240     inline char *get_TargetLatLonStr() { return TargetLatLonStr; }
 241
 242     // utility functions
 243     void MakeTargetLatLonStr( double lat, double lon );
 244     void MakeTargetAltitudeStr( double altitude );
 245     void MakeTargetHeadingStr( double bearing );
 246     void MakeTargetWPStr( double distance );
 247     void update_old_control_values();
 248
 249     // accessors
 250     inline double get_MaxRoll() {
 251         return fgGetFloat( "/autopilot/config/max-roll-deg" );
 252     }
 253     inline double get_RollOut() {
 254         return fgGetFloat( "/autopilot/config/roll-out-deg" );
 255     }
 256     inline double get_MaxAileron() {
 257         return fgGetFloat( "/autopilot/config/max-aileron" );
 258     }
 259     inline double get_RollOutSmooth() {
 260         return fgGetFloat( "/autopilot/config/roll-out-smooth-deg" );
 261     }
 262     inline void set_MaxRoll( double val ) {
 263         fgSetFloat( "/autopilot/config/max-roll-deg", val );
 264     }
 265     inline void set_RollOut( double val ) {
 266         fgSetFloat( "/autopilot/config/roll-out-deg", val );
 267     }
 268     inline void set_MaxAileron( double val ) {
 269         fgSetFloat( "/autopilot/config/max-aileron", val );
 270     }
 271     inline void set_RollOutSmooth( double val ) {
 272         fgSetFloat( "/autopilot/config/roll-out-smooth-deg", val );
 273     }
 274
 275 private:
 276
 277     bool getAPAltitudeLock () const;
 278     void setAPAltitudeLock (bool lock);
 279     double getAPAltitude () const;
 280     void setAPAltitude (double altitude);
 281     bool getAPGSLock () const;
 282     void setAPGSLock (bool lock);
 283     bool getAPVertSpeedLock () const;
 284     void setAPVertSpeedLock (bool lock);
 285     bool getAPTerrainLock () const;
 286     void setAPTerrainLock (bool lock);
 287     double getAPClimb () const;
 288     void setAPClimb (double rate);
 289     bool getAPHeadingLock () const;
 290     void setAPHeadingLock (bool lock);
 291     double getAPHeadingBug () const;
 292     void setAPHeadingBug (double heading);
 293     const char * getAPwaypoint () const;
 294     void setAPwaypoint (const char * apt);
 295     bool getAPWingLeveler () const;
 296     void setAPWingLeveler (bool lock);
 297     bool getAPNAV1Lock () const;
 298     void setAPNAV1Lock (bool lock);
 299     bool getAPAutoThrottleLock () const;
 300     void setAPAutoThrottleLock (bool lock);
 301     double getAPAutoThrottle () const;
 302     void setAPAutoThrottle (double altitude);
 303     double getAPRudderControl () const;
 304     void setAPRudderControl (double value);
 305     double getAPElevatorControl () const;
 306     void setAPElevatorControl (double value);
 307     double getAPThrottleControl () const;
 308     void setAPThrottleControl (double value);
 309     double getAPVertSpeed () const;
 310     void setAPVertSpeed (double speed);
 311
 312 };
 313
 314
 315 #define DEFAULT_AP_HEADING_LOCK FGAutopilot::FG_DG_HEADING_LOCK
 316 #define DEFAULT_AP_ALTITUDE_LOCK FGAutopilot::FG_ALTITUDE_LOCK
 317 //#define DEFAULT_AP_HEADING_LOCK FGAutopilot::FG_TRUE_HEADING_LOCK
 318 //#define DEFAULT_AP_ALTITUDE_LOCK FGAutopilot::FG_TRUE_ALTITUDE_LOCK
 319
 320 /**
 321  * static functions for autopilot properties
 322  */
 323
 324 #endif // _NEWAUTO_HXX
 325