add breakpoint and slope params for TPA

src/modules/mc_att_control/mc_att_control_main.cpp

@@ -185,6 +185,8 @@ private:
 		param_t pitch_rate_i;
 		param_t pitch_rate_d;
 		param_t pitch_rate_ff;
+		param_t tpa_breakpoint;
+		param_t tpa_slope;
 		param_t yaw_p;
 		param_t yaw_rate_p;
 		param_t yaw_rate_i;
@@ -217,6 +219,9 @@ private:
 		math::Vector<3>	rate_ff;			/**< Feedforward gain for desired rates */
 		float yaw_ff;						/**< yaw control feed-forward */
 
+		float tpa_breakpoint;				/**< Throttle PID Attenuation breakpoint */
+		float tpa_slope;					/**< Throttle PID Attenuation slope */
+
 		float roll_rate_max;
 		float pitch_rate_max;
 		float yaw_rate_max;
@@ -381,6 +386,8 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_params_handles.pitch_rate_i	= 	param_find("MC_PITCHRATE_I");
 	_params_handles.pitch_rate_d	= 	param_find("MC_PITCHRATE_D");
 	_params_handles.pitch_rate_ff 	= 	param_find("MC_PITCHRATE_FF");
+	_params_handles.tpa_breakpoint 	= 	param_find("MC_TPA_BREAK");
+	_params_handles.tpa_slope	 	= 	param_find("MC_TPA_SLOPE");
 	_params_handles.yaw_p			=	param_find("MC_YAW_P");
 	_params_handles.yaw_rate_p		= 	param_find("MC_YAWRATE_P");
 	_params_handles.yaw_rate_i		= 	param_find("MC_YAWRATE_I");
@@ -477,6 +484,11 @@ MulticopterAttitudeControl::parameters_update()
 	param_get(_params_handles.pitch_rate_ff, &v);
 	_params.rate_ff(1) = v;
 
+	param_get(_params_handles.tpa_breakpoint, &v);
+	_params.tpa_breakpoint = v;
+	param_get(_params_handles.tpa_slope, &v);
+	_params.tpa_slope = v;
+
 	/* yaw gains */
 	param_get(_params_handles.yaw_p, &v);
 	_params.att_p(2) = v;
@@ -772,7 +784,7 @@ MulticopterAttitudeControl::control_attitude_rates(float dt)
 	rates(2) = _ctrl_state.yaw_rate;
 
 	/* throttle pid attenuation factor */
-	float tpa =  fminf(1.0f, 1.5f - fabsf(_v_rates_sp.thrust));
+	float tpa =  fmaxf(0.0f, fminf(1.0f, 1.0f - _params.tpa_slope * (fabsf(_v_rates_sp.thrust) - _params.tpa_breakpoint)));
 
 	/* angular rates error */
 	math::Vector<3> rates_err = _rates_sp - rates;

src/modules/mc_att_control/mc_att_control_params.c

@@ -375,3 +375,31 @@ PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX, 360.0f);
  * @group Multicopter Attitude Control
  */
 PARAM_DEFINE_FLOAT(MC_RATT_TH, 1.0f);
+
+/**
+ * Threshold for 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, 1.0f);
+
+/**
+ * Slope for 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 - slope*(abs(throttle)-breakpoint))
+ *
+ * @min 0.0
+ * @max 2.0
+ * @decimal 2
+ * @increment 0.1
+ * @group Multicopter Attitude Control
+ */
+PARAM_DEFINE_FLOAT(MC_TPA_SLOPE, 1.0f);