AP_Camera: Load CAMERA_INFORMATION and VIDEO_STREAM_INFORMATION from Lua

Tools/scripts/build_options.py

@@ -143,6 +143,7 @@ def config_option(self):
     Feature('Camera', 'Camera_Solo', 'AP_CAMERA_SOLOGIMBAL_ENABLED', 'Enable Solo gimbal', 0, 'Camera'),
     Feature('Camera', 'Camera_FOV_Status', 'AP_CAMERA_SEND_FOV_STATUS_ENABLED', 'Enable GCS camera FOV status', 0, 'Camera,MOUNT'),  # noqa: E501
     Feature('Camera', 'Camera_ThermalRange', 'AP_CAMERA_SEND_THERMAL_RANGE_ENABLED', 'Enable GCS camera thermal range', 0, 'Camera,MOUNT'),  # noqa: E501
+    Feature('Camera', 'Camera_Info_From_Script', 'AP_CAMERA_INFO_FROM_SCRIPT_ENABLED', 'Enable Camera information messages via Lua script', 0, 'Camera,SCRIPTING'), # noqa
 
     Feature('Camera', 'RUNCAM', 'HAL_RUNCAM_ENABLED', 'Enable RunCam control', 0, None),
 
@@ -373,6 +374,7 @@ def config_option(self):
     Feature('MAVLink', 'MAVLINK_MSG_RC_CHANNELS_RAW', 'AP_MAVLINK_MSG_RC_CHANNELS_RAW_ENABLED', 'Enable RC_CHANNELS_RAW MAVLink messages', 0, None),  # noqa
     Feature('MAVLink', 'AP_MAVLINK_FTP_ENABLED', 'AP_MAVLINK_FTP_ENABLED', 'Enable MAVLink FTP protocol', 0, None),  # noqa
     Feature('MAVLink', 'MAV_CMD_SET_HAGL', 'AP_MAVLINK_MAV_CMD_SET_HAGL_ENABLED', 'Enable MAVLink HAGL command', 0, None),  # noqa
+    Feature('MAVLink', 'VIDEO_STREAM_INFORMATION', 'AP_MAVLINK_MSG_VIDEO_STREAM_INFORMATION_ENABLED', 'Enable MAVLink VIDEO_STREAM_INFORMATION message', 0, "Camera"), # noqa
 
     Feature('Developer', 'KILL_IMU', 'AP_INERTIALSENSOR_KILL_IMU_ENABLED', 'Allow IMUs to be disabled at runtime', 0, None),
     Feature('Developer', 'CRASHCATCHER', 'AP_CRASHDUMP_ENABLED', 'Enable CrashCatcher', 0, None),

Tools/scripts/extract_features.py

@@ -137,6 +137,7 @@ def __init__(self, filename, nm="arm-none-eabi-nm", strings="strings"):
             ('AP_CAMERA_{type}_ENABLED', 'AP_Camera_(?P<type>.*)::trigger_pic',),
             ('AP_CAMERA_SEND_FOV_STATUS_ENABLED', 'AP_Camera::send_camera_fov_status'),
             ('AP_CAMERA_SEND_THERMAL_RANGE_ENABLED', 'AP_Camera::send_camera_thermal_range'),
+            ('AP_CAMERA_INFO_FROM_SCRIPT_ENABLED', 'AP_Camera_Backend::set_camera_information'),
             ('HAL_RUNCAM_ENABLED', 'AP_RunCam::AP_RunCam',),
 
             ('HAL_PROXIMITY_ENABLED', 'AP_Proximity::AP_Proximity',),
@@ -254,6 +255,7 @@ def __init__(self, filename, nm="arm-none-eabi-nm", strings="strings"):
             ('AP_MAVLINK_MSG_RC_CHANNELS_RAW_ENABLED', r'GCS_MAVLINK::send_rc_channels_raw\b'),
             ('AP_MAVLINK_FTP_ENABLED', 'GCS_MAVLINK::ftp_worker'),
             ('AP_MAVLINK_MAV_CMD_SET_HAGL_ENABLED', 'Plane::handle_external_hagl'),
