Feature/TPA

ROMFS/px4fmu_common/init.d/4002_qavr5

@@ -28,8 +28,8 @@ then
 	param set MC_YAWRATE_I 0.1
 	param set MC_YAWRATE_D 0.0
 	param set MC_YAW_FF 0.5
-	param set MC_TPA_BREAK 0.7
-	param set MC_TPA_SLOPE 1.0
+	param set MC_TPA_BREAK_P 0.7
+	param set MC_TPA_RATE_P 0.3
 	param set PWM_MIN 1075
 	param set MPC_THR_MIN 0.06
 	param set MPC_MANTHR_MIN 0.06

ROMFS/px4fmu_common/init.d/4009_qav250

@@ -33,6 +33,6 @@ then
 	param set MPC_MANTHR_MIN 0.06
 	param set CBRK_IO_SAFETY 22027
 	param set ATT_BIAS_MAX 0.0
-	param set MC_TPA_BREAK 0.5
- 	param set MC_TPA_SLOPE 1.0
+	param set MC_TPA_BREAK_P 0.5
+	param set MC_TPA_RATE_P 0.5
 fi

ROMFS/px4fmu_common/init.d/4050_generic_250

@@ -33,8 +33,8 @@ then
 	param set MC_YAWRATE_MAX 400.0
 	param set MC_ACRO_R_MAX 360.0
 	param set MC_ACRO_P_MAX 360.0
-	param set MC_TPA_BREAK 0.5
-	param set MC_TPA_SLOPE 1.0
+	param set MC_TPA_BREAK_P 0.5
+	param set MC_TPA_RATE_P 0.5
 
 
 	param set PWM_MIN 1075

ROMFS/px4fmu_common/init.d/4080_zmr250

@@ -0,0 +1,55 @@
+#!nsh
+#
+# @name ZMR250 Racer
+#
+# @type Quadrotor x
+#
+# @output AUX1 feed-through of RC AUX1 channel
+# @output AUX2 feed-through of RC AUX2 channel
+#
+# @maintainer Anton Matosov <anton.matosov@gmail.com>
+#
+
+sh /etc/init.d/rc.mc_defaults
+
+set MIXER zmr250
+set PWM_OUT 1234
+
+if [ $AUTOCNF == yes ]
+then
+	param set MC_ROLL_P 2.0
+	param set MC_ROLLRATE_P 0.05
+	param set MC_ROLLRATE_I 0.2
+	param set MC_ROLLRATE_D	0.0015
+	param set MC_ROLL_TC 0.18
+
+	param set MC_PITCH_P 2.0
+	param set MC_PITCHRATE_P 0.05
+	param set MC_PITCHRATE_I 0.2
+	param set MC_PITCHRATE_D 0.0015
+	param set MC_PITCH_TC 0.18
+
+	param set MC_YAW_P 1.0
+	param set MC_YAWRATE_P 0.15
+	param set MC_YAWRATE_I 0.2
+	param set MC_YAWRATE_D 0.0
+	param set MC_YAW_FF 0.5
+
+	param set MC_ACRO_R_MAX 1080.0
+	param set MC_ACRO_P_MAX 1080.0
+	param set MC_ACRO_Y_MAX 1080.0
+
+	param set MC_TPA_BREAK_P 0.5
+	param set MC_TPA_BREAK_D 0.7
+	param set MC_TPA_RATE_P 0.5
+	param set MC_TPA_RATE_D 0.5
+
+	param set PWM_MIN 1075
+	param set PWM_RATE 400
+	param set PWM_DISARMED 900
+	param set FAILSAFE 100
+
+	# param set NAV_RCL_ACT 6 # Lockdown
+
+	param set CBRK_IO_SAFETY 22027
+fi

ROMFS/px4fmu_common/mixers/zmr250.main.mix

@@ -0,0 +1,10 @@
+# R: <geometry> <roll scale> <pitch scale> <yaw scale> <deadband>
+R: 4x 7654 10000 10000 0
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 4  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 5  10000  10000      0 -10000  10000

src/modules/mc_att_control/mc_att_control_main.cpp

@@ -100,6 +100,7 @@ extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]);
 #define YAW_DEADZONE	0.05f
 #define MIN_TAKEOFF_THRUST    0.2f
 #define RATES_I_LIMIT	0.3f
