From 80ea3a09bb40d3ab692465ad823581eacd09cd0a Mon Sep 17 00:00:00 2001 From: Peter Breuer Date: Wed, 19 Mar 2025 18:29:17 -0700 Subject: [PATCH] fix: change MANUAL_CONTROL MAVLink message output throttle field range from [-1000, 1000] to [0, 1000] --- src/modules/mavlink/streams/MANUAL_CONTROL.hpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/streams/MANUAL_CONTROL.hpp b/src/modules/mavlink/streams/MANUAL_CONTROL.hpp index 3feabf124d..d62aec1b0a 100644 --- a/src/modules/mavlink/streams/MANUAL_CONTROL.hpp +++ b/src/modules/mavlink/streams/MANUAL_CONTROL.hpp @@ -70,7 +70,9 @@ private: msg.target = mavlink_system.sysid; msg.x = manual_control_setpoint.pitch * 1000.f; msg.y = manual_control_setpoint.roll * 1000.f; - msg.z = manual_control_setpoint.throttle * 1000.f; + // Convert from internal range [-1, 1] to [0, 1000] + // ([-1, 1] + 1) / 2 * 1000 = [0, 1000] + msg.z = (manual_control_setpoint.throttle + 1.0f) * 500.0f; msg.r = manual_control_setpoint.yaw * 1000.f; manual_control_switches_s manual_control_switches{};