fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-22 15:37:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

Add feedforward setpoint term to feedforward

Browse Source
This commit is contained in:
Jaeyoung-Lim
2024-12-17 16:53:56 +01:00
parent e46247e426
commit 14240f4739
1 changed files with 3 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_rate_control/FixedwingRateControl.cpp
+3 -1
View File
@@ -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;