+#define TPA_RATE_LOWER_LIMIT 0.05f
 #define MANUAL_THROTTLE_MAX_MULTICOPTER	0.9f
 #define ATTITUDE_TC_DEFAULT 0.2f
 
@@ -188,8 +189,12 @@ class MulticopterAttitudeControl
 		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 tpa_breakpoint_p;
+		param_t tpa_breakpoint_i;
+		param_t tpa_breakpoint_d;
+		param_t tpa_rate_p;
+		param_t tpa_rate_i;
+		param_t tpa_rate_d;
 		param_t yaw_p;
 		param_t yaw_rate_p;
 		param_t yaw_rate_i;
@@ -224,8 +229,12 @@ class MulticopterAttitudeControl
 		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 tpa_breakpoint_p;				/**< Throttle PID Attenuation breakpoint */
+		float tpa_breakpoint_i;				/**< Throttle PID Attenuation breakpoint */
+		float tpa_breakpoint_d;				/**< Throttle PID Attenuation breakpoint */
+		float tpa_rate_p;					/**< Throttle PID Attenuation slope */
+		float tpa_rate_i;					/**< Throttle PID Attenuation slope */
+		float tpa_rate_d;					/**< Throttle PID Attenuation slope */
 
 		float roll_rate_max;
 		float pitch_rate_max;
@@ -289,6 +298,11 @@ class MulticopterAttitudeControl
 	 */
 	void		control_attitude_rates(float dt);
 
+	/**
+	 * Throttle PID attenuation.
+	 */
+	math::Vector<3> pid_attenuations(float tpa_breakpoint, float tpa_rate);
+
 	/**
 	 * Check for vehicle status updates.
 	 */
@@ -398,8 +412,12 @@ 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.tpa_breakpoint_p 	= 	param_find("MC_TPA_BREAK_P");
+	_params_handles.tpa_breakpoint_i 	= 	param_find("MC_TPA_BREAK_I");
+	_params_handles.tpa_breakpoint_d 	= 	param_find("MC_TPA_BREAK_D");
+	_params_handles.tpa_rate_p	 	= 	param_find("MC_TPA_RATE_P");
+	_params_handles.tpa_rate_i	 	= 	param_find("MC_TPA_RATE_I");
+	_params_handles.tpa_rate_d	 	= 	param_find("MC_TPA_RATE_D");
 	_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");
@@ -496,10 +514,12 @@ 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;
+	param_get(_params_handles.tpa_breakpoint_p, &_params.tpa_breakpoint_p);
+	param_get(_params_handles.tpa_breakpoint_i, &_params.tpa_breakpoint_i);
+	param_get(_params_handles.tpa_breakpoint_d, &_params.tpa_breakpoint_d);
+	param_get(_params_handles.tpa_rate_p, &_params.tpa_rate_p);
+	param_get(_params_handles.tpa_rate_i, &_params.tpa_rate_i);
+	param_get(_params_handles.tpa_rate_d, &_params.tpa_rate_d);
 
 	/* yaw gains */
 	param_get(_params_handles.yaw_p, &v);
@@ -790,6 +810,27 @@ MulticopterAttitudeControl::control_attitude(float dt)
 	}
 }
 
