diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index bd3027072d..75ae487b11 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -1060,7 +1060,10 @@ ap_message GCS_MAVLINK::mavlink_id_to_ap_message_id(const uint32_t mavlink_id) c { MAVLINK_MSG_ID_CAMERA_SETTINGS, MSG_CAMERA_SETTINGS}, { MAVLINK_MSG_ID_CAMERA_FOV_STATUS, MSG_CAMERA_FOV_STATUS}, { MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS, MSG_CAMERA_CAPTURE_STATUS}, -#endif +#if AP_CAMERA_SEND_THERMAL_RANGE_ENABLED + { MAVLINK_MSG_ID_CAMERA_THERMAL_RANGE, MSG_CAMERA_THERMAL_RANGE}, +#endif // AP_CAMERA_SEND_THERMAL_RANGE_ENABLED +#endif // AP_CAMERA_ENABLED #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}, @@ -6131,6 +6134,9 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id) case MSG_CAMERA_FOV_STATUS: #endif case MSG_CAMERA_CAPTURE_STATUS: +#if AP_CAMERA_SEND_THERMAL_RANGE_ENABLED + case MSG_CAMERA_THERMAL_RANGE: +#endif { AP_Camera *camera = AP::camera(); if (camera == nullptr) { diff --git a/libraries/GCS_MAVLink/ap_message.h b/libraries/GCS_MAVLink/ap_message.h index 4923317266..d041127dfa 100644 --- a/libraries/GCS_MAVLink/ap_message.h +++ b/libraries/GCS_MAVLink/ap_message.h @@ -57,6 +57,7 @@ enum ap_message : uint8_t { MSG_CAMERA_SETTINGS, MSG_CAMERA_FOV_STATUS, MSG_CAMERA_CAPTURE_STATUS, + MSG_CAMERA_THERMAL_RANGE, MSG_GIMBAL_DEVICE_ATTITUDE_STATUS, MSG_GIMBAL_MANAGER_INFORMATION, MSG_GIMBAL_MANAGER_STATUS,