From 6d489a4b4da35a638cdd50c85e27ed24b4401b35 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 13 Feb 2021 20:33:40 +0100 Subject: [PATCH] FW att control: Robustify for throttle scaling PX4 can support negative (reverse) throttle and the fixed wing controller is not expecting this input range. This hardens it against it. --- src/modules/fw_att_control/FixedwingAttitudeControl.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index ce72df0583..74c4ddc3af 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -165,7 +165,7 @@ FixedwingAttitudeControl::vehicle_manual_poll() -radians(_param_fw_man_p_max.get()), radians(_param_fw_man_p_max.get())); _att_sp.yaw_body = 0.0f; - _att_sp.thrust_body[0] = _manual_control_setpoint.z; + _att_sp.thrust_body[0] = math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f); Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body)); q.copyTo(_att_sp.q_d); @@ -182,7 +182,7 @@ FixedwingAttitudeControl::vehicle_manual_poll() _rates_sp.roll = _manual_control_setpoint.y * radians(_param_fw_acro_x_max.get()); _rates_sp.pitch = -_manual_control_setpoint.x * radians(_param_fw_acro_y_max.get()); _rates_sp.yaw = _manual_control_setpoint.r * radians(_param_fw_acro_z_max.get()); - _rates_sp.thrust_body[0] = _manual_control_setpoint.z; + _rates_sp.thrust_body[0] = math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f); _rate_sp_pub.publish(_rates_sp); @@ -194,7 +194,7 @@ FixedwingAttitudeControl::vehicle_manual_poll() -_manual_control_setpoint.x * _param_fw_man_p_sc.get() + _param_trim_pitch.get(); _actuators.control[actuator_controls_s::INDEX_YAW] = _manual_control_setpoint.r * _param_fw_man_y_sc.get() + _param_trim_yaw.get(); - _actuators.control[actuator_controls_s::INDEX_THROTTLE] = _manual_control_setpoint.z; + _actuators.control[actuator_controls_s::INDEX_THROTTLE] = math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f); } } }