+/*
+ * Throttle PID attenuation
+ * Function visualization available here https://www.desmos.com/calculator/gn4mfoddje
+ * Input: 'tpa_breakpoint', 'tpa_rate', '_thrust_sp'
+ * Output: 'pidAttenuationPerAxis' vector
+ */
+math::Vector<3>
+MulticopterAttitudeControl::pid_attenuations(float tpa_breakpoint, float tpa_rate)
+{
+	/* throttle pid attenuation factor */
+	float tpa = 1.0f - tpa_rate * (fabsf(_v_rates_sp.thrust) - tpa_breakpoint) / (1.0f - tpa_breakpoint);
+	tpa = fmaxf(TPA_RATE_LOWER_LIMIT, fminf(1.0f, tpa));
+
+	math::Vector<3> pidAttenuationPerAxis;
+	pidAttenuationPerAxis(AXIS_INDEX_ROLL) = tpa;
+	pidAttenuationPerAxis(AXIS_INDEX_PITCH) = tpa;
+	pidAttenuationPerAxis(AXIS_INDEX_YAW) = 1.0;
+
+	return pidAttenuationPerAxis;
+}
+
 /*
  * Attitude rates controller.
  * Input: '_rates_sp' vector, '_thrust_sp'
@@ -809,13 +850,16 @@ MulticopterAttitudeControl::control_attitude_rates(float dt)
 	rates(1) = _ctrl_state.pitch_rate;
 	rates(2) = _ctrl_state.yaw_rate;
 
-	/* throttle pid attenuation factor */
-	float tpa =  fmaxf(0.0f, fminf(1.0f, 1.0f - _params.tpa_slope * (fabsf(_v_rates_sp.thrust) - _params.tpa_breakpoint)));
+	math::Vector<3> rates_p_scaled = _params.rate_p.emult(pid_attenuations(_params.tpa_breakpoint_p, _params.tpa_rate_p));
+	math::Vector<3> rates_i_scaled = _params.rate_i.emult(pid_attenuations(_params.tpa_breakpoint_i, _params.tpa_rate_i));
+	math::Vector<3> rates_d_scaled = _params.rate_d.emult(pid_attenuations(_params.tpa_breakpoint_d, _params.tpa_rate_d));
 
 	/* angular rates error */
 	math::Vector<3> rates_err = _rates_sp - rates;
 
-	_att_control = _params.rate_p.emult(rates_err * tpa) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int +
+	_att_control = rates_p_scaled.emult(rates_err) +
+		       _rates_int +
+		       rates_d_scaled.emult(_rates_prev - rates) / dt +
 		       _params.rate_ff.emult(_rates_sp);
 
 	_rates_sp_prev = _rates_sp;
@@ -825,7 +869,7 @@ MulticopterAttitudeControl::control_attitude_rates(float dt)
 	if (_thrust_sp > MIN_TAKEOFF_THRUST && !_motor_limits.lower_limit && !_motor_limits.upper_limit) {
 		for (int i = AXIS_INDEX_ROLL; i < AXIS_COUNT; i++) {
 			if (fabsf(_att_control(i)) < _thrust_sp) {
-				float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt;
+				float rate_i = _rates_int(i) + rates_i_scaled(i) * rates_err(i) * dt;
 
 				if (PX4_ISFINITE(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT &&
 				    _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT &&

src/modules/mc_att_control/mc_att_control_params.c

@@ -72,6 +72,7 @@ PARAM_DEFINE_FLOAT(MC_PITCH_TC, 0.2f);
  *
  * Roll proportional gain, i.e. desired angular speed in rad/s for error 1 rad.
  *
+ * @unit 1/s
  * @min 0.0
  * @max 8
  * @decimal 2
@@ -138,7 +139,7 @@ PARAM_DEFINE_FLOAT(MC_ROLLRATE_FF, 0.0f);
  * @min 0.0
  * @max 10
  * @decimal 2
- * @increment 0.0005
+ * @increment 0.1
  * @group Multicopter Attitude Control
  */
 PARAM_DEFINE_FLOAT(MC_PITCH_P, 6.5f);
@@ -377,8 +378,9 @@ PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX, 360.0f);
 PARAM_DEFINE_FLOAT(MC_RATT_TH, 1.0f);
 
 /**
- * Threshold for Throttle PID Attenuation (TPA)
+ * TPA P Breakpoint
  *
+ * Throttle PID Attenuation (TPA)
  * Magnitude of throttle setpoint at which to begin attenuating roll/pitch P gain
  *
  * @min 0.0
@@ -387,23 +389,83 @@ PARAM_DEFINE_FLOAT(MC_RATT_TH, 1.0f);
  * @increment 0.1
  * @group Multicopter Attitude Control
  */
-PARAM_DEFINE_FLOAT(MC_TPA_BREAK, 1.0f);
+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);
 
 /**
- * Slope for Throttle PID Attenuation (TPA)
+ * 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 - slope*(abs(throttle)-breakpoint))
+ * Above the setpoint, the attenuation factor is (1 - rate * (throttle - breakpoint) / (1.0 - breakpoint))
  *
  * @min 0.0
- * @max 2.0
+ * @max 1.0
  * @decimal 2
- * @increment 0.1
+ * @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_SLOPE, 1.0f);
+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);
 
 /**
  * Whether to scale outputs by battery power level