From 6ca9033ddeb0ed92e944a94672c58e9ecaa38263 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 19 Sep 2020 10:29:41 +1000 Subject: [PATCH] APM_Control: run AC_PID in parallel with old PID for roll/pitch --- libraries/APM_Control/AP_PitchController.cpp | 270 ++++++++++++++++--- libraries/APM_Control/AP_PitchController.h | 35 ++- libraries/APM_Control/AP_RollController.cpp | 244 +++++++++++++++-- libraries/APM_Control/AP_RollController.h | 26 +- 4 files changed, 498 insertions(+), 77 deletions(-) diff --git a/libraries/APM_Control/AP_PitchController.cpp b/libraries/APM_Control/AP_PitchController.cpp index fe0f9b2f37..85a460cd60 100644 --- a/libraries/APM_Control/AP_PitchController.cpp +++ b/libraries/APM_Control/AP_PitchController.cpp @@ -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); } diff --git a/libraries/APM_Control/AP_PitchController.h b/libraries/APM_Control/AP_PitchController.h index 5dee18b1d9..042caab4c8 100644 --- a/libraries/APM_Control/AP_PitchController.h +++ b/libraries/APM_Control/AP_PitchController.h @@ -6,6 +6,8 @@ #include "AP_AutoTune.h" #include #include +#include +#include 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; }; diff --git a/libraries/APM_Control/AP_RollController.cpp b/libraries/APM_Control/AP_RollController.cpp index ffa3b8a4aa..ac301dd99c 100644 --- a/libraries/APM_Control/AP_RollController.cpp +++ b/libraries/APM_Control/AP_RollController.cpp @@ -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); } diff --git a/libraries/APM_Control/AP_RollController.h b/libraries/APM_Control/AP_RollController.h index 9463754d1d..b60fbba99c 100644 --- a/libraries/APM_Control/AP_RollController.h +++ b/libraries/APM_Control/AP_RollController.h @@ -6,6 +6,7 @@ #include "AP_AutoTune.h" #include #include +#include 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) - };