From 3927521aa6415d77a63257dad593762663faa26d Mon Sep 17 00:00:00 2001 From: Asif Khan Date: Sat, 26 Aug 2023 02:15:03 +0530 Subject: [PATCH] AP_Mount: move handling of all do-set-roi to AP_Mount library --- libraries/AP_Mount/AP_Mount.cpp | 16 ++++++++++++++++ libraries/AP_Mount/AP_Mount.h | 6 ++++++ 2 files changed, 22 insertions(+) diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index 1d2288b0da..7cbec7f675 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -499,6 +499,18 @@ void AP_Mount::handle_command_gimbal_manager_set_pitchyaw(const mavlink_message_ } } +MAV_RESULT AP_Mount::handle_command_do_set_roi_sysid(const mavlink_command_int_t &packet) +{ + set_target_sysid((uint8_t)packet.param1); + return MAV_RESULT_ACCEPTED; +} + +MAV_RESULT AP_Mount::handle_command_do_set_roi_none() +{ + set_mode_to_default(); + return MAV_RESULT_ACCEPTED; +} + MAV_RESULT AP_Mount::handle_command(const mavlink_command_int_t &packet, const mavlink_message_t &msg) { switch (packet.command) { @@ -510,6 +522,10 @@ MAV_RESULT AP_Mount::handle_command(const mavlink_command_int_t &packet, const m return handle_command_do_gimbal_manager_pitchyaw(packet); case MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE: return handle_command_do_gimbal_manager_configure(packet, msg); + case MAV_CMD_DO_SET_ROI_SYSID: + return handle_command_do_set_roi_sysid(packet); + case MAV_CMD_DO_SET_ROI_NONE: + return handle_command_do_set_roi_none(); default: return MAV_RESULT_UNSUPPORTED; } diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index 7030695728..462df7bac5 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -177,6 +177,12 @@ public: void set_target_sysid(uint8_t sysid) { set_target_sysid(_primary, sysid); } void set_target_sysid(uint8_t instance, uint8_t sysid); + // handling of set_roi_sysid message + MAV_RESULT handle_command_do_set_roi_sysid(const mavlink_command_int_t &packet); + + // handling of set_roi_none message + MAV_RESULT handle_command_do_set_roi_none(); + // mavlink message handling: MAV_RESULT handle_command(const mavlink_command_int_t &packet, const mavlink_message_t &msg); void handle_param_value(const mavlink_message_t &msg);