From 1e50fa5357fc8d0002718627ebc29abe8b3ef99f Mon Sep 17 00:00:00 2001
From: Nick Exton <NExton@sypaq.com.au>
Date: Thu, 8 Aug 2024 17:42:01 +1000
Subject: [PATCH] AP_Camera: Add capability to set VIDEO_STREAM_INFORMATION
 from Lua

---
 libraries/AP_Camera/AP_Camera.cpp         | 44 +++++++++++++++++++++++
 libraries/AP_Camera/AP_Camera.h           |  7 ++++
 libraries/AP_Camera/AP_Camera_Backend.cpp | 21 +++++++++++
 libraries/AP_Camera/AP_Camera_Backend.h   |  7 ++++
 4 files changed, 79 insertions(+)

diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp
index 506da611ab29db..ea31391d7e1705 100644
--- a/libraries/AP_Camera/AP_Camera.cpp
+++ b/libraries/AP_Camera/AP_Camera.cpp
@@ -461,6 +461,12 @@ bool AP_Camera::send_mavlink_message(GCS_MAVLINK &link, const enum ap_message ms
         CHECK_PAYLOAD_SIZE2(CAMERA_CAPTURE_STATUS);
         send_camera_capture_status(chan);
         break;
+#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
@@ -574,6 +580,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)
 {
@@ -806,6 +827,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 3c09f3bda9a6e2..4028bb28af9709 100644
--- a/libraries/AP_Camera/AP_Camera.h
+++ b/libraries/AP_Camera/AP_Camera.h
@@ -186,6 +186,9 @@ class AP_Camera {
 #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
@@ -231,6 +234,10 @@ class AP_Camera {
     // 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 14299b7077adf2..7c74caf3c1078c 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 5cb62947a0ade4..ba15a6b9686e73 100644
--- a/libraries/AP_Camera/AP_Camera_Backend.h
+++ b/libraries/AP_Camera/AP_Camera_Backend.h
@@ -115,8 +115,14 @@ class AP_Camera_Backend
     // 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
@@ -179,6 +185,7 @@ class AP_Camera_Backend
 
 #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