From c0c412570d985a459a47232642aed34f85686b61 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 1 Aug 2024 13:20:45 +1200 Subject: [PATCH] gimbal: Fix deg/rad for angular rates The command is in degrees, but the rest in radians. Also, set roll to NAN, rather than 0 when not set. --- src/modules/gimbal/input_mavlink.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/gimbal/input_mavlink.cpp b/src/modules/gimbal/input_mavlink.cpp index f9f5f4c526..d78e069ba7 100644 --- a/src/modules/gimbal/input_mavlink.cpp +++ b/src/modules/gimbal/input_mavlink.cpp @@ -882,8 +882,8 @@ InputMavlinkGimbalV2::_process_command(ControlData &control_data, const vehicle_ const matrix::Eulerf euler(0.0f, math::radians(vehicle_command.param1), math::radians(vehicle_command.param2)); const matrix::Quatf q(euler); - const matrix::Vector3f angular_velocity(0.0f, vehicle_command.param3, - vehicle_command.param4); + const matrix::Vector3f angular_velocity(NAN, math::radians(vehicle_command.param3), + math::radians(vehicle_command.param4)); const uint32_t flags = vehicle_command.param5; // TODO: support gimbal device id for multiple gimbals