aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

APM_Control: move to only ACPID

Browse Source
This commit is contained in:
Andrew Tridgell 2021-02-27 20:56:41 +11:00
parent 6ca9033dde
commit ead011c7c2
6 changed files with 88 additions and 442 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
libraries/APM_Control/AP_AutoTune.cpp
View File

@ -55,9 +55,9 @@ extern const AP_HAL::HAL& hal;
// step size for decreasing gains, percentage
#define AUTOTUNE_DECREASE_STEP 8
// min/max P gains
#define AUTOTUNE_MAX_P 5.0f
#define AUTOTUNE_MIN_P 0.3f
// min/max FF gains
#define AUTOTUNE_MAX_FF 2.0f
#define AUTOTUNE_MIN_FF 0.05f
// tau ranges
#define AUTOTUNE_MAX_TAU 0.7f
@ -68,9 +68,11 @@ extern const AP_HAL::HAL& hal;
// constructor
AP_AutoTune::AP_AutoTune(ATGains &_gains, ATType _type,
const AP_Vehicle::FixedWing &parms) :
running(false),
const AP_Vehicle::FixedWing &parms,
AC_PID &_rpid) :
current(_gains),
rpid(_rpid),
running(false),
type(_type),
aparm(parms),
saturated_surfaces(false)
@ -91,7 +93,7 @@ AP_AutoTune::AP_AutoTune(ATGains &_gains, ATType _type,
*/
static const struct {
float tau;
float Dratio;
float Pratio;
float rmax;
} tuning_table[] = {
{ 0.70f, 0.050f, 20 }, // level 1
@ -131,18 +133,13 @@ void AP_AutoTune::start(void)
}
current.rmax.set(tuning_table[level-1].rmax);
// D gain is scaled to a fixed ratio of P gain
current.D.set(tuning_table[level-1].Dratio * current.P);
current.tau.set(tuning_table[level-1].tau);
current.imax = constrain_float(current.imax, AUTOTUNE_MIN_IMAX, AUTOTUNE_MAX_IMAX);
// force a fixed ratio of I to D gain on the rate feedback path
current.I = 0.5f * current.D / current.tau;
next_save = current;
Debug("START P -> %.3f\n", current.P.get());
Debug("START FF -> %.3f\n", rpid.ff().get());
}
/*
@ -211,24 +208,22 @@ void AP_AutoTune::check_state_exit(uint32_t state_time_ms)
// we increase P if we have not saturated the surfaces during
// this state, and we have
if (state_time_ms >= AUTOTUNE_UNDERSHOOT_TIME && !saturated_surfaces) {
current.P.set(current.P * (100+AUTOTUNE_INCREASE_STEP) * 0.01f);
if (current.P > AUTOTUNE_MAX_P) {
current.P = AUTOTUNE_MAX_P;
rpid.ff().set(rpid.ff() * (100+AUTOTUNE_INCREASE_STEP) * 0.01f);
if (rpid.ff() > AUTOTUNE_MAX_FF) {
rpid.ff().set(AUTOTUNE_MAX_FF);
}
Debug("UNDER P -> %.3f\n", current.P.get());
Debug("UNDER FF -> %.3f\n", rpid.ff().get());
}
current.D.set(tuning_table[aparm.autotune_level-1].Dratio * current.P);
break;
case DEMAND_OVER_POS:
case DEMAND_OVER_NEG:
if (state_time_ms >= AUTOTUNE_OVERSHOOT_TIME) {
current.P.set(current.P * (100-AUTOTUNE_DECREASE_STEP) * 0.01f);
if (current.P < AUTOTUNE_MIN_P) {
current.P = AUTOTUNE_MIN_P;
rpid.ff().set(rpid.ff() * (100-AUTOTUNE_DECREASE_STEP) * 0.01f);
if (rpid.ff() < AUTOTUNE_MIN_FF) {
rpid.ff().set(AUTOTUNE_MIN_FF);
}
Debug("OVER P -> %.3f\n", current.P.get());
Debug("OVER FF -> %.3f\n", rpid.ff().get());
}
current.D.set(tuning_table[aparm.autotune_level-1].Dratio * current.P);
break;
}
}
@ -249,7 +244,7 @@ void AP_AutoTune::check_save(void)
ATGains tmp = current;
save_gains(next_save);
Debug("SAVE P -> %.3f\n", current.P.get());
Debug("SAVE FF -> %.3f\n", rpid.ff().get());
// restore our current gains
current = tmp;
@ -318,11 +313,14 @@ void AP_AutoTune::save_gains(const ATGains &v)
{
current = last_save;
save_float_if_changed(current.tau, v.tau, "TCONST");
save_float_if_changed(current.P, v.P, "P");
save_float_if_changed(current.I, v.I, "I");
save_float_if_changed(current.D, v.D, "D");
#if 0
save_float_if_changed(rpid.kP(), v.P, "P");
save_float_if_changed(rpid.kI(), v.I, "I");
save_float_if_changed(rpid.kD(), v.D, "D");
save_float_if_changed(rpid.ff(), v.FF, "FF");
save_int16_if_changed(current.rmax, v.rmax, "RMAX");
save_int16_if_changed(current.imax, v.imax, "IMAX");
#endif
last_save = current;
}
@ -341,7 +339,7 @@ void AP_AutoTune::write_log(float servo, float demanded, float achieved)
servo : (int16_t)(servo*100),
demanded : demanded,
achieved : achieved,
P : current.P.get()
P : rpid.kP().get()
};
logger->WriteBlock(&pkt, sizeof(pkt));
}

8
libraries/APM_Control/AP_AutoTune.h
View File

@ -4,15 +4,12 @@
#include <AP_Logger/LogStructure.h>
#include <AP_Param/AP_Param.h>
#include <AP_Vehicle/AP_Vehicle.h>
#include <AC_PID/AC_PID.h>
class AP_AutoTune {
public:
struct ATGains {
AP_Float tau;
AP_Float P;
AP_Float I;
AP_Float D;
AP_Float FF;
AP_Int16 rmax;
AP_Int16 imax;
};
@ -35,7 +32,7 @@ public:
// constructor
AP_AutoTune(ATGains &_gains, ATType type, const AP_Vehicle::FixedWing &parms);
AP_AutoTune(ATGains &_gains, ATType type, const AP_Vehicle::FixedWing &parms, AC_PID &rpid);
// called when autotune mode is entered
void start(void);
@ -54,6 +51,7 @@ public:
private:
// the current gains
ATGains &current;
AC_PID &rpid;
// what type of autotune is this
ATType type;

224
libraries/APM_Control/AP_PitchController.cpp
View File

@ -32,29 +32,7 @@ const AP_Param::GroupInfo AP_PitchController::var_info[] = {
// @User: Advanced
AP_GROUPINFO("2SRV_TCONST", 0, AP_PitchController, gains.tau, 0.5f),
// @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("2SRV_P", 1, AP_PitchController, gains.P, 1.0f),
// @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("2SRV_D", 2, AP_PitchController, gains.D, 0.04f),
// @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("2SRV_I", 3, AP_PitchController, gains.I, 0.3f),
// index 1 to 3 reserved for old PID values
// @Param: 2SRV_RMAX_UP
// @DisplayName: Pitch up max rate
@ -90,13 +68,7 @@ const AP_Param::GroupInfo AP_PitchController::var_info[] = {
// @User: Advanced
AP_GROUPINFO("2SRV_IMAX", 7, AP_PitchController, gains.imax, 3000),
// @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("2SRV_FF", 8, AP_PitchController, gains.FF, 0.0f),
// index 8 reserved for old FF
// @Param: 2SRV_SRMAX
// @DisplayName: Servo slew rate limit
@ -195,158 +167,11 @@ const AP_Param::GroupInfo AP_PitchController::var_info[] = {
AP_GROUPEND
};
/*
Function returns an equivalent elevator deflection in centi-degrees in the range from -4500 to 4500
A positive demand is up
Inputs are:
1) demanded pitch rate in degrees/second
2) control gain scaler = scaling_speed / aspeed
3) boolean which is true when stabilise mode is active
4) minimum FBW airspeed (metres/sec)
5) maximum FBW airspeed (metres/sec)
*/
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;
if (_last_t == 0 || dt > 1000) {
dt = 0;
}
_last_t = tnow;
float delta_time = (float)dt * 0.001f;
// Get body rate vector (radians/sec)
float omega_y = _ahrs.get_gyro().y;
// Calculate the pitch rate error (deg/sec) and scale
float achieved_rate = ToDeg(omega_y);
_pid_info.error = desired_rate - achieved_rate;
float rate_error = _pid_info.error * scaler;
_pid_info.target = desired_rate;
_pid_info.actual = achieved_rate;
// Multiply pitch rate error by _ki_rate and integrate
// Scaler is applied before integrator so that integrator state relates directly to elevator deflection
// This means elevator trim offset doesn't change as the value of scaler changes with airspeed
// Don't integrate if in stabilise mode as the integrator will wind up against the pilots inputs
if (!disable_integrator && gains.I > 0) {
float k_I = gains.I;
if (is_zero(gains.FF)) {
/*
if the user hasn't set a direct FF then assume they are
not doing sophisticated tuning. Set a minimum I value of
0.15 to ensure that the time constant for trimming in
pitch is not too long. We have had a lot of user issues
with very small I value leading to very slow pitch
trimming, which causes a lot of problems for TECS. A
value of 0.15 is still quite small, but a lot better
than what many users are running.
*/
k_I = MAX(k_I, 0.15f);
}
float ki_rate = k_I * gains.tau;
//only integrate if gain and time step are positive and airspeed above min value.
if (dt > 0 && aspeed > 0.5f*float(aparm.airspeed_min)) {
float integrator_delta = rate_error * ki_rate * delta_time * scaler;
if (_last_out < -45) {
// prevent the integrator from increasing if surface defln demand is above the upper limit
integrator_delta = MAX(integrator_delta , 0);
} else if (_last_out > 45) {
// prevent the integrator from decreasing if surface defln demand is below the lower limit
integrator_delta = MIN(integrator_delta , 0);
}
_pid_info.I += integrator_delta;
}
} else {
_pid_info.I = 0;
}
// Scale the integration limit
float intLimScaled = gains.imax * 0.01f;
// Constrain the integrator state
_pid_info.I = constrain_float(_pid_info.I, -intLimScaled, intLimScaled);
// Calculate equivalent gains so that values for K_P and K_I can be taken across from the old PID law
// No conversion is required for K_D
float eas2tas = _ahrs.get_EAS2TAS();
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.
// This is because acceleration scales with speed^2, but rate scales with speed.
_pid_info.P = desired_rate * kp_ff * scaler;
_pid_info.FF = desired_rate * k_ff * scaler;
_pid_info.D = rate_error * gains.D * scaler;
if (dt > 0 && _slew_rate_max > 0) {
// 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);
// rectify and apply a decaying envelope filter
float alpha = 1.0f - constrain_float(delta_time/_slew_rate_tau, 0.0f , 1.0f);
_slew_rate_amplitude = fmaxf(fabsf(Dterm_slew_rate), alpha * _slew_rate_amplitude);
_slew_rate_amplitude = fminf(_slew_rate_amplitude, 10.0f*_slew_rate_max);
// Calculate and apply the D gain adjustment
_pid_info.Dmod = _slew_rate_max / fmaxf(_slew_rate_amplitude, _slew_rate_max);
_pid_info.D *= _pid_info.Dmod;
}
_last_out = _pid_info.D + _pid_info.FF + _pid_info.P;
if (autotune.running && aspeed > aparm.airspeed_min) {
// let autotune have a go at the values
// Note that we don't pass the integrator component so we get
// a better idea of how much the base PD controller
// contributed
autotune.update(desired_rate, achieved_rate, _last_out);
// set down rate to rate up when auto-tuning
_max_rate_neg.set_and_save_ifchanged(gains.rmax);
}
_last_out += _pid_info.I;
/*
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);
}
// Convert to centi-degrees and constrain
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)
int32_t AP_PitchController::_get_rate_out(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;
@ -425,24 +250,6 @@ int32_t AP_PitchController::_get_rate_out_ac_pid(float desired_rate, float scale
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
@ -563,18 +370,25 @@ void AP_PitchController::reset_I()
*/
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()) {
if (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);
float old_ff=0, old_p=1.0, old_i=0.3, old_d=0.08;
bool have_old = AP_Param::get_param_by_index(this, 1, AP_PARAM_FLOAT, &old_p);
have_old |= AP_Param::get_param_by_index(this, 3, AP_PARAM_FLOAT, &old_i);
have_old |= AP_Param::get_param_by_index(this, 2, AP_PARAM_FLOAT, &old_d);
have_old |= AP_Param::get_param_by_index(this, 8, AP_PARAM_FLOAT, &old_ff);
if (!have_old) {
// none of the old gains were set
return;
}
const float kp_ff = MAX((old_p - old_i * gains.tau) * gains.tau - old_d, 0);
rate_pid.ff().set_and_save(old_ff + kp_ff);
rate_pid.kI().set_and_save_ifchanged(old_i * gains.tau);
rate_pid.kP().set_and_save_ifchanged(old_d);
rate_pid.kD().set_and_save_ifchanged(0);
rate_pid.kIMAX().set_and_save_ifchanged(gains.imax/4500.0);
}

