From 462eb46c8b885efc9181068d13ff4b6ef60e81fe Mon Sep 17 00:00:00 2001 From: davidsastresas Date: Sat, 30 Dec 2023 13:30:08 +0100 Subject: [PATCH] AP_Mount.cpp: send gimbal_manager_status msg when control changes Co-authored-by: Randy Mackay By default we are sending this message at 0.2 Hz. This is totally fine as no more rate is needed, but whenever control changes it is interesting to notify as soon as possible, so the rest of the mavlink network understands the change in control as soon as possible --- libraries/AP_Mount/AP_Mount_Backend.cpp | 8 ++++++++ libraries/AP_Mount/AP_Mount_Backend.h | 6 +++++- 2 files changed, 13 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp index 7e1f28d3d5..b6080a6f22 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend.cpp @@ -358,6 +358,9 @@ MAV_RESULT AP_Mount_Backend::handle_command_do_gimbal_manager_configure(const ma return MAV_RESULT_FAILED; } + // backup the current values so we can detect a change + mavlink_control_id_t prev_control_id = mavlink_control_id; + // convert negative packet1 and packet2 values int16_t new_sysid = packet.param1; switch (new_sysid) { @@ -382,6 +385,11 @@ MAV_RESULT AP_Mount_Backend::handle_command_do_gimbal_manager_configure(const ma break; } + // send gimbal_manager_status if control has changed + if (prev_control_id != mavlink_control_id) { + gcs().send_message(MSG_GIMBAL_MANAGER_STATUS); + } + return MAV_RESULT_ACCEPTED; } diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index d2f9727360..f75ea29da5 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -321,9 +321,13 @@ protected: // structure holding mavlink sysid and compid of controller of this gimbal // see MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE and GIMBAL_MANAGER_STATUS - struct { + struct mavlink_control_id_t { uint8_t sysid; uint8_t compid; + + // equality operators + bool operator==(const mavlink_control_id_t &rhs) const { return (sysid == rhs.sysid && compid == rhs.compid); } + bool operator!=(const mavlink_control_id_t &rhs) const { return !(*this == rhs); } } mavlink_control_id; };