+            ('AP_MAVLINK_MSG_VIDEO_STREAM_INFORMATION_ENABLED', 'AP_Camera::send_video_stream_information'),
 
             ('AP_DRONECAN_HIMARK_SERVO_SUPPORT', 'AP_DroneCAN::SRV_send_himark'),
             ('AP_DRONECAN_HOBBYWING_ESC_SUPPORT', 'AP_DroneCAN::hobbywing_ESC_update'),

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)
 {
@@ -816,6 +837,55 @@ bool AP_Camera::change_setting(uint8_t instance, CameraSetting setting, float va
 
 #endif // #if AP_CAMERA_SCRIPTING_ENABLED
 
+
+#if AP_CAMERA_INFO_FROM_SCRIPT_ENABLED
+void AP_Camera::set_camera_information(mavlink_camera_information_t camera_info)
+{
+    WITH_SEMAPHORE(_rsem);
+
+    if (primary == nullptr) {
+        return;
+    }
+    return primary->set_camera_information(camera_info);
+}
+
+void AP_Camera::set_camera_information(uint8_t instance, mavlink_camera_information_t camera_info)
+{
+    WITH_SEMAPHORE(_rsem);
+
+    auto *backend = get_instance(instance);
+    if (backend == nullptr) {
+        return;
+    }
+
+    // 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
 AP_Camera_Backend *AP_Camera::get_instance(uint8_t instance) const
 {

libraries/AP_Camera/AP_Camera.h

@@ -186,6 +186,14 @@ class AP_Camera {
     bool change_setting(uint8_t instance, CameraSetting setting, float value);
 #endif
 
+#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
     bool get_legacy_relay_index(int8_t &index) const;
 
@@ -229,6 +237,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);
 

libraries/AP_Camera/AP_Camera_Backend.cpp

@@ -18,6 +18,17 @@ AP_Camera_Backend::AP_Camera_Backend(AP_Camera &frontend, AP_Camera_Params &para
     _instance(instance)
 {}
 
+void AP_Camera_Backend::init()
+{
+#if AP_CAMERA_INFO_FROM_SCRIPT_ENABLED
+    _camera_info.focal_length = NaNf;
+    _camera_info.sensor_size_h = NaNf;
+    _camera_info.sensor_size_v = NaNf;
+
+    _camera_info.flags = CAMERA_CAP_FLAGS_CAPTURE_IMAGE;
+#endif // AP_CAMERA_INFO_FROM_SCRIPT_ENABLED
+}
+
 // update - should be called at 50hz
 void AP_Camera_Backend::update()
 {
@@ -212,30 +223,61 @@ void AP_Camera_Backend::send_camera_feedback(mavlink_channel_t chan)
 // send camera information message to GCS
 void AP_Camera_Backend::send_camera_information(mavlink_channel_t chan) const
 {
-    // prepare vendor, model and cam definition strings
-    const uint8_t vendor_name[32] {};
-    const uint8_t model_name[32] {};
-    const char cam_definition_uri[140] {};
-    const uint32_t cap_flags = CAMERA_CAP_FLAGS_CAPTURE_IMAGE;
-
-    // send CAMERA_INFORMATION message
-    mavlink_msg_camera_information_send(
-        chan,
-        AP_HAL::millis(),       // time_boot_ms
-        vendor_name,            // vendor_name uint8_t[32]
-        model_name,             // model_name uint8_t[32]
-        0,                      // firmware version uint32_t
-        NaNf,                   // focal_length float (mm)
-        NaNf,                   // sensor_size_h float (mm)
-        NaNf,                   // sensor_size_v float (mm)
-        0,                      // resolution_h uint16_t (pix)
-        0,                      // resolution_v uint16_t (pix)
-        0,                      // lens_id, uint8_t
-        cap_flags,              // flags uint32_t (CAMERA_CAP_FLAGS)
-        0,                      // cam_definition_version uint16_t
-        cam_definition_uri,     // cam_definition_uri char[140]
-        get_gimbal_device_id());// gimbal_device_id uint8_t
+    mavlink_camera_information_t camera_info;
+#if AP_CAMERA_INFO_FROM_SCRIPT_ENABLED
+
+    camera_info = _camera_info;
+
+#else
+
+    memset(&camera_info, 0, sizeof(camera_info));
+
+    camera_info.focal_length = NaNf;
+    camera_info.sensor_size_h = NaNf;
+    camera_info.sensor_size_v = NaNf;
+
+    camera_info.flags = CAMERA_CAP_FLAGS_CAPTURE_IMAGE;
+
+#endif // AP_CAMERA_INFO_FROM_SCRIPT_ENABLED
+
+    // Set fixed fields
+    // lens_id is populated with the instance number, to disambiguate multiple cameras
+    camera_info.lens_id = _instance;
+    camera_info.gimbal_device_id = get_gimbal_device_id();
+    camera_info.time_boot_ms = AP_HAL::millis();
+
+    // Send CAMERA_INFORMATION message
+    mavlink_msg_camera_information_send_struct(chan, &camera_info);
+}
+
+#if AP_CAMERA_INFO_FROM_SCRIPT_ENABLED
+void AP_Camera_Backend::set_camera_information(mavlink_camera_information_t camera_info)
+{
+    GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Camera %u CAMERA_INFORMATION (%s %s) set from script", _instance, camera_info.vendor_name, camera_info.model_name);
+    _camera_info = camera_info;
+};
+#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

libraries/AP_Camera/AP_Camera_Backend.h

@@ -45,7 +45,7 @@ class AP_Camera_Backend
     }
 
     // init - performs any required initialisation
-    virtual void init() {};
+    virtual void init();
 
     // update - should be called at 50hz
     virtual void update();
@@ -115,6 +115,16 @@ 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
     virtual void send_camera_settings(mavlink_channel_t chan) const;
 
@@ -181,6 +191,11 @@ class AP_Camera_Backend
     // get mavlink gimbal device id which is normally mount_instance+1
     uint8_t get_gimbal_device_id() const;
 
+#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
     uint8_t _instance;      // this instance's number
     bool timer_installed;   // true if feedback pin change detected using timer

libraries/AP_Camera/AP_Camera_config.h

@@ -58,3 +58,7 @@
 #ifndef HAL_RUNCAM_ENABLED
 #define HAL_RUNCAM_ENABLED 1
 #endif
+
+#ifndef AP_CAMERA_INFO_FROM_SCRIPT_ENABLED
+#define AP_CAMERA_INFO_FROM_SCRIPT_ENABLED AP_CAMERA_SCRIPTING_ENABLED
+#endif

libraries/AP_HAL_ChibiOS/hwdef/include/minimize_common.inc

@@ -133,3 +133,7 @@ define AP_AIRSPEED_DRONECAN_ENABLED HAL_ENABLE_DRONECAN_DRIVERS
 define AP_MISSION_NAV_PAYLOAD_PLACE_ENABLED 0
 # don't need the payload place flight behaviour either:
 define AC_PAYLOAD_PLACE_ENABLED 0
+
+# disable extra camera messages
+define AP_CAMERA_INFO_FROM_SCRIPT_ENABLED 0
+define AP_MAVLINK_MSG_VIDEO_STREAM_INFORMATION_ENABLED 0