#define _NEWAUTO_HXX
-#include <simgear/misc/props.hxx>
+#include <simgear/props/props.hxx>
+#include <simgear/structure/subsystem_mgr.hxx>
#include <simgear/route/waypoint.hxx>
#include <Main/fg_props.hxx>
// Structures
-class FGAutopilot {
+class FGAutopilot : public SGSubsystem
+{
public:
enum fgAutoHeadingMode {
- FG_DG_HEADING_LOCK = 0, // follow bug on directional gryo
- FG_TC_HEADING_LOCK = 1, // keep turn coordinator zero'd
- FG_TRUE_HEADING_LOCK = 2, // lock to true heading (i.e. a perfect world)
- FG_HEADING_NAV1 = 3, // follow nav1 radial
- FG_HEADING_NAV2 = 4, // follow nav2 radial
+ FG_DG_HEADING_LOCK = 0, // follow bug on directional gryo (vacuum)
+ FG_TC_HEADING_LOCK = 1, // keep turn coordinator zero'd
+ FG_TRUE_HEADING_LOCK = 2, // lock to true heading (i.e. a perfect world)
+ FG_HEADING_NAV1 = 3, // follow nav1 radial
+ FG_HEADING_NAV2 = 4, // follow nav2 radial
FG_HEADING_WAYPOINT = 5 // track next waypoint
};
enum fgAutoAltitudeMode {
- FG_ALTITUDE_LOCK = 0, // lock to a specific altitude
- FG_ALTITUDE_TERRAIN = 1, // try to maintain a specific AGL
- FG_ALTITUDE_GS1 = 2, // follow glide slope 1
- FG_ALTITUDE_GS2 = 3, // follow glide slope 2
- FG_ALTITUDE_ARM = 4 // ascend to selected altitude
+ FG_ALTITUDE_LOCK = 0, // lock to a specific altitude (indicated)
+ FG_ALTITUDE_TERRAIN = 1, // try to maintain a specific AGL
+ FG_ALTITUDE_GS1 = 2, // follow glide slope 1
+ FG_ALTITUDE_GS2 = 3, // follow glide slope 2
+ FG_ALTITUDE_ARM = 4, // ascend to selected altitude
+ FG_TRUE_ALTITUDE_LOCK = 5, // lock to a specific true altitude
+ // (i.e. a perfect world)
+ FG_VERT_SPEED = 6 // ascend or descend at selected fpm
};
-
private:
- bool heading_hold; // the current state of the heading hold
- bool altitude_hold; // the current state of the altitude hold
- bool auto_throttle; // the current state of the auto throttle
+ bool heading_hold; // the current state of the heading hold
+ bool altitude_hold; // the current state of the altitude hold
+ bool auto_throttle; // the current state of the auto throttle
fgAutoHeadingMode heading_mode;
fgAutoAltitudeMode altitude_mode;
- SGWayPoint waypoint; // the waypoint the AP should steer to.
+ SGWayPoint waypoint; // the waypoint the AP should steer to.
- // double TargetLatitude; // the latitude the AP should steer to.
- // double TargetLongitude; // the longitude the AP should steer to.
- double TargetDistance; // the distance to Target.
+ // double TargetLatitude; // the latitude the AP should steer to.
+ // double TargetLongitude; // the longitude the AP should steer to.
+ double TargetDistance; // the distance to Target.
double DGTargetHeading; // the apparent DG heading to steer towards.
- double TargetHeading; // the true heading the AP should steer to.
- double TargetAltitude; // altitude to hold
- double TargetAGL; // the terrain separation
+ double TargetHeading; // the true heading the AP should steer to.
+ double TargetAltitude; // altitude to hold
+ double TargetAGL; // the terrain separation
+
+ double TargetVertSpeed; // vertical speed to shoot for
- double TargetSpeed; // speed to shoot for
+ double TargetSpeed; // speed to shoot for
double alt_error_accum; // altitude error accumulator
- double climb_error_accum; // climb error accumulator (for GS)
- double speed_error_accum; // speed error accumulator
-
- double TargetSlope; // the glide slope hold value
-
- double MaxRoll ; // the max the plane can roll for the turn
- double RollOut; // when the plane should roll out
- // measured from Heading
- double MaxAileron; // how far to move the aleroin from center
- double RollOutSmooth; // deg to use for smoothing Aileron Control
- double MaxElevator; // the maximum elevator allowed
- double SlopeSmooth; // smoothing angle for elevator
-
+ double climb_error_accum; // climb error accumulator (for GS)
+ double speed_error_accum; // speed error accumulator
+
+ double current_ap_throttle; // current ap stored throttle setting used to set all engines
+ double previous_speed; // used to detect acceleration rate
+
+ double TargetSlope; // the glide slope hold value
+
+ double MaxRoll ; // the max the plane can roll for the turn
+ double RollOut; // when the plane should roll out
+ // measured from Heading
+ double MaxAileron; // how far to move the aleroin from center
+ double RollOutSmooth; // deg to use for smoothing Aileron Control
+ double MaxElevator; // the maximum elevator allowed
+ double SlopeSmooth; // smoothing angle for elevator
+
// following for testing disengagement of autopilot upon pilot
// interaction with controls
double old_aileron;
double old_elevator;
double old_elevator_trim;
double old_rudder;
-
+
// manual controls override beyond this value
double disengage_threshold;
SGPropertyNode *altitude_agl_node;
SGPropertyNode *vertical_speed_node;
SGPropertyNode *heading_node;
+ SGPropertyNode *indicated_heading_node;
SGPropertyNode *roll_node;
SGPropertyNode *pitch_node;
+ SGPropertyNode *airspeed_node;
SGPropertyNode *min_climb; // minimum climb speed
SGPropertyNode *best_climb; // best climb speed
// component of the pid
SGPropertyNode *zero_pitch_throttle; // amount of throttle at which the aircraft does not pitch up
SGPropertyNode *zero_pitch_trim_full_throttle; // amount of trim required to level at full throttle
- SGPropertyNode *TargetClimbRate; // target climb rate
- SGPropertyNode *TargetDescentRate; // target decent rate
+ SGPropertyNode *max_aileron_node; // maximum aileron setting range -1 ~ 1
+ SGPropertyNode *max_roll_node; // maximum roll setting in degrees
+ SGPropertyNode *roll_out_node; // start rollout offset from desired heading in degrees
+ SGPropertyNode *roll_out_smooth_node; // rollout smoothing offset in degrees
+ SGPropertyNode *throttle_adj_factor; // factor to optimize autothrottle adjustments
+ SGPropertyNode *throttle_integral; // amount of contribution of the integral
+ // component of the pid
+ SGPropertyNode *speed_change_node; // anticipated speed change
+
+ SGPropertyNode *TargetClimbRate; // target climb rate
+ SGPropertyNode *TargetDescentRate; // target decent rate
SGPropertyNode *current_throttle; // current throttle (engine 0)
+ SGPropertyNode *terrain_follow_factor; // modifies the climb rate to
+ // permit more control when using terrain following mode
public:
// destructor
~FGAutopilot();
- // Initialize autopilot system
- void init();
+\f
+ ////////////////////////////////////////////////////////////////////
+ // Implementation of SGSubsystem.
+ ////////////////////////////////////////////////////////////////////
+
+ void init ();
+ void bind ();
+ void unbind ();
+ void update (double dt);
// Reset the autopilot system
void reset(void);
- // run an interation of the autopilot (updates control positions)
- int run();
-
void AltitudeSet( double new_altitude );
void AltitudeAdjust( double inc );
void HeadingAdjust( double inc );
void set_AutoThrottleEnabled( bool value );
/* inline void set_WayPoint( const double lon, const double lat,
- const double alt, const string s ) {
- waypoint = SGWayPoint( lon, lat, alt, SGWayPoint::WGS84, "Current WP" );
+ const double alt, const string s ) {
+ waypoint = SGWayPoint( lon, lat, alt, SGWayPoint::WGS84, "Current WP" );
} */
inline double get_TargetLatitude() const {
- return waypoint.get_target_lat();
+ return waypoint.get_target_lat();
}
inline double get_TargetLongitude() const {
- return waypoint.get_target_lon();
+ return waypoint.get_target_lon();
}
inline void set_old_lat( double val ) { old_lat = val; }
inline void set_old_lon( double val ) { old_lon = val; }
inline void set_TargetDistance( double val ) { TargetDistance = val; }
inline double get_TargetAltitude() const { return TargetAltitude; }
inline void set_TargetAltitude( double val ) { TargetAltitude = val; }
+ inline double get_TargetSpeed() const { return TargetSpeed; }
+ inline void set_TargetSpeed( double val ) { TargetSpeed = val; }
inline double get_TargetAGL() const { return TargetAGL; }
inline void set_TargetAGL( double val ) { TargetAGL = val; }
inline double get_TargetClimbRate() const {
void update_old_control_values();
// accessors
- inline double get_MaxRoll() const { return MaxRoll; }
- inline void set_MaxRoll( double val ) { MaxRoll = val; }
- inline double get_RollOut() const { return RollOut; }
- inline void set_RollOut( double val ) { RollOut = val; }
+ inline double get_MaxRoll() {
+ return fgGetFloat( "/autopilot/config/max-roll-deg" );
+ }
+ inline double get_RollOut() {
+ return fgGetFloat( "/autopilot/config/roll-out-deg" );
+ }
+ inline double get_MaxAileron() {
+ return fgGetFloat( "/autopilot/config/max-aileron" );
+ }
+ inline double get_RollOutSmooth() {
+ return fgGetFloat( "/autopilot/config/roll-out-smooth-deg" );
+ }
+ inline void set_MaxRoll( double val ) {
+ fgSetFloat( "/autopilot/config/max-roll-deg", val );
+ }
+ inline void set_RollOut( double val ) {
+ fgSetFloat( "/autopilot/config/roll-out-deg", val );
+ }
+ inline void set_MaxAileron( double val ) {
+ fgSetFloat( "/autopilot/config/max-aileron", val );
+ }
+ inline void set_RollOutSmooth( double val ) {
+ fgSetFloat( "/autopilot/config/roll-out-smooth-deg", val );
+ }
- inline double get_MaxAileron() const { return MaxAileron; }
- inline void set_MaxAileron( double val ) { MaxAileron = val; }
- inline double get_RollOutSmooth() const { return RollOutSmooth; }
- inline void set_RollOutSmooth( double val ) { RollOutSmooth = val; }
+private:
-};
+ bool getAPAltitudeLock () const;
+ void setAPAltitudeLock (bool lock);
+ double getAPAltitude () const;
+ void setAPAltitude (double altitude);
+ bool getAPGSLock () const;
+ void setAPGSLock (bool lock);
+ bool getAPVertSpeedLock () const;
+ void setAPVertSpeedLock (bool lock);
+ bool getAPTerrainLock () const;
+ void setAPTerrainLock (bool lock);
+ double getAPClimb () const;
+ void setAPClimb (double rate);
+ bool getAPHeadingLock () const;
+ void setAPHeadingLock (bool lock);
+ double getAPHeadingBug () const;
+ void setAPHeadingBug (double heading);
+ const char * getAPwaypoint () const;
+ void setAPwaypoint (const char * apt);
+ bool getAPWingLeveler () const;
+ void setAPWingLeveler (bool lock);
+ bool getAPNAV1Lock () const;
+ void setAPNAV1Lock (bool lock);
+ bool getAPAutoThrottleLock () const;
+ void setAPAutoThrottleLock (bool lock);
+ double getAPAutoThrottle () const;
+ void setAPAutoThrottle (double altitude);
+ double getAPRudderControl () const;
+ void setAPRudderControl (double value);
+ double getAPElevatorControl () const;
+ void setAPElevatorControl (double value);
+ double getAPThrottleControl () const;
+ void setAPThrottleControl (double value);
+ double getAPVertSpeed () const;
+ void setAPVertSpeed (double speed);
+};
-extern FGAutopilot *current_autopilot;
#define DEFAULT_AP_HEADING_LOCK FGAutopilot::FG_DG_HEADING_LOCK
-// #define DEFAULT_AP_HEADING_LOCK FGAutopilot::FG_TRUE_HEADING_LOCK
-
+#define DEFAULT_AP_ALTITUDE_LOCK FGAutopilot::FG_ALTITUDE_LOCK
+//#define DEFAULT_AP_HEADING_LOCK FGAutopilot::FG_TRUE_HEADING_LOCK
+//#define DEFAULT_AP_ALTITUDE_LOCK FGAutopilot::FG_TRUE_ALTITUDE_LOCK
/**
* static functions for autopilot properties
*/
-/**
- * Get the autopilot altitude lock (true=on).
- */
-static bool
-getAPAltitudeLock ()
-{
- return (current_autopilot->get_AltitudeEnabled() &&
- current_autopilot->get_AltitudeMode()
- == FGAutopilot::FG_ALTITUDE_LOCK);
-}
-
-
-/**
- * Set the autopilot altitude lock (true=on).
- */
-static void
-setAPAltitudeLock (bool lock)
-{
- if (lock)
- current_autopilot->set_AltitudeMode(FGAutopilot::FG_ALTITUDE_LOCK);
- if (current_autopilot->get_AltitudeMode() == FGAutopilot::FG_ALTITUDE_LOCK)
- current_autopilot->set_AltitudeEnabled(lock);
-}
-
-
-/**
- * Get the autopilot target altitude in feet.
- */
-static double
-getAPAltitude ()
-{
- return current_autopilot->get_TargetAltitude() * SG_METER_TO_FEET;
-}
-
-
-/**
- * Set the autopilot target altitude in feet.
- */
-static void
-setAPAltitude (double altitude)
-{
- current_autopilot->set_TargetAltitude( altitude * SG_FEET_TO_METER );
-}
-
-/**
- * Get the autopilot altitude lock (true=on).
- */
-static bool
-getAPGSLock ()
-{
- return (current_autopilot->get_AltitudeEnabled() &&
- (current_autopilot->get_AltitudeMode()
- == FGAutopilot::FG_ALTITUDE_GS1));
-}
-
-
-/**
- * Set the autopilot altitude lock (true=on).
- */
-static void
-setAPGSLock (bool lock)
-{
- if (lock)
- current_autopilot->set_AltitudeMode(FGAutopilot::FG_ALTITUDE_GS1);
- if (current_autopilot->get_AltitudeMode() == FGAutopilot::FG_ALTITUDE_GS1)
- current_autopilot->set_AltitudeEnabled(lock);
-}
-
-
-/**
- * Get the autopilot terrain lock (true=on).
- */
-static bool
-getAPTerrainLock ()
-{
- return (current_autopilot->get_AltitudeEnabled() &&
- (current_autopilot->get_AltitudeMode()
- == FGAutopilot::FG_ALTITUDE_TERRAIN));
-}
-
-
-/**
- * Set the autopilot terrain lock (true=on).
- */
-static void
-setAPTerrainLock (bool lock)
-{
- if (lock) {
- current_autopilot->set_AltitudeMode(FGAutopilot::FG_ALTITUDE_TERRAIN);
- current_autopilot
- ->set_TargetAGL(fgGetFloat("/position/altitude-agl-ft") *
- SG_FEET_TO_METER);
- cout << "Target AGL = "
- << fgGetFloat("/position/altitude-agl-ft") * SG_FEET_TO_METER
- << endl;
- }
- if (current_autopilot->get_AltitudeMode() == FGAutopilot::FG_ALTITUDE_TERRAIN)
- current_autopilot->set_AltitudeEnabled(lock);
-}
-
-
-/**
- * Get the autopilot target altitude in feet.
- */
-static double
-getAPClimb ()
-{
- return current_autopilot->get_TargetClimbRate() * SG_METER_TO_FEET;
-}
-
-
-/**
- * Set the autopilot target altitude in feet.
- */
-static void
-setAPClimb (double rate)
-{
- current_autopilot->set_TargetClimbRate( rate * SG_FEET_TO_METER );
-}
-
-
-/**
- * Get the autopilot heading lock (true=on).
- */
-static bool
-getAPHeadingLock ()
-{
- return
- (current_autopilot->get_HeadingEnabled() &&
- current_autopilot->get_HeadingMode() == DEFAULT_AP_HEADING_LOCK);
-}
-
-
-/**
- * Set the autopilot heading lock (true=on).
- */
-static void
-setAPHeadingLock (bool lock)
-{
- if (lock)
- current_autopilot->set_HeadingMode(DEFAULT_AP_HEADING_LOCK);
- if (current_autopilot->get_HeadingMode() == DEFAULT_AP_HEADING_LOCK)
- current_autopilot->set_HeadingEnabled(lock);
-}
-
-
-/**
- * Get the autopilot heading bug in degrees.
- */
-static double
-getAPHeadingBug ()
-{
- return current_autopilot->get_DGTargetHeading();
-}
-
-
-/**
- * Set the autopilot heading bug in degrees.
- */
-static void
-setAPHeadingBug (double heading)
-{
- current_autopilot->set_DGTargetHeading( heading );
-}
-
-
-/**
- * Get the autopilot wing leveler lock (true=on).
- */
-static bool
-getAPWingLeveler ()
-{
- return
- (current_autopilot->get_HeadingEnabled() &&
- current_autopilot->get_HeadingMode() == FGAutopilot::FG_TC_HEADING_LOCK);
-}
-
-
-/**
- * Set the autopilot wing leveler lock (true=on).
- */
-static void
-setAPWingLeveler (bool lock)
-{
- if (lock)
- current_autopilot->set_HeadingMode(FGAutopilot::FG_TC_HEADING_LOCK);
- if (current_autopilot->get_HeadingMode() == FGAutopilot::FG_TC_HEADING_LOCK)
- current_autopilot->set_HeadingEnabled(lock);
-}
-
-/**
- * Return true if the autopilot is locked to NAV1.
- */
-static bool
-getAPNAV1Lock ()
-{
- return
- (current_autopilot->get_HeadingEnabled() &&
- current_autopilot->get_HeadingMode() == FGAutopilot::FG_HEADING_NAV1);
-}
-
-
-/**
- * Set the autopilot NAV1 lock.
- */
-static void
-setAPNAV1Lock (bool lock)
-{
- if (lock)
- current_autopilot->set_HeadingMode(FGAutopilot::FG_HEADING_NAV1);
- if (current_autopilot->get_HeadingMode() == FGAutopilot::FG_HEADING_NAV1)
- current_autopilot->set_HeadingEnabled(lock);
-}
-
-/**
- * Get the autopilot autothrottle lock.
- */
-static bool
-getAPAutoThrottleLock ()
-{
- return current_autopilot->get_AutoThrottleEnabled();
-}
-
-
-/**
- * Set the autothrottle lock.
- */
-static void
-setAPAutoThrottleLock (bool lock)
-{
- current_autopilot->set_AutoThrottleEnabled(lock);
-}
-
-
-// kludge
-static double
-getAPRudderControl ()
-{
- if (getAPHeadingLock())
- return current_autopilot->get_TargetHeading();
- else
- return globals->get_controls()->get_rudder();
-}
-
-// kludge
-static void
-setAPRudderControl (double value)
-{
- if (getAPHeadingLock()) {
- SG_LOG(SG_GENERAL, SG_DEBUG, "setAPRudderControl " << value );
- value -= current_autopilot->get_TargetHeading();
- current_autopilot->HeadingAdjust(value < 0.0 ? -1.0 : 1.0);
- } else {
- globals->get_controls()->set_rudder(value);
- }
-}
-
-// kludge
-static double
-getAPElevatorControl ()
-{
- if (getAPAltitudeLock())
- return current_autopilot->get_TargetAltitude();
- else
- return globals->get_controls()->get_elevator();
-}
-
-// kludge
-static void
-setAPElevatorControl (double value)
-{
- if (value != 0 && getAPAltitudeLock()) {
- SG_LOG(SG_GENERAL, SG_DEBUG, "setAPElevatorControl " << value );
- value -= current_autopilot->get_TargetAltitude();
- current_autopilot->AltitudeAdjust(value < 0.0 ? 100.0 : -100.0);
- } else {
- globals->get_controls()->set_elevator(value);
- }
-}
-
-// kludge
-static double
-getAPThrottleControl ()
-{
- if (getAPAutoThrottleLock())
- return 0.0; // always resets
- else
- return globals->get_controls()->get_throttle(0);
-}
-
-// kludge
-static void
-setAPThrottleControl (double value)
-{
- if (getAPAutoThrottleLock())
- current_autopilot->AutoThrottleAdjust(value < 0.0 ? -0.01 : 0.01);
- else
- globals->get_controls()->set_throttle(FGControls::ALL_ENGINES, value);
-}
-
#endif // _NEWAUTO_HXX
+
projects / flightgear.git / blobdiff