From d5d2ce26d7bdabc494fa08d626431f817d52cb91 Mon Sep 17 00:00:00 2001 From: Jacob Dahl <37091262+dakejahl@users.noreply.github.com> Date: Tue, 31 Mar 2026 08:52:25 -0800 Subject: [PATCH] fix(dshot): map MAVLink standard ACTUATOR_OUTPUT_FUNCTION to PX4 OutputFunction (#26909) The MAVLink standard defines ACTUATOR_OUTPUT_FUNCTION_MOTOR1=1..MOTOR16=16, but PX4 internally uses OutputFunction::Motor1=101..Motor12=112. The DShot driver only handled PX4 internal values (101+) and QGC legacy values (1101+), so any standards-compliant GCS sending the MAVLink enum values would get VEHICLE_CMD_RESULT_UNSUPPORTED back from MAV_CMD_CONFIGURE_ACTUATOR. Add a mapping from MAVLink standard values (1-16) to PX4 internal values (101-116) by adding 100, matching the existing QGC backwards-compat pattern. --- src/drivers/dshot/DShot.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/drivers/dshot/DShot.cpp b/src/drivers/dshot/DShot.cpp index b4af5fd7d2..2920ff668f 100644 --- a/src/drivers/dshot/DShot.cpp +++ b/src/drivers/dshot/DShot.cpp @@ -785,6 +785,11 @@ void DShot::handle_configure_actuator(const vehicle_command_s &command) if (function > 1000) { // NOTE: backwards compatibility for QGC - 1101=Motor1, 1102=Motor2, etc function -= 1000; + + } else if (function >= 1 && function <= 16) { + // MAVLink standard: ACTUATOR_OUTPUT_FUNCTION_MOTOR1=1 .. MOTOR16=16 + // PX4 internal: OutputFunction::Motor1=101 .. Motor12=112 + function += 100; } int motor_index = -1;