From eb03f878633d431ee69555a1f466f477cba612b2 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 10 Oct 2024 15:14:53 +1300 Subject: [PATCH] camera_manager: mavlink update The update requires to set the camera_device_id in this case signalling that it is a MAVLink camera with its own MAVLink component ID. --- src/gazebo_camera_manager_plugin.cpp | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/src/gazebo_camera_manager_plugin.cpp b/src/gazebo_camera_manager_plugin.cpp index 3f1cb22f3d..4c0d374198 100644 --- a/src/gazebo_camera_manager_plugin.cpp +++ b/src/gazebo_camera_manager_plugin.cpp @@ -657,7 +657,8 @@ void CameraManagerPlugin::_handle_request_camera_settings(const mavlink_message_ 0, // time_boot_ms _mode, // Camera Mode 1.0E2 * (_zoom - 1.0)/ (_maxZoom - 1.0), // Zoom level - NAN); // Focus level + NAN, // Focus level + 0); // MAVLink camera with its own component ID _send_mavlink_message(&msg, srcaddr); } @@ -678,18 +679,19 @@ void CameraManagerPlugin::_handle_request_video_stream_status(const mavlink_mess mavlink_message_t msg; mavlink_msg_video_stream_status_pack_chan( _systemID, - _componentID, // Component ID + _componentID, // Component ID MAVLINK_COMM_1, &msg, static_cast(sid), // Stream ID VIDEO_STREAM_STATUS_FLAGS_RUNNING, // Flags (It's always running) 30, // Frame rate - _width, // Horizontal resolution + _width, // Horizontal resolution _height, // Vertical resolution 2048, // Bit rate (made up) 0, // Rotation (none) - 90 // FOV (made up) - ); + 90, // FOV (made up) + 0); // MAVLink camera with its own component ID + _send_mavlink_message(&msg, srcaddr); } @@ -801,7 +803,8 @@ void CameraManagerPlugin::_send_capture_status(struct sockaddr* srcaddr) interval, // image interval recording_time_ms, // recording time in ms available_mib, // available storage capacity - _imageCounter); // total number of images + _imageCounter, // total number of images + 0); // MAVLink camera with its own component ID _send_mavlink_message(&msg, srcaddr); }