From 2cb97e79b901eae446edf62996d2d7015a8e8e0c Mon Sep 17 00:00:00 2001 From: Pernilla Date: Thu, 18 Sep 2025 11:09:09 +0200 Subject: [PATCH] FlightTaskManualAccelerationSlow: Acquire gimbal control until expected IDs are reported (#25565) * Acquire control until expected id s are reported * require an updated message and allow release in case early exit of FlightTask * FlightTask Gimbal: acquiring logic simplification suggestion * formatting --------- Co-authored-by: Matthias Grob --- .../tasks/Utility/Gimbal.cpp | 40 +++++++++++-------- .../tasks/Utility/Gimbal.hpp | 5 ++- 2 files changed, 26 insertions(+), 19 deletions(-) diff --git a/src/modules/flight_mode_manager/tasks/Utility/Gimbal.cpp b/src/modules/flight_mode_manager/tasks/Utility/Gimbal.cpp index 21e97f5fc3..f56ee48804 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/Gimbal.cpp +++ b/src/modules/flight_mode_manager/tasks/Utility/Gimbal.cpp @@ -63,29 +63,35 @@ bool Gimbal::checkForTelemetry(const hrt_abstime now) void Gimbal::acquireGimbalControlIfNeeded() { - if (!_have_gimbal_control) { - _have_gimbal_control = true; + gimbal_manager_status_s gimbal_manager_status; - vehicle_command_s vehicle_command{}; - vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE; - vehicle_command.param1 = _param_mav_sys_id.get(); - vehicle_command.param2 = _param_mav_comp_id.get(); - vehicle_command.param3 = -1.0f; // Leave unchanged. - vehicle_command.param4 = -1.0f; // Leave unchanged. - vehicle_command.timestamp = hrt_absolute_time(); - vehicle_command.source_system = _param_mav_sys_id.get(); - vehicle_command.source_component = _param_mav_comp_id.get(); - vehicle_command.target_system = _param_mav_sys_id.get(); - vehicle_command.target_component = _param_mav_sys_id.get(); - vehicle_command.from_external = false; - _vehicle_command_pub.publish(vehicle_command); + if (_gimbal_manager_status_sub.updated()) { + _gimbal_manager_status_sub.copy(&gimbal_manager_status); + + if (gimbal_manager_status.primary_control_compid != _param_mav_comp_id.get() || + gimbal_manager_status.primary_control_sysid != _param_mav_sys_id.get()) { + _tried_to_have_gimbal_control = true; + vehicle_command_s vehicle_command{}; + vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE; + vehicle_command.param1 = _param_mav_sys_id.get(); + vehicle_command.param2 = _param_mav_comp_id.get(); + vehicle_command.param3 = -1.0f; // Leave unchanged. + vehicle_command.param4 = -1.0f; // Leave unchanged. + vehicle_command.timestamp = hrt_absolute_time(); + vehicle_command.source_system = _param_mav_sys_id.get(); + vehicle_command.source_component = _param_mav_comp_id.get(); + vehicle_command.target_system = _param_mav_sys_id.get(); + vehicle_command.target_component = _param_mav_sys_id.get(); + vehicle_command.from_external = false; + _vehicle_command_pub.publish(vehicle_command); + } } } void Gimbal::releaseGimbalControlIfNeeded() { - if (_have_gimbal_control) { - _have_gimbal_control = false; + if (_tried_to_have_gimbal_control) { + _tried_to_have_gimbal_control = false; // Restore default flags, setting rate setpoints to NAN lead to unexpected behavior publishGimbalManagerSetAttitude(FLAGS_ROLL_PITCH_LOCKED, diff --git a/src/modules/flight_mode_manager/tasks/Utility/Gimbal.hpp b/src/modules/flight_mode_manager/tasks/Utility/Gimbal.hpp index 938ef32aa2..627979fa23 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/Gimbal.hpp +++ b/src/modules/flight_mode_manager/tasks/Utility/Gimbal.hpp @@ -45,6 +45,7 @@ #include #include #include +#include #include #include "Sticks.hpp" @@ -73,9 +74,9 @@ public: | gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_PITCH_LOCK; private: - bool _have_gimbal_control{false}; - + bool _tried_to_have_gimbal_control{false}; uORB::Subscription _gimbal_device_attitude_status_sub{ORB_ID(gimbal_device_attitude_status)}; + uORB::Subscription _gimbal_manager_status_sub{ORB_ID(gimbal_manager_status)}; hrt_abstime _telemtry_timestamp{}; uint16_t _telemetry_flags{}; float _telemetry_yaw{};