From 14240f4739b39d0501274fd3ebf5265a83e79094 Mon Sep 17 00:00:00 2001 From: Jaeyoung-Lim Date: Tue, 17 Dec 2024 16:53:56 +0100 Subject: [PATCH] Add feedforward setpoint term to feedforward --- src/modules/fw_rate_control/FixedwingRateControl.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/fw_rate_control/FixedwingRateControl.cpp b/src/modules/fw_rate_control/FixedwingRateControl.cpp index 1f2cff00d6..1793bcf7c4 100644 --- a/src/modules/fw_rate_control/FixedwingRateControl.cpp +++ b/src/modules/fw_rate_control/FixedwingRateControl.cpp @@ -348,6 +348,8 @@ void FixedwingRateControl::Run() _rates_sp_sub.update(&_rates_sp); Vector3f body_rates_setpoint = Vector3f(_rates_sp.roll, _rates_sp.pitch, _rates_sp.yaw); + const Vector3f feedforward_setpoint = Vector3f(_rates_sp.body_rates_ff[0], _rates_sp.body_rates_ff[1], + _rates_sp.body_rates_ff[2]); // Tailsitter: rotate setpoint from hover to fixed-wing frame (controller is in fixed-wing frame, interface in hover) if (_vehicle_status.is_vtol_tailsitter) { @@ -359,7 +361,7 @@ void FixedwingRateControl::Run() _landed); const Vector3f gain_ff(_param_fw_rr_ff.get(), _param_fw_pr_ff.get(), _param_fw_yr_ff.get()); - const Vector3f feedforward = gain_ff.emult(body_rates_setpoint) * _airspeed_scaling; + const Vector3f feedforward = gain_ff.emult(body_rates_setpoint) * _airspeed_scaling + feedforward_setpoint; Vector3f control_u = angular_acceleration_setpoint * _airspeed_scaling * _airspeed_scaling + feedforward;