diff --git a/addons/main/functions/extract_data/fn_extract_marker_callsign.sqf b/addons/main/functions/extract_data/fn_extract_marker_callsign.sqf index e38d41b..bc0c2a0 100644 --- a/addons/main/functions/extract_data/fn_extract_marker_callsign.sqf +++ b/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 (_markerCallsignOverride isNotEqualTo "") then { - _callsign = _markerCallsignOverride; +if (_callsign isEqualTo "") then { + _callsign = _displayName; }; _callsign diff --git a/addons/main/functions/extract_data/fn_extract_uas_camera_data.sqf b/addons/main/functions/extract_data/fn_extract_uas_camera_data.sqf index d4347d1..a21dc30 100644 --- a/addons/main/functions/extract_data/fn_extract_uas_camera_data.sqf +++ b/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,16 +22,18 @@ private _cameraDir = []; private _spiASL = []; private _slantRange = 0; -private _laserTarget = laserTarget _drone; -if (!isNull _laserTarget) then { - private _laserTargetWorld = getPosWorld _laserTarget; - private _laserTargetAslZ = (getPosASL _laserTarget) select 2; - _spiASL = [_laserTargetWorld select 0, _laserTargetWorld select 1, _laserTargetAslZ]; - _cameraDir = _spiASL vectorDiff _originASL; - _slantRange = _originASL vectorDistance _spiASL; +if (_cameraMode isNotEqualTo "fpv") then { + private _laserTarget = laserTarget _drone; + if (!isNull _laserTarget) then { + private _laserTargetWorld = getPosWorld _laserTarget; + private _laserTargetAslZ = (getPosASL _laserTarget) select 2; + _spiASL = [_laserTargetWorld select 0, _laserTargetWorld select 1, _laserTargetAslZ]; + _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 = []; diff --git a/addons/uav/functions/fnc_updateMavlinkBroadcast.sqf b/addons/uav/functions/fnc_updateMavlinkBroadcast.sqf index f7d0286..1cf18fa 100644 --- a/addons/uav/functions/fnc_updateMavlinkBroadcast.sqf +++ b/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 (!isNull _laser) then { - private _originASL = getPosASL _uav; - private _targetWorld = getPosWorld _laser; - private _targetAslZ = (getPosASL _laser) select 2; - private _spiASL = [_targetWorld select 0, _targetWorld select 1, _targetAslZ]; +if (_cameraData isEqualType [] && {(count _cameraData) >= 6}) then { + _gimbalYaw = _cameraData param [0, _yaw]; + _gimbalPitch = _cameraData param [1, _pitch]; + _hfov = _cameraData param [2, _hfov]; + _vfov = _uav getVariable ["armatak_uas_vfov", (_hfov * 0.5625)]; - private _los = _spiASL vectorDiff _originASL; - private _losMag = vectorMagnitude _los; - - if (_losMag > 0) then { - _los = _los vectorMultiply (1 / _losMag); - - 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; + private _spiGeo = _cameraData param [5, []]; + if (_spiGeo isEqualType [] && {(count _spiGeo) >= 3}) then { + _imageLat = _spiGeo select 0; + _imageLon = _spiGeo select 1; + _imageAlt = _spiGeo select 2; }; }; @@ -113,7 +101,8 @@ private _systemPayload = [ _vfov, _imageLat, _imageLon, - _imageAlt + _imageAlt, + parseNumber _hasTurretCamera ]; "armatak" callExtension ["uas:send_uas_system", [_systemPayload]]; diff --git a/src/uas/callbacks.rs b/src/uas/callbacks.rs index c2603dc..36a9ce0 100644 --- a/src/uas/callbacks.rs +++ b/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::>() @@ -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 { 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= 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 confirmation = *payload.get(32)?; + let target_component = payload.get(31).copied().unwrap_or(0); + 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 None, } } + +pub(crate) fn mavlink_response_packets(bytes: &[u8]) -> Vec> { + 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> { + 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> { + 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> { + 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, + ) +} diff --git a/src/uas/constants.rs b/src/uas/constants.rs index e4e9d49..e3439e6 100644 --- a/src/uas/constants.rs +++ b/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; diff --git a/src/uas/endpoint.rs b/src/uas/endpoint.rs index 507b44e..1c6935a 100644 --- a/src/uas/endpoint.rs +++ b/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> = Mutex::new(None); pub(crate) fn socket_for_send() -> Option { - MAVLINK_ENDPOINT - .lock() - .ok() - .and_then(|endpoint| endpoint.as_ref().and_then(|entry| entry.socket.try_clone().ok())) + MAVLINK_ENDPOINT.lock().ok().and_then(|endpoint| { + endpoint + .as_ref() + .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) { - let _ = listener_ctx.callback_data("MAVLINK UDP", event.function, event.data); + if let Some(event) = mavlink_callback_event(&buffer[..received], &source_string) + { + 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) { diff --git a/src/uas/identity.rs b/src/uas/identity.rs index 1c54152..2c8af29 100644 --- a/src/uas/identity.rs +++ b/src/uas/identity.rs @@ -1,6 +1,4 @@ -use super::constants::{ - MAV_TYPE_FIXED_WING, MAV_TYPE_HELICOPTER, MAV_TYPE_QUADROTOR, -}; +use super::constants::{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]; diff --git a/src/uas/mod.rs b/src/uas/mod.rs index a6a10ae..34b1b8d 100644 --- a/src/uas/mod.rs +++ b/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)] diff --git a/src/uas/packets.rs b/src/uas/packets.rs index df0c2fd..3a3c1d7 100644 --- a/src/uas/packets.rs +++ b/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, - GIMBAL_COMPONENT_ID, MAV_AUTOPILOT_GENERIC, MAV_AUTOPILOT_INVALID, - MAV_LANDED_STATE_IN_AIR, MAV_LANDED_STATE_ON_GROUND, MAV_LANDED_STATE_UNDEFINED, - MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, MAV_MODE_FLAG_SAFETY_ARMED, - MAV_PROTOCOL_CAPABILITY_COMMAND_INT, MAV_PROTOCOL_CAPABILITY_COMPONENT_IMPLEMENTS_GIMBAL_MANAGER, - MAV_PROTOCOL_CAPABILITY_FTP, MAV_PROTOCOL_CAPABILITY_MAVLINK2, - MAV_PROTOCOL_CAPABILITY_MISSION_INT, MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_GLOBAL_INT, - MAV_STATE_ACTIVE, MAV_STATE_STANDBY, 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, + AUTOPILOT_COMPONENT_ID, CAMERA_CAP_FLAGS_CAPTURE_IMAGE, CAMERA_CAP_FLAGS_CAPTURE_VIDEO, + CAMERA_CAP_FLAGS_HAS_BASIC_ZOOM, CAMERA_CAP_FLAGS_HAS_MODES, CAMERA_CAP_FLAGS_HAS_VIDEO_STREAM, + GIMBAL_COMPONENT_ID, GIMBAL_MANAGER_CAP_FLAGS_BASIC_PITCH_YAW, MAV_AUTOPILOT_ARDUPILOTMEGA, + MAV_AUTOPILOT_INVALID, MAV_LANDED_STATE_IN_AIR, MAV_LANDED_STATE_ON_GROUND, + MAV_LANDED_STATE_UNDEFINED, MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, MAV_MODE_FLAG_SAFETY_ARMED, + MAV_PROTOCOL_CAPABILITY_COMMAND_INT, + MAV_PROTOCOL_CAPABILITY_COMPONENT_IMPLEMENTS_GIMBAL_MANAGER, MAV_PROTOCOL_CAPABILITY_FTP, + MAV_PROTOCOL_CAPABILITY_MAVLINK2, MAV_PROTOCOL_CAPABILITY_MISSION_INT, + MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_GLOBAL_INT, MAV_STATE_ACTIVE, MAV_STATE_STANDBY, + 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 { 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 { 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 { + 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 { 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 { pub(crate) fn global_position_int_packet(payload: &UasTelemetryPayload) -> Vec { 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 vy = (payload.groundspeed_mps * payload.heading_deg.to_radians().cos() * 100.0).round() as i16; + let vx = + (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 { pub(crate) fn system_status_packet(system_id: u8) -> Vec { let fields = [ - FieldSpec { ty: "uint32_t", name: "onboard_control_sensors_present", array_len: 0 }, - FieldSpec { ty: "uint32_t", name: "onboard_control_sensors_enabled", array_len: 0 }, - FieldSpec { ty: "uint32_t", 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 }, + FieldSpec { + ty: "uint32_t", + name: "onboard_control_sensors_present", + array_len: 0, + }, + FieldSpec { + ty: "uint32_t", + name: "onboard_control_sensors_enabled", + array_len: 0, + }, + FieldSpec { + ty: "uint32_t", + 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 { pub(crate) fn extended_sys_state_packet(system_id: u8, flying: bool) -> Vec { let fields = [ - FieldSpec { ty: "uint8_t", name: "vtol_state", array_len: 0 }, - FieldSpec { ty: "uint8_t", name: "landed_state", array_len: 0 }, + FieldSpec { + 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 pub(crate) fn autopilot_version_packet(system_id: u8, entity_uuid: &str) -> Vec { let fields = [ - FieldSpec { ty: "uint64_t", name: "capabilities", array_len: 0 }, - FieldSpec { ty: "uint32_t", name: "flight_sw_version", 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 }, + FieldSpec { + ty: "uint64_t", + name: "capabilities", + array_len: 0, + }, + FieldSpec { + ty: "uint32_t", + name: "flight_sw_version", + 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 { +pub(crate) fn home_position_packet( + system_id: u8, + lat_deg: f64, + lon_deg: f64, + alt_msl_m: f32, + heading_deg: f32, +) -> Vec { + 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 { 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 { +pub(crate) fn camera_information_packet_for_component( + system_id: u8, + component_id: u8, + callsign: &str, + gimbal_device_id: u8, +) -> Vec { 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 Vec Vec { 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(1); + msg.push(stream_id); + 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 { + 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 { + 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 { + 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 { + 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 { + 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, + ] } diff --git a/src/uas/payload.rs b/src/uas/payload.rs index 4b61bc5..f5cf971 100644 --- a/src/uas/payload.rs +++ b/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, }) } } diff --git a/src/uas/send.rs b/src/uas/send.rs index 6d2859d..ff82f39 100644 --- a/src/uas/send.rs +++ b/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, - component_heartbeat_packet, extended_sys_state_packet, global_position_int_packet, - gps_raw_int_packet, heartbeat_packet, system_status_packet, vfr_hud_packet, - video_stream_information_packet, video_stream_status_packet, + attitude_packet, autopilot_version_packet, camera_fov_status_packet_for_component, + camera_information_packet_for_component, component_heartbeat_packet, extended_sys_state_packet, + gimbal_manager_information_packet, global_position_int_packet, gps_raw_int_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 { if let Some(socket) = socket_for_send() { @@ -21,7 +30,11 @@ fn sending_socket(ctx: &Context, error_prefix: &str) -> Result 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()); - info!("MAVLink mock failed sending packet {} to {}: {}", index, payload.address, error); + let _ = ctx.callback_data( + "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, - payload.video_uri + system_id, 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()); - info!("MAVLink system failed sending packet {} to {}: {}", index, payload.address, error); + let _ = ctx.callback_data( + "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, + ) +} diff --git a/src/uas/state.rs b/src/uas/state.rs new file mode 100644 index 0000000..7dca78f --- /dev/null +++ b/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> = 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 { + 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; + } + } +}