AP_Camera: Add capability to set VIDEO_STREAM_INFORMATION from Lua

ArduPilot · Aug 10, 2024 · cd73f5f · cd73f5f
1 parent 29acede
commit cd73f5f
Show file tree

Hide file tree

Showing 4 changed files with 79 additions and 0 deletions.
diff --git 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
@@ -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
@@ -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
@@ -112,8 +112,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
@@ -176,6 +182,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