Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-23 09:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

APM_Control: run AC_PID in parallel with old PID for roll/pitch

Browse Source
This commit is contained in:
Andrew Tridgell 2020-09-19 10:29:41 +10:00
parent d2d06af751
commit 6ca9033dde
4 changed files with 498 additions and 77 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

270
libraries/APM_Control/AP_PitchController.cpp
View File

@ -23,99 +23,175 @@ extern const AP_HAL::HAL& hal;
const AP_Param::GroupInfo AP_PitchController::var_info[] = {
// @Param: TCONST
// @Param: 2SRV_TCONST
// @DisplayName: Pitch Time Constant
// @Description: Time constant in seconds from demanded to achieved pitch angle. Most models respond well to 0.5. May be reduced for faster responses, but setting lower than a model can achieve will not help.
// @Range: 0.4 1.0
// @Units: s
// @Increment: 0.1
// @User: Advanced
AP_GROUPINFO("TCONST", 0, AP_PitchController, gains.tau, 0.5f),
AP_GROUPINFO("2SRV_TCONST", 0, AP_PitchController, gains.tau, 0.5f),
// @Param: P
// @Param: 2SRV_P
// @DisplayName: Proportional Gain
// @Description: Proportional gain from pitch angle demands to elevator. Higher values allow more servo response but can cause oscillations. Automatically set and adjusted by AUTOTUNE mode.
// @Range: 0.1 3.0
// @Increment: 0.1
// @User: Standard
AP_GROUPINFO("P", 1, AP_PitchController, gains.P, 1.0f),
// @User: Standard
AP_GROUPINFO("2SRV_P", 1, AP_PitchController, gains.P, 1.0f),
// @Param: D
// @Param: 2SRV_D
// @DisplayName: Damping Gain
// @Description: Damping gain from pitch acceleration to elevator. Higher values reduce pitching in turbulence, but can cause oscillations. Automatically set and adjusted by AUTOTUNE mode.
// @Range: 0 0.2
// @Increment: 0.01
// @User: Standard
AP_GROUPINFO("D", 2, AP_PitchController, gains.D, 0.04f),
// @User: Standard
AP_GROUPINFO("2SRV_D", 2, AP_PitchController, gains.D, 0.04f),
// @Param: I
// @Param: 2SRV_I
// @DisplayName: Integrator Gain
// @Description: Integrator gain from long-term pitch angle offsets to elevator. Higher values "trim" out offsets faster but can cause oscillations. Automatically set and adjusted by AUTOTUNE mode.
// @Range: 0 0.5
// @Increment: 0.05
// @User: Standard
AP_GROUPINFO("I", 3, AP_PitchController, gains.I, 0.3f),
// @User: Standard
AP_GROUPINFO("2SRV_I", 3, AP_PitchController, gains.I, 0.3f),
// @Param: RMAX_UP
// @Param: 2SRV_RMAX_UP
// @DisplayName: Pitch up max rate
// @Description: Maximum pitch up rate that the pitch controller demands (degrees/sec) in ACRO mode.
// @Range: 0 100
// @Units: deg/s
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("RMAX_UP", 4, AP_PitchController, gains.rmax, 0.0f),
AP_GROUPINFO("2SRV_RMAX_UP", 4, AP_PitchController, gains.rmax, 0.0f),
// @Param: RMAX_DN
// @Param: 2SRV_RMAX_DN
// @DisplayName: Pitch down max rate
// @Description: This sets the maximum nose down pitch rate that the controller will demand (degrees/sec). Setting it to zero disables the limit.
// @Range: 0 100
// @Units: deg/s
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("RMAX_DN", 5, AP_PitchController, _max_rate_neg, 0.0f),
AP_GROUPINFO("2SRV_RMAX_DN", 5, AP_PitchController, _max_rate_neg, 0.0f),
// @Param: RLL
// @Param: 2SRV_RLL
// @DisplayName: Roll compensation
// @Description: Gain added to pitch to keep aircraft from descending or ascending in turns. Increase in increments of 0.05 to reduce altitude loss. Decrease for altitude gain.
// @Range: 0.7 1.5
// @Increment: 0.05
// @User: Standard
AP_GROUPINFO("RLL", 6, AP_PitchController, _roll_ff, 1.0f),
// @User: Standard
AP_GROUPINFO("2SRV_RLL", 6, AP_PitchController, _roll_ff, 1.0f),
// @Param: IMAX
// @Param: 2SRV_IMAX
// @DisplayName: Integrator limit
// @Description: Limit of pitch integrator gain in centi-degrees of servo travel. Servos are assumed to have +/- 4500 centi-degrees of travel, so a value of 3000 allows trim of up to 2/3 of servo travel range.
// @Range: 0 4500
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("IMAX", 7, AP_PitchController, gains.imax, 3000),
AP_GROUPINFO("2SRV_IMAX", 7, AP_PitchController, gains.imax, 3000),
// @Param: FF
// @Param: 2SRV_FF
// @DisplayName: Feed forward Gain
// @Description: Gain from demanded rate to elevator output.
// @Range: 0.1 4.0
// @Increment: 0.1
// @User: Standard
AP_GROUPINFO("FF", 8, AP_PitchController, gains.FF, 0.0f),
// @User: Standard
AP_GROUPINFO("2SRV_FF", 8, AP_PitchController, gains.FF, 0.0f),
// @Param: SRMAX
// @Param: 2SRV_SRMAX
// @DisplayName: Servo slew rate limit
// @Description: Sets an upper limit on the servo slew rate produced by the D-gain (pitch rate feedback). If the amplitude of the control action produced by the pitch rate feedback exceeds this value, then the D-gain is reduced to respect the limit. This limits the amplitude of high frequency oscillations caused by an excessive D-gain. The limit should be set to no more than 25% of the servo's specified slew rate to allow for inertia and aerodynamic load effects. Note: The D-gain will not be reduced to less than 10% of the nominal value. A value of zero will disable this feature.
// @Units: deg/s
// @Range: 0 500
// @Increment: 10.0
// @User: Advanced
AP_GROUPINFO("SRMAX", 9, AP_PitchController, _slew_rate_max, 150.0f),
AP_GROUPINFO("2SRV_SRMAX", 9, AP_PitchController, _slew_rate_max, 150.0f),
// @Param: SRTAU
// @Param: 2SRV_SRTAU
// @DisplayName: Servo slew rate decay time constant
// @Description: This sets the time constant used to recover the D gain after it has been reduced due to excessive servo slew rate.
// @Units: s
// @Range: 0.5 5.0
// @Increment: 0.1
// @User: Advanced
AP_GROUPINFO("SRTAU", 10, AP_PitchController, _slew_rate_tau, 1.0f),
AP_GROUPINFO("2SRV_SRTAU", 10, AP_PitchController, _slew_rate_tau, 1.0f),
// @Param: _RATE_P
// @DisplayName: Pitch axis rate controller P gain
// @Description: Pitch axis rate controller P gain. Converts the difference between desired roll rate and actual roll rate into a motor speed output
// @Range: 0.08 0.35
// @Increment: 0.005
// @User: Standard
// @Param: _RATE_I
// @DisplayName: Pitch axis rate controller I gain
// @Description: Pitch axis rate controller I gain. Corrects long-term difference in desired roll rate vs actual roll rate
// @Range: 0.01 0.6
// @Increment: 0.01
// @User: Standard
// @Param: _RATE_IMAX
// @DisplayName: Pitch axis rate controller I gain maximum
// @Description: Pitch axis rate controller I gain maximum. Constrains the maximum motor output that the I gain will output
// @Range: 0 1
// @Increment: 0.01
// @User: Standard
// @Param: _RATE_D
// @DisplayName: Pitch axis rate controller D gain
// @Description: Pitch axis rate controller D gain. Compensates for short-term change in desired roll rate vs actual roll rate
// @Range: 0.001 0.03
// @Increment: 0.001
// @User: Standard
// @Param: _RATE_FF
// @DisplayName: Pitch axis rate controller feed forward
// @Description: Pitch axis rate controller feed forward
// @Range: 0 3.0
// @Increment: 0.001
// @User: Standard
// @Param: _RATE_FLTT
// @DisplayName: Pitch axis rate controller target frequency in Hz
// @Description: Pitch axis rate controller target frequency in Hz
// @Range: 2 50
// @Increment: 1
// @Units: Hz
// @User: Standard
// @Param: _RATE_FLTE
// @DisplayName: Pitch axis rate controller error frequency in Hz
// @Description: Pitch axis rate controller error frequency in Hz
// @Range: 2 50
// @Increment: 1
// @Units: Hz
// @User: Standard
// @Param: _RATE_FLTD
// @DisplayName: Pitch axis rate controller derivative frequency in Hz
// @Description: Pitch axis rate controller derivative frequency in Hz
// @Range: 0 50
// @Increment: 1
// @Units: Hz
// @User: Standard
// @Param: _RATE_SMAX
// @DisplayName: Pitch slew rate limit
// @Description: Sets an upper limit on the slew rate produced by the combined P and D gains. If the amplitude of the control action produced by the rate feedback exceeds this value, then the D+P gain is reduced to respect the limit. This limits the amplitude of high frequency oscillations caused by an excessive gain. The limit should be set to no more than 25% of the actuators maximum slew rate to allow for load effects. Note: The gain will not be reduced to less than 10% of the nominal value. A value of zero will disable this feature.
// @Range: 0 200
// @Increment: 0.5
// @User: Advanced
// @Param: _RATE_STAU
// @DisplayName: Pitch slew rate decay time constant
// @Description: This sets the time constant used to recover the P+D gain after it has been reduced due to excessive slew rate.
// @Units: s
// @Range: 0.5 5.0
// @Increment: 0.1
// @User: Advanced
AP_SUBGROUPINFO(rate_pid, "_RATE_", 11, AP_PitchController, AC_PID),
AP_GROUPEND
};
@ -129,7 +205,7 @@ const AP_Param::GroupInfo AP_PitchController::var_info[] = {
4) minimum FBW airspeed (metres/sec)
5) maximum FBW airspeed (metres/sec)
*/
int32_t AP_PitchController::_get_rate_out(float desired_rate, float scaler, bool disable_integrator, float aspeed)
int32_t AP_PitchController::_get_rate_out_old(float desired_rate, float scaler, bool disable_integrator, float aspeed)
{
uint32_t tnow = AP_HAL::millis();
uint32_t dt = tnow - _last_t;
@ -199,6 +275,8 @@ int32_t AP_PitchController::_get_rate_out(float desired_rate, float scaler, bool
float kp_ff = MAX((gains.P - gains.I * gains.tau) * gains.tau - gains.D , 0) / eas2tas;
float k_ff = gains.FF / eas2tas;
const float last_pid_info_D = _pid_info.D;
// Calculate the demanded control surface deflection
// Note the scaler is applied again. We want a 1/speed scaler applied to the feed-forward
// path, but want a 1/speed^2 scaler applied to the rate error path.
@ -211,7 +289,7 @@ int32_t AP_PitchController::_get_rate_out(float desired_rate, float scaler, bool
// Calculate the slew rate amplitude produced by the unmodified D term
// calculate a low pass filtered slew rate
float Dterm_slew_rate = _slew_rate_filter.apply((fabsf(_pid_info.D - _last_pid_info_D)/ delta_time), delta_time);
float Dterm_slew_rate = _slew_rate_filter.apply((fabsf(_pid_info.D - last_pid_info_D)/ delta_time), delta_time);
// rectify and apply a decaying envelope filter
float alpha = 1.0f - constrain_float(delta_time/_slew_rate_tau, 0.0f , 1.0f);
@ -219,12 +297,10 @@ int32_t AP_PitchController::_get_rate_out(float desired_rate, float scaler, bool
_slew_rate_amplitude = fminf(_slew_rate_amplitude, 10.0f*_slew_rate_max);
// Calculate and apply the D gain adjustment
_pid_info.Dmod = _D_gain_modifier = _slew_rate_max / fmaxf(_slew_rate_amplitude, _slew_rate_max);
_pid_info.D *= _D_gain_modifier;
_pid_info.Dmod = _slew_rate_max / fmaxf(_slew_rate_amplitude, _slew_rate_max);
_pid_info.D *= _pid_info.Dmod;
}
_last_pid_info_D = _pid_info.D;
_last_out = _pid_info.D + _pid_info.FF + _pid_info.P;
if (autotune.running && aspeed > aparm.airspeed_min) {
@ -264,6 +340,109 @@ int32_t AP_PitchController::_get_rate_out(float desired_rate, float scaler, bool
return constrain_float(_last_out * 100, -4500, 4500);
}
/*
AC_PID based rate controller
*/
int32_t AP_PitchController::_get_rate_out_ac_pid(float desired_rate, float scaler, bool disable_integrator, float aspeed)
{
convert_pid();
const float dt = AP::scheduler().get_loop_period_s();
const float eas2tas = _ahrs.get_EAS2TAS();
bool limit_I = fabsf(last_ac_out) >= 45;
float rate_y = _ahrs.get_gyro().y;
float old_I = rate_pid.get_i();
rate_pid.set_dt(dt);
bool underspeed = aspeed <= 0.5*float(aparm.airspeed_min);
if (underspeed) {
limit_I = true;
}
// the P and I elements are scaled by sq(scaler). To use an
// unmodified AC_PID object we scale the inputs and calculate FF separately
//
// note that we run AC_PID in radians so that the normal scaling
// range for IMAX in AC_PID applies (usually an IMAX value less than 1.0)
rate_pid.update_all(radians(desired_rate) * scaler * scaler, rate_y * scaler * scaler, limit_I);
if (underspeed) {
// when underspeed we lock the integrator
rate_pid.set_integrator(old_I);
}
// FF should be scaled by scaler/eas2tas, but since we have scaled
// the AC_PID target above by scaler*scaler we need to instead
// divide by scaler*eas2tas to get the right scaling
const float ff = degrees(rate_pid.get_ff() / (scaler * eas2tas));
if (disable_integrator) {
rate_pid.reset_I();
}
// convert AC_PID info object to same scale as old controller
_pid_info_ac_pid = rate_pid.get_pid_info();
auto &pinfo = _pid_info_ac_pid;
const float deg_scale = degrees(1);
pinfo.FF = ff;
pinfo.P *= deg_scale;
pinfo.I *= deg_scale;
pinfo.D *= deg_scale;
// fix the logged target and actual values to not have the scalers applied
pinfo.target = desired_rate;
pinfo.actual = degrees(rate_y);
// sum components
float out = pinfo.FF + pinfo.P + pinfo.I + pinfo.D;
/*
when we are past the users defined roll limit for the
aircraft our priority should be to bring the aircraft back
within the roll limit. Using elevator for pitch control at
large roll angles is ineffective, and can be counter
productive as it induces earth-frame yaw which can reduce
the ability to roll. We linearly reduce elevator input when
beyond the configured roll limit, reducing to zero at 90
degrees
*/
float roll_wrapped = labs(_ahrs.roll_sensor);
if (roll_wrapped > 9000) {
roll_wrapped = 18000 - roll_wrapped;
}
if (roll_wrapped > aparm.roll_limit_cd + 500 && aparm.roll_limit_cd < 8500 &&
labs(_ahrs.pitch_sensor) < 7000) {
float roll_prop = (roll_wrapped - (aparm.roll_limit_cd+500)) / (float)(9000 - aparm.roll_limit_cd);
_last_out *= (1 - roll_prop);
}
// remember the last output to trigger the I limit
last_ac_out = out;
// output is scaled to notional centidegrees of deflection
return constrain_int32(out * 100, -4500, 4500);
}
/*
rate controller selector
*/
int32_t AP_PitchController::_get_rate_out(float desired_rate, float scaler, bool disable_integrator, float aspeed)
{
int32_t ret_ac_pid = _get_rate_out_ac_pid(desired_rate, scaler, disable_integrator, aspeed);
int32_t ret_old = _get_rate_out_old(desired_rate, scaler, disable_integrator, aspeed);
const auto &pinfo_ac = _pid_info_ac_pid;
const auto &pinfo_old = _pid_info;
AP::logger().Write("PIXP", "TimeUS,AC,Old,ACSum,OldSum", "Qiiff",
AP_HAL::micros64(),
ret_ac_pid,
ret_old,
pinfo_ac.FF + pinfo_ac.P + pinfo_ac.I + pinfo_ac.D,
pinfo_old.FF + pinfo_old.P + pinfo_old.I + pinfo_old.D);
return use_ac_pid ? ret_ac_pid : ret_old;
}
/*
Function returns an equivalent elevator deflection in centi-degrees in the range from -4500 to 4500
A positive demand is up
@ -374,5 +553,28 @@ int32_t AP_PitchController::get_servo_out(int32_t angle_err, float scaler, bool
void AP_PitchController::reset_I()
{
_pid_info.I = 0;
_pid_info.I = 0;
rate_pid.reset_I();
}
/*
convert from old to new PIDs
this is a temporary conversion function during development
*/
void AP_PitchController::convert_pid()
{
if (done_init && is_positive(rate_pid.ff())) {
return;
}
done_init = true;
AP_Float &ff = rate_pid.ff();
if (is_positive(ff) && ff.configured_in_storage()) {
return;
}
const float kp_ff = MAX((gains.P - gains.I * gains.tau) * gains.tau - gains.D, 0);
rate_pid.ff().set_and_save(gains.FF + kp_ff);
rate_pid.kI().set_and_save_ifchanged(gains.I * gains.tau);
rate_pid.kP().set_and_save_ifchanged(gains.D);
rate_pid.kD().set_and_save_ifchanged(0);
rate_pid.kIMAX().set_and_save_ifchanged(gains.imax/4500.0);
}

35
libraries/APM_Control/AP_PitchController.h
View File

@ -6,6 +6,8 @@
#include "AP_AutoTune.h"
#include <AP_Logger/AP_Logger.h>
#include <AP_Math/AP_Math.h>
#include <AC_PID/AC_PID.h>
#include <Filter/SlewLimiter.h>
class AP_PitchController {
public:
@ -34,13 +36,16 @@ public:
void decay_I() {
// this reduces integrator by 95% over 2s
_pid_info.I *= 0.995f;
rate_pid.set_integrator(rate_pid.get_i() * 0.995);
}
void autotune_start(void) { autotune.start(); }
void autotune_restore(void) { autotune.stop(); }
const AP_Logger::PID_Info& get_pid_info(void) const { return _pid_info; }
const AP_Logger::PID_Info& get_pid_info(void) const { return use_ac_pid?_pid_info_ac_pid:_pid_info; }
const AP_Logger::PID_Info& get_old_pid_info(void) const { return _pid_info; }
const AP_Logger::PID_Info& get_ac_pid_info(void) const { return _pid_info_ac_pid; }
static const struct AP_Param::GroupInfo var_info[];
AP_Float &kP(void) { return gains.P; }
@ -48,6 +53,10 @@ public:
AP_Float &kD(void) { return gains.D; }
AP_Float &kFF(void) { return gains.FF; }
void set_ac_pid(bool set) {
use_ac_pid = set;
}
private:
const AP_Vehicle::FixedWing &aparm;
AP_AutoTune::ATGains gains;
@ -55,21 +64,29 @@ private:
AP_Int16 _max_rate_neg;
AP_Float _roll_ff;
uint32_t _last_t;
float _last_out;
AP_Logger::PID_Info _pid_info;
float _last_out;
AC_PID rate_pid{0.04, 0.15, 0, 0.345, 0.666, 10, 10, 10, 0.02, 150, 1};
int32_t _get_rate_out(float desired_rate, float scaler, bool disable_integrator, float aspeed);
AP_Logger::PID_Info _pid_info;
AP_Logger::PID_Info _pid_info_ac_pid;
int32_t _get_rate_out(float desired_rate, float scaler, bool disable_integrator, float aspeed);
int32_t _get_rate_out_old(float desired_rate, float scaler, bool disable_integrator, float aspeed);
int32_t _get_rate_out_ac_pid(float desired_rate, float scaler, bool disable_integrator, float aspeed);
float _get_coordination_rate_offset(float &aspeed, bool &inverted) const;
void convert_pid();
bool done_init;
AP_AHRS &_ahrs;
// D gain limit cycle control
float _last_pid_info_D; // value of the D term (angular rate control feedback) from the previous time step (deg)
LowPassFilterFloat _slew_rate_filter; // LPF applied to the derivative of the control action generated by the angular rate feedback
float _slew_rate_amplitude; // Amplitude of the servo slew rate produced by the angular rate feedback (deg/sec)
float _D_gain_modifier = 1.0f; // Gain modifier applied to the angular rate feedback to prevent excessive slew rate
AP_Float _slew_rate_max; // Maximum permitted angular rate control feedback servo slew rate (deg/sec)
AP_Float _slew_rate_tau; // Time constant used to recover gain after a slew rate exceedance (sec)
bool use_ac_pid;
float last_ac_out;
};

244
libraries/APM_Control/AP_RollController.cpp
View File

@ -23,82 +23,158 @@
extern const AP_HAL::HAL& hal;
const AP_Param::GroupInfo AP_RollController::var_info[] = {
// @Param: TCONST
// @Param: 2SRV_TCONST
// @DisplayName: Roll Time Constant
// @Description: Time constant in seconds from demanded to achieved roll angle. Most models respond well to 0.5. May be reduced for faster responses, but setting lower than a model can achieve will not help.
// @Range: 0.4 1.0
// @Units: s
// @Increment: 0.1
// @User: Advanced
AP_GROUPINFO("TCONST", 0, AP_RollController, gains.tau, 0.5f),
AP_GROUPINFO("2SRV_TCONST", 0, AP_RollController, gains.tau, 0.5f),
// @Param: P
// @Param: 2SRV_P
// @DisplayName: Proportional Gain
// @Description: Proportional gain from roll angle demands to ailerons. Higher values allow more servo response but can cause oscillations. Automatically set and adjusted by AUTOTUNE mode.
// @Range: 0.1 4.0
// @Increment: 0.1
// @User: Standard
AP_GROUPINFO("P", 1, AP_RollController, gains.P, 1.0f),
// @User: Standard
AP_GROUPINFO("2SRV_P", 1, AP_RollController, gains.P, 1.0f),
// @Param: D
// @Param: 2SRV_D
// @DisplayName: Damping Gain
// @Description: Damping gain from roll acceleration to ailerons. Higher values reduce rolling in turbulence, but can cause oscillations. Automatically set and adjusted by AUTOTUNE mode.
// @Range: 0 0.2
// @Increment: 0.01
// @User: Standard
AP_GROUPINFO("D", 2, AP_RollController, gains.D, 0.08f),
// @User: Standard
AP_GROUPINFO("2SRV_D", 2, AP_RollController, gains.D, 0.08f),
// @Param: I
// @Param: 2SRV_I
// @DisplayName: Integrator Gain
// @Description: Integrator gain from long-term roll angle offsets to ailerons. Higher values "trim" out offsets faster but can cause oscillations. Automatically set and adjusted by AUTOTUNE mode.
// @Range: 0 1.0
// @Increment: 0.05
// @User: Standard
AP_GROUPINFO("I", 3, AP_RollController, gains.I, 0.3f),
// @User: Standard
AP_GROUPINFO("2SRV_I", 3, AP_RollController, gains.I, 0.3f),
// @Param: RMAX
// @Param: 2SRV_RMAX
// @DisplayName: Maximum Roll Rate
// @Description: Maximum roll rate that the roll controller demands (degrees/sec) in ACRO mode.
// @Range: 0 180
// @Units: deg/s
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("RMAX", 4, AP_RollController, gains.rmax, 0),
AP_GROUPINFO("2SRV_RMAX", 4, AP_RollController, gains.rmax, 0),
// @Param: IMAX
// @Param: 2SRV_IMAX
// @DisplayName: Integrator limit
// @Description: Limit of roll integrator gain in centi-degrees of servo travel. Servos are assumed to have +/- 4500 centi-degrees of travel, so a value of 3000 allows trim of up to 2/3 of servo travel range.
// @Range: 0 4500
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("IMAX", 5, AP_RollController, gains.imax, 3000),
AP_GROUPINFO("2SRV_IMAX", 5, AP_RollController, gains.imax, 3000),
// @Param: FF
// @Param: 2SRV_FF
// @DisplayName: Feed forward Gain
// @Description: Gain from demanded rate to aileron output.
// @Range: 0.1 4.0
// @Increment: 0.1
// @User: Standard
AP_GROUPINFO("FF", 6, AP_RollController, gains.FF, 0.0f),
// @User: Standard
AP_GROUPINFO("2SRV_FF", 6, AP_RollController, gains.FF, 0.0f),
// @Param: SRMAX
// @Param: 2SRV_SRMAX
// @DisplayName: Servo slew rate limit
// @Description: Sets an upper limit on the servo slew rate produced by the D-gain (roll rate feedback). If the amplitude of the control action produced by the roll rate feedback exceeds this value, then the D-gain is reduced to respect the limit. This limits the amplitude of high frequency oscillations caused by an excessive D-gain. The parameter should be set to no more than 25% of the servo's specified slew rate to allow for inertia and aerodynamic load effects. Note: The D-gain will not be reduced to less than 10% of the nominal value. A valule of zero will disable this feature.
// @Units: deg/s
// @Range: 0 500
// @Increment: 10.0
// @User: Advanced
AP_GROUPINFO("SRMAX", 7, AP_RollController, _slew_rate_max, 150.0f),
AP_GROUPINFO("2SRV_SRMAX", 7, AP_RollController, _slew_rate_max, 150.0f),
// @Param: SRTAU
// @Param: 2SRV_SRTAU
// @DisplayName: Servo slew rate decay time constant
// @Description: This sets the time constant used to recover the D-gain after it has been reduced due to excessive servo slew rate.
// @Units: s
// @Range: 0.5 5.0
// @Increment: 0.1
// @User: Advanced
AP_GROUPINFO("SRTAU", 8, AP_RollController, _slew_rate_tau, 1.0f),
AP_GROUPINFO("2SRV_SRTAU", 8, AP_RollController, _slew_rate_tau, 1.0f),
// @Param: _RATE_P
// @DisplayName: Roll axis rate controller P gain
// @Description: Roll axis rate controller P gain. Converts the difference between desired roll rate and actual roll rate into a motor speed output
// @Range: 0.08 0.35
// @Increment: 0.005
// @User: Standard
// @Param: _RATE_I
// @DisplayName: Roll axis rate controller I gain
// @Description: Roll axis rate controller I gain. Corrects long-term difference in desired roll rate vs actual roll rate
// @Range: 0.01 0.6
// @Increment: 0.01
// @User: Standard
// @Param: _RATE_IMAX
// @DisplayName: Roll axis rate controller I gain maximum
// @Description: Roll axis rate controller I gain maximum. Constrains the maximum motor output that the I gain will output
// @Range: 0 1
// @Increment: 0.01
// @User: Standard
// @Param: _RATE_D
// @DisplayName: Roll axis rate controller D gain
// @Description: Roll axis rate controller D gain. Compensates for short-term change in desired roll rate vs actual roll rate
// @Range: 0.001 0.03
// @Increment: 0.001
// @User: Standard
// @Param: _RATE_FF
// @DisplayName: Roll axis rate controller feed forward
// @Description: Roll axis rate controller feed forward
// @Range: 0 3.0
// @Increment: 0.001
// @User: Standard
// @Param: _RATE_FLTT
// @DisplayName: Roll axis rate controller target frequency in Hz
// @Description: Roll axis rate controller target frequency in Hz
// @Range: 2 50
// @Increment: 1
// @Units: Hz
// @User: Standard
// @Param: _RATE_FLTE
// @DisplayName: Roll axis rate controller error frequency in Hz
// @Description: Roll axis rate controller error frequency in Hz
// @Range: 2 50
// @Increment: 1
// @Units: Hz
// @User: Standard
// @Param: _RATE_FLTD
// @DisplayName: Roll axis rate controller derivative frequency in Hz
// @Description: Roll axis rate controller derivative frequency in Hz
// @Range: 0 50
// @Increment: 1
// @Units: Hz
// @User: Standard
// @Param: _RATE_SMAX
// @DisplayName: Roll slew rate limit
// @Description: Sets an upper limit on the slew rate produced by the combined P and D gains. If the amplitude of the control action produced by the rate feedback exceeds this value, then the D+P gain is reduced to respect the limit. This limits the amplitude of high frequency oscillations caused by an excessive gain. The limit should be set to no more than 25% of the actuators maximum slew rate to allow for load effects. Note: The gain will not be reduced to less than 10% of the nominal value. A value of zero will disable this feature.
// @Range: 0 200
// @Increment: 0.5
// @User: Advanced
// @Param: _RATE_STAU
// @DisplayName: Roll slew rate decay time constant
// @Description: This sets the time constant used to recover the P+D gain after it has been reduced due to excessive slew rate.
// @Units: s
// @Range: 0.5 5.0
// @Increment: 0.1
// @User: Advanced
AP_SUBGROUPINFO(rate_pid, "_RATE_", 9, AP_RollController, AC_PID),
AP_GROUPEND
};
@ -107,7 +183,7 @@ const AP_Param::GroupInfo AP_RollController::var_info[] = {
internal rate controller, called by attitude and rate controller
public functions
*/
int32_t AP_RollController::_get_rate_out(float desired_rate, float scaler, bool disable_integrator)
int32_t AP_RollController::_get_rate_out_old(float desired_rate, float scaler, bool disable_integrator)
{
uint32_t tnow = AP_HAL::millis();
uint32_t dt = tnow - _last_t;
@ -169,7 +245,9 @@ int32_t AP_RollController::_get_rate_out(float desired_rate, float scaler, bool
// Calculate the demanded control surface deflection
// Note the scaler is applied again. We want a 1/speed scaler applied to the feed-forward
// path, but want a 1/speed^2 scaler applied to the rate error path.
// This is because acceleration scales with speed^2, but rate scales with speed.
// This is because acceleration scales with speed^2, but rate scales with speed.
const float last_pid_info_D = _pid_info.D;
_pid_info.D = rate_error * gains.D * scaler;
_pid_info.P = desired_rate * kp_ff * scaler;
_pid_info.FF = desired_rate * k_ff * scaler;
@ -178,7 +256,7 @@ int32_t AP_RollController::_get_rate_out(float desired_rate, float scaler, bool
// Calculate the slew rate amplitude produced by the unmodified D term
// calculate a low pass filtered slew rate
float Dterm_slew_rate = _slew_rate_filter.apply((fabsf(_pid_info.D - _last_pid_info_D)/ delta_time), delta_time);
float Dterm_slew_rate = _slew_rate_filter.apply((fabsf(_pid_info.D - last_pid_info_D)/ delta_time), delta_time);
// rectify and apply a decaying envelope filter
float alpha = 1.0f - constrain_float(delta_time/_slew_rate_tau, 0.0f , 1.0f);
@ -186,12 +264,10 @@ int32_t AP_RollController::_get_rate_out(float desired_rate, float scaler, bool
_slew_rate_amplitude = fminf(_slew_rate_amplitude, 10.0f*_slew_rate_max);
// Calculate and apply the D gain adjustment
_pid_info.Dmod = _D_gain_modifier = _slew_rate_max / fmaxf(_slew_rate_amplitude, _slew_rate_max);
_pid_info.D *= _D_gain_modifier;
_pid_info.Dmod = _slew_rate_max / fmaxf(_slew_rate_amplitude, _slew_rate_max);
_pid_info.D *= _pid_info.Dmod;
}
_last_pid_info_D = _pid_info.D;
_last_out = _pid_info.D + _pid_info.FF + _pid_info.P;
if (autotune.running && aspeed > aparm.airspeed_min) {
@ -208,6 +284,93 @@ int32_t AP_RollController::_get_rate_out(float desired_rate, float scaler, bool
return constrain_float(_last_out * 100, -4500, 4500);
}
/*
AC_PID based rate controller
*/
int32_t AP_RollController::_get_rate_out_ac_pid(float desired_rate, float scaler, bool disable_integrator)
{
convert_pid();
const float dt = AP::scheduler().get_loop_period_s();
const float eas2tas = _ahrs.get_EAS2TAS();
bool limit_I = fabsf(last_ac_out) >= 45;
float rate_x = _ahrs.get_gyro().x;
float aspeed;
float old_I = rate_pid.get_i();
rate_pid.set_dt(dt);
if (!_ahrs.airspeed_estimate(aspeed)) {
aspeed = 0;
}
bool underspeed = aspeed <= float(aparm.airspeed_min);
if (underspeed) {
limit_I = true;
}
// the P and I elements are scaled by sq(scaler). To use an
// unmodified AC_PID object we scale the inputs and calculate FF separately
//
// note that we run AC_PID in radians so that the normal scaling
// range for IMAX in AC_PID applies (usually an IMAX value less than 1.0)
rate_pid.update_all(radians(desired_rate) * scaler * scaler, rate_x * scaler * scaler, limit_I);
if (underspeed) {
// when underspeed we lock the integrator
rate_pid.set_integrator(old_I);
}
// FF should be scaled by scaler/eas2tas, but since we have scaled
// the AC_PID target above by scaler*scaler we need to instead
// divide by scaler*eas2tas to get the right scaling
const float ff = degrees(rate_pid.get_ff() / (scaler * eas2tas));
if (disable_integrator) {
rate_pid.reset_I();
}
// convert AC_PID info object to same scale as old controller
_pid_info_ac_pid = rate_pid.get_pid_info();
auto &pinfo = _pid_info_ac_pid;
const float deg_scale = degrees(1);
pinfo.FF = ff;
pinfo.P *= deg_scale;
pinfo.I *= deg_scale;
pinfo.D *= deg_scale;
// fix the logged target and actual values to not have the scalers applied
pinfo.target = desired_rate;
pinfo.actual = degrees(rate_x);
// sum components
float out = pinfo.FF + pinfo.P + pinfo.I + pinfo.D;
// remember the last output to trigger the I limit
last_ac_out = out;
// output is scaled to notional centidegrees of deflection
return constrain_int32(out * 100, -4500, 4500);
}
/*
rate controller selector
*/
int32_t AP_RollController::_get_rate_out(float desired_rate, float scaler, bool disable_integrator)
{
int32_t ret_ac_pid = _get_rate_out_ac_pid(desired_rate, scaler, disable_integrator);
int32_t ret_old = _get_rate_out_old(desired_rate, scaler, disable_integrator);
const auto &pinfo_ac = _pid_info_ac_pid;
const auto &pinfo_old = _pid_info;
AP::logger().Write("PIXR", "TimeUS,AC,Old,ACSum,OldSum", "Qiiff",
AP_HAL::micros64(),
ret_ac_pid,
ret_old,
pinfo_ac.FF + pinfo_ac.P + pinfo_ac.I + pinfo_ac.D,
pinfo_old.FF + pinfo_old.P + pinfo_old.I + pinfo_old.D);
return use_ac_pid ? ret_ac_pid : ret_old;
}
/*
Function returns an equivalent elevator deflection in centi-degrees in the range from -4500 to 4500
@ -252,4 +415,27 @@ int32_t AP_RollController::get_servo_out(int32_t angle_err, float scaler, bool d
void AP_RollController::reset_I()
{
_pid_info.I = 0;
rate_pid.reset_I();
}
/*
convert from old to new PIDs
this is a temporary conversion function during development
*/
void AP_RollController::convert_pid()
{
if (done_init && is_positive(rate_pid.ff())) {
return;
}
done_init = true;
AP_Float &ff = rate_pid.ff();
if (is_positive(ff) && ff.configured_in_storage()) {
return;
}
const float kp_ff = MAX((gains.P - gains.I * gains.tau) * gains.tau - gains.D, 0);
rate_pid.ff().set_and_save(gains.FF + kp_ff);
rate_pid.kI().set_and_save_ifchanged(gains.I * gains.tau);
rate_pid.kP().set_and_save_ifchanged(gains.D);
rate_pid.kD().set_and_save_ifchanged(0);
rate_pid.kIMAX().set_and_save_ifchanged(gains.imax/4500.0);
}

26
libraries/APM_Control/AP_RollController.h
View File

@ -6,6 +6,7 @@
#include "AP_AutoTune.h"
#include <AP_Logger/AP_Logger.h>
#include <AP_Math/AP_Math.h>
#include <AC_PID/AC_PID.h>
class AP_RollController {
public:
@ -34,12 +35,16 @@ public:
void decay_I() {
// this reduces integrator by 95% over 2s
_pid_info.I *= 0.995f;
rate_pid.set_integrator(rate_pid.get_i() * 0.995);
}
void autotune_start(void) { autotune.start(); }
void autotune_restore(void) { autotune.stop(); }
const AP_Logger::PID_Info& get_pid_info(void) const { return _pid_info; }
const AP_Logger::PID_Info& get_pid_info(void) const { return use_ac_pid?_pid_info_ac_pid:_pid_info; }
const AP_Logger::PID_Info& get_old_pid_info(void) const { return _pid_info; }
const AP_Logger::PID_Info& get_ac_pid_info(void) const { return _pid_info_ac_pid; }
static const struct AP_Param::GroupInfo var_info[];
@ -55,25 +60,36 @@ public:
AP_Float &kD(void) { return gains.D; }
AP_Float &kFF(void) { return gains.FF; }
void set_ac_pid(bool set) {
use_ac_pid = set;
}
private:
const AP_Vehicle::FixedWing &aparm;
AP_AutoTune::ATGains gains;
AP_AutoTune autotune;
uint32_t _last_t;
float _last_out;
AC_PID rate_pid{0.08, 0.15, 0, 0.345, 0.666, 10, 10, 10, 0.02, 150, 1};
bool use_ac_pid;
float last_ac_out;
AP_Logger::PID_Info _pid_info;
AP_Logger::PID_Info _pid_info_ac_pid;
int32_t _get_rate_out(float desired_rate, float scaler, bool disable_integrator);
int32_t _get_rate_out_old(float desired_rate, float scaler, bool disable_integrator);
int32_t _get_rate_out_ac_pid(float desired_rate, float scaler, bool disable_integrator);
int32_t _get_rate_out(float desired_rate, float scaler, bool disable_integrator);
// convert from old to new PID values
void convert_pid();
bool done_init;
AP_AHRS &_ahrs;
// D gain limit cycle control
float _last_pid_info_D; // value of the D term (angular rate control feedback) from the previous time step (deg)
LowPassFilterFloat _slew_rate_filter; // LPF applied to the derivative of the control action generated by the angular rate feedback
float _slew_rate_amplitude; // Amplitude of the servo slew rate produced by the angular rate feedback (deg/sec)
float _D_gain_modifier = 1.0f; // Gain modifier applied to the angular rate feedback to prevent excessive slew rate
AP_Float _slew_rate_max; // Maximum permitted angular rate control feedback servo slew rate (deg/sec)
AP_Float _slew_rate_tau; // Time constant used to recover gain after a slew rate exceedance (sec)
};