From 8a791d60824be7bc81693990b6bcff3656d0f2ba Mon Sep 17 00:00:00 2001 From: Asif Khan Date: Thu, 21 Sep 2023 11:38:00 +0900 Subject: [PATCH] GCS_MAVLink: add SEND_FOV_STATUS support --- libraries/GCS_MAVLink/GCS_Common.cpp | 13 +++++++++++++ libraries/GCS_MAVLink/ap_message.h | 1 + 2 files changed, 14 insertions(+) diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 46e6b3378e..0cff164fd2 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -998,6 +998,7 @@ ap_message GCS_MAVLINK::mavlink_id_to_ap_message_id(const uint32_t mavlink_id) c { MAVLINK_MSG_ID_CAMERA_FEEDBACK, MSG_CAMERA_FEEDBACK}, { MAVLINK_MSG_ID_CAMERA_INFORMATION, MSG_CAMERA_INFORMATION}, { MAVLINK_MSG_ID_CAMERA_SETTINGS, MSG_CAMERA_SETTINGS}, + { MAVLINK_MSG_ID_CAMERA_FOV_STATUS, MSG_CAMERA_FOV_STATUS}, #endif #if HAL_MOUNT_ENABLED { MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS, MSG_GIMBAL_DEVICE_ATTITUDE_STATUS}, @@ -5775,6 +5776,18 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id) camera->send_camera_settings(chan); } break; +#if AP_CAMERA_SEND_FOV_STATUS_ENABLED + case MSG_CAMERA_FOV_STATUS: + { + AP_Camera *camera = AP::camera(); + if (camera == nullptr) { + break; + } + CHECK_PAYLOAD_SIZE(CAMERA_FOV_STATUS); + camera->send_camera_fov_status(chan); + } + break; +#endif #endif case MSG_SYSTEM_TIME: diff --git a/libraries/GCS_MAVLink/ap_message.h b/libraries/GCS_MAVLink/ap_message.h index 6b4fbb0e29..3a707bc462 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_CAMERA_INFORMATION, MSG_CAMERA_SETTINGS, + MSG_CAMERA_FOV_STATUS, MSG_GIMBAL_DEVICE_ATTITUDE_STATUS, MSG_GIMBAL_MANAGER_INFORMATION, MSG_GIMBAL_MANAGER_STATUS,