X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FAutopilot%2Fnewauto.hxx;h=08fa318c22b055e101202bc0320ac42866f09cfc;hb=b4eb6324aaa0d2704a2d809381c616b365783ea0;hp=9a2db9f411a4b20c511b9674c545345ca4d6b775;hpb=3ecf1b8dce76e1ceada10f3106c60b56d5939fc7;p=flightgear.git diff --git a/src/Autopilot/newauto.hxx b/src/Autopilot/newauto.hxx index 9a2db9f41..08fa318c2 100644 --- a/src/Autopilot/newauto.hxx +++ b/src/Autopilot/newauto.hxx @@ -27,25 +27,34 @@ #define _NEWAUTO_HXX +#include +#include + +#include
+ // 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 @@ -55,15 +64,19 @@ private: 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 @@ -94,12 +107,41 @@ private: 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(); @@ -129,22 +171,40 @@ public: 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; } @@ -153,7 +213,7 @@ public: 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 @@ -172,5 +232,312 @@ public: 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