diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp index 66ee16045d..288b0506bb 100644 --- a/libraries/AP_Camera/AP_Camera.cpp +++ b/libraries/AP_Camera/AP_Camera.cpp @@ -467,6 +467,12 @@ bool AP_Camera::send_mavlink_message(GCS_MAVLINK &link, const enum ap_message ms send_camera_thermal_range(chan); break; #endif +#if AP_MAVLINK_MSG_VIDEO_STREAM_INFORMATION_ENABLED + case MSG_VIDEO_STREAM_INFORMATION: + CHECK_PAYLOAD_SIZE2(VIDEO_STREAM_INFORMATION); + send_video_stream_information(chan); + break; +#endif // AP_MAVLINK_MSG_VIDEO_STREAM_INFORMATION_ENABLED default: // should not reach this; should only be called for specific IDs @@ -580,6 +586,21 @@ void AP_Camera::send_camera_information(mavlink_channel_t chan) } } +#if AP_MAVLINK_MSG_VIDEO_STREAM_INFORMATION_ENABLED +// send video stream information message to GCS +void AP_Camera::send_video_stream_information(mavlink_channel_t chan) +{ + WITH_SEMAPHORE(_rsem); + + // call each instance + for (uint8_t instance = 0; instance < AP_CAMERA_MAX_INSTANCES; instance++) { + if (_backends[instance] != nullptr) { + _backends[instance]->send_video_stream_information(chan); + } + } +} +#endif // AP_MAVLINK_MSG_VIDEO_STREAM_INFORMATION_ENABLED + // send camera settings message to GCS void AP_Camera::send_camera_settings(mavlink_channel_t chan) { @@ -840,6 +861,29 @@ void AP_Camera::set_camera_information(uint8_t instance, mavlink_camera_informat // call instance backend->set_camera_information(camera_info); } + +void AP_Camera::set_stream_information(mavlink_video_stream_information_t stream_info) +{ + WITH_SEMAPHORE(_rsem); + + if (primary == nullptr) { + return; + } + return primary->set_stream_information(stream_info); +} + +void AP_Camera::set_stream_information(uint8_t instance, mavlink_video_stream_information_t stream_info) +{ + WITH_SEMAPHORE(_rsem); + + auto *backend = get_instance(instance); + if (backend == nullptr) { + return; + } + + // call instance + backend->set_stream_information(stream_info); +} #endif // AP_CAMERA_INFO_FROM_SCRIPT_ENABLED // return backend for instance number diff --git a/libraries/AP_Camera/AP_Camera.h b/libraries/AP_Camera/AP_Camera.h index f56fe40ff3..a7b7f602ac 100644 --- a/libraries/AP_Camera/AP_Camera.h +++ b/libraries/AP_Camera/AP_Camera.h @@ -189,6 +189,9 @@ public: #if AP_CAMERA_INFO_FROM_SCRIPT_ENABLED void set_camera_information(mavlink_camera_information_t camera_info); void set_camera_information(uint8_t instance, mavlink_camera_information_t camera_info); + + void set_stream_information(mavlink_video_stream_information_t camera_info); + void set_stream_information(uint8_t instance, mavlink_video_stream_information_t camera_info); #endif // AP_CAMERA_INFO_FROM_SCRIPT_ENABLED // Return true and the relay index if relay camera backend is selected, used for conversion to relay functions @@ -234,6 +237,10 @@ private: // send camera information message to GCS void send_camera_information(mavlink_channel_t chan); +#if AP_MAVLINK_MSG_VIDEO_STREAM_INFORMATION_ENABLED + void send_video_stream_information(mavlink_channel_t chan); +#endif // AP_MAVLINK_MSG_VIDEO_STREAM_INFORMATION_ENABLED + // send camera settings message to GCS void send_camera_settings(mavlink_channel_t chan); diff --git a/libraries/AP_Camera/AP_Camera_Backend.cpp b/libraries/AP_Camera/AP_Camera_Backend.cpp index 14be8e7b40..c2d5868c10 100644 --- a/libraries/AP_Camera/AP_Camera_Backend.cpp +++ b/libraries/AP_Camera/AP_Camera_Backend.cpp @@ -258,6 +258,27 @@ void AP_Camera_Backend::set_camera_information(mavlink_camera_information_t came }; #endif // AP_CAMERA_INFO_FROM_SCRIPT_ENABLED +#if AP_MAVLINK_MSG_VIDEO_STREAM_INFORMATION_ENABLED +// send video stream information message to GCS +void AP_Camera_Backend::send_video_stream_information(mavlink_channel_t chan) const +{ +#if AP_CAMERA_INFO_FROM_SCRIPT_ENABLED + + // Send VIDEO_STREAM_INFORMATION message + mavlink_msg_video_stream_information_send_struct(chan, &_stream_info); + +#endif // AP_CAMERA_INFO_FROM_SCRIPT_ENABLED +} +#endif // AP_MAVLINK_MSG_VIDEO_STREAM_INFORMATION_ENABLED + +#if AP_CAMERA_INFO_FROM_SCRIPT_ENABLED +void AP_Camera_Backend::set_stream_information(mavlink_video_stream_information_t stream_info) +{ + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Camera %u VIDEO_STREAM_INFORMATION (%s) set from script", _instance, stream_info.name); + _stream_info = stream_info; +}; +#endif // AP_CAMERA_INFO_FROM_SCRIPT_ENABLED + // send camera settings message to GCS void AP_Camera_Backend::send_camera_settings(mavlink_channel_t chan) const { diff --git a/libraries/AP_Camera/AP_Camera_Backend.h b/libraries/AP_Camera/AP_Camera_Backend.h index 37e07f375a..7d52b6831f 100644 --- a/libraries/AP_Camera/AP_Camera_Backend.h +++ b/libraries/AP_Camera/AP_Camera_Backend.h @@ -115,8 +115,14 @@ public: // send camera information message to GCS virtual void send_camera_information(mavlink_channel_t chan) const; +#if AP_MAVLINK_MSG_VIDEO_STREAM_INFORMATION_ENABLED + // send video stream information message to GCS + virtual void send_video_stream_information(mavlink_channel_t chan) const; +#endif // AP_MAVLINK_MSG_VIDEO_STREAM_INFORMATION_ENABLED + #if AP_CAMERA_INFO_FROM_SCRIPT_ENABLED void set_camera_information(mavlink_camera_information_t camera_info); + void set_stream_information(mavlink_video_stream_information_t stream_info); #endif // AP_CAMERA_INFO_FROM_SCRIPT_ENABLED // send camera settings message to GCS @@ -187,6 +193,7 @@ protected: #if AP_CAMERA_INFO_FROM_SCRIPT_ENABLED mavlink_camera_information_t _camera_info; + mavlink_video_stream_information_t _stream_info; #endif // AP_CAMERA_INFO_FROM_SCRIPT_ENABLED // internal members