From 18f96c16ced96ce72cbc57454e5224de3b59ba0b Mon Sep 17 00:00:00 2001 From: DanielePettenuzzo Date: Thu, 15 Feb 2024 23:16:02 +0100 Subject: [PATCH] fix gimbal driver for mavlink gimbal v2 input and AUX output The main problem was that during initial negotiation the client would request the gimbal_manager_information from px4 but px4 would never send it because in this configuration the device_compid was set to 0. --- src/modules/gimbal/gimbal.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/modules/gimbal/gimbal.cpp b/src/modules/gimbal/gimbal.cpp index c663d31cea..5893d29738 100644 --- a/src/modules/gimbal/gimbal.cpp +++ b/src/modules/gimbal/gimbal.cpp @@ -211,7 +211,10 @@ static int gimbal_thread_main(int argc, char *argv[]) // Reset control as no one is active anymore, or yet. thread_data.control_data.sysid_primary_control = 0; thread_data.control_data.compid_primary_control = 0; - thread_data.control_data.device_compid = 0; + // If the output is set to AUX we still want the input to be able to control the gimbal + // via mavlink, so we set the device_compid to 1. This follows the mavlink spec which states + // that the gimbal_device_id should be between 1 and 6. + thread_data.control_data.device_compid = params.mnt_mode_out == 0 ? 1 : 0; } InputBase::UpdateResult update_result = InputBase::UpdateResult::NoUpdate;