From 9b3cb6c75896a04dcd84647df597774752edd89a Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Fri, 30 Sep 2022 13:45:20 +0200 Subject: [PATCH] FW Attitdue controller: fix throttle in acro (use _rate_sp.throttle_body) (#20330) * FW Attitdue controller: fix throttle in acro (use _rate_sp.throttle_body) Signed-off-by: Silvan Fuhrer * FW Attitude controller: do manual control polling before attitude setpoint polling Signed-off-by: Silvan Fuhrer Signed-off-by: Silvan Fuhrer --- src/modules/fw_att_control/FixedwingAttitudeControl.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index 82fbf2f58c..f967c44455 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -353,13 +353,14 @@ void FixedwingAttitudeControl::Run() const matrix::Eulerf euler_angles(_R); + vehicle_manual_poll(euler_angles.psi()); + vehicle_attitude_setpoint_poll(); // vehicle status update must be before the vehicle_control_mode_poll(), otherwise rate sp are not published during whole transition _vehicle_status_sub.update(&_vehicle_status); vehicle_control_mode_poll(); - vehicle_manual_poll(euler_angles.psi()); vehicle_land_detected_poll(); // the position controller will not emit attitude setpoints in some modes @@ -608,7 +609,7 @@ void FixedwingAttitudeControl::Run() /* throttle passed through if it is finite */ _actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] = - (PX4_ISFINITE(_att_sp.thrust_body[0])) ? _att_sp.thrust_body[0] : 0.0f; + (PX4_ISFINITE(_rates_sp.thrust_body[0])) ? _rates_sp.thrust_body[0] : 0.0f; /* scale effort by battery status */ if (_param_fw_bat_scale_en.get() &&