FW attitude controller: constrain rates correctly

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

src/modules/fw_att_control/ecl_controller.cpp

@@ -103,11 +103,6 @@ void ECL_Controller::set_max_rate(float max_rate)
 	_max_rate = max_rate;
 }
 
-void ECL_Controller::set_bodyrate_setpoint(float rate)
-{
-	_body_rate_setpoint = math::constrain(rate, -_max_rate, _max_rate);
-}
-
 float ECL_Controller::get_euler_rate_setpoint()
 {
 	return _euler_rate_setpoint;

src/modules/fw_att_control/ecl_controller.h

@@ -87,7 +87,6 @@ public:
 	void set_k_ff(float k_ff);
 	void set_integrator_max(float max);
 	void set_max_rate(float max_rate);
-	void set_bodyrate_setpoint(float rate);
 
 	/* Getters */
 	float get_euler_rate_setpoint();

src/modules/fw_att_control/ecl_pitch_controller.cpp

@@ -62,8 +62,9 @@ float ECL_PitchController::control_attitude(const float dt, const ECL_ControlDat
 	_euler_rate_setpoint =  pitch_error / _tc;
 
 	/* Transform setpoint to body angular rates (jacobian) */
-	_body_rate_setpoint = cosf(ctl_data.roll) * _euler_rate_setpoint +
+	const float pitch_body_rate_setpoint_raw = cosf(ctl_data.roll) * _euler_rate_setpoint +
 			cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.euler_yaw_rate_setpoint;
+	_body_rate_setpoint = math::constrain(pitch_body_rate_setpoint_raw, -_max_rate_neg, _max_rate);
 
 	return _body_rate_setpoint;
 }

src/modules/fw_att_control/ecl_pitch_controller.h

@@ -73,11 +73,6 @@ public:
 		_max_rate_neg = max_rate_neg;
 	}
 
-	void set_bodyrate_setpoint(float rate)
-	{
-		_body_rate_setpoint = math::constrain(rate, -_max_rate_neg, _max_rate);
-	}
-
 protected:
 	float _max_rate_neg{0.0f};
 };

src/modules/fw_att_control/ecl_roll_controller.cpp

@@ -61,7 +61,9 @@ float ECL_RollController::control_attitude(const float dt, const ECL_ControlData
 	_euler_rate_setpoint = roll_error / _tc;
 
 	/* Transform setpoint to body angular rates (jacobian) */
-	_body_rate_setpoint = _euler_rate_setpoint - sinf(ctl_data.pitch) * ctl_data.euler_yaw_rate_setpoint;
+	const float roll_body_rate_setpoint_raw = _euler_rate_setpoint - sinf(ctl_data.pitch) *
+			ctl_data.euler_yaw_rate_setpoint;
+	_body_rate_setpoint = math::constrain(roll_body_rate_setpoint_raw, -_max_rate, _max_rate);
 
 	return _body_rate_setpoint;
 }

src/modules/fw_att_control/ecl_yaw_controller.cpp

@@ -85,8 +85,9 @@ float ECL_YawController::control_attitude(const float dt, const ECL_ControlData
 				       ctl_data.airspeed_min ? ctl_data.airspeed_min : ctl_data.airspeed);
 
 		/* Transform setpoint to body angular rates (jacobian) */
-		_body_rate_setpoint = -sinf(ctl_data.roll) * ctl_data.euler_pitch_rate_setpoint +
+		const float yaw_body_rate_setpoint_raw = -sinf(ctl_data.roll) * ctl_data.euler_pitch_rate_setpoint +
 				cosf(ctl_data.roll) * cosf(ctl_data.pitch) * _euler_rate_setpoint;
+		_body_rate_setpoint = math::constrain(yaw_body_rate_setpoint_raw, -_max_rate, _max_rate);
 	}
 
 	if (!PX4_ISFINITE(_body_rate_setpoint)) {