Added handler for turreted drones to share multiple camera components to UAS Tool

addons/main/functions/extract_data/fn_extract_marker_callsign.sqf

@@ -6,13 +6,20 @@ params["_unit"];
 
 private _callsign = "";
 private _displayName = localize (getText (configOf _unit >> "displayName"));
+private _markerCallsignOverride = _unit getVariable ["armatak_attribute_marker_callsign", ""];
+
+if (_markerCallsignOverride isNotEqualTo "") exitWith {
+	_markerCallsignOverride
+};
 
 if (_displayName isEqualTo "") then {
 	_displayName = typeOf _unit;
 };
 
+private _vehicleName = vehicleVarName _unit;
+
 if ((([_unit] call BIS_fnc_objectType) select 0) == "Vehicle") then {
-	_callsign = _displayName;
+	_callsign = [_displayName, _vehicleName] select (_vehicleName isNotEqualTo "");
 
 	if (!isNull driver _unit) then {
 		_callsign = _displayName + " | " + ([name (driver _unit)] call armatak_fnc_shorten_name);
@@ -20,7 +27,13 @@ if ((([_unit] call BIS_fnc_objectType) select 0) == "Vehicle") then {
 };
 
 if (unitIsUAV _unit) then {
-	_callsign = _displayName;
+	_callsign = [_displayName, _vehicleName] select (_vehicleName isNotEqualTo "");
+
+	private _uavControl = UAVControl _unit;
+	private _controller = _uavControl param [0, objNull];
+	if (!isNull _controller) then {
+		_callsign = _callsign + " | " + ([name _controller] call armatak_fnc_shorten_name);
+	};
 
 	if (isUAVConnected _unit) then {
 		_callsign = _callsign + " [ON]";
@@ -29,10 +42,8 @@ if (unitIsUAV _unit) then {
 	}
 };
 
-private _markerCallsignOverride = _unit getVariable ["armatak_attribute_marker_callsign", ""];
+if (_callsign isEqualTo "") then {
-
+	_callsign = _displayName;
-if (_markerCallsignOverride isNotEqualTo "") then {
-	_callsign = _markerCallsignOverride;
 };
 
 _callsign

addons/main/functions/extract_data/fn_extract_uas_camera_data.sqf

@@ -1,4 +1,4 @@
-params ["_drone"];
+params ["_drone", ["_cameraMode", "turret"]];
 
 private _override = _drone getVariable ["armatak_uas_camera_data_override", []];
 private _isLocalController = hasInterface && {!isNull player} && {(getConnectedUAV player) isEqualTo _drone};
@@ -22,6 +22,7 @@ private _cameraDir = [];
 private _spiASL = [];
 private _slantRange = 0;
 
+if (_cameraMode isNotEqualTo "fpv") then {
     private _laserTarget = laserTarget _drone;
     if (!isNull _laserTarget) then {
         private _laserTargetWorld = getPosWorld _laserTarget;
@@ -30,8 +31,9 @@ if (!isNull _laserTarget) then {
         _cameraDir = _spiASL vectorDiff _originASL;
         _slantRange = _originASL vectorDistance _spiASL;
     };
+};
 
-if (_cameraDir isEqualTo []) then {
+if (_cameraDir isEqualTo [] && {_cameraMode isNotEqualTo "fpv"}) then {
     private _uavControl = UAVControl _drone;
     private _controlledTurretPath = _uavControl param [1, []];
     private _candidateTurrets = [];

addons/uav/functions/fnc_updateMavlinkBroadcast.sqf

@@ -59,34 +59,22 @@ private _vfov = _uav getVariable ["armatak_uas_vfov", (_hfov * 0.5625)];
 private _imageLat = _pos select 1;
 private _imageLon = _pos select 2;
 private _imageAlt = _pos select 3;
+private _cameraData = [_uav, "turret"] call armatak_fnc_extract_uas_camera_data;
+private _uavControl = UAVControl _uav;
+private _controlledTurretPath = _uavControl param [1, []];
+private _hasTurretCamera = ((_controlledTurretPath isEqualType []) && {_controlledTurretPath isNotEqualTo []}) || {(allTurrets _uav) isNotEqualTo []};
 
-private _laser = laserTarget _uav;
+if (_cameraData isEqualType [] && {(count _cameraData) >= 6}) then {
-if (!isNull _laser) then {
+	_gimbalYaw = _cameraData param [0, _yaw];
-	private _originASL = getPosASL _uav;
+	_gimbalPitch = _cameraData param [1, _pitch];
-	private _targetWorld = getPosWorld _laser;
+	_hfov = _cameraData param [2, _hfov];
-	private _targetAslZ = (getPosASL _laser) select 2;
+	_vfov = _uav getVariable ["armatak_uas_vfov", (_hfov * 0.5625)];
-	private _spiASL = [_targetWorld select 0, _targetWorld select 1, _targetAslZ];
 
-	private _los = _spiASL vectorDiff _originASL;
+	private _spiGeo = _cameraData param [5, []];
-	private _losMag = vectorMagnitude _los;
+	if (_spiGeo isEqualType [] && {(count _spiGeo) >= 3}) then {
-
+		_imageLat = _spiGeo select 0;
-	if (_losMag > 0) then {
+		_imageLon = _spiGeo select 1;
-		_los = _los vectorMultiply (1 / _losMag);
+		_imageAlt = _spiGeo select 2;
-
-		private _dirX = _los select 0;
-		private _dirY = _los select 1;
-		private _dirZ = _los select 2;
-		private _horizontalMag = sqrt ((_dirX * _dirX) + (_dirY * _dirY));
-
-		private _camYaw = ((_dirX atan2 _dirY) + 360) mod 360;
-		private _camPitch = _dirZ atan2 (_horizontalMag max 0.001);
-		_gimbalPitch = _camPitch;
-		_gimbalYaw = _camYaw;
-
-		private _imageGeo = [_targetWorld select 0, _targetWorld select 1, _targetAslZ] call EFUNC(client,convertClientLocation);
-		_imageLat = _imageGeo select 0;
-		_imageLon = _imageGeo select 1;
-		_imageAlt = _imageGeo select 2;
 	};
 };
 
@@ -113,7 +101,8 @@ private _systemPayload = [
 	_vfov,
 	_imageLat,
 	_imageLon,
-	_imageAlt
+	_imageAlt,
+	parseNumber _hasTurretCamera
 ];
 
 "armatak" callExtension ["uas:send_uas_system", [_systemPayload]];

src/uas/callbacks.rs

@@ -3,8 +3,20 @@ pub(crate) struct MavlinkCallbackEvent {
     pub data: String,
 }
 
+use super::constants::{AUTOPILOT_COMPONENT_ID, CAMERA_COMPONENT_ID, TURRET_CAMERA_COMPONENT_ID};
+use super::identity::should_send_video_stream_information;
+use super::packets::{
+    autopilot_version_packet, camera_fov_status_packet_for_component,
+    camera_information_packet_for_component, command_ack_packet, gimbal_manager_information_packet,
+    home_position_packet, mount_orientation_packet_for_component, mount_status_packet,
+    video_stream_information_packet_for_component, video_stream_status_packet_for_component,
+};
+use super::state::{latest_system, set_active_camera};
+use log::info;
+
 pub(crate) fn hex_preview(bytes: &[u8], max_len: usize) -> String {
-    bytes.iter()
+    bytes
+        .iter()
         .take(max_len)
         .map(|byte| format!("{:02X}", byte))
         .collect::<Vec<_>>()
@@ -21,7 +33,12 @@ pub(crate) fn mav_cmd_name(command_id: u16) -> &'static str {
         251 => "VIDEO_STOP_CAPTURE",
         252 => "DO_CONTROL_VIDEO",
         400 => "COMPONENT_ARM_DISARM",
-        521 => "REQUEST_MESSAGE",
+        512 => "REQUEST_MESSAGE",
+        521 => "REQUEST_CAMERA_INFORMATION",
+        2502 => "VIDEO_START_STREAMING",
+        2503 => "VIDEO_STOP_STREAMING",
+        2504 => "REQUEST_VIDEO_STREAM_INFORMATION",
+        2505 => "REQUEST_VIDEO_STREAM_STATUS",
         _ => "UNKNOWN",
     }
 }
@@ -46,10 +63,10 @@ fn read_f32(payload: &[u8], offset: usize) -> Option<f32> {
 
 fn mavlink_message_detail(msg_id: u8, payload: &[u8]) -> String {
     match msg_id {
-        76 if payload.len() >= 33 => {
+        76 if payload.len() >= 31 => {
             let command = u16::from_le_bytes([payload[28], payload[29]]);
             let target_system = payload[30];
-            let target_component = payload[31];
+            let target_component = payload.get(31).copied().unwrap_or(0);
             format!(
                 " command={}({}) target={}:{}",
                 command,
@@ -86,7 +103,8 @@ pub(crate) fn mavlink_packet_summary(bytes: &[u8]) -> String {
             let system_id = bytes[3];
             let component_id = bytes[4];
             let msg_id = bytes[5];
-            let detail = mavlink_message_detail(msg_id, bytes.get(6..6 + payload_len).unwrap_or(&[]));
+            let detail =
+                mavlink_message_detail(msg_id, bytes.get(6..6 + payload_len).unwrap_or(&[]));
             format!(
                 "MAVLink v1 msgid={}{} sysid={} compid={} seq={} payload_len={} preview={}",
                 msg_id,
@@ -104,7 +122,10 @@ pub(crate) fn mavlink_packet_summary(bytes: &[u8]) -> String {
             let system_id = bytes[5];
             let component_id = bytes[6];
             let msg_id = bytes[7] as u32 | ((bytes[8] as u32) << 8) | ((bytes[9] as u32) << 16);
-            let detail = mavlink_message_detail(msg_id as u8, bytes.get(10..10 + payload_len).unwrap_or(&[]));
+            let detail = mavlink_message_detail(
+                msg_id as u8,
+                bytes.get(10..10 + payload_len).unwrap_or(&[]),
+            );
             format!(
                 "MAVLink v2 msgid={}{} sysid={} compid={} seq={} payload_len={} preview={}",
                 msg_id,
@@ -163,11 +184,11 @@ pub(crate) fn mavlink_callback_event(bytes: &[u8], source: &str) -> Option<Mavli
                 ),
             })
         }
-        76 if payload.len() >= 33 => {
+        76 if payload.len() >= 31 => {
             let command = read_u16(payload, 28)?;
             let target_system = *payload.get(30)?;
-            let target_component = *payload.get(31)?;
+            let target_component = payload.get(31).copied().unwrap_or(0);
-            let confirmation = *payload.get(32)?;
+            let confirmation = payload.get(32).copied().unwrap_or(0);
             Some(MavlinkCallbackEvent {
                 function: "COMMAND_LONG",
                 data: format!(
@@ -237,3 +258,374 @@ pub(crate) fn mavlink_callback_event(bytes: &[u8], source: &str) -> Option<Mavli
         _ => None,
     }
 }
+
+pub(crate) fn mavlink_response_packets(bytes: &[u8]) -> Vec<Vec<u8>> {
+    if bytes.len() < 8 {
+        return Vec::new();
+    }
+
+    let (msg_id, payload) = match bytes[0] {
+        0xFE if bytes.len() >= 8 => {
+            let payload_len = bytes[1] as usize;
+            (
+                bytes[5] as u32,
+                bytes.get(6..6 + payload_len).unwrap_or(&[]),
+            )
+        }
+        0xFD if bytes.len() >= 12 => {
+            let payload_len = bytes[1] as usize;
+            (
+                bytes[7] as u32 | ((bytes[8] as u32) << 8) | ((bytes[9] as u32) << 16),
+                bytes.get(10..10 + payload_len).unwrap_or(&[]),
+            )
+        }
+        _ => return Vec::new(),
+    };
+
+    match msg_id {
+        76 if payload.len() >= 31 => command_long_response_packets(payload),
+        75 if payload.len() >= 35 => command_int_response_packets(payload),
+        _ => Vec::new(),
+    }
+}
+
+fn command_long_response_packets(payload: &[u8]) -> Vec<Vec<u8>> {
+    let command = match read_u16(payload, 28) {
+        Some(command) => command,
+        None => return Vec::new(),
+    };
+    let target_system = *payload.get(30).unwrap_or(&0);
+    let target_component = *payload.get(31).unwrap_or(&0);
+    if target_system == 0 {
+        return Vec::new();
+    }
+
+    let ack_component = if target_component == 0 {
+        AUTOPILOT_COMPONENT_ID
+    } else {
+        target_component
+    };
+    let mut packets = vec![command_ack_packet(target_system, ack_component, command, 0)];
+
+    match command {
+        512 => {
+            let requested_message = read_f32(payload, 0).unwrap_or(0.0).round() as u32;
+            packets.extend(requested_message_packets(target_system, requested_message));
+        }
+        521 => {
+            if let Some(system) = latest_system(target_system) {
+                let camera_component =
+                    camera_component_for_target(target_component, system.has_turret_camera);
+                set_active_camera(target_system, camera_component);
+                info!(
+                    "MAVLink camera selection command={} target_system={} target_component={} resolved_camera_component={}",
+                    command, target_system, target_component, camera_component
+                );
+                packets.push(camera_information_packet_for_component(
+                    target_system,
+                    camera_component,
+                    &camera_name(&system.callsign, camera_component, system.has_turret_camera),
+                    gimbal_device_for_target(target_component, system.has_turret_camera),
+                ));
+            }
+        }
+        2502 | 2503 | 2505 => {
+            if let Some(system) = latest_system(target_system) {
+                let camera_component =
+                    camera_component_for_target(target_component, system.has_turret_camera);
+                set_active_camera(target_system, camera_component);
+                info!(
+                    "MAVLink camera selection command={} target_system={} target_component={} resolved_camera_component={}",
+                    command, target_system, target_component, camera_component
+                );
+                packets.push(video_stream_status_packet_for_component(
+                    target_system,
+                    camera_component,
+                    system.hfov_deg,
+                    1,
+                    false,
+                ));
+            }
+        }
+        2504 => {
+            if let Some(system) = latest_system(target_system) {
+                if should_send_video_stream_information(&system.video_uri) {
+                    let camera_component =
+                        camera_component_for_target(target_component, system.has_turret_camera);
+                    set_active_camera(target_system, camera_component);
+                    info!(
+                        "MAVLink camera selection command={} target_system={} target_component={} resolved_camera_component={}",
+                        command, target_system, target_component, camera_component
+                    );
+                    packets.push(video_stream_information_packet_for_component(
+                        target_system,
+                        camera_component,
+                        &camera_name(&system.callsign, camera_component, system.has_turret_camera),
+                        &system.video_uri,
+                        system.hfov_deg,
+                        1,
+                        1,
+                        false,
+                    ));
+                }
+            }
+        }
+        _ => {}
+    }
+
+    packets
+}
+
+fn command_int_response_packets(payload: &[u8]) -> Vec<Vec<u8>> {
+    let command = match read_u16(payload, 28) {
+        Some(command) => command,
+        None => return Vec::new(),
+    };
+    let target_system = *payload.get(30).unwrap_or(&0);
+    let target_component = *payload.get(31).unwrap_or(&AUTOPILOT_COMPONENT_ID);
+    if target_system == 0 {
+        return Vec::new();
+    }
+
+    vec![command_ack_packet(
+        target_system,
+        if target_component == 0 {
+            AUTOPILOT_COMPONENT_ID
+        } else {
+            target_component
+        },
+        command,
+        0,
+    )]
+}
+
+fn requested_message_packets(system_id: u8, requested_message: u32) -> Vec<Vec<u8>> {
+    let Some(system) = latest_system(system_id) else {
+        return Vec::new();
+    };
+
+    match requested_message {
+        148 => vec![autopilot_version_packet(
+            system_id,
+            &system.mavlink_identity,
+        )],
+        242 => vec![home_position_packet(
+            system_id,
+            system.lat_deg,
+            system.lon_deg,
+            system.alt_msl_m - system.rel_alt_m,
+            system.heading_deg,
+        )],
+        259 => {
+            let mut packets = vec![camera_information_packet_for_component(
+                system_id,
+                CAMERA_COMPONENT_ID,
+                &camera_name(
+                    &system.callsign,
+                    CAMERA_COMPONENT_ID,
+                    system.has_turret_camera,
+                ),
+                0,
+            )];
+            if system.has_turret_camera {
+                packets.push(camera_information_packet_for_component(
+                    system_id,
+                    TURRET_CAMERA_COMPONENT_ID,
+                    &camera_name(&system.callsign, TURRET_CAMERA_COMPONENT_ID, true),
+                    super::constants::GIMBAL_COMPONENT_ID,
+                ));
+            }
+            packets
+        }
+        269 => {
+            if should_send_video_stream_information(&system.video_uri) {
+                let mut packets = vec![video_stream_information_packet_for_component(
+                    system_id,
+                    CAMERA_COMPONENT_ID,
+                    &camera_name(
+                        &system.callsign,
+                        CAMERA_COMPONENT_ID,
+                        system.has_turret_camera,
+                    ),
+                    &system.video_uri,
+                    system.hfov_deg,
+                    1,
+                    1,
+                    false,
+                )];
+                if system.has_turret_camera {
+                    packets.push(video_stream_information_packet_for_component(
+                        system_id,
+                        TURRET_CAMERA_COMPONENT_ID,
+                        &camera_name(&system.callsign, TURRET_CAMERA_COMPONENT_ID, true),
+                        &system.video_uri,
+                        system.hfov_deg,
+                        1,
+                        1,
+                        false,
+                    ));
+                }
+                packets
+            } else {
+                Vec::new()
+            }
+        }
+        270 => {
+            let mut packets = vec![video_stream_status_packet_for_component(
+                system_id,
+                CAMERA_COMPONENT_ID,
+                system.hfov_deg,
+                1,
+                false,
+            )];
+            if system.has_turret_camera {
+                packets.push(video_stream_status_packet_for_component(
+                    system_id,
+                    TURRET_CAMERA_COMPONENT_ID,
+                    system.hfov_deg,
+                    1,
+                    false,
+                ));
+            }
+            packets
+        }
+        265 => {
+            let mut packets = vec![mount_orientation_packet_for_component(
+                system_id,
+                CAMERA_COMPONENT_ID,
+                system.fpv_pitch_deg,
+                system.fpv_yaw_deg,
+            )];
+            if system.has_turret_camera {
+                packets.push(mount_orientation_packet_for_component(
+                    system_id,
+                    TURRET_CAMERA_COMPONENT_ID,
+                    system.gimbal_pitch_deg,
+                    system.gimbal_yaw_deg,
+                ));
+            }
+            packets
+        }
+        271 => {
+            let (fpv_image_lat, fpv_image_lon, fpv_image_alt) = fpv_image_point(
+                system.lat_deg,
+                system.lon_deg,
+                system.alt_msl_m,
+                system.rel_alt_m,
+                system.fpv_pitch_deg,
+                system.fpv_yaw_deg,
+            );
+            let mut packets = vec![camera_fov_status_packet_for_component(
+                system_id,
+                CAMERA_COMPONENT_ID,
+                system.lat_deg,
+                system.lon_deg,
+                system.alt_msl_m,
+                fpv_image_lat,
+                fpv_image_lon,
+                fpv_image_alt,
+                0.0,
+                system.fpv_pitch_deg,
+                system.fpv_yaw_deg,
+                system.hfov_deg,
+                system.vfov_deg,
+            )];
+            if system.has_turret_camera {
+                packets.push(camera_fov_status_packet_for_component(
+                    system_id,
+                    TURRET_CAMERA_COMPONENT_ID,
+                    system.lat_deg,
+                    system.lon_deg,
+                    system.alt_msl_m,
+                    system.image_lat_deg,
+                    system.image_lon_deg,
+                    system.image_alt_msl_m,
+                    0.0,
+                    system.gimbal_pitch_deg,
+                    system.gimbal_yaw_deg,
+                    system.hfov_deg,
+                    system.vfov_deg,
+                ));
+            }
+            packets
+        }
+        158 => {
+            let active_component = if system.has_turret_camera {
+                system.active_camera_component
+            } else {
+                CAMERA_COMPONENT_ID
+            };
+            let (pitch, roll, relative_yaw) = if active_component == TURRET_CAMERA_COMPONENT_ID {
+                (
+                    system.gimbal_pitch_deg,
+                    0.0,
+                    normalize_signed_deg(system.gimbal_yaw_deg - system.fpv_yaw_deg),
+                )
+            } else {
+                (system.fpv_pitch_deg, 0.0, 0.0)
+            };
+            vec![mount_status_packet(system_id, pitch, roll, relative_yaw)]
+        }
+        280 => vec![gimbal_manager_information_packet(system_id)],
+        _ => Vec::new(),
+    }
+}
+
+fn camera_component_for_target(target_component: u8, has_turret_camera: bool) -> u8 {
+    if has_turret_camera && target_component == TURRET_CAMERA_COMPONENT_ID {
+        TURRET_CAMERA_COMPONENT_ID
+    } else {
+        CAMERA_COMPONENT_ID
+    }
+}
+
+fn gimbal_device_for_target(target_component: u8, has_turret_camera: bool) -> u8 {
+    if has_turret_camera && target_component == TURRET_CAMERA_COMPONENT_ID {
+        super::constants::GIMBAL_COMPONENT_ID
+    } else {
+        0
+    }
+}
+
+fn camera_name(callsign: &str, component_id: u8, has_turret_camera: bool) -> String {
+    if has_turret_camera && component_id == TURRET_CAMERA_COMPONENT_ID {
+        format!("{callsign} Turret")
+    } else {
+        format!("{callsign} FPV")
+    }
+}
+
+fn normalize_signed_deg(value: f32) -> f32 {
+    let normalized = ((value % 360.0) + 360.0) % 360.0;
+    if normalized > 180.0 {
+        normalized - 360.0
+    } else {
+        normalized
+    }
+}
+
+fn fpv_image_point(
+    lat_deg: f64,
+    lon_deg: f64,
+    alt_msl_m: f32,
+    rel_alt_m: f32,
+    pitch_deg: f32,
+    yaw_deg: f32,
+) -> (f64, f64, f32) {
+    let pitch_rad = pitch_deg.to_radians();
+    let vertical = (-pitch_rad.sin()).max(0.01);
+    let slant_m = (rel_alt_m.max(1.0) / vertical).clamp(1.0, 15_000.0);
+    let ground_m = slant_m * pitch_rad.cos().abs();
+    let yaw_rad = yaw_deg.to_radians();
+    let north_m = ground_m * yaw_rad.cos();
+    let east_m = ground_m * yaw_rad.sin();
+    let lat_rad = lat_deg.to_radians();
+    let meters_per_degree_lat = 111_320.0;
+    let meters_per_degree_lon = (111_320.0 * lat_rad.cos().abs()).max(1.0);
+
+    (
+        lat_deg + north_m as f64 / meters_per_degree_lat,
+        lon_deg + east_m as f64 / meters_per_degree_lon,
+        alt_msl_m - rel_alt_m,
+    )
+}

src/uas/constants.rs

@@ -1,5 +1,6 @@
 pub const AUTOPILOT_COMPONENT_ID: u8 = 1;
 pub const CAMERA_COMPONENT_ID: u8 = 100;
+pub const TURRET_CAMERA_COMPONENT_ID: u8 = 101;
 pub const GIMBAL_COMPONENT_ID: u8 = 154;
 
 pub const MAV_TYPE_FIXED_WING: u8 = 1;
@@ -8,7 +9,7 @@ pub const MAV_TYPE_HELICOPTER: u8 = 4;
 pub const MAV_TYPE_GIMBAL: u8 = 26;
 pub const MAV_TYPE_CAMERA: u8 = 30;
 
-pub const MAV_AUTOPILOT_GENERIC: u8 = 0;
+pub const MAV_AUTOPILOT_ARDUPILOTMEGA: u8 = 3;
 pub const MAV_AUTOPILOT_INVALID: u8 = 8;
 
 pub const MAV_STATE_STANDBY: u8 = 3;
@@ -27,6 +28,10 @@ pub const MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_GLOBAL_INT: u64 = 256;
 pub const MAV_PROTOCOL_CAPABILITY_MAVLINK2: u64 = 8192;
 pub const MAV_PROTOCOL_CAPABILITY_COMPONENT_IMPLEMENTS_GIMBAL_MANAGER: u64 = 262_144;
 
+pub const CAMERA_CAP_FLAGS_CAPTURE_VIDEO: u32 = 1;
+pub const CAMERA_CAP_FLAGS_CAPTURE_IMAGE: u32 = 2;
+pub const CAMERA_CAP_FLAGS_HAS_MODES: u32 = 4;
+pub const CAMERA_CAP_FLAGS_HAS_BASIC_ZOOM: u32 = 64;
 pub const CAMERA_CAP_FLAGS_HAS_VIDEO_STREAM: u32 = 256;
 pub const VIDEO_STREAM_STATUS_FLAGS_RUNNING: u16 = 1;
 pub const VIDEO_STREAM_TYPE_RTSP: u8 = 0;
@@ -34,3 +39,5 @@ pub const VIDEO_STREAM_TYPE_RTPUDP: u8 = 1;
 pub const VIDEO_STREAM_TYPE_TCP_MPEG: u8 = 2;
 pub const VIDEO_STREAM_TYPE_MPEG_TS: u8 = 3;
 pub const VIDEO_STREAM_ENCODING_H264: u8 = 1;
+
+pub const GIMBAL_MANAGER_CAP_FLAGS_BASIC_PITCH_YAW: u32 = 32 | 128 | 256 | 1024 | 4096 | 131_072;

src/uas/endpoint.rs

@@ -6,7 +6,7 @@ use std::sync::{Arc, Mutex};
 use std::thread::{self, JoinHandle};
 use std::time::Duration;
 
-use super::callbacks::{mavlink_callback_event, mavlink_packet_summary};
+use super::callbacks::{mavlink_callback_event, mavlink_packet_summary, mavlink_response_packets};
 
 pub(crate) struct MavlinkEndpoint {
     pub socket: UdpSocket,
@@ -18,10 +18,11 @@ pub(crate) struct MavlinkEndpoint {
 static MAVLINK_ENDPOINT: Mutex<Option<MavlinkEndpoint>> = Mutex::new(None);
 
 pub(crate) fn socket_for_send() -> Option<UdpSocket> {
-    MAVLINK_ENDPOINT
+    MAVLINK_ENDPOINT.lock().ok().and_then(|endpoint| {
-        .lock()
+        endpoint
-        .ok()
+            .as_ref()
-        .and_then(|endpoint| endpoint.as_ref().and_then(|entry| entry.socket.try_clone().ok()))
+            .and_then(|entry| entry.socket.try_clone().ok())
+    })
 }
 
 fn stop_endpoint_internal() {
@@ -102,14 +103,27 @@ pub fn start_endpoint(ctx: Context, bind_port: i32) -> &'static str {
                         source,
                         mavlink_packet_summary(&buffer[..received])
                     );
-                    if let Some(event) = mavlink_callback_event(&buffer[..received], &source_string) {
+                    if let Some(event) = mavlink_callback_event(&buffer[..received], &source_string)
-                        let _ = listener_ctx.callback_data("MAVLINK UDP", event.function, event.data);
+                    {
+                        let _ =
+                            listener_ctx.callback_data("MAVLINK UDP", event.function, event.data);
+                    }
+                    for packet in mavlink_response_packets(&buffer[..received]) {
+                        if let Err(error) = listener_socket.send_to(&packet, source) {
+                            info!(
+                                "MAVLink UDP endpoint failed sending response to {}: {}",
+                                source, error
+                            );
+                            break;
+                        }
                     }
                 }
                 Err(error)
                     if matches!(
                         error.kind(),
-                        std::io::ErrorKind::WouldBlock | std::io::ErrorKind::TimedOut
+                        std::io::ErrorKind::WouldBlock
+                            | std::io::ErrorKind::TimedOut
+                            | std::io::ErrorKind::ConnectionReset
                     ) => {}
                 Err(error) => {
                     if listener_running.load(Ordering::Relaxed) {

src/uas/identity.rs

@@ -1,6 +1,4 @@
-use super::constants::{
+use super::constants::{MAV_TYPE_FIXED_WING, MAV_TYPE_HELICOPTER, MAV_TYPE_QUADROTOR};
-    MAV_TYPE_FIXED_WING, MAV_TYPE_HELICOPTER, MAV_TYPE_QUADROTOR,
-};
 
 pub(crate) fn map_vehicle_type(vehicle_type: u8) -> u8 {
     match vehicle_type {
@@ -25,6 +23,23 @@ pub(crate) fn stable_system_id(entity_uuid: &str) -> u8 {
     ((hash % 250) as u8) + 1
 }
 
+pub(crate) fn stable_mavlink_identity(callsign: &str, entity_uuid: &str) -> String {
+    let mut identity = callsign.trim().to_string();
+
+    for suffix in [" [ON]", " [OFF]"] {
+        if identity.ends_with(suffix) {
+            identity.truncate(identity.len() - suffix.len());
+            break;
+        }
+    }
+
+    if identity.is_empty() {
+        entity_uuid.trim().to_string()
+    } else {
+        identity
+    }
+}
+
 fn uuid16(entity_uuid: &str) -> [u8; 16] {
     let hex = entity_uuid.replace('-', "");
     let mut bytes = [0u8; 16];

src/uas/mod.rs

@@ -6,6 +6,7 @@ mod identity;
 mod packets;
 mod payload;
 mod send;
+mod state;
 
 pub use endpoint::{start_endpoint, stop_endpoint};
 #[allow(unused_imports)]

src/uas/packets.rs

@@ -1,16 +1,17 @@
 use chrono::Utc;
 
 use super::constants::{
-    AUTOPILOT_COMPONENT_ID, CAMERA_CAP_FLAGS_HAS_VIDEO_STREAM, CAMERA_COMPONENT_ID,
+    AUTOPILOT_COMPONENT_ID, CAMERA_CAP_FLAGS_CAPTURE_IMAGE, CAMERA_CAP_FLAGS_CAPTURE_VIDEO,
-    GIMBAL_COMPONENT_ID, MAV_AUTOPILOT_GENERIC, MAV_AUTOPILOT_INVALID,
+    CAMERA_CAP_FLAGS_HAS_BASIC_ZOOM, CAMERA_CAP_FLAGS_HAS_MODES, CAMERA_CAP_FLAGS_HAS_VIDEO_STREAM,
-    MAV_LANDED_STATE_IN_AIR, MAV_LANDED_STATE_ON_GROUND, MAV_LANDED_STATE_UNDEFINED,
+    GIMBAL_COMPONENT_ID, GIMBAL_MANAGER_CAP_FLAGS_BASIC_PITCH_YAW, MAV_AUTOPILOT_ARDUPILOTMEGA,
-    MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, MAV_MODE_FLAG_SAFETY_ARMED,
+    MAV_AUTOPILOT_INVALID, MAV_LANDED_STATE_IN_AIR, MAV_LANDED_STATE_ON_GROUND,
-    MAV_PROTOCOL_CAPABILITY_COMMAND_INT, MAV_PROTOCOL_CAPABILITY_COMPONENT_IMPLEMENTS_GIMBAL_MANAGER,
+    MAV_LANDED_STATE_UNDEFINED, MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, MAV_MODE_FLAG_SAFETY_ARMED,
-    MAV_PROTOCOL_CAPABILITY_FTP, MAV_PROTOCOL_CAPABILITY_MAVLINK2,
+    MAV_PROTOCOL_CAPABILITY_COMMAND_INT,
-    MAV_PROTOCOL_CAPABILITY_MISSION_INT, MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_GLOBAL_INT,
+    MAV_PROTOCOL_CAPABILITY_COMPONENT_IMPLEMENTS_GIMBAL_MANAGER, MAV_PROTOCOL_CAPABILITY_FTP,
-    MAV_STATE_ACTIVE, MAV_STATE_STANDBY, VIDEO_STREAM_ENCODING_H264,
+    MAV_PROTOCOL_CAPABILITY_MAVLINK2, MAV_PROTOCOL_CAPABILITY_MISSION_INT,
-    VIDEO_STREAM_STATUS_FLAGS_RUNNING, VIDEO_STREAM_TYPE_MPEG_TS, VIDEO_STREAM_TYPE_RTPUDP,
+    MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_GLOBAL_INT, MAV_STATE_ACTIVE, MAV_STATE_STANDBY,
-    VIDEO_STREAM_TYPE_RTSP, VIDEO_STREAM_TYPE_TCP_MPEG,
+    VIDEO_STREAM_ENCODING_H264, VIDEO_STREAM_STATUS_FLAGS_RUNNING, VIDEO_STREAM_TYPE_MPEG_TS,
+    VIDEO_STREAM_TYPE_RTPUDP, VIDEO_STREAM_TYPE_RTSP, VIDEO_STREAM_TYPE_TCP_MPEG,
 };
 use super::crc::{build_v1_packet, build_v2_packet, calculate_crc_extra, FieldSpec};
 use super::identity::{fixed_string, normalize_heading_deg, uid2_from_uuid, uid64_from_uuid};
@@ -18,9 +19,10 @@ use super::payload::UasTelemetryPayload;
 
 pub(crate) fn heartbeat_packet(payload: &UasTelemetryPayload) -> Vec<u8> {
     let mut msg = Vec::with_capacity(9);
-    msg.extend_from_slice(&0u32.to_le_bytes());
+    let custom_mode = if payload.flying { 5u32 } else { 0u32 };
+    msg.extend_from_slice(&custom_mode.to_le_bytes());
     msg.push(payload.vehicle_type);
-    msg.push(MAV_AUTOPILOT_GENERIC);
+    msg.push(MAV_AUTOPILOT_ARDUPILOTMEGA);
     msg.push(if payload.flying {
         MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | MAV_MODE_FLAG_SAFETY_ARMED
     } else {
@@ -35,6 +37,18 @@ pub(crate) fn heartbeat_packet(payload: &UasTelemetryPayload) -> Vec<u8> {
     build_v1_packet(payload.system_id, payload.component_id, 0, &msg, 50)
 }
 
+pub(crate) fn command_ack_packet(
+    system_id: u8,
+    component_id: u8,
+    command: u16,
+    result: u8,
+) -> Vec<u8> {
+    let mut msg = Vec::with_capacity(3);
+    msg.extend_from_slice(&command.to_le_bytes());
+    msg.push(result);
+    build_v2_packet(system_id, component_id, 77, &msg, 143)
+}
+
 pub(crate) fn gps_raw_int_packet(payload: &UasTelemetryPayload) -> Vec<u8> {
     let time_usec = (Utc::now().timestamp_millis().max(0) as u64) * 1_000;
     let fix_type = if payload.flying { 3u8 } else { 2u8 };
@@ -55,8 +69,10 @@ pub(crate) fn gps_raw_int_packet(payload: &UasTelemetryPayload) -> Vec<u8> {
 
 pub(crate) fn global_position_int_packet(payload: &UasTelemetryPayload) -> Vec<u8> {
     let time_boot_ms = Utc::now().timestamp_millis().max(0) as u32;
-    let vx = (payload.groundspeed_mps * payload.heading_deg.to_radians().sin() * 100.0).round() as i16;
+    let vx =
-    let vy = (payload.groundspeed_mps * payload.heading_deg.to_radians().cos() * 100.0).round() as i16;
+        (payload.groundspeed_mps * payload.heading_deg.to_radians().sin() * 100.0).round() as i16;
+    let vy =
+        (payload.groundspeed_mps * payload.heading_deg.to_radians().cos() * 100.0).round() as i16;
     let heading = (normalize_heading_deg(payload.heading_deg) * 100.0).round() as u16;
 
     let mut msg = Vec::with_capacity(28);
@@ -105,19 +121,71 @@ pub(crate) fn vfr_hud_packet(payload: &UasTelemetryPayload) -> Vec<u8> {
 
 pub(crate) fn system_status_packet(system_id: u8) -> Vec<u8> {
     let fields = [
-        FieldSpec { ty: "uint32_t", name: "onboard_control_sensors_present", array_len: 0 },
+        FieldSpec {
-        FieldSpec { ty: "uint32_t", name: "onboard_control_sensors_enabled", array_len: 0 },
+            ty: "uint32_t",
-        FieldSpec { ty: "uint32_t", name: "onboard_control_sensors_health", array_len: 0 },
+            name: "onboard_control_sensors_present",
-        FieldSpec { ty: "uint16_t", name: "load", array_len: 0 },
+            array_len: 0,
-        FieldSpec { ty: "uint16_t", name: "voltage_battery", array_len: 0 },
+        },
-        FieldSpec { ty: "int16_t", name: "current_battery", array_len: 0 },
+        FieldSpec {
-        FieldSpec { ty: "uint16_t", name: "drop_rate_comm", array_len: 0 },
+            ty: "uint32_t",
-        FieldSpec { ty: "uint16_t", name: "errors_comm", array_len: 0 },
+            name: "onboard_control_sensors_enabled",
-        FieldSpec { ty: "uint16_t", name: "errors_count1", array_len: 0 },
+            array_len: 0,
-        FieldSpec { ty: "uint16_t", name: "errors_count2", array_len: 0 },
+        },
-        FieldSpec { ty: "uint16_t", name: "errors_count3", array_len: 0 },
+        FieldSpec {
-        FieldSpec { ty: "uint16_t", name: "errors_count4", array_len: 0 },
+            ty: "uint32_t",
-        FieldSpec { ty: "int8_t", name: "battery_remaining", array_len: 0 },
+            name: "onboard_control_sensors_health",
+            array_len: 0,
+        },
+        FieldSpec {
+            ty: "uint16_t",
+            name: "load",
+            array_len: 0,
+        },
+        FieldSpec {
+            ty: "uint16_t",
+            name: "voltage_battery",
+            array_len: 0,
+        },
+        FieldSpec {
+            ty: "int16_t",
+            name: "current_battery",
+            array_len: 0,
+        },
+        FieldSpec {
+            ty: "uint16_t",
+            name: "drop_rate_comm",
+            array_len: 0,
+        },
+        FieldSpec {
+            ty: "uint16_t",
+            name: "errors_comm",
+            array_len: 0,
+        },
+        FieldSpec {
+            ty: "uint16_t",
+            name: "errors_count1",
+            array_len: 0,
+        },
+        FieldSpec {
+            ty: "uint16_t",
+            name: "errors_count2",
+            array_len: 0,
+        },
+        FieldSpec {
+            ty: "uint16_t",
+            name: "errors_count3",
+            array_len: 0,
+        },
+        FieldSpec {
+            ty: "uint16_t",
+            name: "errors_count4",
+            array_len: 0,
+        },
+        FieldSpec {
+            ty: "int8_t",
+            name: "battery_remaining",
+            array_len: 0,
+        },
     ];
     let crc_extra = calculate_crc_extra("SYS_STATUS", &fields);
 
@@ -142,8 +210,16 @@ pub(crate) fn system_status_packet(system_id: u8) -> Vec<u8> {
 
 pub(crate) fn extended_sys_state_packet(system_id: u8, flying: bool) -> Vec<u8> {
     let fields = [
-        FieldSpec { ty: "uint8_t", name: "vtol_state", array_len: 0 },
+        FieldSpec {
-        FieldSpec { ty: "uint8_t", name: "landed_state", array_len: 0 },
+            ty: "uint8_t",
+            name: "vtol_state",
+            array_len: 0,
+        },
+        FieldSpec {
+            ty: "uint8_t",
+            name: "landed_state",
+            array_len: 0,
+        },
     ];
     let crc_extra = calculate_crc_extra("EXTENDED_SYS_STATE", &fields);
 
@@ -160,17 +236,61 @@ pub(crate) fn extended_sys_state_packet(system_id: u8, flying: bool) -> Vec<u8>
 
 pub(crate) fn autopilot_version_packet(system_id: u8, entity_uuid: &str) -> Vec<u8> {
     let fields = [
-        FieldSpec { ty: "uint64_t", name: "capabilities", array_len: 0 },
+        FieldSpec {
-        FieldSpec { ty: "uint32_t", name: "flight_sw_version", array_len: 0 },
+            ty: "uint64_t",
-        FieldSpec { ty: "uint32_t", name: "middleware_sw_version", array_len: 0 },
+            name: "capabilities",
-        FieldSpec { ty: "uint32_t", name: "os_sw_version", array_len: 0 },
+            array_len: 0,
-        FieldSpec { ty: "uint32_t", name: "board_version", array_len: 0 },
+        },
-        FieldSpec { ty: "uint8_t", name: "flight_custom_version", array_len: 8 },
+        FieldSpec {
-        FieldSpec { ty: "uint8_t", name: "middleware_custom_version", array_len: 8 },
+            ty: "uint32_t",
-        FieldSpec { ty: "uint8_t", name: "os_custom_version", array_len: 8 },
+            name: "flight_sw_version",
-        FieldSpec { ty: "uint16_t", name: "vendor_id", array_len: 0 },
+            array_len: 0,
-        FieldSpec { ty: "uint16_t", name: "product_id", array_len: 0 },
+        },
-        FieldSpec { ty: "uint64_t", name: "uid", array_len: 0 },
+        FieldSpec {
+            ty: "uint32_t",
+            name: "middleware_sw_version",
+            array_len: 0,
+        },
+        FieldSpec {
+            ty: "uint32_t",
+            name: "os_sw_version",
+            array_len: 0,
+        },
+        FieldSpec {
+            ty: "uint32_t",
+            name: "board_version",
+            array_len: 0,
+        },
+        FieldSpec {
+            ty: "uint8_t",
+            name: "flight_custom_version",
+            array_len: 8,
+        },
+        FieldSpec {
+            ty: "uint8_t",
+            name: "middleware_custom_version",
+            array_len: 8,
+        },
+        FieldSpec {
+            ty: "uint8_t",
+            name: "os_custom_version",
+            array_len: 8,
+        },
+        FieldSpec {
+            ty: "uint16_t",
+            name: "vendor_id",
+            array_len: 0,
+        },
+        FieldSpec {
+            ty: "uint16_t",
+            name: "product_id",
+            array_len: 0,
+        },
+        FieldSpec {
+            ty: "uint64_t",
+            name: "uid",
+            array_len: 0,
+        },
     ];
     let crc_extra = calculate_crc_extra("AUTOPILOT_VERSION", &fields);
 
@@ -200,7 +320,38 @@ pub(crate) fn autopilot_version_packet(system_id: u8, entity_uuid: &str) -> Vec<
     build_v2_packet(system_id, AUTOPILOT_COMPONENT_ID, 148, &msg, crc_extra)
 }
 
-pub(crate) fn component_heartbeat_packet(system_id: u8, component_id: u8, component_type: u8) -> Vec<u8> {
+pub(crate) fn home_position_packet(
+    system_id: u8,
+    lat_deg: f64,
+    lon_deg: f64,
+    alt_msl_m: f32,
+    heading_deg: f32,
+) -> Vec<u8> {
+    let yaw = normalize_heading_deg(heading_deg).to_radians();
+    let q = [(yaw * 0.5).cos(), 0.0f32, 0.0f32, (yaw * 0.5).sin()];
+
+    let mut msg = Vec::with_capacity(52);
+    msg.extend_from_slice(&((lat_deg * 1e7).round() as i32).to_le_bytes());
+    msg.extend_from_slice(&((lon_deg * 1e7).round() as i32).to_le_bytes());
+    msg.extend_from_slice(&((alt_msl_m * 1000.0).round() as i32).to_le_bytes());
+    msg.extend_from_slice(&0f32.to_le_bytes());
+    msg.extend_from_slice(&0f32.to_le_bytes());
+    msg.extend_from_slice(&0f32.to_le_bytes());
+    for value in q {
+        msg.extend_from_slice(&value.to_le_bytes());
+    }
+    msg.extend_from_slice(&0f32.to_le_bytes());
+    msg.extend_from_slice(&0f32.to_le_bytes());
+    msg.extend_from_slice(&0f32.to_le_bytes());
+
+    build_v2_packet(system_id, AUTOPILOT_COMPONENT_ID, 242, &msg, 85)
+}
+
+pub(crate) fn component_heartbeat_packet(
+    system_id: u8,
+    component_id: u8,
+    component_type: u8,
+) -> Vec<u8> {
     let mut msg = Vec::with_capacity(9);
     msg.extend_from_slice(&0u32.to_le_bytes());
     msg.push(component_type);
@@ -211,10 +362,20 @@ pub(crate) fn component_heartbeat_packet(system_id: u8, component_id: u8, compon
     build_v2_packet(system_id, component_id, 0, &msg, 50)
 }
 
-pub(crate) fn camera_information_packet(system_id: u8, callsign: &str) -> Vec<u8> {
+pub(crate) fn camera_information_packet_for_component(
+    system_id: u8,
+    component_id: u8,
+    callsign: &str,
+    gimbal_device_id: u8,
+) -> Vec<u8> {
     let vendor = fixed_string::<32>("ArmaTAK");
     let model = fixed_string::<32>(callsign);
     let cam_definition_uri = fixed_string::<140>("");
+    let flags = CAMERA_CAP_FLAGS_CAPTURE_VIDEO
+        | CAMERA_CAP_FLAGS_CAPTURE_IMAGE
+        | CAMERA_CAP_FLAGS_HAS_MODES
+        | CAMERA_CAP_FLAGS_HAS_BASIC_ZOOM
+        | CAMERA_CAP_FLAGS_HAS_VIDEO_STREAM;
 
     let mut msg = Vec::with_capacity(235);
     msg.extend_from_slice(&(Utc::now().timestamp_millis().max(0) as u32).to_le_bytes());
@@ -222,7 +383,7 @@ pub(crate) fn camera_information_packet(system_id: u8, callsign: &str) -> Vec<u8
     msg.extend_from_slice(&2.8f32.to_le_bytes());
     msg.extend_from_slice(&6.4f32.to_le_bytes());
     msg.extend_from_slice(&4.8f32.to_le_bytes());
-    msg.extend_from_slice(&CAMERA_CAP_FLAGS_HAS_VIDEO_STREAM.to_le_bytes());
+    msg.extend_from_slice(&flags.to_le_bytes());
     msg.extend_from_slice(&1280u16.to_le_bytes());
     msg.extend_from_slice(&720u16.to_le_bytes());
     msg.extend_from_slice(&0u16.to_le_bytes());
@@ -230,17 +391,21 @@ pub(crate) fn camera_information_packet(system_id: u8, callsign: &str) -> Vec<u8
     msg.extend_from_slice(&model);
     msg.push(0);
     msg.extend_from_slice(&cam_definition_uri);
-    msg.push(GIMBAL_COMPONENT_ID);
+    msg.push(gimbal_device_id);
     msg.push(0);
 
-    build_v2_packet(system_id, CAMERA_COMPONENT_ID, 259, &msg, 92)
+    build_v2_packet(system_id, component_id, 259, &msg, 92)
 }
 
-pub(crate) fn video_stream_information_packet(
+pub(crate) fn video_stream_information_packet_for_component(
     system_id: u8,
+    component_id: u8,
     callsign: &str,
     video_uri: &str,
     hfov_deg: f32,
+    stream_id: u8,
+    stream_count: u8,
+    thermal: bool,
 ) -> Vec<u8> {
     let stream_type = if video_uri.starts_with("rtsp://") {
         VIDEO_STREAM_TYPE_RTSP
@@ -254,42 +419,195 @@ pub(crate) fn video_stream_information_packet(
         VIDEO_STREAM_TYPE_RTSP
     };
 
-    let name = fixed_string::<32>(&format!("{} Video", callsign));
+    let name = fixed_string::<32>(callsign);
     let uri = fixed_string::<160>(video_uri);
+    let flags = VIDEO_STREAM_STATUS_FLAGS_RUNNING | if thermal { 2 } else { 0 };
 
     let mut msg = Vec::with_capacity(208);
     msg.extend_from_slice(&30f32.to_le_bytes());
     msg.extend_from_slice(&4_000_000u32.to_le_bytes());
-    msg.extend_from_slice(&VIDEO_STREAM_STATUS_FLAGS_RUNNING.to_le_bytes());
+    msg.extend_from_slice(&flags.to_le_bytes());
     msg.extend_from_slice(&1280u16.to_le_bytes());
     msg.extend_from_slice(&720u16.to_le_bytes());
     msg.extend_from_slice(&0u16.to_le_bytes());
     msg.extend_from_slice(&(hfov_deg.clamp(1.0, 360.0).round() as u16).to_le_bytes());
-    msg.push(1);
+    msg.push(stream_id);
-    msg.push(1);
+    msg.push(stream_count);
     msg.push(stream_type);
     msg.extend_from_slice(&name);
     msg.extend_from_slice(&uri);
     msg.push(VIDEO_STREAM_ENCODING_H264);
     msg.push(0);
 
-    build_v2_packet(system_id, CAMERA_COMPONENT_ID, 269, &msg, 109)
+    build_v2_packet(system_id, component_id, 269, &msg, 109)
 }
 
-pub(crate) fn video_stream_status_packet(
+pub(crate) fn video_stream_status_packet_for_component(
     system_id: u8,
+    component_id: u8,
     hfov_deg: f32,
+    stream_id: u8,
+    thermal: bool,
 ) -> Vec<u8> {
+    let flags = VIDEO_STREAM_STATUS_FLAGS_RUNNING | if thermal { 2 } else { 0 };
     let mut msg = Vec::with_capacity(19);
     msg.extend_from_slice(&30f32.to_le_bytes());
     msg.extend_from_slice(&4_000_000u32.to_le_bytes());
-    msg.extend_from_slice(&VIDEO_STREAM_STATUS_FLAGS_RUNNING.to_le_bytes());
+    msg.extend_from_slice(&flags.to_le_bytes());
     msg.extend_from_slice(&1280u16.to_le_bytes());
     msg.extend_from_slice(&720u16.to_le_bytes());
     msg.extend_from_slice(&0u16.to_le_bytes());
     msg.extend_from_slice(&(hfov_deg.clamp(1.0, 360.0).round() as u16).to_le_bytes());
-    msg.push(1);
+    msg.push(stream_id);
     msg.push(0);
 
-    build_v2_packet(system_id, CAMERA_COMPONENT_ID, 270, &msg, 59)
+    build_v2_packet(system_id, component_id, 270, &msg, 59)
+}
+
+pub(crate) fn mount_orientation_packet_for_component(
+    system_id: u8,
+    component_id: u8,
+    pitch_deg: f32,
+    yaw_deg: f32,
+) -> Vec<u8> {
+    let mut msg = Vec::with_capacity(16);
+    msg.extend_from_slice(&(Utc::now().timestamp_millis().max(0) as u32).to_le_bytes());
+    msg.extend_from_slice(&pitch_deg.to_le_bytes());
+    msg.extend_from_slice(&0f32.to_le_bytes());
+    msg.extend_from_slice(&normalize_heading_deg(yaw_deg).to_le_bytes());
+
+    build_v2_packet(system_id, component_id, 265, &msg, 26)
+}
+
+pub(crate) fn camera_fov_status_packet_for_component(
+    system_id: u8,
+    component_id: u8,
+    lat_camera_deg: f64,
+    lon_camera_deg: f64,
+    alt_camera_msl_m: f32,
+    lat_image_deg: f64,
+    lon_image_deg: f64,
+    alt_image_msl_m: f32,
+    roll_deg: f32,
+    pitch_deg: f32,
+    yaw_deg: f32,
+    hfov_deg: f32,
+    vfov_deg: f32,
+) -> Vec<u8> {
+    let q = attitude_quaternion(roll_deg, pitch_deg, yaw_deg);
+    let mut msg = Vec::with_capacity(53);
+    msg.extend_from_slice(&(Utc::now().timestamp_millis().max(0) as u32).to_le_bytes());
+    msg.extend_from_slice(&((lat_camera_deg * 1e7).round() as i32).to_le_bytes());
+    msg.extend_from_slice(&((lon_camera_deg * 1e7).round() as i32).to_le_bytes());
+    msg.extend_from_slice(&((alt_camera_msl_m * 1000.0).round() as i32).to_le_bytes());
+    msg.extend_from_slice(&((lat_image_deg * 1e7).round() as i32).to_le_bytes());
+    msg.extend_from_slice(&((lon_image_deg * 1e7).round() as i32).to_le_bytes());
+    msg.extend_from_slice(&((alt_image_msl_m * 1000.0).round() as i32).to_le_bytes());
+    for value in q {
+        msg.extend_from_slice(&value.to_le_bytes());
+    }
+    msg.extend_from_slice(&hfov_deg.to_le_bytes());
+    msg.extend_from_slice(&vfov_deg.to_le_bytes());
+    msg.push(0);
+
+    build_v2_packet(system_id, component_id, 271, &msg, 22)
+}
+
+pub(crate) fn mount_status_packet(
+    system_id: u8,
+    pitch_deg: f32,
+    roll_deg: f32,
+    relative_yaw_deg: f32,
+) -> Vec<u8> {
+    let mut msg = Vec::with_capacity(15);
+    msg.extend_from_slice(&((pitch_deg * 100.0).round() as i32).to_le_bytes());
+    msg.extend_from_slice(&((roll_deg * 100.0).round() as i32).to_le_bytes());
+    msg.extend_from_slice(&((relative_yaw_deg * 100.0).round() as i32).to_le_bytes());
+    msg.push(system_id);
+    msg.push(GIMBAL_COMPONENT_ID);
+    msg.push(2);
+
+    build_v2_packet(system_id, AUTOPILOT_COMPONENT_ID, 158, &msg, 134)
+}
+
+pub(crate) fn gimbal_manager_information_packet(system_id: u8) -> Vec<u8> {
+    let fields = [
+        FieldSpec {
+            ty: "uint32_t",
+            name: "time_boot_ms",
+            array_len: 0,
+        },
+        FieldSpec {
+            ty: "uint32_t",
+            name: "cap_flags",
+            array_len: 0,
+        },
+        FieldSpec {
+            ty: "float",
+            name: "roll_min",
+            array_len: 0,
+        },
+        FieldSpec {
+            ty: "float",
+            name: "roll_max",
+            array_len: 0,
+        },
+        FieldSpec {
+            ty: "float",
+            name: "pitch_min",
+            array_len: 0,
+        },
+        FieldSpec {
+            ty: "float",
+            name: "pitch_max",
+            array_len: 0,
+        },
+        FieldSpec {
+            ty: "float",
+            name: "yaw_min",
+            array_len: 0,
+        },
+        FieldSpec {
+            ty: "float",
+            name: "yaw_max",
+            array_len: 0,
+        },
+        FieldSpec {
+            ty: "uint8_t",
+            name: "gimbal_device_id",
+            array_len: 0,
+        },
+    ];
+    let crc_extra = calculate_crc_extra("GIMBAL_MANAGER_INFORMATION", &fields);
+
+    let mut msg = Vec::with_capacity(33);
+    msg.extend_from_slice(&(Utc::now().timestamp_millis().max(0) as u32).to_le_bytes());
+    msg.extend_from_slice(&GIMBAL_MANAGER_CAP_FLAGS_BASIC_PITCH_YAW.to_le_bytes());
+    msg.extend_from_slice(&0f32.to_le_bytes());
+    msg.extend_from_slice(&0f32.to_le_bytes());
+    msg.extend_from_slice(&(-90f32).to_radians().to_le_bytes());
+    msg.extend_from_slice(&30f32.to_radians().to_le_bytes());
+    msg.extend_from_slice(&(-180f32).to_radians().to_le_bytes());
+    msg.extend_from_slice(&180f32.to_radians().to_le_bytes());
+    msg.push(GIMBAL_COMPONENT_ID);
+
+    build_v2_packet(system_id, GIMBAL_COMPONENT_ID, 280, &msg, crc_extra)
+}
+
+fn attitude_quaternion(roll_deg: f32, pitch_deg: f32, yaw_deg: f32) -> [f32; 4] {
+    let (roll, pitch, yaw) = (
+        roll_deg.to_radians(),
+        pitch_deg.to_radians(),
+        normalize_heading_deg(yaw_deg).to_radians(),
+    );
+    let (sr, cr) = (roll * 0.5).sin_cos();
+    let (sp, cp) = (pitch * 0.5).sin_cos();
+    let (sy, cy) = (yaw * 0.5).sin_cos();
+
+    [
+        cr * cp * cy + sr * sp * sy,
+        sr * cp * cy - cr * sp * sy,
+        cr * sp * cy + sr * cp * sy,
+        cr * cp * sy - sr * sp * cy,
+    ]
 }

src/uas/payload.rs

@@ -42,6 +42,7 @@ pub struct UasSystemPayload {
     pub image_lat_deg: f64,
     pub image_lon_deg: f64,
     pub image_alt_msl_m: f32,
+    pub has_turret_camera: bool,
 }
 
 impl FromArma for UasTelemetryPayload {
@@ -123,6 +124,7 @@ impl FromArma for UasSystemPayload {
             image_lat_deg,
             image_lon_deg,
             image_alt_msl_m,
+            has_turret_camera,
         ) = <(
             String,
             String,
@@ -147,6 +149,7 @@ impl FromArma for UasSystemPayload {
             f64,
             f64,
             f32,
+            i32,
         )>::from_arma(data)?;
 
         Ok(Self {
@@ -173,6 +176,7 @@ impl FromArma for UasSystemPayload {
             image_lat_deg,
             image_lon_deg,
             image_alt_msl_m,
+            has_turret_camera: has_turret_camera != 0,
         })
     }
 }

src/uas/send.rs

@@ -2,16 +2,25 @@ use arma_rs::Context;
 use log::info;
 use std::net::UdpSocket;
 
-use super::constants::{AUTOPILOT_COMPONENT_ID, CAMERA_COMPONENT_ID, GIMBAL_COMPONENT_ID, MAV_TYPE_CAMERA, MAV_TYPE_GIMBAL};
+use super::constants::{
+    AUTOPILOT_COMPONENT_ID, CAMERA_COMPONENT_ID, GIMBAL_COMPONENT_ID, MAV_TYPE_CAMERA,
+    MAV_TYPE_GIMBAL, TURRET_CAMERA_COMPONENT_ID,
+};
 use super::endpoint::socket_for_send;
-use super::identity::{map_vehicle_type, should_send_video_stream_information, stable_system_id};
+use super::identity::{
+    map_vehicle_type, should_send_video_stream_information, stable_mavlink_identity,
+    stable_system_id,
+};
 use super::packets::{
-    attitude_packet, autopilot_version_packet, camera_information_packet,
+    attitude_packet, autopilot_version_packet, camera_fov_status_packet_for_component,
-    component_heartbeat_packet, extended_sys_state_packet, global_position_int_packet,
+    camera_information_packet_for_component, component_heartbeat_packet, extended_sys_state_packet,
-    gps_raw_int_packet, heartbeat_packet, system_status_packet, vfr_hud_packet,
+    gimbal_manager_information_packet, global_position_int_packet, gps_raw_int_packet,
-    video_stream_information_packet, video_stream_status_packet,
+    heartbeat_packet, home_position_packet, mount_orientation_packet_for_component,
+    mount_status_packet, system_status_packet, vfr_hud_packet,
+    video_stream_information_packet_for_component, video_stream_status_packet_for_component,
 };
 use super::payload::{UasSystemPayload, UasTelemetryPayload};
+use super::state::{latest_system, record_system};
 
 fn sending_socket(ctx: &Context, error_prefix: &str) -> Result<UdpSocket, &'static str> {
     if let Some(socket) = socket_for_send() {
@@ -21,7 +30,11 @@ fn sending_socket(ctx: &Context, error_prefix: &str) -> Result<UdpSocket, &'stat
     match UdpSocket::bind("0.0.0.0:0") {
         Ok(socket) => Ok(socket),
         Err(error) => {
-            let _ = ctx.callback_data("MAVLINK MOCK ERROR", "Failed to bind UDP socket", error.to_string());
+            let _ = ctx.callback_data(
+                "MAVLINK MOCK ERROR",
+                "Failed to bind UDP socket",
+                error.to_string(),
+            );
             info!("{} failed to bind UDP socket: {}", error_prefix, error);
             Err("Failed to bind MAVLink mock socket")
         }
@@ -58,24 +71,41 @@ pub fn send_uas_telemetry(ctx: Context, payload: UasTelemetryPayload) -> &'stati
 
     for (index, packet) in packets.iter().enumerate() {
         if let Err(error) = socket.send_to(packet, &payload.address) {
-            let _ = ctx.callback_data("MAVLINK MOCK ERROR", "Failed to send MAVLink packet", error.to_string());
+            let _ = ctx.callback_data(
-            info!("MAVLink mock failed sending packet {} to {}: {}", index, payload.address, error);
+                "MAVLINK MOCK ERROR",
+                "Failed to send MAVLink packet",
+                error.to_string(),
+            );
+            info!(
+                "MAVLink mock failed sending packet {} to {}: {}",
+                index, payload.address, error
+            );
             return "Failed to send MAVLink mock telemetry";
         }
     }
 
-    info!("MAVLink mock sent {} packets to {}", packets.len(), payload.address);
+    info!(
+        "MAVLink mock sent {} packets to {}",
+        packets.len(),
+        payload.address
+    );
     "Sent MAVLink mock telemetry"
 }
 
 pub fn send_uas_system(ctx: Context, payload: UasSystemPayload) -> &'static str {
-    let system_id = stable_system_id(&payload.entity_uuid);
+    let mavlink_identity = stable_mavlink_identity(&payload.callsign, &payload.entity_uuid);
+    let system_id = stable_system_id(&mavlink_identity);
     let vehicle_type = map_vehicle_type(payload.vehicle_type);
+    record_system(system_id, &mavlink_identity, &payload);
+    let active_camera_component = latest_system(system_id)
+        .map(|system| system.active_camera_component)
+        .unwrap_or(CAMERA_COMPONENT_ID);
 
     info!(
-        "MAVLink system send requested to {} entity_uuid={} sysid={} callsign={} lat={} lon={} alt_msl={} rel_alt={} heading={} gimbal_pitch={} gimbal_yaw={} video_uri={}",
+        "MAVLink system send requested to {} entity_uuid={} mavlink_identity={} sysid={} callsign={} lat={} lon={} alt_msl={} rel_alt={} heading={} gimbal_pitch={} gimbal_yaw={} video_uri={}",
         payload.address,
         payload.entity_uuid,
+        mavlink_identity,
         system_id,
         payload.callsign,
         payload.lat_deg,
@@ -92,6 +122,46 @@ pub fn send_uas_system(ctx: Context, payload: UasSystemPayload) -> &'static str
         Ok(socket) => socket,
         Err(message) => return message,
     };
+    let (fpv_image_lat, fpv_image_lon, fpv_image_alt) = fpv_image_point(
+        payload.lat_deg,
+        payload.lon_deg,
+        payload.alt_msl_m,
+        payload.rel_alt_m,
+        payload.pitch_deg,
+        payload.yaw_deg,
+    );
+    let active_is_turret =
+        payload.has_turret_camera && active_camera_component == TURRET_CAMERA_COMPONENT_ID;
+    info!(
+        "MAVLink active camera sysid={} active_component={} has_turret={} active_is_turret={}",
+        system_id, active_camera_component, payload.has_turret_camera, active_is_turret
+    );
+    let (
+        primary_pitch,
+        primary_roll,
+        primary_yaw,
+        primary_image_lat,
+        primary_image_lon,
+        primary_image_alt,
+    ) = if active_is_turret {
+        (
+            payload.gimbal_pitch_deg,
+            payload.gimbal_roll_deg,
+            payload.gimbal_yaw_deg,
+            payload.image_lat_deg,
+            payload.image_lon_deg,
+            payload.image_alt_msl_m,
+        )
+    } else {
+        (
+            payload.pitch_deg,
+            payload.roll_deg,
+            payload.yaw_deg,
+            fpv_image_lat,
+            fpv_image_lon,
+            fpv_image_alt,
+        )
+    };
 
     let autopilot_payload = UasTelemetryPayload {
         address: payload.address.clone(),
@@ -118,25 +188,139 @@ pub fn send_uas_system(ctx: Context, payload: UasSystemPayload) -> &'static str
         vfr_hud_packet(&autopilot_payload),
         system_status_packet(system_id),
         extended_sys_state_packet(system_id, payload.flying),
-        autopilot_version_packet(system_id, &payload.entity_uuid),
+        autopilot_version_packet(system_id, &mavlink_identity),
+        home_position_packet(
+            system_id,
+            payload.lat_deg,
+            payload.lon_deg,
+            payload.alt_msl_m - payload.rel_alt_m,
+            payload.heading_deg,
+        ),
         component_heartbeat_packet(system_id, CAMERA_COMPONENT_ID, MAV_TYPE_CAMERA),
         component_heartbeat_packet(system_id, GIMBAL_COMPONENT_ID, MAV_TYPE_GIMBAL),
-        camera_information_packet(system_id, &payload.callsign),
+        camera_information_packet_for_component(
+            system_id,
+            CAMERA_COMPONENT_ID,
+            &format!("{} FPV", payload.callsign),
+            0,
+        ),
+        mount_orientation_packet_for_component(
+            system_id,
+            CAMERA_COMPONENT_ID,
+            primary_pitch,
+            primary_yaw,
+        ),
+        camera_fov_status_packet_for_component(
+            system_id,
+            CAMERA_COMPONENT_ID,
+            payload.lat_deg,
+            payload.lon_deg,
+            payload.alt_msl_m,
+            primary_image_lat,
+            primary_image_lon,
+            primary_image_alt,
+            primary_roll,
+            primary_pitch,
+            primary_yaw,
+            payload.hfov_deg,
+            payload.vfov_deg,
+        ),
+        gimbal_manager_information_packet(system_id),
     ];
 
+    if payload.has_turret_camera {
+        packets.push(component_heartbeat_packet(
+            system_id,
+            TURRET_CAMERA_COMPONENT_ID,
+            MAV_TYPE_CAMERA,
+        ));
+        packets.push(camera_information_packet_for_component(
+            system_id,
+            TURRET_CAMERA_COMPONENT_ID,
+            &format!("{} Turret", payload.callsign),
+            GIMBAL_COMPONENT_ID,
+        ));
+        packets.push(mount_orientation_packet_for_component(
+            system_id,
+            TURRET_CAMERA_COMPONENT_ID,
+            payload.gimbal_pitch_deg,
+            payload.gimbal_yaw_deg,
+        ));
+        packets.push(camera_fov_status_packet_for_component(
+            system_id,
+            TURRET_CAMERA_COMPONENT_ID,
+            payload.lat_deg,
+            payload.lon_deg,
+            payload.alt_msl_m,
+            payload.image_lat_deg,
+            payload.image_lon_deg,
+            payload.image_alt_msl_m,
+            payload.gimbal_roll_deg,
+            payload.gimbal_pitch_deg,
+            payload.gimbal_yaw_deg,
+            payload.hfov_deg,
+            payload.vfov_deg,
+        ));
+    }
+
+    let (active_pitch, active_roll, active_relative_yaw) = if active_is_turret {
+        (
+            payload.gimbal_pitch_deg,
+            payload.gimbal_roll_deg,
+            normalize_signed_deg(payload.gimbal_yaw_deg - payload.yaw_deg),
+        )
+    } else {
+        (payload.pitch_deg, payload.roll_deg, 0.0)
+    };
+    packets.push(mount_status_packet(
+        system_id,
+        active_pitch,
+        active_roll,
+        active_relative_yaw,
+    ));
+
     if should_send_video_stream_information(&payload.video_uri) {
         info!(
             "Sending VIDEO_STREAM_INFORMATION for sysid={} uri={}",
-            system_id,
+            system_id, payload.video_uri
-            payload.video_uri
         );
-        packets.push(video_stream_information_packet(
+        packets.push(video_stream_information_packet_for_component(
             system_id,
-            &payload.callsign,
+            CAMERA_COMPONENT_ID,
+            &format!("{} FPV", payload.callsign),
             &payload.video_uri,
             payload.hfov_deg,
+            1,
+            1,
+            false,
         ));
-        packets.push(video_stream_status_packet(system_id, payload.hfov_deg));
+        packets.push(video_stream_status_packet_for_component(
+            system_id,
+            CAMERA_COMPONENT_ID,
+            payload.hfov_deg,
+            1,
+            false,
+        ));
+
+        if payload.has_turret_camera {
+            packets.push(video_stream_information_packet_for_component(
+                system_id,
+                TURRET_CAMERA_COMPONENT_ID,
+                &format!("{} Turret", payload.callsign),
+                &payload.video_uri,
+                payload.hfov_deg,
+                1,
+                1,
+                false,
+            ));
+            packets.push(video_stream_status_packet_for_component(
+                system_id,
+                TURRET_CAMERA_COMPONENT_ID,
+                payload.hfov_deg,
+                1,
+                false,
+            ));
+        }
     } else if !payload.video_uri.trim().is_empty() {
         info!(
             "Skipping VIDEO_STREAM_INFORMATION for sysid={} because URI is not a supported stream URI: {}",
@@ -146,8 +330,15 @@ pub fn send_uas_system(ctx: Context, payload: UasSystemPayload) -> &'static str
 
     for (index, packet) in packets.iter().enumerate() {
         if let Err(error) = socket.send_to(packet, &payload.address) {
-            let _ = ctx.callback_data("MAVLINK MOCK ERROR", "Failed to send MAVLink packet", error.to_string());
+            let _ = ctx.callback_data(
-            info!("MAVLink system failed sending packet {} to {}: {}", index, payload.address, error);
+                "MAVLINK MOCK ERROR",
+                "Failed to send MAVLink packet",
+                error.to_string(),
+            );
+            info!(
+                "MAVLink system failed sending packet {} to {}: {}",
+                index, payload.address, error
+            );
             return "Failed to send MAVLink system telemetry";
         }
     }
@@ -162,3 +353,38 @@ pub fn send_uas_system(ctx: Context, payload: UasSystemPayload) -> &'static str
     );
     "Sent MAVLink system telemetry"
 }
+
+fn normalize_signed_deg(value: f32) -> f32 {
+    let normalized = ((value % 360.0) + 360.0) % 360.0;
+    if normalized > 180.0 {
+        normalized - 360.0
+    } else {
+        normalized
+    }
+}
+
+fn fpv_image_point(
+    lat_deg: f64,
+    lon_deg: f64,
+    alt_msl_m: f32,
+    rel_alt_m: f32,
+    pitch_deg: f32,
+    yaw_deg: f32,
+) -> (f64, f64, f32) {
+    let pitch_rad = pitch_deg.to_radians();
+    let vertical = (-pitch_rad.sin()).max(0.01);
+    let slant_m = (rel_alt_m.max(1.0) / vertical).clamp(1.0, 15_000.0);
+    let ground_m = slant_m * pitch_rad.cos().abs();
+    let yaw_rad = yaw_deg.to_radians();
+    let north_m = ground_m * yaw_rad.cos();
+    let east_m = ground_m * yaw_rad.sin();
+    let lat_rad = lat_deg.to_radians();
+    let meters_per_degree_lat = 111_320.0;
+    let meters_per_degree_lon = (111_320.0 * lat_rad.cos().abs()).max(1.0);
+
+    (
+        lat_deg + north_m as f64 / meters_per_degree_lat,
+        lon_deg + east_m as f64 / meters_per_degree_lon,
+        alt_msl_m - rel_alt_m,
+    )
+}

src/uas/state.rs

@@ -0,0 +1,82 @@
+use std::collections::HashMap;
+use std::sync::Mutex;
+
+use lazy_static::lazy_static;
+
+use super::payload::UasSystemPayload;
+
+#[derive(Clone)]
+pub(crate) struct LatestUasSystem {
+    pub mavlink_identity: String,
+    pub callsign: String,
+    pub lat_deg: f64,
+    pub lon_deg: f64,
+    pub alt_msl_m: f32,
+    pub rel_alt_m: f32,
+    pub heading_deg: f32,
+    pub fpv_pitch_deg: f32,
+    pub fpv_yaw_deg: f32,
+    pub gimbal_pitch_deg: f32,
+    pub gimbal_yaw_deg: f32,
+    pub video_uri: String,
+    pub hfov_deg: f32,
+    pub vfov_deg: f32,
+    pub image_lat_deg: f64,
+    pub image_lon_deg: f64,
+    pub image_alt_msl_m: f32,
+    pub has_turret_camera: bool,
+    pub active_camera_component: u8,
+}
+
+lazy_static! {
+    static ref LATEST_UAS_SYSTEMS: Mutex<HashMap<u8, LatestUasSystem>> = Mutex::new(HashMap::new());
+}
+
+pub(crate) fn record_system(system_id: u8, mavlink_identity: &str, payload: &UasSystemPayload) {
+    if let Ok(mut systems) = LATEST_UAS_SYSTEMS.lock() {
+        let active_camera_component = systems
+            .get(&system_id)
+            .map(|system| system.active_camera_component)
+            .unwrap_or(super::constants::CAMERA_COMPONENT_ID);
+
+        systems.insert(
+            system_id,
+            LatestUasSystem {
+                mavlink_identity: mavlink_identity.to_string(),
+                callsign: payload.callsign.clone(),
+                lat_deg: payload.lat_deg,
+                lon_deg: payload.lon_deg,
+                alt_msl_m: payload.alt_msl_m,
+                rel_alt_m: payload.rel_alt_m,
+                heading_deg: payload.heading_deg,
+                fpv_pitch_deg: payload.pitch_deg,
+                fpv_yaw_deg: payload.yaw_deg,
+                gimbal_pitch_deg: payload.gimbal_pitch_deg,
+                gimbal_yaw_deg: payload.gimbal_yaw_deg,
+                video_uri: payload.video_uri.clone(),
+                hfov_deg: payload.hfov_deg,
+                vfov_deg: payload.vfov_deg,
+                image_lat_deg: payload.image_lat_deg,
+                image_lon_deg: payload.image_lon_deg,
+                image_alt_msl_m: payload.image_alt_msl_m,
+                has_turret_camera: payload.has_turret_camera,
+                active_camera_component,
+            },
+        );
+    }
+}
+
+pub(crate) fn latest_system(system_id: u8) -> Option<LatestUasSystem> {
+    LATEST_UAS_SYSTEMS
+        .lock()
+        .ok()
+        .and_then(|systems| systems.get(&system_id).cloned())
+}
+
+pub(crate) fn set_active_camera(system_id: u8, component_id: u8) {
+    if let Ok(mut systems) = LATEST_UAS_SYSTEMS.lock() {
+        if let Some(system) = systems.get_mut(&system_id) {
+            system.active_camera_component = component_id;
+        }
+    }
+}