mc_att_control: remove TPA

because it's mostly unused and we have thrust curve correction
see parameter THR_MDL_FAC

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

src/modules/mc_att_control/RateControl/RateControl.hpp

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

src/modules/mc_att_control/RateControl/RateControlTest.cpp

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

src/modules/mc_att_control/mc_att_control.hpp

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

src/modules/mc_att_control/mc_att_control_main.cpp

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

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
  *