#define _NEWAUTO_HXX
+#include <simgear/misc/props.hxx>
+#include <simgear/route/waypoint.hxx>
+
+#include <Main/fg_props.hxx>
+
// Structures
class FGAutopilot {
public:
enum fgAutoHeadingMode {
- FG_HEADING_LOCK = 0,
- FG_HEADING_WAYPOINT = 1,
- FG_HEADING_NAV1 = 2,
- FG_HEADING_NAV2 = 3
+ 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_HEADING_WAYPOINT = 5 // track next waypoint
};
enum fgAutoAltitudeMode {
- FG_ALTITUDE_LOCK = 0,
- FG_ALTITUDE_TERRAIN = 1,
- FG_ALTITUDE_GS1 = 2,
- FG_ALTITUDE_GS2 = 3
+ 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
};
+
private:
bool heading_hold; // the current state of the heading hold
fgAutoHeadingMode heading_mode;
fgAutoAltitudeMode altitude_mode;
- double TargetLatitude; // the latitude the AP should steer to.
- double TargetLongitude; // the longitude 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 TargetHeading; // the heading the AP should steer to.
+ 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 TargetClimbRate; // climb rate to shoot for
+
double TargetSpeed; // speed to shoot for
- double alt_error_accum; // altitude error accumulator
+ 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
char TargetLatitudeStr[64];
char TargetLongitudeStr[64];
char TargetLatLonStr[64];
- char TargetDistanceStr[64];
+ char TargetWP1Str[64];
+ char TargetWP2Str[64];
+ char TargetWP3Str[64];
char TargetHeadingStr[64];
char TargetAltitudeStr[64];
+ // property nodes
+ SGPropertyNode *latitude_node;
+ SGPropertyNode *longitude_node;
+ SGPropertyNode *altitude_node;
+ SGPropertyNode *altitude_agl_node;
+ SGPropertyNode *vertical_speed_node;
+ SGPropertyNode *heading_node;
+ SGPropertyNode *roll_node;
+ SGPropertyNode *pitch_node;
+
+ SGPropertyNode *min_climb; // minimum climb speed
+ SGPropertyNode *best_climb; // best climb speed
+ SGPropertyNode *elevator_adj_factor; // factor to optimize altitude hold adjustments
+ SGPropertyNode *integral_contrib; // amount of contribution of the integral
+ // 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 *current_throttle; // current throttle (engine 0)
+
public:
+ // constructor
+ FGAutopilot();
+
+ // destructor
+ ~FGAutopilot();
+
// Initialize autopilot system
void init();
inline bool get_AutoThrottleEnabled() const { return auto_throttle; }
void set_AutoThrottleEnabled( bool value );
- inline double get_TargetLatitude() const { return TargetLatitude; }
- inline void set_TargetLatitude( double val ) { TargetLatitude = val; }
+ /* 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" );
+ } */
+ inline double get_TargetLatitude() const {
+ return waypoint.get_target_lat();
+ }
+ inline double get_TargetLongitude() const {
+ return waypoint.get_target_lon();
+ }
inline void set_old_lat( double val ) { old_lat = val; }
- inline double get_TargetLongitude() const { return TargetLongitude; }
- inline void set_TargetLongitude( double val ) { TargetLongitude = val; }
inline void set_old_lon( double val ) { old_lon = val; }
inline double get_TargetHeading() const { return TargetHeading; }
inline void set_TargetHeading( double val ) { TargetHeading = val; }
+ inline double get_DGTargetHeading() const { return DGTargetHeading; }
+ inline void set_DGTargetHeading( double val ) { DGTargetHeading = val; }
inline double get_TargetDistance() const { return TargetDistance; }
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_TargetAGL() const { return TargetAGL; }
+ inline void set_TargetAGL( double val ) { TargetAGL = val; }
+ inline double get_TargetClimbRate() const {
+ return TargetClimbRate->getFloatValue();
+ }
+ inline void set_TargetClimbRate( double val ) {
+ fgSetFloat( "/autopilot/config/target-climb-rate-fpm", val);
+ }
inline char *get_TargetLatitudeStr() { return TargetLatitudeStr; }
inline char *get_TargetLongitudeStr() { return TargetLongitudeStr; }
- inline char *get_TargetDistanceStr() { return TargetDistanceStr; }
+ inline char *get_TargetWP1Str() { return TargetWP1Str; }
+ inline char *get_TargetWP2Str() { return TargetWP2Str; }
+ inline char *get_TargetWP3Str() { return TargetWP3Str; }
inline char *get_TargetHeadingStr() { return TargetHeadingStr; }
inline char *get_TargetAltitudeStr() { return TargetAltitudeStr; }
inline char *get_TargetLatLonStr() { return TargetLatLonStr; }
void MakeTargetLatLonStr( double lat, double lon );
void MakeTargetAltitudeStr( double altitude );
void MakeTargetHeadingStr( double bearing );
- void MakeTargetDistanceStr( double distance );
+ void MakeTargetWPStr( double distance );
void update_old_control_values();
// accessors
extern FGAutopilot *current_autopilot;
+#define DEFAULT_AP_HEADING_LOCK FGAutopilot::FG_DG_HEADING_LOCK
+// #define DEFAULT_AP_HEADING_LOCK FGAutopilot::FG_TRUE_HEADING_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