From 15219fbfe50d3786d236ff53bbb65b75a352f052 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 13 Feb 2021 20:34:44 +0100 Subject: [PATCH] MC att control: Robustify for throttle scaling PX4 can support negative (reverse) throttle and the multicopter controller is not expecting this input range. This hardens it against it. --- src/modules/mc_att_control/mc_att_control_main.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index c1a7327047..684d3c4c64 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -127,7 +127,8 @@ MulticopterAttitudeControl::generate_attitude_setpoint(const Quatf &q, float dt, if (reset_yaw_sp) { _man_yaw_sp = yaw; - } else if (_manual_control_setpoint.z > 0.05f || _param_mc_airmode.get() == (int32_t)Mixer::Airmode::roll_pitch_yaw) { + } else if (math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f) > 0.05f + || _param_mc_airmode.get() == (int32_t)Mixer::Airmode::roll_pitch_yaw) { const float yaw_rate = math::radians(_param_mpc_man_y_max.get()); attitude_setpoint.yaw_sp_move_rate = _manual_control_setpoint.r * yaw_rate; @@ -210,7 +211,7 @@ MulticopterAttitudeControl::generate_attitude_setpoint(const Quatf &q, float dt, Quatf q_sp = Eulerf(attitude_setpoint.roll_body, attitude_setpoint.pitch_body, attitude_setpoint.yaw_body); q_sp.copyTo(attitude_setpoint.q_d); - attitude_setpoint.thrust_body[2] = -throttle_curve(_manual_control_setpoint.z); + attitude_setpoint.thrust_body[2] = -throttle_curve(math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f)); attitude_setpoint.timestamp = hrt_absolute_time(); _vehicle_attitude_setpoint_pub.publish(attitude_setpoint);