add FF to FW rate controllers

src/modules/fw_att_control/ecl_controller.h

@@ -80,7 +80,7 @@ public:
 	virtual ~ECL_Controller() = default;
 
 	virtual float control_attitude(const float dt, const ECL_ControlData &ctl_data) = 0;
-	virtual float control_euler_rate(const float dt, const ECL_ControlData &ctl_data) = 0;
+	virtual float control_euler_rate(const float dt, const ECL_ControlData &ctl_data, float bodyrate_ff) = 0;
 	virtual float control_bodyrate(const float dt, const ECL_ControlData &ctl_data) = 0;
 
 	/* Setters */

src/modules/fw_att_control/ecl_pitch_controller.cpp

@@ -111,11 +111,11 @@ float ECL_PitchController::control_bodyrate(const float dt, const ECL_ControlDat
 	return math::constrain(_last_output, -1.0f, 1.0f);
 }
 
-float ECL_PitchController::control_euler_rate(const float dt, const ECL_ControlData &ctl_data)
+float ECL_PitchController::control_euler_rate(const float dt, const ECL_ControlData &ctl_data, float bodyrate_ff)
 {
 	/* Transform setpoint to body angular rates (jacobian) */
 	_bodyrate_setpoint = cosf(ctl_data.roll) * _rate_setpoint +
-			     cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.yaw_rate_setpoint;
+			     cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.yaw_rate_setpoint + bodyrate_ff;
 
 	set_bodyrate_setpoint(_bodyrate_setpoint);
 

src/modules/fw_att_control/ecl_pitch_controller.h

@@ -61,7 +61,7 @@ public:
 	~ECL_PitchController() = default;
 
 	float control_attitude(const float dt, const ECL_ControlData &ctl_data) override;
-	float control_euler_rate(const float dt, const ECL_ControlData &ctl_data) override;
+	float control_euler_rate(const float dt, const ECL_ControlData &ctl_data, float bodyrate_ff) override;
 	float control_bodyrate(const float dt, const ECL_ControlData &ctl_data) override;
 
 	/* Additional Setters */

src/modules/fw_att_control/ecl_roll_controller.cpp

@@ -108,10 +108,10 @@ float ECL_RollController::control_bodyrate(const float dt, const ECL_ControlData
 	return math::constrain(_last_output, -1.0f, 1.0f);
 }
 
-float ECL_RollController::control_euler_rate(const float dt, const ECL_ControlData &ctl_data)
+float ECL_RollController::control_euler_rate(const float dt, const ECL_ControlData &ctl_data, float bodyrate_ff)
 {
 	/* Transform setpoint to body angular rates (jacobian) */
-	_bodyrate_setpoint = ctl_data.roll_rate_setpoint - sinf(ctl_data.pitch) * ctl_data.yaw_rate_setpoint;
+	_bodyrate_setpoint = ctl_data.roll_rate_setpoint - sinf(ctl_data.pitch) * ctl_data.yaw_rate_setpoint + bodyrate_ff;
 
 	set_bodyrate_setpoint(_bodyrate_setpoint);
 

src/modules/fw_att_control/ecl_roll_controller.h

@@ -59,7 +59,7 @@ public:
 	~ECL_RollController() = default;
 
 	float control_attitude(const float dt, const ECL_ControlData &ctl_data) override;
-	float control_euler_rate(const float dt, const ECL_ControlData &ctl_data) override;
+	float control_euler_rate(const float dt, const ECL_ControlData &ctl_data, float bodyrate_ff) override;
 	float control_bodyrate(const float dt, const ECL_ControlData &ctl_data) override;
 };
 

src/modules/fw_att_control/ecl_wheel_controller.h

@@ -62,7 +62,7 @@ public:
 
 	float control_bodyrate(const float dt, const ECL_ControlData &ctl_data) override;
 
-	float control_euler_rate(const float dt, const ECL_ControlData &ctl_data) override { (void)ctl_data; return 0; }
+	float control_euler_rate(const float dt, const ECL_ControlData &ctl_data, float bodyrate_ff) override { (void)ctl_data; return 0; }
 };
 
 #endif // ECL_HEADING_CONTROLLER_H

src/modules/fw_att_control/ecl_yaw_controller.cpp

@@ -142,11 +142,11 @@ float ECL_YawController::control_bodyrate(const float dt, const ECL_ControlData
 	return math::constrain(_last_output, -1.0f, 1.0f);
 }
 
-float ECL_YawController::control_euler_rate(const float dt, const ECL_ControlData &ctl_data)
+float ECL_YawController::control_euler_rate(const float dt, const ECL_ControlData &ctl_data, float bodyrate_ff)
 {
 	/* Transform setpoint to body angular rates (jacobian) */
 	_bodyrate_setpoint = -sinf(ctl_data.roll) * ctl_data.pitch_rate_setpoint +
-			     cosf(ctl_data.roll) * cosf(ctl_data.pitch) * _rate_setpoint;
+			     cosf(ctl_data.roll) * cosf(ctl_data.pitch) * _rate_setpoint + bodyrate_ff;
 
 	set_bodyrate_setpoint(_bodyrate_setpoint);
 

src/modules/fw_att_control/ecl_yaw_controller.h

@@ -59,7 +59,7 @@ public:
 	~ECL_YawController() = default;
 
 	float control_attitude(const float dt, const ECL_ControlData &ctl_data) override;
-	float control_euler_rate(const float dt, const ECL_ControlData &ctl_data) override;
+	float control_euler_rate(const float dt, const ECL_ControlData &ctl_data, float bodyrate_ff) override;
 	float control_bodyrate(const float dt, const ECL_ControlData &ctl_data) override;
 
 protected: