From e4428f538d4d6c15a48deaaa550769368cc8582c Mon Sep 17 00:00:00 2001 From: davidsastresas Date: Tue, 9 May 2023 21:25:33 +0900 Subject: [PATCH] GCS_MAVLink: support gimbal_manager_status and do_gimbal_manager_configure Co-authored-by: Randy Mackay --- libraries/GCS_MAVLink/GCS.h | 3 ++- libraries/GCS_MAVLink/GCS_Common.cpp | 36 ++++++++++++++++++++-------- libraries/GCS_MAVLink/ap_message.h | 1 + 3 files changed, 29 insertions(+), 11 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index c46ffffb43..c3645b863c 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -368,6 +368,7 @@ public: void send_vibration() const; void send_gimbal_device_attitude_status() const; void send_gimbal_manager_information() const; + void send_gimbal_manager_status() const; void send_named_float(const char *name, float value) const; void send_home_position() const; void send_gps_global_origin() const; @@ -638,7 +639,7 @@ protected: MAV_RESULT handle_command_do_set_roi_sysid(const mavlink_command_long_t &packet); MAV_RESULT handle_command_do_set_roi_none(); - virtual MAV_RESULT handle_command_mount(const mavlink_command_long_t &packet); + virtual MAV_RESULT handle_command_mount(const mavlink_command_long_t &packet, const mavlink_message_t &msg); MAV_RESULT handle_command_mag_cal(const mavlink_command_long_t &packet); virtual MAV_RESULT handle_command_long_packet(const mavlink_command_long_t &packet); MAV_RESULT handle_command_camera(const mavlink_command_long_t &packet); diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 0f8b9c9b6d..59641dd8ea 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -990,6 +990,7 @@ ap_message GCS_MAVLINK::mavlink_id_to_ap_message_id(const uint32_t mavlink_id) c { MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS, MSG_GIMBAL_DEVICE_ATTITUDE_STATUS}, { MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE, MSG_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE}, { MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION, MSG_GIMBAL_MANAGER_INFORMATION}, + { MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS, MSG_GIMBAL_MANAGER_STATUS}, #endif #if AP_OPTICALFLOW_ENABLED { MAVLINK_MSG_ID_OPTICAL_FLOW, MSG_OPTICAL_FLOW}, @@ -4540,14 +4541,14 @@ MAV_RESULT GCS_MAVLINK::handle_command_accelcal_vehicle_pos(const mavlink_comman } #endif // HAL_INS_ACCELCAL_ENABLED -MAV_RESULT GCS_MAVLINK::handle_command_mount(const mavlink_command_long_t &packet) +MAV_RESULT GCS_MAVLINK::handle_command_mount(const mavlink_command_long_t &packet, const mavlink_message_t &msg) { #if HAL_MOUNT_ENABLED AP_Mount *mount = AP::mount(); if (mount == nullptr) { return MAV_RESULT_UNSUPPORTED; } - return mount->handle_command_long(packet); + return mount->handle_command_long(packet, msg); #else return MAV_RESULT_UNSUPPORTED; #endif @@ -4678,12 +4679,6 @@ MAV_RESULT GCS_MAVLINK::handle_command_long_packet(const mavlink_command_long_t break; #endif - case MAV_CMD_DO_MOUNT_CONFIGURE: - case MAV_CMD_DO_MOUNT_CONTROL: - case MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW: - result = handle_command_mount(packet); - break; - #if AP_MAVLINK_MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES_ENABLED case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES: { result = handle_command_request_autopilot_capabilities(packet); @@ -4892,6 +4887,14 @@ void GCS_MAVLINK::handle_command_long(const mavlink_message_t &msg) result = handle_command_preflight_calibration(packet, msg); break; + case MAV_CMD_DO_MOUNT_CONFIGURE: + case MAV_CMD_DO_MOUNT_CONTROL: + case MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW: + case MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE: + // some mount commands require the source sysid and compid + result = handle_command_mount(packet, msg); + break; + default: result = handle_command_long_packet(packet); break; @@ -5334,9 +5337,7 @@ void GCS_MAVLINK::send_gimbal_device_attitude_status() const } mount->send_gimbal_device_attitude_status(chan); } -#endif -#if HAL_MOUNT_ENABLED void GCS_MAVLINK::send_gimbal_manager_information() const { AP_Mount *mount = AP::mount(); @@ -5345,6 +5346,15 @@ void GCS_MAVLINK::send_gimbal_manager_information() const } mount->send_gimbal_manager_information(chan); } + +void GCS_MAVLINK::send_gimbal_manager_status() const +{ + AP_Mount *mount = AP::mount(); + if (mount == nullptr) { + return; + } + mount->send_gimbal_manager_status(chan); +} #endif void GCS_MAVLINK::send_set_position_target_global_int(uint8_t target_system, uint8_t target_component, const Location& loc) @@ -5675,6 +5685,12 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id) #if HAL_MOUNT_ENABLED CHECK_PAYLOAD_SIZE(GIMBAL_MANAGER_INFORMATION); send_gimbal_manager_information(); +#endif + break; + case MSG_GIMBAL_MANAGER_STATUS: +#if HAL_MOUNT_ENABLED + CHECK_PAYLOAD_SIZE(GIMBAL_MANAGER_STATUS); + send_gimbal_manager_status(); #endif break; case MSG_OPTICAL_FLOW: diff --git a/libraries/GCS_MAVLink/ap_message.h b/libraries/GCS_MAVLink/ap_message.h index 9179c4b90b..c11d75f81e 100644 --- a/libraries/GCS_MAVLink/ap_message.h +++ b/libraries/GCS_MAVLink/ap_message.h @@ -51,6 +51,7 @@ enum ap_message : uint8_t { MSG_CAMERA_FEEDBACK, MSG_GIMBAL_DEVICE_ATTITUDE_STATUS, MSG_GIMBAL_MANAGER_INFORMATION, + MSG_GIMBAL_MANAGER_STATUS, MSG_OPTICAL_FLOW, MSG_MAG_CAL_PROGRESS, MSG_MAG_CAL_REPORT,