fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

mc_att_control: remove TPA

Browse Source 
because it's mostly unused and we have thrust curve correction
see parameter THR_MDL_FAC
This commit is contained in:
Matthias Grob 2019-10-16 15:07:30 +02:00 committed by Daniel Agar
parent f9ddbd7e2a
commit 08ec6a28f0
6 changed files with 9 additions and 155 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
src/modules/mc_att_control/RateControl/RateControl.cpp
View File

@ -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);
}

26
src/modules/mc_att_control/RateControl/RateControl.hpp
View File

@ -80,13 +80,6 @@ public:
*/
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 @@ public:
* @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 @@ public:
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 @@ private:
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;
};

2
src/modules/mc_att_control/RateControl/RateControlTest.cpp
View File

@ -39,6 +39,6 @@ using namespace matrix;
TEST(RateControlTest, AllZeroCase)
{
RateControl rate_control;
Vector3f torque = rate_control.update(Vector3f(), Vector3f(), 0.f, false, 0.f);
Vector3f torque = rate_control.update(Vector3f(), Vector3f(), 0.f, false);
EXPECT_EQ(torque, Vector3f());
}

12
src/modules/mc_att_control/mc_att_control.hpp
View File

@ -135,11 +135,6 @@ private:
*/
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 @@ private:
(ParamFloat<px4::params::MC_DTERM_CUTOFF>) _param_mc_dterm_cutoff, /**< Cutoff frequency for the D-term filter */
(ParamFloat<px4::params::MC_TPA_BREAK_P>) _param_mc_tpa_break_p, /**< Throttle PID Attenuation breakpoint */
(ParamFloat<px4::params::MC_TPA_BREAK_I>) _param_mc_tpa_break_i, /**< Throttle PID Attenuation breakpoint */
(ParamFloat<px4::params::MC_TPA_BREAK_D>) _param_mc_tpa_break_d, /**< Throttle PID Attenuation breakpoint */
(ParamFloat<px4::params::MC_TPA_RATE_P>) _param_mc_tpa_rate_p, /**< Throttle PID Attenuation slope */
(ParamFloat<px4::params::MC_TPA_RATE_I>) _param_mc_tpa_rate_i, /**< Throttle PID Attenuation slope */
(ParamFloat<px4::params::MC_TPA_RATE_D>) _param_mc_tpa_rate_d, /**< Throttle PID Attenuation slope */
(ParamFloat<px4::params::MC_ROLLRATE_MAX>) _param_mc_rollrate_max,
(ParamFloat<px4::params::MC_PITCHRATE_MAX>) _param_mc_pitchrate_max,
(ParamFloat<px4::params::MC_YAWRATE_MAX>) _param_mc_yawrate_max,

2
src/modules/mc_att_control/mc_att_control_main.cpp
View File

@ -377,7 +377,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

90
src/modules/mc_att_control/mc_att_control_params.c
View File

@ -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
*