Skip to content

Commit

Permalink
mc_att_control: remove TPA
Browse files Browse the repository at this point in the history 
because it's mmostly unused and we have thrust curve correction
see parameter THR_MDL_FAC
  • Loading branch information
@MaEtUgR
MaEtUgR committed Oct 16, 2019
1 parent 45a5372 commit 1055c73
Show file tree
Hide file tree
Showing 5 changed files with 8 additions and 154 deletions.
32 changes: 5 additions & 27 deletions src/modules/mc_att_control/RateControl/RateControl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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;

Expand All @@ -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
Expand All @@ -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)) {
Expand All @@ -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 changes: 2 additions & 24 deletions src/modules/mc_att_control/RateControl/RateControl.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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
Expand All @@ -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
Expand All @@ -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;
};
12 changes: 0 additions & 12 deletions src/modules/mc_att_control/mc_att_control.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -135,11 +135,6 @@ class MulticopterAttitudeControl : public ModuleBase<MulticopterAttitudeControl>
*/
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

Expand Down Expand Up @@ -230,13 +225,6 @@ class MulticopterAttitudeControl : public ModuleBase<MulticopterAttitudeControl>

(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,
Expand Down
2 changes: 1 addition & 1 deletion src/modules/mc_att_control/mc_att_control_main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
90 changes: 0 additions & 90 deletions src/modules/mc_att_control/mc_att_control_params.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
*
Expand Down

0 comments on commit 1055c73

Please sign in to comment.