From f892d1a688b7272c6f13b56b2a8dfb2179cf9020 Mon Sep 17 00:00:00 2001 From: Nick Exton Date: Thu, 8 Aug 2024 16:02:42 +1000 Subject: [PATCH 1/9] AP_Camera: Add capability to set CAMERA_INFORMATION from Lua --- libraries/AP_Camera/AP_Camera.cpp | 26 +++++++++ libraries/AP_Camera/AP_Camera.h | 5 ++ libraries/AP_Camera/AP_Camera_Backend.cpp | 67 +++++++++++++++-------- libraries/AP_Camera/AP_Camera_Backend.h | 10 +++- libraries/AP_Camera/AP_Camera_config.h | 4 ++ 5 files changed, 88 insertions(+), 24 deletions(-) diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp index 06a1147dc2ada..66ee16045d15d 100644 --- a/libraries/AP_Camera/AP_Camera.cpp +++ b/libraries/AP_Camera/AP_Camera.cpp @@ -816,6 +816,32 @@ 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); +} +#endif // AP_CAMERA_INFO_FROM_SCRIPT_ENABLED + // return backend for instance number AP_Camera_Backend *AP_Camera::get_instance(uint8_t instance) const { diff --git a/libraries/AP_Camera/AP_Camera.h b/libraries/AP_Camera/AP_Camera.h index 37f33942273fe..f56fe40ff3142 100644 --- a/libraries/AP_Camera/AP_Camera.h +++ b/libraries/AP_Camera/AP_Camera.h @@ -186,6 +186,11 @@ 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); +#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; diff --git a/libraries/AP_Camera/AP_Camera_Backend.cpp b/libraries/AP_Camera/AP_Camera_Backend.cpp index 190a36ad6c709..14be8e7b40843 100644 --- a/libraries/AP_Camera/AP_Camera_Backend.cpp +++ b/libraries/AP_Camera/AP_Camera_Backend.cpp @@ -18,6 +18,17 @@ AP_Camera_Backend::AP_Camera_Backend(AP_Camera &frontend, AP_Camera_Params ¶ _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,31 +223,41 @@ 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 + // 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 c9b55949e61db..37e07f375a045 100644 --- a/libraries/AP_Camera/AP_Camera_Backend.h +++ b/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,10 @@ class AP_Camera_Backend // send camera information message to GCS virtual void send_camera_information(mavlink_channel_t chan) const; +#if AP_CAMERA_INFO_FROM_SCRIPT_ENABLED + void set_camera_information(mavlink_camera_information_t camera_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 +185,10 @@ 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; +#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 diff --git a/libraries/AP_Camera/AP_Camera_config.h b/libraries/AP_Camera/AP_Camera_config.h index 6ff54ec978b57..90f69ea7da44c 100644 --- a/libraries/AP_Camera/AP_Camera_config.h +++ b/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 From f42e2fc26a64b817eab5cbbdc29d34f683217efc Mon Sep 17 00:00:00 2001 From: Nick Exton Date: Fri, 9 Aug 2024 08:40:42 +1000 Subject: [PATCH 2/9] AP_HAL_ChibiOS: Disable CAMERA_INFO_FROM_SCRIPT in minimize_common.inc --- libraries/AP_HAL_ChibiOS/hwdef/include/minimize_common.inc | 3 +++ 1 file changed, 3 insertions(+) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/include/minimize_common.inc b/libraries/AP_HAL_ChibiOS/hwdef/include/minimize_common.inc index 91bd8acd8b000..e075cf78f1275 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/include/minimize_common.inc +++ b/libraries/AP_HAL_ChibiOS/hwdef/include/minimize_common.inc @@ -133,3 +133,6 @@ 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 From 812b73dd615461e20916288f4c67cab8f3ad5505 Mon Sep 17 00:00:00 2001 From: Nick Exton Date: Fri, 9 Aug 2024 09:55:23 +1000 Subject: [PATCH 3/9] Tools: Add CAMERA_INFO_FROM_SCRIPT guard to build_options.py --- Tools/scripts/build_options.py | 1 + Tools/scripts/extract_features.py | 1 + 2 files changed, 2 insertions(+) diff --git a/Tools/scripts/build_options.py b/Tools/scripts/build_options.py index c975752aa4a2a..081862880f92a 100644 --- a/Tools/scripts/build_options.py +++ b/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), diff --git a/Tools/scripts/extract_features.py b/Tools/scripts/extract_features.py index f71c3eea7712f..2c7ea1be9ec2d 100755 --- a/Tools/scripts/extract_features.py +++ b/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.*)::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',), From aa5acb847af02bc1c70ff9303e9cdad8c3fa3762 Mon Sep 17 00:00:00 2001 From: Nick Exton Date: Thu, 8 Aug 2024 16:03:22 +1000 Subject: [PATCH 4/9] AP_Scripting: Add binding for AP_Camera::set_camera_information() --- libraries/AP_Scripting/docs/docs.lua | 130 ++++++++++++++++++ .../examples/set_CAMERA_INFORMATION.lua | 38 +++++ .../generator/description/bindings.desc | 18 +++ 3 files changed, 186 insertions(+) create mode 100644 libraries/AP_Scripting/examples/set_CAMERA_INFORMATION.lua diff --git a/libraries/AP_Scripting/docs/docs.lua b/libraries/AP_Scripting/docs/docs.lua index 72de9a5b7e4f4..a375ede3bc63f 100644 --- a/libraries/AP_Scripting/docs/docs.lua +++ b/libraries/AP_Scripting/docs/docs.lua @@ -1419,6 +1419,136 @@ function camera:get_state(instance) end ---@return boolean function camera:change_setting(instance, setting, value) end +-- The MAVLink CAMERA_INFORMATION message struct +---@class (exact) mavlink_camera_information_t_ud +local mavlink_camera_information_t_ud = {} + +---@return mavlink_camera_information_t_ud +function mavlink_camera_information_t() end + +-- get field +---@return uint32_t_ud +function mavlink_camera_information_t_ud:time_boot_ms() end + +-- set field +---@param value uint32_t_ud|integer|number +function mavlink_camera_information_t_ud:time_boot_ms(value) end + +-- get field +---@return uint32_t_ud +function mavlink_camera_information_t_ud:firmware_version() end + + -- set field +---@param value uint32_t_ud|integer|number +function mavlink_camera_information_t_ud:firmware_version(value) end + +-- get field +---@return number +function mavlink_camera_information_t_ud:focal_length() end + + -- set field +---@param value number +function mavlink_camera_information_t_ud:focal_length(value) end + +-- get field +---@return number +function mavlink_camera_information_t_ud:sensor_size_h() end + + -- set field +---@param value number +function mavlink_camera_information_t_ud:sensor_size_h(value) end + +-- get field +---@return number +function mavlink_camera_information_t_ud:sensor_size_v() end + + -- set field +---@param value number +function mavlink_camera_information_t_ud:sensor_size_v(value) end + +-- get field +---@return uint32_t_ud +function mavlink_camera_information_t_ud:flags() end + + -- set field +---@param value uint32_t_ud|integer|number +function mavlink_camera_information_t_ud:flags(value) end + +-- get field +---@return integer +function mavlink_camera_information_t_ud:resolution_h() end + + -- set field +---@param value integer +function mavlink_camera_information_t_ud:resolution_h(value) end + +-- get field +---@return integer +function mavlink_camera_information_t_ud:resolution_v() end + + -- set field +---@param value integer +function mavlink_camera_information_t_ud:resolution_v(value) end + +-- get field +---@return integer +function mavlink_camera_information_t_ud:cam_definition_version() end + + -- set field +---@param value integer +function mavlink_camera_information_t_ud:cam_definition_version(value) end + +-- get array field +---@param index integer +---@return integer +function mavlink_camera_information_t_ud:vendor_name(index) end + +-- set array field +---@param index integer +---@param value integer +function mavlink_camera_information_t_ud:vendor_name(index, value) end + +-- get array field +---@param index integer +---@return integer +function mavlink_camera_information_t_ud:model_name(index) end + +-- set array field +---@param index integer +---@param value integer +function mavlink_camera_information_t_ud:model_name(index, value) end + +-- get field +---@return integer +function mavlink_camera_information_t_ud:lens_id() end + + -- set field +---@param value integer +function mavlink_camera_information_t_ud:lens_id(value) end + +-- get array field +---@param index integer +---@return integer +function mavlink_camera_information_t_ud:cam_definition_uri(index) end + +-- set array field +---@param index integer +---@param value integer +function mavlink_camera_information_t_ud:cam_definition_uri(index, value) end + +-- get field +---@return integer +function mavlink_camera_information_t_ud:gimbal_device_id() end + + -- set field +---@param value integer +function mavlink_camera_information_t_ud:gimbal_device_id(value) end + +-- Populate the fields of the CAMERA_INFORMATION message +---@param instance integer +---@param cam_info mavlink_camera_information_t_ud +function camera:set_camera_information(instance, cam_info) end + -- desc mount = {} diff --git a/libraries/AP_Scripting/examples/set_CAMERA_INFORMATION.lua b/libraries/AP_Scripting/examples/set_CAMERA_INFORMATION.lua new file mode 100644 index 0000000000000..76ec4ccd38e0a --- /dev/null +++ b/libraries/AP_Scripting/examples/set_CAMERA_INFORMATION.lua @@ -0,0 +1,38 @@ + --[[ + Populate the fields of the CAMERA_INFORMATION message sent by the selected camera instance. + --]] + function set_camera_information() + -- set the Camera Information data + local cam_info = mavlink_camera_information_t() + + local INSTANCE = 0 + local vendor_name = 'Unknown' + local model_name = 'Camera' + local uri = '' + + -- "time_boot_ms" is populated automatically by the camera backend + for i = 0, #vendor_name do + cam_info:vendor_name(i, vendor_name:byte(i+1)) + end + for i = 0, #model_name do + cam_info:model_name(i, model_name:byte(i+1)) + end + cam_info:firmware_version(0) + cam_info:focal_length(1.6) + cam_info:sensor_size_h(3840) + cam_info:sensor_size_v(2160) + cam_info:resolution_h(1920) + cam_info:resolution_v(1080) + -- "lens_id" is populated automatically by the camera backend + cam_info:flags(256) -- CAMERA_CAP_FLAGS_HAS_VIDEO_STREAM + cam_info:cam_definition_version(0) + for i = 0, #uri do + cam_info:cam_definition_uri(i, uri:byte(i+1)) + end + -- "gimbal_device_id" is populated automatically by the camera backend + + camera:set_camera_information(INSTANCE, cam_info) + +end + +return set_camera_information() diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index 6c6d9ba5f8202..cde3185702ff8 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -778,6 +778,22 @@ singleton AP_Mount method get_angle_target boolean uint8_t'skip_check float'Null singleton AP_Mount method get_location_target boolean uint8_t'skip_check Location'Null singleton AP_Mount method set_attitude_euler void uint8_t'skip_check float'skip_check float'skip_check float'skip_check +userdata mavlink_camera_information_t depends AP_CAMERA_INFO_FROM_SCRIPT_ENABLED +userdata mavlink_camera_information_t field time_boot_ms uint32_t'skip_check read write +userdata mavlink_camera_information_t field firmware_version uint32_t'skip_check read write +userdata mavlink_camera_information_t field focal_length float'skip_check read write +userdata mavlink_camera_information_t field sensor_size_h float'skip_check read write +userdata mavlink_camera_information_t field sensor_size_v float'skip_check read write +userdata mavlink_camera_information_t field flags uint32_t'skip_check read write +userdata mavlink_camera_information_t field resolution_h uint16_t'skip_check read write +userdata mavlink_camera_information_t field resolution_v uint16_t'skip_check read write +userdata mavlink_camera_information_t field cam_definition_version uint16_t'skip_check read write +userdata mavlink_camera_information_t field vendor_name'array 32 uint8_t'skip_check read write +userdata mavlink_camera_information_t field model_name'array 32 uint8_t'skip_check read write +userdata mavlink_camera_information_t field lens_id uint8_t'skip_check read write +userdata mavlink_camera_information_t field cam_definition_uri'array 140 uint8_t'skip_check read write +userdata mavlink_camera_information_t field gimbal_device_id uint8_t'skip_check read write + include AP_Camera/AP_Camera.h singleton AP_Camera depends AP_CAMERA_ENABLED && (AP_CAMERA_SCRIPTING_ENABLED == 1) singleton AP_Camera rename camera @@ -797,6 +813,8 @@ userdata AP_Camera::camera_state_t field tracking_p1 Vector2f read userdata AP_Camera::camera_state_t field tracking_p2 Vector2f read singleton AP_Camera method get_state boolean uint8_t'skip_check AP_Camera::camera_state_t'Null singleton AP_Camera method change_setting boolean uint8_t'skip_check CameraSetting'enum CameraSetting::THERMAL_PALETTE CameraSetting::THERMAL_RAW_DATA float'skip_check +singleton AP_Camera method set_camera_information void uint8_t'skip_check mavlink_camera_information_t +singleton AP_Camera method set_camera_information depends AP_CAMERA_INFO_FROM_SCRIPT_ENABLED include AP_Winch/AP_Winch.h singleton AP_Winch depends AP_WINCH_ENABLED && APM_BUILD_COPTER_OR_HELI From 23070e4b4518311b1f7ea66c9853bb5e2f3d7d04 Mon Sep 17 00:00:00 2001 From: Nick Exton Date: Thu, 8 Aug 2024 17:21:16 +1000 Subject: [PATCH 5/9] GCS_MAVLink: Add handler for VIDEO_STREAM_INFORMATION request --- libraries/GCS_MAVLink/GCS_Common.cpp | 6 ++++++ libraries/GCS_MAVLink/GCS_config.h | 4 ++++ libraries/GCS_MAVLink/ap_message.h | 1 + 3 files changed, 11 insertions(+) diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 05c2d8f85c81e..a564972a67421 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -1063,6 +1063,9 @@ ap_message GCS_MAVLINK::mavlink_id_to_ap_message_id(const uint32_t mavlink_id) c #if AP_CAMERA_SEND_THERMAL_RANGE_ENABLED { MAVLINK_MSG_ID_CAMERA_THERMAL_RANGE, MSG_CAMERA_THERMAL_RANGE}, #endif // AP_CAMERA_SEND_THERMAL_RANGE_ENABLED +#if AP_MAVLINK_MSG_VIDEO_STREAM_INFORMATION_ENABLED + { MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION, MSG_VIDEO_STREAM_INFORMATION}, +#endif // AP_MAVLINK_MSG_VIDEO_STREAM_INFORMATION_ENABLED #endif // AP_CAMERA_ENABLED #if HAL_MOUNT_ENABLED { MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS, MSG_GIMBAL_DEVICE_ATTITUDE_STATUS}, @@ -6189,6 +6192,9 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id) #if AP_CAMERA_SEND_THERMAL_RANGE_ENABLED case MSG_CAMERA_THERMAL_RANGE: #endif +#if AP_MAVLINK_MSG_VIDEO_STREAM_INFORMATION_ENABLED + case MSG_VIDEO_STREAM_INFORMATION: +#endif // AP_MAVLINK_MSG_VIDEO_STREAM_INFORMATION_ENABLED { AP_Camera *camera = AP::camera(); if (camera == nullptr) { diff --git a/libraries/GCS_MAVLink/GCS_config.h b/libraries/GCS_MAVLink/GCS_config.h index 0bf028f64f6df..5f059cb9a1042 100644 --- a/libraries/GCS_MAVLink/GCS_config.h +++ b/libraries/GCS_MAVLink/GCS_config.h @@ -135,3 +135,7 @@ #ifndef AP_MAVLINK_MAV_CMD_SET_HAGL_ENABLED #define AP_MAVLINK_MAV_CMD_SET_HAGL_ENABLED (BOARD_FLASH_SIZE > 1024) #endif + +#ifndef AP_MAVLINK_MSG_VIDEO_STREAM_INFORMATION_ENABLED +#define AP_MAVLINK_MSG_VIDEO_STREAM_INFORMATION_ENABLED HAL_GCS_ENABLED +#endif diff --git a/libraries/GCS_MAVLink/ap_message.h b/libraries/GCS_MAVLink/ap_message.h index 347e85d797dc0..570c708f682a7 100644 --- a/libraries/GCS_MAVLink/ap_message.h +++ b/libraries/GCS_MAVLink/ap_message.h @@ -63,6 +63,7 @@ enum ap_message : uint8_t { MSG_GIMBAL_DEVICE_ATTITUDE_STATUS, MSG_GIMBAL_MANAGER_INFORMATION, MSG_GIMBAL_MANAGER_STATUS, + MSG_VIDEO_STREAM_INFORMATION, MSG_OPTICAL_FLOW, MSG_MAG_CAL_PROGRESS, MSG_MAG_CAL_REPORT, From a144bf9e18d68b6371be8cc116c6721d2b9a411e Mon Sep 17 00:00:00 2001 From: Nick Exton Date: Fri, 9 Aug 2024 08:41:06 +1000 Subject: [PATCH 6/9] AP_HAL_ChibiOS: Disable VIDEO_STREAM_INFORMATION in minimize_common.inc --- libraries/AP_HAL_ChibiOS/hwdef/include/minimize_common.inc | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/include/minimize_common.inc b/libraries/AP_HAL_ChibiOS/hwdef/include/minimize_common.inc index e075cf78f1275..005fb89895002 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/include/minimize_common.inc +++ b/libraries/AP_HAL_ChibiOS/hwdef/include/minimize_common.inc @@ -136,3 +136,4 @@ 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 From a0e95258d2b8457279a79448a7138fcfc927b570 Mon Sep 17 00:00:00 2001 From: Nick Exton Date: Fri, 9 Aug 2024 09:55:36 +1000 Subject: [PATCH 7/9] Tools: Add VIDEO_STREAM_INFORMATION guard to build_options.py --- Tools/scripts/build_options.py | 1 + Tools/scripts/extract_features.py | 1 + 2 files changed, 2 insertions(+) diff --git a/Tools/scripts/build_options.py b/Tools/scripts/build_options.py index 081862880f92a..3f5dd534ff8f1 100644 --- a/Tools/scripts/build_options.py +++ b/Tools/scripts/build_options.py @@ -374,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), diff --git a/Tools/scripts/extract_features.py b/Tools/scripts/extract_features.py index 2c7ea1be9ec2d..eec1399c02a3b 100755 --- a/Tools/scripts/extract_features.py +++ b/Tools/scripts/extract_features.py @@ -255,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'), From 9781b0e9c4fd0fe0e3a9f7bd3eab1c369ea833b6 Mon Sep 17 00:00:00 2001 From: Nick Exton Date: Thu, 8 Aug 2024 17:42:01 +1000 Subject: [PATCH 8/9] 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 66ee16045d15d..288b0506bb54e 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 f56fe40ff3142..a7b7f602ac43e 100644 --- a/libraries/AP_Camera/AP_Camera.h +++ b/libraries/AP_Camera/AP_Camera.h @@ -189,6 +189,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 @@ -234,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); diff --git a/libraries/AP_Camera/AP_Camera_Backend.cpp b/libraries/AP_Camera/AP_Camera_Backend.cpp index 14be8e7b40843..c2d5868c10aca 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 37e07f375a045..7d52b6831f9a9 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 @@ -187,6 +193,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 From d5b0a7cf56ed8c6f4a6e049388af98e0e0dbed7c Mon Sep 17 00:00:00 2001 From: Nick Exton Date: Thu, 8 Aug 2024 17:02:37 +1000 Subject: [PATCH 9/9] AP_Scripting: Add binding for AP_Camera::set_stream_information() --- libraries/AP_Scripting/docs/docs.lua | 112 ++++++++++++++++++ .../examples/set_VIDEO_STREAM_INFORMATION.lua | 33 ++++++ .../generator/description/bindings.desc | 16 +++ 3 files changed, 161 insertions(+) create mode 100644 libraries/AP_Scripting/examples/set_VIDEO_STREAM_INFORMATION.lua diff --git a/libraries/AP_Scripting/docs/docs.lua b/libraries/AP_Scripting/docs/docs.lua index a375ede3bc63f..de2796c57cbe6 100644 --- a/libraries/AP_Scripting/docs/docs.lua +++ b/libraries/AP_Scripting/docs/docs.lua @@ -1549,6 +1549,118 @@ function mavlink_camera_information_t_ud:gimbal_device_id(value) end ---@param cam_info mavlink_camera_information_t_ud function camera:set_camera_information(instance, cam_info) end +-- The MAVLink VIDEO_STREAM_INFORMATION message struct +---@class (exact) mavlink_video_stream_information_t_ud +local mavlink_video_stream_information_t_ud = {} + +---@return mavlink_video_stream_information_t_ud +function mavlink_video_stream_information_t() end + +-- get field +---@return number +function mavlink_video_stream_information_t_ud:framerate() end + +-- set field +---@param value number +function mavlink_video_stream_information_t_ud:framerate(value) end + +-- get field +---@return uint32_t_ud +function mavlink_video_stream_information_t_ud:bitrate() end + +-- set field +---@param value uint32_t_ud|integer|number +function mavlink_video_stream_information_t_ud:bitrate(value) end + +-- get field +---@return integer +function mavlink_video_stream_information_t_ud:flags() end + +-- set field +---@param value integer +function mavlink_video_stream_information_t_ud:flags(value) end + +-- get field +---@return integer +function mavlink_video_stream_information_t_ud:resolution_h() end + +-- set field +---@param value integer +function mavlink_video_stream_information_t_ud:resolution_h(value) end + +-- get field +---@return integer +function mavlink_video_stream_information_t_ud:resolution_v() end + +-- set field +---@param value integer +function mavlink_video_stream_information_t_ud:resolution_v(value) end + +-- get field +---@return integer +function mavlink_video_stream_information_t_ud:rotation() end + +-- set field +---@param value integer +function mavlink_video_stream_information_t_ud:rotation(value) end + +-- get field +---@return integer +function mavlink_video_stream_information_t_ud:hfov() end + +-- set field +---@param value integer +function mavlink_video_stream_information_t_ud:hfov(value) end + +-- get field +---@return integer +function mavlink_video_stream_information_t_ud:stream_id() end + +-- set field +---@param value integer +function mavlink_video_stream_information_t_ud:stream_id(value) end + +-- get field +---@return integer +function mavlink_video_stream_information_t_ud:count() end + +-- set field +---@param value integer +function mavlink_video_stream_information_t_ud:count(value) end + +-- get field +---@return integer +function mavlink_video_stream_information_t_ud:type() end + +-- set field +---@param value integer +function mavlink_video_stream_information_t_ud:type(value) end + +-- get array field +---@param index integer +---@return integer +function mavlink_video_stream_information_t_ud:name(index) end + +-- set array field +---@param index integer +---@param value integer +function mavlink_video_stream_information_t_ud:name(index, value) end + +-- get array field +---@param index integer +---@return integer +function mavlink_video_stream_information_t_ud:uri(index) end + +-- set array field +---@param index integer +---@param value integer +function mavlink_video_stream_information_t_ud:uri(index, value) end + +-- Populate the fields of the VIDEO_STREAM_INFORMATION message +---@param instance integer +---@param stream_info mavlink_video_stream_information_t_ud +function camera:set_stream_information(instance, stream_info) end + -- desc mount = {} diff --git a/libraries/AP_Scripting/examples/set_VIDEO_STREAM_INFORMATION.lua b/libraries/AP_Scripting/examples/set_VIDEO_STREAM_INFORMATION.lua new file mode 100644 index 0000000000000..7f8a20844e8ba --- /dev/null +++ b/libraries/AP_Scripting/examples/set_VIDEO_STREAM_INFORMATION.lua @@ -0,0 +1,33 @@ + --[[ + Populate the fields of the VIDEO_STREAM_INFORMATION message sent by the selected camera instance. + --]] + function set_video_stream_information() + -- set the Video Stream Information data + local stream_info = mavlink_video_stream_information_t() + + local INSTANCE = 0 + local name = 'Video' + local uri = '127.0.0.1:5600' + + stream_info:framerate(30) + stream_info:bitrate(1500) + stream_info:flags(1) -- VIDEO_STREAM_STATUS_FLAGS_RUNNING + stream_info:resolution_h(1920) + stream_info:resolution_v(1080) + stream_info:rotation(0) + stream_info:hfov(50) + stream_info:stream_id(1) + stream_info:count(1) + stream_info:type(1) -- VIDEO_STREAM_TYPE_RTPUDP + for i = 0, #name do + stream_info:name(i, name:byte(i+1)) + end + for i = 0, #uri do + stream_info:uri(i, uri:byte(i+1)) + end + + camera:set_stream_information(INSTANCE, stream_info) + +end + +return set_video_stream_information() diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index cde3185702ff8..08c0abe1857b5 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -794,6 +794,20 @@ userdata mavlink_camera_information_t field lens_id uint8_t'skip_check read writ userdata mavlink_camera_information_t field cam_definition_uri'array 140 uint8_t'skip_check read write userdata mavlink_camera_information_t field gimbal_device_id uint8_t'skip_check read write +userdata mavlink_video_stream_information_t depends AP_CAMERA_INFO_FROM_SCRIPT_ENABLED +userdata mavlink_video_stream_information_t field framerate float'skip_check read write +userdata mavlink_video_stream_information_t field bitrate uint32_t'skip_check read write +userdata mavlink_video_stream_information_t field flags uint16_t'skip_check read write +userdata mavlink_video_stream_information_t field resolution_h uint16_t'skip_check read write +userdata mavlink_video_stream_information_t field resolution_v uint16_t'skip_check read write +userdata mavlink_video_stream_information_t field rotation uint16_t'skip_check read write +userdata mavlink_video_stream_information_t field hfov uint16_t'skip_check read write +userdata mavlink_video_stream_information_t field stream_id uint8_t'skip_check read write +userdata mavlink_video_stream_information_t field count uint8_t'skip_check read write +userdata mavlink_video_stream_information_t field type uint8_t'skip_check read write +userdata mavlink_video_stream_information_t field name'array 32 uint8_t'skip_check read write +userdata mavlink_video_stream_information_t field uri'array 160 uint8_t'skip_check read write + include AP_Camera/AP_Camera.h singleton AP_Camera depends AP_CAMERA_ENABLED && (AP_CAMERA_SCRIPTING_ENABLED == 1) singleton AP_Camera rename camera @@ -815,6 +829,8 @@ singleton AP_Camera method get_state boolean uint8_t'skip_check AP_Camera::camer singleton AP_Camera method change_setting boolean uint8_t'skip_check CameraSetting'enum CameraSetting::THERMAL_PALETTE CameraSetting::THERMAL_RAW_DATA float'skip_check singleton AP_Camera method set_camera_information void uint8_t'skip_check mavlink_camera_information_t singleton AP_Camera method set_camera_information depends AP_CAMERA_INFO_FROM_SCRIPT_ENABLED +singleton AP_Camera method set_stream_information void uint8_t'skip_check mavlink_video_stream_information_t +singleton AP_Camera method set_stream_information depends AP_CAMERA_INFO_FROM_SCRIPT_ENABLED include AP_Winch/AP_Winch.h singleton AP_Winch depends AP_WINCH_ENABLED && APM_BUILD_COPTER_OR_HELI