From a048a8e8a0b44d2596c97e7bb6ba6794a80bb87c Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Fri, 21 Mar 2025 10:55:20 +0100 Subject: [PATCH] mavlink_receiver: refactor manual_control.throttle extraction to exactly match MAVLink output --- src/modules/mavlink/mavlink_receiver.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 39b626b4d2..cf683eaa10 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -2093,8 +2093,10 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) if (math::isInRange((int)mavlink_manual_control.y, -1000, 1000)) { manual_control_setpoint.roll = mavlink_manual_control.y / 1000.f; } - // For backwards compatibility at the moment interpret throttle in range [0,1000] - if (math::isInRange((int)mavlink_manual_control.z, 0, 1000)) { manual_control_setpoint.throttle = ((mavlink_manual_control.z / 1000.f) * 2.f) - 1.f; } + // For backwards compatibility we need to interpret throttle in range [0,1000] + // Convert from [0, 1000] to internal range [-1, 1] + // (([0, 1000] / 1000 * 2) - 1 = [-1, 1] + if (math::isInRange((int)mavlink_manual_control.z, 0, 1000)) { manual_control_setpoint.throttle = (mavlink_manual_control.z / 500.f) - 1.f; } if (math::isInRange((int)mavlink_manual_control.r, -1000, 1000)) { manual_control_setpoint.yaw = mavlink_manual_control.r / 1000.f; }