19
libraries/APM_Control/AP_PitchController.h
View File

@ -13,7 +13,7 @@ class AP_PitchController {
public:
AP_PitchController(AP_AHRS &ahrs, const AP_Vehicle::FixedWing &parms)
: aparm(parms)
, autotune(gains, AP_AutoTune::AUTOTUNE_PITCH, parms)
, autotune(gains, AP_AutoTune::AUTOTUNE_PITCH, parms, rate_pid)
, _ahrs(ahrs)
{
AP_Param::setup_object_defaults(this, var_info);
@ -48,15 +48,17 @@ public:
static const struct AP_Param::GroupInfo var_info[];
AP_Float &kP(void) { return gains.P; }
AP_Float &kI(void) { return gains.I; }
AP_Float &kD(void) { return gains.D; }
AP_Float &kFF(void) { return gains.FF; }
AP_Float &kP(void) { return rate_pid.kP(); }
AP_Float &kI(void) { return rate_pid.kI(); }
AP_Float &kD(void) { return rate_pid.kD(); }
AP_Float &kFF(void) { return rate_pid.ff(); }
void set_ac_pid(bool set) {
use_ac_pid = set;
}
void convert_pid();
private:
const AP_Vehicle::FixedWing &aparm;
AP_AutoTune::ATGains gains;
@ -71,14 +73,9 @@ private:
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;
AP_AHRS &_ahrs;
// D gain limit cycle control
LowPassFilterFloat _slew_rate_filter; // LPF applied to the derivative of the control action generated by the angular rate feedback

188
libraries/APM_Control/AP_RollController.cpp
View File

@ -32,29 +32,7 @@ const AP_Param::GroupInfo AP_RollController::var_info[] = {
// @User: Advanced
AP_GROUPINFO("2SRV_TCONST", 0, AP_RollController, gains.tau, 0.5f),
// @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("2SRV_P", 1, AP_RollController, gains.P, 1.0f),
// @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("2SRV_D", 2, AP_RollController, gains.D, 0.08f),
// @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("2SRV_I", 3, AP_RollController, gains.I, 0.3f),
// index 1 to 3 reserved for old PID values
// @Param: 2SRV_RMAX
// @DisplayName: Maximum Roll Rate
@ -73,13 +51,7 @@ const AP_Param::GroupInfo AP_RollController::var_info[] = {
// @User: Advanced
AP_GROUPINFO("2SRV_IMAX", 5, AP_RollController, gains.imax, 3000),
// @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("2SRV_FF", 6, AP_RollController, gains.FF, 0.0f),
// index 6 reserved for old FF
// @Param: 2SRV_SRMAX
// @DisplayName: Servo slew rate limit
@ -179,118 +151,11 @@ 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_old(float desired_rate, float scaler, bool disable_integrator)
{
uint32_t tnow = AP_HAL::millis();
uint32_t dt = tnow - _last_t;
if (_last_t == 0 || dt > 1000) {
dt = 0;
}
_last_t = tnow;
// Calculate equivalent gains so that values for K_P and K_I can be taken across from the old PID law
// No conversion is required for K_D
float ki_rate = gains.I * gains.tau;
float eas2tas = _ahrs.get_EAS2TAS();
float kp_ff = MAX((gains.P - gains.I * gains.tau) * gains.tau - gains.D , 0) / eas2tas;
float k_ff = gains.FF / eas2tas;
float delta_time = (float)dt * 0.001f;
// Get body rate vector (radians/sec)
float omega_x = _ahrs.get_gyro().x;
// Calculate the roll rate error (deg/sec) and apply gain scaler
float achieved_rate = ToDeg(omega_x);
_pid_info.error = desired_rate - achieved_rate;
float rate_error = _pid_info.error * scaler;
_pid_info.target = desired_rate;
_pid_info.actual = achieved_rate;
// Get an airspeed estimate - default to zero if none available
float aspeed;
if (!_ahrs.airspeed_estimate(aspeed)) {
aspeed = 0.0f;
}
// Multiply roll rate error by _ki_rate, apply scaler and integrate
// Scaler is applied before integrator so that integrator state relates directly to aileron deflection
// This means aileron trim offset doesn't change as the value of scaler changes with airspeed
// Don't integrate if in stabilise mode as the integrator will wind up against the pilots inputs
if (!disable_integrator && ki_rate > 0) {
//only integrate if gain and time step are positive and airspeed above min value.
if (dt > 0 && aspeed > float(aparm.airspeed_min)) {
float integrator_delta = rate_error * ki_rate * delta_time * scaler;
// prevent the integrator from increasing if surface defln demand is above the upper limit
if (_last_out < -45) {
integrator_delta = MAX(integrator_delta , 0);
} else if (_last_out > 45) {
// prevent the integrator from decreasing if surface defln demand is below the lower limit
integrator_delta = MIN(integrator_delta, 0);
}
_pid_info.I += integrator_delta;
}
} else {
_pid_info.I = 0;
}
// Scale the integration limit
float intLimScaled = gains.imax * 0.01f;
// Constrain the integrator state
_pid_info.I = constrain_float(_pid_info.I, -intLimScaled, intLimScaled);
// 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.
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;
if (dt > 0 && _slew_rate_max > 0) {
// 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);
// rectify and apply a decaying envelope filter
float alpha = 1.0f - constrain_float(delta_time/_slew_rate_tau, 0.0f , 1.0f);
_slew_rate_amplitude = fmaxf(fabsf(Dterm_slew_rate), alpha * _slew_rate_amplitude);
_slew_rate_amplitude = fminf(_slew_rate_amplitude, 10.0f*_slew_rate_max);
// Calculate and apply the D gain adjustment
_pid_info.Dmod = _slew_rate_max / fmaxf(_slew_rate_amplitude, _slew_rate_max);
_pid_info.D *= _pid_info.Dmod;
}
_last_out = _pid_info.D + _pid_info.FF + _pid_info.P;
if (autotune.running && aspeed > aparm.airspeed_min) {
// let autotune have a go at the values
// Note that we don't pass the integrator component so we get
// a better idea of how much the base PD controller
// contributed
autotune.update(desired_rate, achieved_rate, _last_out);
}
_last_out += _pid_info.I;
// Convert to centi-degrees and constrain
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)
int32_t AP_RollController::_get_rate_out(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;
@ -330,8 +195,8 @@ int32_t AP_RollController::_get_rate_out_ac_pid(float desired_rate, float scaler
}
// 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;
_pid_info = rate_pid.get_pid_info();
auto &pinfo = _pid_info;
const float deg_scale = degrees(1);
pinfo.FF = ff;
@ -353,25 +218,6 @@ int32_t AP_RollController::_get_rate_out_ac_pid(float desired_rate, float scaler
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
A positive demand is up
@ -424,18 +270,24 @@ void AP_RollController::reset_I()
*/
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()) {
if (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);
float old_ff=0, old_p=1.0, old_i=0.3, old_d=0.08;
bool have_old = AP_Param::get_param_by_index(this, 1, AP_PARAM_FLOAT, &old_p);
have_old |= AP_Param::get_param_by_index(this, 3, AP_PARAM_FLOAT, &old_i);
have_old |= AP_Param::get_param_by_index(this, 2, AP_PARAM_FLOAT, &old_d);
have_old |= AP_Param::get_param_by_index(this, 6, AP_PARAM_FLOAT, &old_ff);
if (!have_old) {
// none of the old gains were set
return;
}
const float kp_ff = MAX((old_p - old_i * gains.tau) * gains.tau - old_d, 0);
rate_pid.ff().set_and_save(old_ff + kp_ff);
rate_pid.kI().set_and_save_ifchanged(old_i * gains.tau);
rate_pid.kP().set_and_save_ifchanged(old_d);
rate_pid.kD().set_and_save_ifchanged(0);
rate_pid.kIMAX().set_and_save_ifchanged(gains.imax/4500.0);
}

39
libraries/APM_Control/AP_RollController.h
View File

@ -12,7 +12,7 @@ class AP_RollController {
public:
AP_RollController(AP_AHRS &ahrs, const AP_Vehicle::FixedWing &parms)
: aparm(parms)
, autotune(gains, AP_AutoTune::AUTOTUNE_ROLL, parms)
, autotune(gains, AP_AutoTune::AUTOTUNE_ROLL, parms, rate_pid)
, _ahrs(ahrs)
{
AP_Param::setup_object_defaults(this, var_info);
@ -41,28 +41,23 @@ public:
void autotune_start(void) { autotune.start(); }
void autotune_restore(void) { autotune.stop(); }
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_pid_info(void) const { return _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[];
static const struct AP_Param::GroupInfo var_info[];
// tuning accessors
void kP(float v) { gains.P.set(v); }
void kI(float v) { gains.I.set(v); }
void kD(float v) { gains.D.set(v); }
void kFF(float v) { gains.FF.set(v); }
void kP(float v) { rate_pid.kP().set(v); }
void kI(float v) { rate_pid.kI().set(v); }
void kD(float v) { rate_pid.kD().set(v); }
void kFF(float v) { rate_pid.ff().set(v); }
AP_Float &kP(void) { return gains.P; }
AP_Float &kI(void) { return gains.I; }
AP_Float &kD(void) { return gains.D; }
AP_Float &kFF(void) { return gains.FF; }
AP_Float &kP(void) { return rate_pid.kP(); }
AP_Float &kI(void) { return rate_pid.kI(); }
AP_Float &kD(void) { return rate_pid.kD(); }
AP_Float &kFF(void) { return rate_pid.ff(); }
void set_ac_pid(bool set) {
use_ac_pid = set;
}
void convert_pid();
private:
const AP_Vehicle::FixedWing &aparm;
@ -71,21 +66,13 @@ private:
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_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;
AP_AHRS &_ahrs;
// D gain limit cycle control
LowPassFilterFloat _slew_rate_filter; // LPF applied to the derivative of the control action generated by the angular rate feedback