diff --git a/src/modules/mc_att_control/RateControl/RateControl.cpp b/src/modules/mc_att_control/RateControl/RateControl.cpp index f9815894aecf..a728c8ec98d0 100644 --- a/src/modules/mc_att_control/RateControl/RateControl.cpp +++ b/src/modules/mc_att_control/RateControl/RateControl.cpp @@ -56,12 +56,6 @@ void RateControl::setDTermCutoff(const float loop_rate, const float cutoff, cons } } -void RateControl::setTPA(const Vector3f &breakpoint, const Vector3f &rate) -{ - _tpa_breakpoint = breakpoint; - _tpa_rate = rate; -} - void RateControl::setSaturationStatus(const MultirotorMixer::saturation_status &status) { _mixer_saturation_positive[0] = status.flags.roll_pos; @@ -72,13 +66,8 @@ void RateControl::setSaturationStatus(const MultirotorMixer::saturation_status & _mixer_saturation_negative[2] = status.flags.yaw_neg; } -Vector3f RateControl::update(const Vector3f rate, const Vector3f rate_sp, const float dt, const bool landed, - const float thrust_sp) +Vector3f RateControl::update(const Vector3f rate, const Vector3f rate_sp, const float dt, const bool landed) { - Vector3f gain_p_tpa = _gain_p.emult(tpa_attenuations(_tpa_breakpoint(0), _tpa_rate(0), thrust_sp)); - Vector3f gain_i_tpa = _gain_i.emult(tpa_attenuations(_tpa_breakpoint(1), _tpa_rate(1), thrust_sp)); - Vector3f gain_d_tpa = _gain_d.emult(tpa_attenuations(_tpa_breakpoint(2), _tpa_rate(2), thrust_sp)); - // angular rates error Vector3f rate_error = rate_sp - rate; @@ -91,20 +80,20 @@ Vector3f RateControl::update(const Vector3f rate, const Vector3f rate_sp, const } // PID control with feed forward - Vector3f torque = gain_p_tpa.emult(rate_error) + _rate_int - gain_d_tpa.emult(rate_d) + _gain_ff.emult(rate_sp); + Vector3f torque = _gain_p.emult(rate_error) + _rate_int - _gain_d.emult(rate_d) + _gain_ff.emult(rate_sp); _rate_prev = rate; _rate_prev_filtered = rate_filtered; // update integral only if we are not landed if (!landed) { - updateIntegral(rate_error, dt, gain_i_tpa); + updateIntegral(rate_error, dt); } return torque; } -void RateControl::updateIntegral(Vector3f &rate_error, const float dt, const Vector3f &gain_i_tpa) +void RateControl::updateIntegral(Vector3f &rate_error, const float dt) { for (int i = 0; i < 3; i++) { // prevent further positive control saturation @@ -127,7 +116,7 @@ void RateControl::updateIntegral(Vector3f &rate_error, const float dt, const Vec i_factor = math::max(0.0f, 1.f - i_factor * i_factor); // Perform the integration using a first order method - float rate_i = _rate_int(i) + i_factor * gain_i_tpa(i) * rate_error(i) * dt; + float rate_i = _rate_int(i) + i_factor * _gain_i(i) * rate_error(i) * dt; // do not propagate the result if out of range or invalid if (PX4_ISFINITE(rate_i)) { @@ -142,14 +131,3 @@ void RateControl::getRateControlStatus(rate_ctrl_status_s &rate_ctrl_status) rate_ctrl_status.pitchspeed_integ = _rate_int(1); rate_ctrl_status.yawspeed_integ = _rate_int(2); } - -Vector3f RateControl::tpa_attenuations(float tpa_breakpoint, float tpa_rate, float thrust_sp) -{ - static constexpr float tpa_rate_lower_limit = 0.05f; - - /* throttle pid attenuation factor */ - float tpa = 1.0f - tpa_rate * (fabsf(thrust_sp) - tpa_breakpoint) / (1.0f - tpa_breakpoint); - tpa = fmaxf(tpa_rate_lower_limit, fminf(1.0f, tpa)); - - return Vector3f(tpa, tpa, 1.f); -} diff --git a/src/modules/mc_att_control/RateControl/RateControl.hpp b/src/modules/mc_att_control/RateControl/RateControl.hpp index 7acbd68f1256..38f688969af9 100644 --- a/src/modules/mc_att_control/RateControl/RateControl.hpp +++ b/src/modules/mc_att_control/RateControl/RateControl.hpp @@ -80,13 +80,6 @@ class RateControl */ void setFeedForwardGain(const matrix::Vector3f &FF) { _gain_ff = FF; }; - /** - * Set the rate control gains - * @param breakpoint parameter 3D vector for P, I and D - * @param rate parameter 3D vector for P, I and D - */ - void setTPA(const matrix::Vector3f &breakpoint, const matrix::Vector3f &rate); - /** * Set saturation status * @param status message from mixer reporting about saturation @@ -98,11 +91,9 @@ class RateControl * @param rate estimation of the current vehicle angular rate * @param rate_sp desired vehicle angular rate setpoint * @param dt desired vehicle angular rate setpoint - * @param thrust_sp total thrust setpoint to be used for TPA * @return [-1,1] normalized torque vector to apply to the vehicle */ - matrix::Vector3f update(const matrix::Vector3f rate, const matrix::Vector3f rate_sp, const float dt, const bool landed, - const float thrust_sp); + matrix::Vector3f update(const matrix::Vector3f rate, const matrix::Vector3f rate_sp, const float dt, const bool landed); /** * Set the integral term to 0 to prevent windup @@ -117,7 +108,7 @@ class RateControl void getRateControlStatus(rate_ctrl_status_s &rate_ctrl_status); private: - void updateIntegral(matrix::Vector3f &rate_error, const float dt, const matrix::Vector3f &gain_i_tpa); + void updateIntegral(matrix::Vector3f &rate_error, const float dt); // Gains matrix::Vector3f _gain_p; ///< rate control proportional gain for all axes x, y, z @@ -133,17 +124,4 @@ class RateControl math::LowPassFilter2pVector3f _lp_filters_d{0.f, 0.f}; ///< low-pass filters for D-term (roll, pitch & yaw) bool _mixer_saturation_positive[3] {}; bool _mixer_saturation_negative[3] {}; - - /* - * Throttle PID attenuation - * Lowers the overall gain of the PID controller linearly depending on total thrust. - * Function visualization available here https://www.desmos.com/calculator/gn4mfoddje - * @param tpa_breakpoint - * @param tpa_rate - * @param thrust_sp - * @return attenuation [0,1] per axis in a vector - */ - matrix::Vector3f tpa_attenuations(float tpa_breakpoint, float tpa_rate, float thrust_sp); - matrix::Vector3f _tpa_breakpoint; - matrix::Vector3f _tpa_rate; }; diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp index ca7396e32ba0..49fa149582ab 100644 --- a/src/modules/mc_att_control/mc_att_control.hpp +++ b/src/modules/mc_att_control/mc_att_control.hpp @@ -135,11 +135,6 @@ class MulticopterAttitudeControl : public ModuleBase */ void control_attitude_rates(float dt, const matrix::Vector3f &rates); - /** - * Throttle PID attenuation. - */ - matrix::Vector3f pid_attenuations(float tpa_breakpoint, float tpa_rate); - AttitudeControl _attitude_control; ///< class for attitude control calculations RateControl _rate_control; ///< class for rate control calculations @@ -230,13 +225,6 @@ class MulticopterAttitudeControl : public ModuleBase (ParamFloat) _param_mc_dterm_cutoff, /**< Cutoff frequency for the D-term filter */ - (ParamFloat) _param_mc_tpa_break_p, /**< Throttle PID Attenuation breakpoint */ - (ParamFloat) _param_mc_tpa_break_i, /**< Throttle PID Attenuation breakpoint */ - (ParamFloat) _param_mc_tpa_break_d, /**< Throttle PID Attenuation breakpoint */ - (ParamFloat) _param_mc_tpa_rate_p, /**< Throttle PID Attenuation slope */ - (ParamFloat) _param_mc_tpa_rate_i, /**< Throttle PID Attenuation slope */ - (ParamFloat) _param_mc_tpa_rate_d, /**< Throttle PID Attenuation slope */ - (ParamFloat) _param_mc_rollrate_max, (ParamFloat) _param_mc_pitchrate_max, (ParamFloat) _param_mc_yawrate_max, diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index cf0b6fff56fb..b9809b40a282 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -376,7 +376,7 @@ MulticopterAttitudeControl::control_attitude_rates(float dt, const Vector3f &rat const bool landed = _vehicle_land_detected.maybe_landed || _vehicle_land_detected.landed; _rate_control.setSaturationStatus(_saturation_status); - _att_control = _rate_control.update(rates, _rates_sp, dt, landed, _thrust_sp); + _att_control = _rate_control.update(rates, _rates_sp, dt, landed); } void diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c index 0cafa88fcf6c..2d0a2d766991 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -502,96 +502,6 @@ PARAM_DEFINE_FLOAT(MC_RATT_TH, 0.8f); */ PARAM_DEFINE_INT32(MC_BAT_SCALE_EN, 0); -/** - * TPA P Breakpoint - * - * Throttle PID Attenuation (TPA) - * Magnitude of throttle setpoint at which to begin attenuating roll/pitch P gain - * - * @min 0.0 - * @max 1.0 - * @decimal 2 - * @increment 0.1 - * @group Multicopter Attitude Control - */ -PARAM_DEFINE_FLOAT(MC_TPA_BREAK_P, 1.0f); - -/** - * TPA I Breakpoint - * - * Throttle PID Attenuation (TPA) - * Magnitude of throttle setpoint at which to begin attenuating roll/pitch I gain - * - * @min 0.0 - * @max 1.0 - * @decimal 2 - * @increment 0.1 - * @group Multicopter Attitude Control - */ -PARAM_DEFINE_FLOAT(MC_TPA_BREAK_I, 1.0f); - -/** - * TPA D Breakpoint - * - * Throttle PID Attenuation (TPA) - * Magnitude of throttle setpoint at which to begin attenuating roll/pitch D gain - * - * @min 0.0 - * @max 1.0 - * @decimal 2 - * @increment 0.1 - * @group Multicopter Attitude Control - */ -PARAM_DEFINE_FLOAT(MC_TPA_BREAK_D, 1.0f); - -/** - * TPA Rate P - * - * Throttle PID Attenuation (TPA) - * Rate at which to attenuate roll/pitch P gain - * Attenuation factor is 1.0 when throttle magnitude is below the setpoint - * Above the setpoint, the attenuation factor is (1 - rate * (throttle - breakpoint) / (1.0 - breakpoint)) - * - * @min 0.0 - * @max 1.0 - * @decimal 2 - * @increment 0.05 - * @group Multicopter Attitude Control - */ -PARAM_DEFINE_FLOAT(MC_TPA_RATE_P, 0.0f); - -/** - * TPA Rate I - * - * Throttle PID Attenuation (TPA) - * Rate at which to attenuate roll/pitch I gain - * Attenuation factor is 1.0 when throttle magnitude is below the setpoint - * Above the setpoint, the attenuation factor is (1 - rate * (throttle - breakpoint) / (1.0 - breakpoint)) - * - * @min 0.0 - * @max 1.0 - * @decimal 2 - * @increment 0.05 - * @group Multicopter Attitude Control - */ -PARAM_DEFINE_FLOAT(MC_TPA_RATE_I, 0.0f); - -/** - * TPA Rate D - * - * Throttle PID Attenuation (TPA) - * Rate at which to attenuate roll/pitch D gain - * Attenuation factor is 1.0 when throttle magnitude is below the setpoint - * Above the setpoint, the attenuation factor is (1 - rate * (throttle - breakpoint) / (1.0 - breakpoint)) - * - * @min 0.0 - * @max 1.0 - * @decimal 2 - * @increment 0.05 - * @group Multicopter Attitude Control - */ -PARAM_DEFINE_FLOAT(MC_TPA_RATE_D, 0.0f); - /** * Cutoff frequency for the low pass filter on the D-term in the rate controller *