diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 5a358a6b4b..0ae1e3ce79 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -986,6 +986,8 @@ ap_message GCS_MAVLINK::mavlink_id_to_ap_message_id(const uint32_t mavlink_id) c { MAVLINK_MSG_ID_BATTERY2, MSG_BATTERY2}, #endif { MAVLINK_MSG_ID_CAMERA_FEEDBACK, MSG_CAMERA_FEEDBACK}, + { MAVLINK_MSG_ID_CAMERA_INFORMATION, MSG_CAMERA_INFORMATION}, + { MAVLINK_MSG_ID_CAMERA_SETTINGS, MSG_CAMERA_SETTINGS}, #if HAL_MOUNT_ENABLED { 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}, @@ -5689,6 +5691,26 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id) camera->send_feedback(chan); } break; + case MSG_CAMERA_INFORMATION: + { + AP_Camera *camera = AP::camera(); + if (camera == nullptr) { + break; + } + CHECK_PAYLOAD_SIZE(CAMERA_INFORMATION); + camera->send_camera_information(chan); + } + break; + case MSG_CAMERA_SETTINGS: + { + AP_Camera *camera = AP::camera(); + if (camera == nullptr) { + break; + } + CHECK_PAYLOAD_SIZE(CAMERA_INFORMATION); + camera->send_camera_settings(chan); + } + break; #endif case MSG_SYSTEM_TIME: diff --git a/libraries/GCS_MAVLink/ap_message.h b/libraries/GCS_MAVLink/ap_message.h index c11d75f81e..271a009e58 100644 --- a/libraries/GCS_MAVLink/ap_message.h +++ b/libraries/GCS_MAVLink/ap_message.h @@ -49,6 +49,8 @@ enum ap_message : uint8_t { MSG_TERRAIN, MSG_BATTERY2, MSG_CAMERA_FEEDBACK, + MSG_CAMERA_INFORMATION, + MSG_CAMERA_SETTINGS, MSG_GIMBAL_DEVICE_ATTITUDE_STATUS, MSG_GIMBAL_MANAGER_INFORMATION, MSG_GIMBAL_MANAGER_STATUS,