fly/flightgear
1
0
Fork 0
Code Activity

Jim Wilson:

Browse source 
These changes should preserve previous functionality (with the exception of a
couple bug fixes).

Bugs fixed:
- AP no longer resets the error accumulator when switching altitude modes or
just closing the autopilot GUI.  It will not be necessary to collect the barf
bags after selecting a new altitude anymore.  Makes things much smoother.
- climb_rate calculation in the altitude hold mode included a factor that made
sense for the c172.  It is now scaled according to the configuration's target
climb rate.

Additions:
Autothrottle (supports speed control only) is more configurable and accurate.
VerticalSpeed mode added (automatically arms to altitude if flown toward
altitude setting).
Exposed various properties, added new lock properties.
This commit is contained in:
ehofman 2003-07-17 08:52:36 +00:00
parent 36183cbc3d
commit 9c22c5678c
2 changed files with 175 additions and 36 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

186
src/Autopilot/newauto.cxx
View file

@ -98,10 +98,6 @@ void FGAutopilot::MakeTargetHeadingStr( double bearing ) {
}
static inline double get_speed( void ) {
return( cur_fdm_state->get_V_equiv_kts() );
}
static inline double get_ground_speed() {
// starts in ft/s so we convert to kts
static const SGPropertyNode * speedup_node = fgGetNode("/sim/speed-up");
@ -213,10 +209,22 @@ void FGAutopilot::init ()
altitude_node = fgGetNode("/position/altitude-ft", true);
altitude_agl_node = fgGetNode("/position/altitude-agl-ft", true);
vertical_speed_node = fgGetNode("/velocities/vertical-speed-fps", true);
airspeed_node = fgGetNode("/velocities/airspeed-kt", true);
heading_node = fgGetNode("/orientation/heading-deg", true);
dg_heading_node
// support non-dg systems that indicate magnetic heading (e.g. 747-400)
if (fgGetBool("autopilot/config/indicated-heading-magnetic")) {
// use magnetic heading indicated
indicated_heading_node
= fgGetNode("/orientation/heading-magnetic-deg",
true);
} else {
// use dg heading indicated
indicated_heading_node
= fgGetNode("/instrumentation/heading-indicator/indicated-heading-deg",
true);
}
roll_node = fgGetNode("/orientation/roll-deg", true);
pitch_node = fgGetNode("/orientation/pitch-deg", true);
@ -241,6 +249,13 @@ void FGAutopilot::init ()
max_roll_node = fgGetNode("/autopilot/config/max-roll-deg", true);
roll_out_node = fgGetNode("/autopilot/config/roll-out-deg", true);
roll_out_smooth_node = fgGetNode("/autopilot/config/roll-out-smooth-deg", true);
throttle_adj_factor
= fgGetNode("/autopilot/config/throttle-adj-factor", true);
throttle_integral
= fgGetNode("/autopilot/config/throttle-integral", true);
speed_change_node
= fgGetNode("/autopilot/output/speed_change_anticipated_kt", true);
terrain_follow_factor = fgGetNode("/autopilot/config/terrain-follow-factor", true);
current_throttle = fgGetNode("/controls/engines/engine/throttle");
@ -270,6 +285,10 @@ void FGAutopilot::init ()
fgSetFloat( "/autopilot/config/roll-out-deg", 20 );
if ( roll_out_smooth_node->getFloatValue() < 0.0000001 )
fgSetFloat( "/autopilot/config/roll-out-smooth-deg", 10 );
if (throttle_adj_factor->getFloatValue() < 1)
fgSetFloat( "/autopilot/config/throttle-adj-factor", 5000 );
if ( throttle_integral->getFloatValue() < 0.0000001 )
fgSetFloat( "/autopilot/config/throttle-integral", 0.001 );
if (terrain_follow_factor->getFloatValue() < 1)
fgSetFloat( "/autopilot/config/terrain-follow-factor", 16 );
@ -283,6 +302,7 @@ void FGAutopilot::init ()
DGTargetHeading = fgGetDouble("/autopilot/settings/heading-bug-deg");
TargetHeading = fgGetDouble("/autopilot/settings/heading-bug-deg");
TargetAltitude = fgGetDouble("/autopilot/settings/altitude-ft") * SG_FEET_TO_METER;
TargetSpeed = fgGetDouble("/autopilot/settings/speed-kt");
// Initialize target location to startup location
old_lat = latitude_node->getDoubleValue();
@ -315,6 +335,9 @@ void FGAutopilot::init ()
// set default aileron max deflection
MaxAileron = 0.5;
// used to calculate acceleration
previous_speed = 0;
#if !defined( USING_SLIDER_CLASS )
MaxRollAdjust = 2 * MaxRoll;
RollOutAdjust = 2 * RollOut;
@ -349,6 +372,11 @@ FGAutopilot::bind ()
&FGAutopilot::getAPHeadingLock, &FGAutopilot::setAPHeadingLock);
fgSetArchivable("/autopilot/locks/heading");
fgTie("/autopilot/locks/vert-speed", this,
&FGAutopilot::getAPVertSpeedLock, &FGAutopilot::setAPVertSpeedLock);
fgSetArchivable("/autopilot/locks/vert-speed");
fgTie("/autopilot/settings/heading-bug-deg", this,
&FGAutopilot::getAPHeadingBug, &FGAutopilot::setAPHeadingBug);
fgSetArchivable("/autopilot/settings/heading-bug-deg");
@ -369,6 +397,9 @@ FGAutopilot::bind ()
&FGAutopilot::getAPAutoThrottleLock,
&FGAutopilot::setAPAutoThrottleLock);
fgSetArchivable("/autopilot/locks/auto-throttle");
fgTie("/autopilot/settings/speed-kt", this,
&FGAutopilot::getAPAutoThrottle, &FGAutopilot::setAPAutoThrottle);
fgSetArchivable("/autopilot/settings/altitude-ft");
fgTie("/autopilot/control-overrides/rudder", this,
&FGAutopilot::getAPRudderControl,
&FGAutopilot::setAPRudderControl);
@ -381,6 +412,12 @@ FGAutopilot::bind ()
&FGAutopilot::getAPThrottleControl,
&FGAutopilot::setAPThrottleControl);
fgSetArchivable("/autopilot/control-overrides/throttle");
fgTie("/autopilot/settings/vertical-speed-fpm", this,
&FGAutopilot::getAPVertSpeed, &FGAutopilot::setAPVertSpeed);
fgSetArchivable("/autopilot/settings/vertical-speed-fpm");
fgSetDouble("/autopilot/settings/vertical-speed-fpm", 0.0f);
}
void
@ -452,11 +489,8 @@ static double LinearExtrapolate( double x, double x1, double y1, double x2, doub
void
FGAutopilot::update (double dt)
{
// Remove the following lines when the calling funcitons start
// passing in the data pointer
// get control settings
double lat = latitude_node->getDoubleValue();
double lon = longitude_node->getDoubleValue();
double alt = altitude_node->getDoubleValue() * SG_FEET_TO_METER;
@ -499,7 +533,7 @@ FGAutopilot::update (double dt)
if ( heading_hold == true ) {
if ( heading_mode == FG_DG_HEADING_LOCK ) {
double dg_error = heading_node->getDoubleValue()
- dg_heading_node->getDoubleValue();
- indicated_heading_node->getDoubleValue();
TargetHeading = DGTargetHeading + dg_error;
// cout << "dg_error = " << dg_error << endl;
while ( TargetHeading < 0.0 ) { TargetHeading += 360.0; }
@ -712,9 +746,9 @@ FGAutopilot::update (double dt)
if ( altitude_mode == FG_ALTITUDE_LOCK ) {
climb_rate =
( TargetAltitude -
fgGetDouble("/instrumentation/altimeter/indicated-altitude-ft") * SG_FEET_TO_METER ) * 8.0;
fgGetDouble("/instrumentation/altimeter/indicated-altitude-ft") * SG_FEET_TO_METER ) * (TargetClimbRate->getDoubleValue() * 0.016);
} else if ( altitude_mode == FG_TRUE_ALTITUDE_LOCK ) {
climb_rate = ( TargetAltitude - alt ) * 8.0;
climb_rate = ( TargetAltitude - alt ) * (TargetClimbRate->getDoubleValue() * 0.016);
} else if ( altitude_mode == FG_ALTITUDE_GS1 ) {
double x = current_radiostack->get_navcom1()->get_nav_gs_dist();
double y = (altitude_node->getDoubleValue()
@ -740,12 +774,18 @@ FGAutopilot::update (double dt)
( TargetAGL - altitude_agl_node->getDoubleValue()
* SG_FEET_TO_METER )
* terrain_follow_factor->getFloatValue();
} else if ( altitude_mode == FG_VERT_SPEED ) {
climb_rate = TargetVertSpeed * SG_FEET_TO_METER;
// switch to altitude hold, if set, within 500ft of target
if (fabs(TargetAltitude * SG_METER_TO_FEET - altitude_node->getDoubleValue()) < 500) {
set_AltitudeMode( FG_ALTITUDE_LOCK );
}
} else {
// just try to zero out rate of climb ...
climb_rate = 0.0;
}
speed = get_speed();
speed = airspeed_node->getFloatValue();
if ( speed < min_climb->getFloatValue() ) {
max_climb = 0.0;
@ -760,24 +800,28 @@ FGAutopilot::update (double dt)
+ fabs(TargetClimbRate->getFloatValue() * SG_FEET_TO_METER);
}
// this first one could be optional if we wanted to allow
// better climb performance assuming we have the airspeed to
// support it.
if ( climb_rate >
if (altitude_mode != FG_VERT_SPEED) {
// this first one could be optional if we wanted to allow
// better climb performance assuming we have the airspeed to
// support it.
if ( climb_rate >
fabs(TargetClimbRate->getFloatValue() * SG_FEET_TO_METER) ) {
climb_rate
climb_rate
= fabs(TargetClimbRate->getFloatValue() * SG_FEET_TO_METER);
}
}
if ( climb_rate > max_climb ) {
climb_rate = max_climb;
}
if ( climb_rate > max_climb ) {
climb_rate = max_climb;
}
if ( climb_rate <
if ( climb_rate <
-fabs(TargetDescentRate->getFloatValue() * SG_FEET_TO_METER) ) {
climb_rate
climb_rate
= -fabs(TargetDescentRate->getFloatValue() * SG_FEET_TO_METER);
}
}
}
error = vertical_speed_node->getDoubleValue() * 60
- climb_rate * SG_METER_TO_FEET;
@ -824,8 +868,17 @@ FGAutopilot::update (double dt)
double error;
double prop_error, int_error;
double prop_adj, int_adj, total_adj;
double lookahead;
error = TargetSpeed - get_speed();
// estimate speed in 10 seconds
lookahead = airspeed_node->getFloatValue() + ( airspeed_node->getFloatValue() - previous_speed) * (10/dt);
previous_speed = airspeed_node->getFloatValue();
// compare targetspeed to anticipated airspeed
error = TargetSpeed - lookahead;
// output anticipated speed change...
speed_change_node->setDoubleValue(lookahead - airspeed_node->getFloatValue());
// accumulate the error under the curve ... this really should
// be *= delta t
@ -841,13 +894,17 @@ FGAutopilot::update (double dt)
int_error = speed_error_accum;
// printf("error = %.2f int_error = %.2f\n", error, int_error);
int_adj = int_error / 200.0;
int_adj = int_error / throttle_adj_factor->getFloatValue();
// caclulate proportional error
prop_error = error;
prop_adj = 0.5 + prop_error / 50.0;
prop_adj = prop_error / throttle_adj_factor->getFloatValue();
total_adj = (1.0 - throttle_integral->getFloatValue()) * prop_adj +
throttle_integral->getFloatValue() * int_adj;
total_adj = current_throttle->getFloatValue() + total_adj;
total_adj = 0.9 * prop_adj + 0.1 * int_adj;
if ( total_adj > 1.0 ) {
total_adj = 1.0;
}
@ -956,8 +1013,11 @@ void FGAutopilot::set_HeadingMode( fgAutoHeadingMode mode ) {
void FGAutopilot::set_AltitudeMode( fgAutoAltitudeMode mode ) {
altitude_mode = mode;
alt_error_accum = 0.0;
// only reset accum error if not in altitude mode for smooth transitions
// from one altitude mode to another until pitch control damping added.
if (!altitude_hold) {
alt_error_accum = 0.0;
}
if ( altitude_mode == DEFAULT_AP_ALTITUDE_LOCK ) {
if ( TargetAltitude < altitude_agl_node->getDoubleValue()
@ -1098,15 +1158,34 @@ void FGAutopilot::HeadingSet( double new_heading ) {
void FGAutopilot::AutoThrottleAdjust( double inc ) {
double target = ( int ) ( TargetSpeed / inc ) * inc + inc;
TargetSpeed = target;
set_TargetSpeed( target );
}
/**
* Set the autothrottle speed
*/
void
FGAutopilot::setAPAutoThrottle (double speed_kt)
{
set_TargetSpeed( speed_kt );
}
/**
* Get the autothrottle speed
*/
double
FGAutopilot::getAPAutoThrottle () const
{
return get_TargetSpeed();
}
void FGAutopilot::set_AutoThrottleEnabled( bool value ) {
auto_throttle = value;
if ( auto_throttle == true ) {
TargetSpeed = fgGetDouble("/velocities/airspeed-kt");
if (TargetSpeed < 0.0001) {
TargetSpeed = fgGetDouble("/velocities/airspeed-kt");
}
speed_error_accum = 0.0;
}
@ -1221,6 +1300,30 @@ FGAutopilot::setAPTerrainLock (bool lock)
set_AltitudeEnabled(lock);
}
/**
* Get the autopilot vertical speed lock (true=on).
*/
bool
FGAutopilot::getAPVertSpeedLock () const
{
return (get_AltitudeEnabled() &&
(get_AltitudeMode()
== FGAutopilot::FG_VERT_SPEED));
}
/**
* Set the autopilot vert speed lock (true=on).
*/
void
FGAutopilot::setAPVertSpeedLock (bool lock)
{
if (lock)
set_AltitudeMode(FGAutopilot::FG_VERT_SPEED);
if (get_AltitudeMode() == FGAutopilot::FG_VERT_SPEED)
set_AltitudeEnabled(lock);
}
/**
* Get the autopilot target altitude in feet.
@ -1469,5 +1572,24 @@ FGAutopilot::setAPThrottleControl (double value)
globals->get_controls()->set_throttle(FGControls::ALL_ENGINES, value);
}
/**
* Get the vertical speed selected
*/
double
FGAutopilot::getAPVertSpeed () const
{
return TargetVertSpeed;
}
/**
* Set the selected vertical speed
*/
void
FGAutopilot::setAPVertSpeed (double speed)
{
TargetVertSpeed = speed;
}
// end of newauto.cxx

25
src/Autopilot/newauto.hxx
View file

@ -54,11 +54,11 @@ public:
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_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
@ -78,11 +78,15 @@ private:
double TargetAltitude; // altitude to hold
double TargetAGL; // the terrain separation
double TargetVertSpeed; // vertical 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 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
@ -124,9 +128,10 @@ private:
SGPropertyNode *altitude_agl_node;
SGPropertyNode *vertical_speed_node;
SGPropertyNode *heading_node;
SGPropertyNode *dg_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
@ -139,6 +144,10 @@ private:
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
@ -207,6 +216,8 @@ public:
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 {
@ -266,6 +277,8 @@ private:
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;
@ -282,12 +295,16 @@ private:
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);
};