From a6cb16ce365ba6f5f8638d03f9e1e538e84c2838 Mon Sep 17 00:00:00 2001 From: curt Date: Fri, 1 Mar 2002 16:54:50 +0000 Subject: [PATCH] This is the change where autopilot bindings are moved from fg_props.cxx to newauto.cxx. Basically everything is the same functionally except for a changed in the initial altitude setting to 3000ft instead of meters (the panel is in feet). --- src/Autopilot/newauto.cxx | 129 +++++++++------ src/Autopilot/newauto.hxx | 333 +++++++++++++++++++++++++++++++++++-- src/Main/fg_props.cxx | 335 -------------------------------------- 3 files changed, 404 insertions(+), 393 deletions(-) diff --git a/src/Autopilot/newauto.cxx b/src/Autopilot/newauto.cxx index 78a1dcf29..eaf69cdf7 100644 --- a/src/Autopilot/newauto.cxx +++ b/src/Autopilot/newauto.cxx @@ -48,12 +48,6 @@ FGAutopilot *current_autopilot; -// Climb speed constants -// const double min_climb = 70.0; // kts -// const double best_climb = 75.0; // kts -// const double ideal_climb_rate = 500.0 * SG_FEET_TO_METER; // fpm -> mpm -// const double ideal_decent_rate = 1000.0 * SG_FEET_TO_METER; // fpm -> mpm - /// These statics will eventually go into the class /// they are just here while I am experimenting -- NHV :-) // AutoPilot Gain Adjuster members @@ -72,38 +66,6 @@ extern char *coord_format_lon(float); // constructor FGAutopilot::FGAutopilot() { - TargetClimbRate - = fgGetNode("/autopilot/config/target-climb-rate-fpm", true); - TargetDescentRate - = fgGetNode("/autopilot/config/target-descent-rate-fpm", true); - min_climb = fgGetNode("/autopilot/config/min-climb-speed-kt", true); - best_climb = fgGetNode("/autopilot/config/best-climb-speed-kt", true); - elevator_adj_factor - = fgGetNode("/autopilot/config/elevator-adj-factor", true); - integral_contrib - = fgGetNode("/autopilot/config/integral-contribution", true); - zero_pitch_throttle - = fgGetNode("/autopilot/config/zero-pitch-throttle", true); - zero_pitch_trim_full_throttle - = fgGetNode("/autopilot/config/zero-pitch-trim-full-throttle", true); - current_throttle = fgGetNode("/controls/throttle"); - // initialize with defaults (in case config isn't there) - if ( TargetClimbRate->getFloatValue() < 1 ) - fgSetFloat( "/autopilot/config/target-climb-rate-fpm", 500); - if ( TargetDescentRate->getFloatValue() < 1 ) - fgSetFloat( "/autopilot/config/target-descent-rate-fpm", 1000 ); - if ( min_climb->getFloatValue() < 1) - fgSetFloat( "/autopilot/config/min-climb-speed-kt", 70 ); - if (best_climb->getFloatValue() < 1) - fgSetFloat( "/autopilot/config/best-climb-speed-kt", 120 ); - if (elevator_adj_factor->getFloatValue() < 1) - fgSetFloat( "/autopilot/config/elevator-adj-factor", 5000 ); - if ( integral_contrib->getFloatValue() < 0.0000001 ) - fgSetFloat( "/autopilot/config/integral-contribution", 0.01 ); - if ( zero_pitch_throttle->getFloatValue() < 0.0000001 ) - fgSetFloat( "/autopilot/config/zero-pitch-throttle", 0.60 ); - if ( zero_pitch_trim_full_throttle->getFloatValue() < 0.0000001 ) - fgSetFloat( "/autopilot/config/zero-pitch-trim-full-throttle", 0.15 ); } // destructor @@ -249,6 +211,43 @@ void FGAutopilot::update_old_control_values() { void FGAutopilot::init() { SG_LOG( SG_AUTOPILOT, SG_INFO, "Init AutoPilot Subsystem" ); + // Autopilot control property static get/set bindings + fgTie("/autopilot/locks/altitude", getAPAltitudeLock, setAPAltitudeLock); + fgSetArchivable("/autopilot/locks/altitude"); + fgTie("/autopilot/settings/altitude-ft", getAPAltitude, setAPAltitude); + fgSetArchivable("/autopilot/settings/altitude-ft"); + fgTie("/autopilot/locks/glide-slope", getAPGSLock, setAPGSLock); + fgSetDouble("/autopilot/settings/altitude-ft", 3000.0f); + fgSetArchivable("/autopilot/locks/glide-slope"); + fgTie("/autopilot/locks/terrain", getAPTerrainLock, setAPTerrainLock); + fgSetArchivable("/autopilot/locks/terrain"); + fgTie("/autopilot/settings/climb-rate-fpm", getAPClimb, setAPClimb, false); + fgSetArchivable("/autopilot/settings/climb-rate-fpm"); + fgTie("/autopilot/locks/heading", getAPHeadingLock, setAPHeadingLock); + fgSetArchivable("/autopilot/locks/heading"); + fgTie("/autopilot/settings/heading-bug-deg", + getAPHeadingBug, setAPHeadingBug); + fgSetArchivable("/autopilot/settings/heading-bug-deg"); + fgSetDouble("/autopilot/settings/heading-bug-deg", 0.0f); + fgTie("/autopilot/locks/wing-leveler", getAPWingLeveler, setAPWingLeveler); + fgSetArchivable("/autopilot/locks/wing-leveler"); + fgTie("/autopilot/locks/nav[0]", getAPNAV1Lock, setAPNAV1Lock); + fgSetArchivable("/autopilot/locks/nav[0]"); + fgTie("/autopilot/locks/auto-throttle", + getAPAutoThrottleLock, setAPAutoThrottleLock); + fgSetArchivable("/autopilot/locks/auto-throttle"); + fgTie("/autopilot/control-overrides/rudder", + getAPRudderControl, setAPRudderControl); + fgSetArchivable("/autopilot/control-overrides/rudder"); + fgTie("/autopilot/control-overrides/elevator", + getAPElevatorControl, setAPElevatorControl); + fgSetArchivable("/autopilot/control-overrides/elevator"); + fgTie("/autopilot/control-overrides/throttle", + getAPThrottleControl, setAPThrottleControl); + fgSetArchivable("/autopilot/control-overrides/throttle"); + + + // bind data input property nodes... latitude_node = fgGetNode("/position/latitude-deg", true); longitude_node = fgGetNode("/position/longitude-deg", true); altitude_node = fgGetNode("/position/altitude-ft", true); @@ -256,14 +255,52 @@ void FGAutopilot::init() { vertical_speed_node = fgGetNode("/velocities/vertical-speed-fps", true); heading_node = fgGetNode("/orientation/heading-deg", true); roll_node = fgGetNode("/orientation/roll-deg", true); + pitch_node = fgGetNode("/orientation/pitch-deg", true); + + // bind config property nodes... + TargetClimbRate + = fgGetNode("/autopilot/config/target-climb-rate-fpm", true); + TargetDescentRate + = fgGetNode("/autopilot/config/target-descent-rate-fpm", true); + min_climb = fgGetNode("/autopilot/config/min-climb-speed-kt", true); + best_climb = fgGetNode("/autopilot/config/best-climb-speed-kt", true); + elevator_adj_factor + = fgGetNode("/autopilot/config/elevator-adj-factor", true); + integral_contrib + = fgGetNode("/autopilot/config/integral-contribution", true); + zero_pitch_throttle + = fgGetNode("/autopilot/config/zero-pitch-throttle", true); + zero_pitch_trim_full_throttle + = fgGetNode("/autopilot/config/zero-pitch-trim-full-throttle", true); + current_throttle = fgGetNode("/controls/throttle"); + + // initialize config properties with defaults (in case config isn't there) + if ( TargetClimbRate->getFloatValue() < 1 ) + fgSetFloat( "/autopilot/config/target-climb-rate-fpm", 500); + if ( TargetDescentRate->getFloatValue() < 1 ) + fgSetFloat( "/autopilot/config/target-descent-rate-fpm", 1000 ); + if ( min_climb->getFloatValue() < 1) + fgSetFloat( "/autopilot/config/min-climb-speed-kt", 70 ); + if (best_climb->getFloatValue() < 1) + fgSetFloat( "/autopilot/config/best-climb-speed-kt", 120 ); + if (elevator_adj_factor->getFloatValue() < 1) + fgSetFloat( "/autopilot/config/elevator-adj-factor", 5000 ); + if ( integral_contrib->getFloatValue() < 0.0000001 ) + fgSetFloat( "/autopilot/config/integral-contribution", 0.01 ); + if ( zero_pitch_throttle->getFloatValue() < 0.0000001 ) + fgSetFloat( "/autopilot/config/zero-pitch-throttle", 0.60 ); + if ( zero_pitch_trim_full_throttle->getFloatValue() < 0.0000001 ) + fgSetFloat( "/autopilot/config/zero-pitch-trim-full-throttle", 0.15 ); + /* set defaults */ heading_hold = false ; // turn the heading hold off altitude_hold = false ; // turn the altitude hold off auto_throttle = false ; // turn the auto throttle off heading_mode = DEFAULT_AP_HEADING_LOCK; - sg_srandom_time(); - DGTargetHeading = sg_random() * 360.0; + DGTargetHeading = fgGetDouble("/autopilot/settings/heading-bug-deg"); + TargetHeading = fgGetDouble("/autopilot/settings/heading-bug-deg"); + TargetAltitude = fgGetDouble("/autopilot/settings/altitude-ft") * SG_FEET_TO_METER; // Initialize target location to startup location old_lat = latitude_node->getDoubleValue(); @@ -272,8 +309,6 @@ void FGAutopilot::init() { MakeTargetLatLonStr( get_TargetLatitude(), get_TargetLongitude() ); - TargetHeading = 0.0; // default direction, due north - TargetAltitude = 3000; // default altitude in meters alt_error_accum = 0.0; climb_error_accum = 0.0; @@ -726,11 +761,11 @@ int FGAutopilot::run() { + (double) integral_contrib->getFloatValue() * int_adj; // stop on autopilot trim at 30% +/- - if ( total_adj > 0.3 ) { - total_adj = 0.3; - } else if ( total_adj < -0.3 ) { - total_adj = -0.3; - } +// if ( total_adj > 0.3 ) { +// total_adj = 0.3; +// } else if ( total_adj < -0.3 ) { +// total_adj = -0.3; +// } // adjust for throttle pitch gain total_adj += ((current_throttle->getFloatValue() - zero_pitch_throttle->getFloatValue()) diff --git a/src/Autopilot/newauto.hxx b/src/Autopilot/newauto.hxx index 252280ade..08fa318c2 100644 --- a/src/Autopilot/newauto.hxx +++ b/src/Autopilot/newauto.hxx @@ -50,9 +50,11 @@ public: 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_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 @@ -71,16 +73,7 @@ private: double TargetHeading; // the true heading the AP should steer to. double TargetAltitude; // altitude to hold double TargetAGL; // the terrain separation - 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) + double TargetSpeed; // speed to shoot for double alt_error_accum; // altitude error accumulator double climb_error_accum; // climb error accumulator (for GS) @@ -120,6 +113,7 @@ private: char TargetHeadingStr[64]; char TargetAltitudeStr[64]; + // property nodes SGPropertyNode *latitude_node; SGPropertyNode *longitude_node; SGPropertyNode *altitude_node; @@ -127,6 +121,18 @@ private: 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: @@ -229,4 +235,309 @@ 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 diff --git a/src/Main/fg_props.cxx b/src/Main/fg_props.cxx index 6a598b6bd..b7966352e 100644 --- a/src/Main/fg_props.cxx +++ b/src/Main/fg_props.cxx @@ -29,7 +29,6 @@ #include STL_IOSTREAM #include -#include #include #include