AP_Mount.cpp: send gimbal_manager_status msg when control changes

Co-authored-by: Randy Mackay <rmackay9@yahoo.com>

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
This commit is contained in:
davidsastresas 2023-12-30 13:30:08 +01:00 committed by Andrew Tridgell
parent 7e5e55a97b
commit 462eb46c8b
2 changed files with 13 additions and 1 deletions

View File

@ -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;
}

View File

@ -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;
};