From 52edf94b17b59167a58211cf727ffa5be584860c Mon Sep 17 00:00:00 2001 From: Valmo Trindade Date: Thu, 7 May 2026 03:37:22 -0300 Subject: [PATCH] refactored mavlink mocking to "uas" module on the extension --- src/lib.rs | 10 +- src/mavlink_mock.rs | 239 ------------------------------------- src/uas/callbacks.rs | 239 +++++++++++++++++++++++++++++++++++++ src/uas/constants.rs | 36 ++++++ src/uas/crc.rs | 102 ++++++++++++++++ src/uas/endpoint.rs | 146 +++++++++++++++++++++++ src/uas/identity.rs | 80 +++++++++++++ src/uas/mod.rs | 13 ++ src/uas/packets.rs | 277 +++++++++++++++++++++++++++++++++++++++++++ src/uas/payload.rs | 178 +++++++++++++++++++++++++++ src/uas/send.rs | 158 ++++++++++++++++++++++++ 11 files changed, 1236 insertions(+), 242 deletions(-) delete mode 100644 src/mavlink_mock.rs create mode 100644 src/uas/callbacks.rs create mode 100644 src/uas/constants.rs create mode 100644 src/uas/crc.rs create mode 100644 src/uas/endpoint.rs create mode 100644 src/uas/identity.rs create mode 100644 src/uas/mod.rs create mode 100644 src/uas/packets.rs create mode 100644 src/uas/payload.rs create mode 100644 src/uas/send.rs diff --git a/src/lib.rs b/src/lib.rs index fae878d..5ac4097 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -1,6 +1,6 @@ use arma_rs::{arma, Extension, Group}; use rustls::crypto::aws_lc_rs; -mod mavlink_mock; +mod uas; mod mdns; mod structs; mod tcp; @@ -42,8 +42,12 @@ pub fn init() -> Extension { .command("uuid", utils::uuid::get_uuid) .command("log", utils::log::log_info) .group( - "mavlink_mock", - Group::new().command("send_uas_telemetry", mavlink_mock::send_uas_telemetry), + "uas", + Group::new() + .command("start_endpoint", uas::start_endpoint) + .command("stop_endpoint", uas::stop_endpoint) + .command("send_uas_telemetry", uas::send_uas_telemetry) + .command("send_uas_system", uas::send_uas_system), ) .group( "mdns", diff --git a/src/mavlink_mock.rs b/src/mavlink_mock.rs deleted file mode 100644 index b781fce..0000000 --- a/src/mavlink_mock.rs +++ /dev/null @@ -1,239 +0,0 @@ -use arma_rs::{Context, FromArma, FromArmaError}; -use chrono::Utc; -use log::info; -use std::net::UdpSocket; -use std::sync::atomic::{AtomicU8, Ordering}; - -static MAVLINK_SEQUENCE: AtomicU8 = AtomicU8::new(0); - -pub struct UasTelemetryPayload { - pub address: String, - pub system_id: u8, - pub component_id: u8, - pub vehicle_type: u8, - pub lat_deg: f64, - pub lon_deg: f64, - pub alt_msl_m: f32, - pub rel_alt_m: f32, - pub heading_deg: f32, - pub groundspeed_mps: f32, - pub roll_deg: f32, - pub pitch_deg: f32, - pub yaw_deg: f32, - pub flying: bool, -} - -impl FromArma for UasTelemetryPayload { - fn from_arma(data: String) -> Result { - let ( - address, - system_id, - component_id, - vehicle_type, - lat_deg, - lon_deg, - alt_msl_m, - rel_alt_m, - heading_deg, - groundspeed_mps, - roll_deg, - pitch_deg, - yaw_deg, - flying, - ) = <( - String, - i32, - i32, - i32, - f64, - f64, - f32, - f32, - f32, - f32, - f32, - f32, - f32, - i32, - )>::from_arma(data)?; - - Ok(Self { - address, - system_id: system_id.clamp(1, 255) as u8, - component_id: component_id.clamp(1, 255) as u8, - vehicle_type: vehicle_type.clamp(0, 255) as u8, - lat_deg, - lon_deg, - alt_msl_m, - rel_alt_m, - heading_deg, - groundspeed_mps, - roll_deg, - pitch_deg, - yaw_deg, - flying: flying != 0, - }) - } -} - -fn crc_accumulate(byte: u8, crc: &mut u16) { - let mut tmp = byte ^ (*crc as u8); - tmp ^= tmp << 4; - *crc = (*crc >> 8) ^ ((tmp as u16) << 8) ^ ((tmp as u16) << 3) ^ ((tmp as u16) >> 4); -} - -fn mavlink_crc(header_and_payload: &[u8], crc_extra: u8) -> u16 { - let mut crc = 0xFFFFu16; - for byte in header_and_payload { - crc_accumulate(*byte, &mut crc); - } - crc_accumulate(crc_extra, &mut crc); - crc -} - -fn build_v1_packet(system_id: u8, component_id: u8, msg_id: u8, payload: &[u8], crc_extra: u8) -> Vec { - let seq = MAVLINK_SEQUENCE.fetch_add(1, Ordering::Relaxed); - let mut packet = Vec::with_capacity(payload.len() + 8); - packet.push(0xFE); - packet.push(payload.len() as u8); - packet.push(seq); - packet.push(system_id); - packet.push(component_id); - packet.push(msg_id); - packet.extend_from_slice(payload); - - let crc = mavlink_crc(&packet[1..], crc_extra); - packet.push((crc & 0xFF) as u8); - packet.push((crc >> 8) as u8); - packet -} - -fn heartbeat_packet(payload: &UasTelemetryPayload) -> Vec { - let mut msg = Vec::with_capacity(9); - msg.extend_from_slice(&0u32.to_le_bytes()); - msg.push(payload.vehicle_type); - msg.push(0); - msg.push(if payload.flying { 0x81 } else { 0x01 }); - msg.push(if payload.flying { 4 } else { 3 }); - msg.push(3); - build_v1_packet(payload.system_id, payload.component_id, 0, &msg, 50) -} - -fn gps_raw_int_packet(payload: &UasTelemetryPayload) -> Vec { - let now_us = Utc::now().timestamp_micros().max(0) as u64; - let lat = (payload.lat_deg * 1e7).round() as i32; - let lon = (payload.lon_deg * 1e7).round() as i32; - let alt = (payload.alt_msl_m * 1000.0).round() as i32; - let speed_cms = (payload.groundspeed_mps.max(0.0) * 100.0).round() as u16; - let cog_cdeg = (((payload.heading_deg % 360.0 + 360.0) % 360.0) * 100.0).round() as u16; - - let mut msg = Vec::with_capacity(30); - msg.extend_from_slice(&now_us.to_le_bytes()); - msg.extend_from_slice(&lat.to_le_bytes()); - msg.extend_from_slice(&lon.to_le_bytes()); - msg.extend_from_slice(&alt.to_le_bytes()); - msg.extend_from_slice(&100u16.to_le_bytes()); - msg.extend_from_slice(&100u16.to_le_bytes()); - msg.extend_from_slice(&speed_cms.to_le_bytes()); - msg.extend_from_slice(&cog_cdeg.to_le_bytes()); - msg.push(3); - msg.push(10); - build_v1_packet(payload.system_id, payload.component_id, 24, &msg, 24) -} - -fn global_position_int_packet(payload: &UasTelemetryPayload) -> Vec { - let now_ms = Utc::now().timestamp_millis().max(0) as u32; - let lat = (payload.lat_deg * 1e7).round() as i32; - let lon = (payload.lon_deg * 1e7).round() as i32; - let alt = (payload.alt_msl_m * 1000.0).round() as i32; - let rel_alt = (payload.rel_alt_m.max(0.0) * 1000.0).round() as i32; - let speed_cms = (payload.groundspeed_mps.max(0.0) * 100.0).round() as i16; - let hdg_cdeg = (((payload.heading_deg % 360.0 + 360.0) % 360.0) * 100.0).round() as u16; - - let mut msg = Vec::with_capacity(28); - msg.extend_from_slice(&now_ms.to_le_bytes()); - msg.extend_from_slice(&lat.to_le_bytes()); - msg.extend_from_slice(&lon.to_le_bytes()); - msg.extend_from_slice(&alt.to_le_bytes()); - msg.extend_from_slice(&rel_alt.to_le_bytes()); - msg.extend_from_slice(&speed_cms.to_le_bytes()); - msg.extend_from_slice(&0i16.to_le_bytes()); - msg.extend_from_slice(&0i16.to_le_bytes()); - msg.extend_from_slice(&hdg_cdeg.to_le_bytes()); - build_v1_packet(payload.system_id, payload.component_id, 33, &msg, 104) -} - -fn attitude_packet(payload: &UasTelemetryPayload) -> Vec { - let now_ms = Utc::now().timestamp_millis().max(0) as u32; - let roll = payload.roll_deg.to_radians(); - let pitch = payload.pitch_deg.to_radians(); - let yaw = payload.yaw_deg.to_radians(); - - let mut msg = Vec::with_capacity(28); - msg.extend_from_slice(&now_ms.to_le_bytes()); - msg.extend_from_slice(&roll.to_le_bytes()); - msg.extend_from_slice(&pitch.to_le_bytes()); - msg.extend_from_slice(&yaw.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_v1_packet(payload.system_id, payload.component_id, 30, &msg, 39) -} - -fn vfr_hud_packet(payload: &UasTelemetryPayload) -> Vec { - let heading = (((payload.heading_deg % 360.0 + 360.0) % 360.0).round()) as i16; - let throttle = if payload.flying { 50u16 } else { 0u16 }; - - let mut msg = Vec::with_capacity(20); - msg.extend_from_slice(&payload.groundspeed_mps.to_le_bytes()); - msg.extend_from_slice(&payload.groundspeed_mps.to_le_bytes()); - msg.extend_from_slice(&payload.alt_msl_m.to_le_bytes()); - msg.extend_from_slice(&0f32.to_le_bytes()); - msg.extend_from_slice(&heading.to_le_bytes()); - msg.extend_from_slice(&throttle.to_le_bytes()); - build_v1_packet(payload.system_id, payload.component_id, 74, &msg, 20) -} - -pub fn send_uas_telemetry(ctx: Context, payload: UasTelemetryPayload) -> &'static str { - info!( - "MAVLink mock send requested to {} sysid={} compid={} lat={} lon={} alt_msl={} rel_alt={} heading={} speed={} flying={}", - payload.address, - payload.system_id, - payload.component_id, - payload.lat_deg, - payload.lon_deg, - payload.alt_msl_m, - payload.rel_alt_m, - payload.heading_deg, - payload.groundspeed_mps, - payload.flying - ); - - let socket = match UdpSocket::bind("0.0.0.0:0") { - Ok(socket) => socket, - Err(error) => { - let _ = ctx.callback_data("MAVLINK MOCK ERROR", "Failed to bind UDP socket", error.to_string()); - info!("MAVLink mock failed to bind UDP socket: {}", error); - return "Failed to bind MAVLink mock socket"; - } - }; - - let packets = [ - heartbeat_packet(&payload), - gps_raw_int_packet(&payload), - global_position_int_packet(&payload), - attitude_packet(&payload), - vfr_hud_packet(&payload), - ]; - - 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); - return "Failed to send MAVLink mock telemetry"; - } - } - - info!("MAVLink mock sent {} packets to {}", packets.len(), payload.address); - "Sent MAVLink mock telemetry" -} diff --git a/src/uas/callbacks.rs b/src/uas/callbacks.rs new file mode 100644 index 0000000..c2603dc --- /dev/null +++ b/src/uas/callbacks.rs @@ -0,0 +1,239 @@ +pub(crate) struct MavlinkCallbackEvent { + pub function: &'static str, + pub data: String, +} + +pub(crate) fn hex_preview(bytes: &[u8], max_len: usize) -> String { + bytes.iter() + .take(max_len) + .map(|byte| format!("{:02X}", byte)) + .collect::>() + .join(" ") +} + +pub(crate) fn mav_cmd_name(command_id: u16) -> &'static str { + match command_id { + 22 => "NAV_TAKEOFF", + 176 => "DO_SET_MODE", + 200 => "IMAGE_START_CAPTURE", + 201 => "IMAGE_STOP_CAPTURE", + 250 => "VIDEO_START_CAPTURE", + 251 => "VIDEO_STOP_CAPTURE", + 252 => "DO_CONTROL_VIDEO", + 400 => "COMPONENT_ARM_DISARM", + 521 => "REQUEST_MESSAGE", + _ => "UNKNOWN", + } +} + +fn read_i16(payload: &[u8], offset: usize) -> Option { + payload + .get(offset..offset + 2) + .map(|bytes| i16::from_le_bytes([bytes[0], bytes[1]])) +} + +fn read_u16(payload: &[u8], offset: usize) -> Option { + payload + .get(offset..offset + 2) + .map(|bytes| u16::from_le_bytes([bytes[0], bytes[1]])) +} + +fn read_f32(payload: &[u8], offset: usize) -> Option { + payload + .get(offset..offset + 4) + .map(|bytes| f32::from_le_bytes([bytes[0], bytes[1], bytes[2], bytes[3]])) +} + +fn mavlink_message_detail(msg_id: u8, payload: &[u8]) -> String { + match msg_id { + 76 if payload.len() >= 33 => { + let command = u16::from_le_bytes([payload[28], payload[29]]); + let target_system = payload[30]; + let target_component = payload[31]; + format!( + " command={}({}) target={}:{}", + command, + mav_cmd_name(command), + target_system, + target_component + ) + } + 75 if payload.len() >= 35 => { + let command = u16::from_le_bytes([payload[28], payload[29]]); + let target_system = payload[30]; + let target_component = payload[31]; + format!( + " command={}({}) target={}:{}", + command, + mav_cmd_name(command), + target_system, + target_component + ) + } + _ => String::new(), + } +} + +pub(crate) fn mavlink_packet_summary(bytes: &[u8]) -> String { + if bytes.is_empty() { + return "empty datagram".to_string(); + } + + match bytes[0] { + 0xFE if bytes.len() >= 8 => { + let payload_len = bytes[1] as usize; + let seq = bytes[2]; + 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(&[])); + format!( + "MAVLink v1 msgid={}{} sysid={} compid={} seq={} payload_len={} preview={}", + msg_id, + detail, + system_id, + component_id, + seq, + payload_len, + hex_preview(bytes, 24) + ) + } + 0xFD if bytes.len() >= 12 => { + let payload_len = bytes[1] as usize; + let seq = bytes[4]; + 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(&[])); + format!( + "MAVLink v2 msgid={}{} sysid={} compid={} seq={} payload_len={} preview={}", + msg_id, + detail, + system_id, + component_id, + seq, + payload_len, + hex_preview(bytes, 24) + ) + } + _ => format!("Unknown UDP payload preview={}", hex_preview(bytes, 24)), + } +} + +pub(crate) fn mavlink_callback_event(bytes: &[u8], source: &str) -> Option { + if bytes.len() < 8 { + return None; + } + + let (msg_id, system_id, component_id, payload) = match bytes[0] { + 0xFE if bytes.len() >= 8 => { + let payload_len = bytes[1] as usize; + ( + bytes[5] as u32, + bytes[3], + bytes[4], + 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[5], + bytes[6], + bytes.get(10..10 + payload_len).unwrap_or(&[]), + ) + } + _ => return None, + }; + + match msg_id { + 69 if payload.len() >= 11 => { + let x = read_i16(payload, 0)?; + let y = read_i16(payload, 2)?; + let z = read_i16(payload, 4)?; + let r = read_i16(payload, 6)?; + let buttons = read_u16(payload, 8)?; + let target = *payload.get(10)?; + Some(MavlinkCallbackEvent { + function: "MANUAL_CONTROL", + data: format!( + "source={};sysid={};compid={};target={};x={};y={};z={};r={};buttons={}", + source, system_id, component_id, target, x, y, z, r, buttons + ), + }) + } + 76 if payload.len() >= 33 => { + let command = read_u16(payload, 28)?; + let target_system = *payload.get(30)?; + let target_component = *payload.get(31)?; + let confirmation = *payload.get(32)?; + Some(MavlinkCallbackEvent { + function: "COMMAND_LONG", + data: format!( + "source={};sysid={};compid={};command={};command_name={};target_system={};target_component={};confirmation={};param1={:.3};param2={:.3};param3={:.3};param4={:.3};param5={:.3};param6={:.3};param7={:.3}", + source, + system_id, + component_id, + command, + mav_cmd_name(command), + target_system, + target_component, + confirmation, + read_f32(payload, 0).unwrap_or(0.0), + read_f32(payload, 4).unwrap_or(0.0), + read_f32(payload, 8).unwrap_or(0.0), + read_f32(payload, 12).unwrap_or(0.0), + read_f32(payload, 16).unwrap_or(0.0), + read_f32(payload, 20).unwrap_or(0.0), + read_f32(payload, 24).unwrap_or(0.0), + ), + }) + } + 75 if payload.len() >= 35 => { + let command = read_u16(payload, 28)?; + let target_system = *payload.get(30)?; + let target_component = *payload.get(31)?; + let frame = *payload.get(32)?; + let current = *payload.get(33)?; + let autocontinue = *payload.get(34)?; + Some(MavlinkCallbackEvent { + function: "COMMAND_INT", + data: format!( + "source={};sysid={};compid={};command={};command_name={};target_system={};target_component={};frame={};current={};autocontinue={};param1={:.3};param2={:.3};param3={:.3};param4={:.3}", + source, + system_id, + component_id, + command, + mav_cmd_name(command), + target_system, + target_component, + frame, + current, + autocontinue, + read_f32(payload, 0).unwrap_or(0.0), + read_f32(payload, 4).unwrap_or(0.0), + read_f32(payload, 8).unwrap_or(0.0), + read_f32(payload, 12).unwrap_or(0.0), + ), + }) + } + 77 if payload.len() >= 3 => { + let command = read_u16(payload, 0)?; + let result = *payload.get(2)?; + Some(MavlinkCallbackEvent { + function: "COMMAND_ACK", + data: format!( + "source={};sysid={};compid={};command={};command_name={};result={}", + source, + system_id, + component_id, + command, + mav_cmd_name(command), + result + ), + }) + } + _ => None, + } +} diff --git a/src/uas/constants.rs b/src/uas/constants.rs new file mode 100644 index 0000000..e4e9d49 --- /dev/null +++ b/src/uas/constants.rs @@ -0,0 +1,36 @@ +pub const AUTOPILOT_COMPONENT_ID: u8 = 1; +pub const CAMERA_COMPONENT_ID: u8 = 100; +pub const GIMBAL_COMPONENT_ID: u8 = 154; + +pub const MAV_TYPE_FIXED_WING: u8 = 1; +pub const MAV_TYPE_QUADROTOR: u8 = 2; +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_INVALID: u8 = 8; + +pub const MAV_STATE_STANDBY: u8 = 3; +pub const MAV_STATE_ACTIVE: u8 = 4; + +pub const MAV_MODE_FLAG_CUSTOM_MODE_ENABLED: u8 = 1; +pub const MAV_MODE_FLAG_SAFETY_ARMED: u8 = 128; +pub const MAV_LANDED_STATE_UNDEFINED: u8 = 0; +pub const MAV_LANDED_STATE_ON_GROUND: u8 = 1; +pub const MAV_LANDED_STATE_IN_AIR: u8 = 2; + +pub const MAV_PROTOCOL_CAPABILITY_MISSION_INT: u64 = 4; +pub const MAV_PROTOCOL_CAPABILITY_COMMAND_INT: u64 = 8; +pub const MAV_PROTOCOL_CAPABILITY_FTP: u64 = 32; +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_HAS_VIDEO_STREAM: u32 = 256; +pub const VIDEO_STREAM_STATUS_FLAGS_RUNNING: u16 = 1; +pub const VIDEO_STREAM_TYPE_RTSP: u8 = 0; +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; diff --git a/src/uas/crc.rs b/src/uas/crc.rs new file mode 100644 index 0000000..101e5df --- /dev/null +++ b/src/uas/crc.rs @@ -0,0 +1,102 @@ +use std::sync::atomic::{AtomicU8, Ordering}; + +static MAVLINK_SEQUENCE: AtomicU8 = AtomicU8::new(0); + +#[derive(Clone, Copy)] +pub(crate) struct FieldSpec { + pub ty: &'static str, + pub name: &'static str, + pub array_len: usize, +} + +fn crc_accumulate(byte: u8, crc: &mut u16) { + let mut tmp = byte ^ (*crc as u8); + tmp ^= tmp << 4; + *crc = (*crc >> 8) ^ ((tmp as u16) << 8) ^ ((tmp as u16) << 3) ^ ((tmp as u16) >> 4); +} + +fn mavlink_crc(bytes: &[u8], crc_extra: u8) -> u16 { + let mut crc = 0xFFFFu16; + for byte in bytes { + crc_accumulate(*byte, &mut crc); + } + crc_accumulate(crc_extra, &mut crc); + crc +} + +pub(crate) fn calculate_crc_extra(message_name: &str, base_fields: &[FieldSpec]) -> u8 { + let mut crc = 0xFFFFu16; + + for byte in message_name.as_bytes() { + crc_accumulate(*byte, &mut crc); + } + crc_accumulate(b' ', &mut crc); + + for field in base_fields { + for byte in field.ty.as_bytes() { + crc_accumulate(*byte, &mut crc); + } + crc_accumulate(b' ', &mut crc); + + for byte in field.name.as_bytes() { + crc_accumulate(*byte, &mut crc); + } + crc_accumulate(b' ', &mut crc); + + if field.array_len > 0 { + crc_accumulate(field.array_len as u8, &mut crc); + } + } + + ((crc & 0xFF) ^ (crc >> 8)) as u8 +} + +pub(crate) fn build_v1_packet( + system_id: u8, + component_id: u8, + msg_id: u8, + payload: &[u8], + crc_extra: u8, +) -> Vec { + let seq = MAVLINK_SEQUENCE.fetch_add(1, Ordering::Relaxed); + let mut packet = Vec::with_capacity(payload.len() + 8); + packet.push(0xFE); + packet.push(payload.len() as u8); + packet.push(seq); + packet.push(system_id); + packet.push(component_id); + packet.push(msg_id); + packet.extend_from_slice(payload); + + let crc = mavlink_crc(&packet[1..], crc_extra); + packet.push((crc & 0xFF) as u8); + packet.push((crc >> 8) as u8); + packet +} + +pub(crate) fn build_v2_packet( + system_id: u8, + component_id: u8, + msg_id: u32, + payload: &[u8], + crc_extra: u8, +) -> Vec { + let seq = MAVLINK_SEQUENCE.fetch_add(1, Ordering::Relaxed); + let mut packet = Vec::with_capacity(payload.len() + 12); + packet.push(0xFD); + packet.push(payload.len() as u8); + packet.push(0); + packet.push(0); + packet.push(seq); + packet.push(system_id); + packet.push(component_id); + packet.push((msg_id & 0xFF) as u8); + packet.push(((msg_id >> 8) & 0xFF) as u8); + packet.push(((msg_id >> 16) & 0xFF) as u8); + packet.extend_from_slice(payload); + + let crc = mavlink_crc(&packet[1..], crc_extra); + packet.push((crc & 0xFF) as u8); + packet.push((crc >> 8) as u8); + packet +} diff --git a/src/uas/endpoint.rs b/src/uas/endpoint.rs new file mode 100644 index 0000000..507b44e --- /dev/null +++ b/src/uas/endpoint.rs @@ -0,0 +1,146 @@ +use arma_rs::Context; +use log::info; +use std::net::UdpSocket; +use std::sync::atomic::{AtomicBool, Ordering}; +use std::sync::{Arc, Mutex}; +use std::thread::{self, JoinHandle}; +use std::time::Duration; + +use super::callbacks::{mavlink_callback_event, mavlink_packet_summary}; + +pub(crate) struct MavlinkEndpoint { + pub socket: UdpSocket, + pub running: Arc, + pub listener: Option>, + pub bind_port: u16, +} + +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())) +} + +fn stop_endpoint_internal() { + let endpoint = MAVLINK_ENDPOINT.lock().unwrap().take(); + + if let Some(mut endpoint) = endpoint { + endpoint.running.store(false, Ordering::Relaxed); + info!( + "Stopping MAVLink UDP endpoint on 0.0.0.0:{}", + endpoint.bind_port + ); + + if let Some(listener) = endpoint.listener.take() { + let _ = listener.join(); + } + } +} + +pub fn start_endpoint(ctx: Context, bind_port: i32) -> &'static str { + let bind_port = bind_port.clamp(1, 65535) as u16; + + stop_endpoint_internal(); + + let socket = match UdpSocket::bind(format!("0.0.0.0:{bind_port}")) { + Ok(socket) => socket, + Err(error) => { + let _ = ctx.callback_data( + "MAVLINK UDP ERROR", + "failed to bind MAVLink UDP endpoint", + error.to_string(), + ); + info!( + "Failed to bind MAVLink UDP endpoint on 0.0.0.0:{}: {}", + bind_port, error + ); + return "Failed to bind MAVLink UDP endpoint"; + } + }; + + if let Err(error) = socket.set_read_timeout(Some(Duration::from_millis(500))) { + info!( + "Failed to set MAVLink UDP endpoint read timeout on 0.0.0.0:{}: {}", + bind_port, error + ); + } + + let listener_socket = match socket.try_clone() { + Ok(listener_socket) => listener_socket, + Err(error) => { + let _ = ctx.callback_data( + "MAVLINK UDP ERROR", + "failed to clone MAVLink UDP endpoint socket", + error.to_string(), + ); + info!( + "Failed to clone MAVLink UDP endpoint socket on 0.0.0.0:{}: {}", + bind_port, error + ); + return "Failed to clone MAVLink UDP endpoint socket"; + } + }; + + let running = Arc::new(AtomicBool::new(true)); + let listener_running = Arc::clone(&running); + + let listener_ctx = ctx; + let listener = thread::spawn(move || { + let mut buffer = [0u8; 2048]; + info!("MAVLink UDP endpoint listening on 0.0.0.0:{}", bind_port); + + while listener_running.load(Ordering::Relaxed) { + match listener_socket.recv_from(&mut buffer) { + Ok((received, source)) => { + let source_string = source.to_string(); + info!( + "MAVLink UDP endpoint received {} bytes from {}: {}", + received, + 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); + } + } + Err(error) + if matches!( + error.kind(), + std::io::ErrorKind::WouldBlock | std::io::ErrorKind::TimedOut + ) => {} + Err(error) => { + if listener_running.load(Ordering::Relaxed) { + info!( + "MAVLink UDP endpoint listener error on 0.0.0.0:{}: {}", + bind_port, error + ); + } + break; + } + } + } + + info!( + "MAVLink UDP endpoint listener stopped on 0.0.0.0:{}", + bind_port + ); + }); + + *MAVLINK_ENDPOINT.lock().unwrap() = Some(MavlinkEndpoint { + socket, + running, + listener: Some(listener), + bind_port, + }); + + info!("Started MAVLink UDP endpoint on 0.0.0.0:{}", bind_port); + "Started MAVLink UDP endpoint" +} + +pub fn stop_endpoint(_ctx: Context) -> &'static str { + stop_endpoint_internal(); + "Stopped MAVLink UDP endpoint" +} diff --git a/src/uas/identity.rs b/src/uas/identity.rs new file mode 100644 index 0000000..1c54152 --- /dev/null +++ b/src/uas/identity.rs @@ -0,0 +1,80 @@ +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 { + 1 => MAV_TYPE_FIXED_WING, + 3 => MAV_TYPE_HELICOPTER, + 2 => MAV_TYPE_QUADROTOR, + value => value, + } +} + +pub(crate) fn normalize_heading_deg(value: f32) -> f32 { + ((value % 360.0) + 360.0) % 360.0 +} + +pub(crate) fn stable_system_id(entity_uuid: &str) -> u8 { + let mut hash: u32 = 0x811C9DC5; + for byte in entity_uuid.as_bytes() { + hash ^= *byte as u32; + hash = hash.wrapping_mul(0x01000193); + } + + ((hash % 250) as u8) + 1 +} + +fn uuid16(entity_uuid: &str) -> [u8; 16] { + let hex = entity_uuid.replace('-', ""); + let mut bytes = [0u8; 16]; + + for index in 0..16 { + let start = index * 2; + if start + 2 <= hex.len() { + if let Ok(value) = u8::from_str_radix(&hex[start..start + 2], 16) { + bytes[index] = value; + } + } + } + + bytes +} + +pub(crate) fn uid64_from_uuid(entity_uuid: &str) -> u64 { + let uuid = uuid16(entity_uuid); + let mut bytes = [0u8; 8]; + bytes.copy_from_slice(&uuid[..8]); + u64::from_le_bytes(bytes) +} + +pub(crate) fn uid2_from_uuid(entity_uuid: &str) -> [u8; 18] { + let uuid = uuid16(entity_uuid); + let mut uid2 = [0u8; 18]; + uid2[..16].copy_from_slice(&uuid); + + let checksum = entity_uuid + .as_bytes() + .iter() + .fold(0u16, |acc, value| acc.wrapping_add(*value as u16)); + uid2[16] = (checksum & 0xFF) as u8; + uid2[17] = (checksum >> 8) as u8; + uid2 +} + +pub(crate) fn fixed_string(value: &str) -> [u8; N] { + let mut bytes = [0u8; N]; + let raw = value.as_bytes(); + let len = raw.len().min(N.saturating_sub(1)); + bytes[..len].copy_from_slice(&raw[..len]); + bytes +} + +pub(crate) fn should_send_video_stream_information(video_uri: &str) -> bool { + let trimmed = video_uri.trim().to_ascii_lowercase(); + trimmed.starts_with("rtsp://") + || trimmed.starts_with("rtp://") + || trimmed.starts_with("udp://") + || trimmed.starts_with("mpegts://") + || trimmed.starts_with("tcp://") +} diff --git a/src/uas/mod.rs b/src/uas/mod.rs new file mode 100644 index 0000000..a6a10ae --- /dev/null +++ b/src/uas/mod.rs @@ -0,0 +1,13 @@ +mod callbacks; +mod constants; +mod crc; +mod endpoint; +mod identity; +mod packets; +mod payload; +mod send; + +pub use endpoint::{start_endpoint, stop_endpoint}; +#[allow(unused_imports)] +pub use payload::{UasSystemPayload, UasTelemetryPayload}; +pub use send::{send_uas_system, send_uas_telemetry}; diff --git a/src/uas/packets.rs b/src/uas/packets.rs new file mode 100644 index 0000000..e66d9df --- /dev/null +++ b/src/uas/packets.rs @@ -0,0 +1,277 @@ +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, +}; +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}; +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()); + msg.push(payload.vehicle_type); + msg.push(MAV_AUTOPILOT_GENERIC); + msg.push(if payload.flying { + MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | MAV_MODE_FLAG_SAFETY_ARMED + } else { + MAV_MODE_FLAG_CUSTOM_MODE_ENABLED + }); + msg.push(if payload.flying { + MAV_STATE_ACTIVE + } else { + MAV_STATE_STANDBY + }); + msg.push(3); + build_v1_packet(payload.system_id, payload.component_id, 0, &msg, 50) +} + +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 }; + + let mut msg = Vec::with_capacity(30); + msg.extend_from_slice(&time_usec.to_le_bytes()); + msg.extend_from_slice(&((payload.lat_deg * 1e7).round() as i32).to_le_bytes()); + msg.extend_from_slice(&((payload.lon_deg * 1e7).round() as i32).to_le_bytes()); + msg.extend_from_slice(&((payload.alt_msl_m * 1000.0).round() as i32).to_le_bytes()); + msg.extend_from_slice(&u16::MAX.to_le_bytes()); + msg.extend_from_slice(&u16::MAX.to_le_bytes()); + msg.extend_from_slice(&u16::MAX.to_le_bytes()); + msg.extend_from_slice(&u16::MAX.to_le_bytes()); + msg.push(fix_type); + msg.push(10); + build_v1_packet(payload.system_id, payload.component_id, 24, &msg, 24) +} + +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 heading = (normalize_heading_deg(payload.heading_deg) * 100.0).round() as u16; + + let mut msg = Vec::with_capacity(28); + msg.extend_from_slice(&time_boot_ms.to_le_bytes()); + msg.extend_from_slice(&((payload.lat_deg * 1e7).round() as i32).to_le_bytes()); + msg.extend_from_slice(&((payload.lon_deg * 1e7).round() as i32).to_le_bytes()); + msg.extend_from_slice(&((payload.alt_msl_m * 1000.0).round() as i32).to_le_bytes()); + msg.extend_from_slice(&((payload.rel_alt_m * 1000.0).round() as i32).to_le_bytes()); + msg.extend_from_slice(&vx.to_le_bytes()); + msg.extend_from_slice(&vy.to_le_bytes()); + msg.extend_from_slice(&0i16.to_le_bytes()); + msg.extend_from_slice(&heading.to_le_bytes()); + build_v1_packet(payload.system_id, payload.component_id, 33, &msg, 104) +} + +pub(crate) fn attitude_packet(payload: &UasTelemetryPayload) -> Vec { + let now_ms = Utc::now().timestamp_millis().max(0) as u32; + let roll = payload.roll_deg.to_radians(); + let pitch = payload.pitch_deg.to_radians(); + let yaw = payload.yaw_deg.to_radians(); + + let mut msg = Vec::with_capacity(28); + msg.extend_from_slice(&now_ms.to_le_bytes()); + msg.extend_from_slice(&roll.to_le_bytes()); + msg.extend_from_slice(&pitch.to_le_bytes()); + msg.extend_from_slice(&yaw.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_v1_packet(payload.system_id, payload.component_id, 30, &msg, 39) +} + +pub(crate) fn vfr_hud_packet(payload: &UasTelemetryPayload) -> Vec { + let heading = normalize_heading_deg(payload.heading_deg).round() as i16; + let throttle = if payload.flying { 50u16 } else { 0u16 }; + + let mut msg = Vec::with_capacity(20); + msg.extend_from_slice(&payload.groundspeed_mps.to_le_bytes()); + msg.extend_from_slice(&payload.groundspeed_mps.to_le_bytes()); + msg.extend_from_slice(&payload.alt_msl_m.to_le_bytes()); + msg.extend_from_slice(&0f32.to_le_bytes()); + msg.extend_from_slice(&heading.to_le_bytes()); + msg.extend_from_slice(&throttle.to_le_bytes()); + build_v1_packet(payload.system_id, payload.component_id, 74, &msg, 20) +} + +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 }, + ]; + let crc_extra = calculate_crc_extra("SYS_STATUS", &fields); + + let sensors = 0x2000u32 | 0x4000u32 | 0x8000u32 | 0x20u32; + let mut msg = Vec::with_capacity(31); + msg.extend_from_slice(&sensors.to_le_bytes()); + msg.extend_from_slice(&sensors.to_le_bytes()); + msg.extend_from_slice(&sensors.to_le_bytes()); + msg.extend_from_slice(&500u16.to_le_bytes()); + msg.extend_from_slice(&12000u16.to_le_bytes()); + msg.extend_from_slice(&(-1i16).to_le_bytes()); + msg.extend_from_slice(&0u16.to_le_bytes()); + msg.extend_from_slice(&0u16.to_le_bytes()); + msg.extend_from_slice(&0u16.to_le_bytes()); + msg.extend_from_slice(&0u16.to_le_bytes()); + msg.extend_from_slice(&0u16.to_le_bytes()); + msg.extend_from_slice(&0u16.to_le_bytes()); + msg.push(100u8); + + build_v2_packet(system_id, AUTOPILOT_COMPONENT_ID, 1, &msg, crc_extra) +} + +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 }, + ]; + let crc_extra = calculate_crc_extra("EXTENDED_SYS_STATE", &fields); + + let mut msg = Vec::with_capacity(2); + msg.push(MAV_LANDED_STATE_UNDEFINED); + msg.push(if flying { + MAV_LANDED_STATE_IN_AIR + } else { + MAV_LANDED_STATE_ON_GROUND + }); + + build_v2_packet(system_id, AUTOPILOT_COMPONENT_ID, 245, &msg, crc_extra) +} + +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 }, + ]; + let crc_extra = calculate_crc_extra("AUTOPILOT_VERSION", &fields); + + let capabilities = MAV_PROTOCOL_CAPABILITY_MISSION_INT + | MAV_PROTOCOL_CAPABILITY_COMMAND_INT + | MAV_PROTOCOL_CAPABILITY_FTP + | MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_GLOBAL_INT + | MAV_PROTOCOL_CAPABILITY_MAVLINK2 + | MAV_PROTOCOL_CAPABILITY_COMPONENT_IMPLEMENTS_GIMBAL_MANAGER; + let uid = uid64_from_uuid(entity_uuid); + let uid2 = uid2_from_uuid(entity_uuid); + + let mut msg = Vec::with_capacity(78); + msg.extend_from_slice(&capabilities.to_le_bytes()); + msg.extend_from_slice(&0x010100FFu32.to_le_bytes()); + msg.extend_from_slice(&0u32.to_le_bytes()); + msg.extend_from_slice(&0u32.to_le_bytes()); + msg.extend_from_slice(&0u32.to_le_bytes()); + msg.extend_from_slice(&[0u8; 8]); + msg.extend_from_slice(&[0u8; 8]); + msg.extend_from_slice(&[0u8; 8]); + msg.extend_from_slice(&0x5441u16.to_le_bytes()); + msg.extend_from_slice(&0x5541u16.to_le_bytes()); + msg.extend_from_slice(&uid.to_le_bytes()); + msg.extend_from_slice(&uid2); + + 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 { + let mut msg = Vec::with_capacity(9); + msg.extend_from_slice(&0u32.to_le_bytes()); + msg.push(component_type); + msg.push(MAV_AUTOPILOT_INVALID); + msg.push(0); + msg.push(MAV_STATE_ACTIVE); + msg.push(3); + build_v2_packet(system_id, component_id, 0, &msg, 50) +} + +pub(crate) fn camera_information_packet(system_id: u8, callsign: &str) -> Vec { + let vendor = fixed_string::<32>("ArmaTAK"); + let model = fixed_string::<32>(callsign); + let cam_definition_uri = fixed_string::<140>(""); + + let mut msg = Vec::with_capacity(235); + msg.extend_from_slice(&(Utc::now().timestamp_millis().max(0) as u32).to_le_bytes()); + msg.extend_from_slice(&0x010100FFu32.to_le_bytes()); + 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(&1280u16.to_le_bytes()); + msg.extend_from_slice(&720u16.to_le_bytes()); + msg.extend_from_slice(&0u16.to_le_bytes()); + msg.extend_from_slice(&vendor); + msg.extend_from_slice(&model); + msg.push(0); + msg.extend_from_slice(&cam_definition_uri); + msg.push(GIMBAL_COMPONENT_ID); + msg.push(0); + + build_v2_packet(system_id, CAMERA_COMPONENT_ID, 259, &msg, 92) +} + +pub(crate) fn video_stream_information_packet( + system_id: u8, + callsign: &str, + video_uri: &str, + hfov_deg: f32, +) -> Vec { + let stream_type = if video_uri.starts_with("rtsp://") { + VIDEO_STREAM_TYPE_RTSP + } else if video_uri.starts_with("rtp://") { + VIDEO_STREAM_TYPE_RTPUDP + } else if video_uri.starts_with("tcp://") { + VIDEO_STREAM_TYPE_TCP_MPEG + } else if video_uri.starts_with("mpegts://") || video_uri.starts_with("udp://") { + VIDEO_STREAM_TYPE_MPEG_TS + } else { + VIDEO_STREAM_TYPE_RTSP + }; + + let name = fixed_string::<32>(&format!("{} Video", callsign)); + let uri = fixed_string::<160>(video_uri); + + 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(&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(0); + msg.push(1); + msg.push(stream_type); + msg.extend_from_slice(&name); + msg.extend_from_slice(&uri); + msg.push(VIDEO_STREAM_ENCODING_H264); + msg.push(CAMERA_COMPONENT_ID); + + build_v2_packet(system_id, CAMERA_COMPONENT_ID, 269, &msg, 109) +} diff --git a/src/uas/payload.rs b/src/uas/payload.rs new file mode 100644 index 0000000..4b61bc5 --- /dev/null +++ b/src/uas/payload.rs @@ -0,0 +1,178 @@ +use arma_rs::{FromArma, FromArmaError}; + +pub struct UasTelemetryPayload { + pub address: String, + pub system_id: u8, + pub component_id: u8, + pub vehicle_type: u8, + pub lat_deg: f64, + pub lon_deg: f64, + pub alt_msl_m: f32, + pub rel_alt_m: f32, + pub heading_deg: f32, + pub groundspeed_mps: f32, + pub roll_deg: f32, + pub pitch_deg: f32, + pub yaw_deg: f32, + pub flying: bool, +} + +#[allow(dead_code)] +pub struct UasSystemPayload { + pub address: String, + pub entity_uuid: String, + pub callsign: String, + pub vehicle_type: u8, + pub lat_deg: f64, + pub lon_deg: f64, + pub alt_msl_m: f32, + pub rel_alt_m: f32, + pub heading_deg: f32, + pub groundspeed_mps: f32, + pub roll_deg: f32, + pub pitch_deg: f32, + pub yaw_deg: f32, + pub flying: bool, + pub gimbal_roll_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, +} + +impl FromArma for UasTelemetryPayload { + fn from_arma(data: String) -> Result { + let ( + address, + system_id, + component_id, + vehicle_type, + lat_deg, + lon_deg, + alt_msl_m, + rel_alt_m, + heading_deg, + groundspeed_mps, + roll_deg, + pitch_deg, + yaw_deg, + flying, + ) = <( + String, + i32, + i32, + i32, + f64, + f64, + f32, + f32, + f32, + f32, + f32, + f32, + f32, + i32, + )>::from_arma(data)?; + + Ok(Self { + address, + system_id: system_id.clamp(1, 255) as u8, + component_id: component_id.clamp(1, 255) as u8, + vehicle_type: vehicle_type.clamp(0, 255) as u8, + lat_deg, + lon_deg, + alt_msl_m, + rel_alt_m, + heading_deg, + groundspeed_mps, + roll_deg, + pitch_deg, + yaw_deg, + flying: flying != 0, + }) + } +} + +impl FromArma for UasSystemPayload { + fn from_arma(data: String) -> Result { + let ( + address, + entity_uuid, + callsign, + vehicle_type, + lat_deg, + lon_deg, + alt_msl_m, + rel_alt_m, + heading_deg, + groundspeed_mps, + roll_deg, + pitch_deg, + yaw_deg, + flying, + gimbal_roll_deg, + gimbal_pitch_deg, + gimbal_yaw_deg, + video_uri, + hfov_deg, + vfov_deg, + image_lat_deg, + image_lon_deg, + image_alt_msl_m, + ) = <( + String, + String, + String, + i32, + f64, + f64, + f32, + f32, + f32, + f32, + f32, + f32, + f32, + i32, + f32, + f32, + f32, + String, + f32, + f32, + f64, + f64, + f32, + )>::from_arma(data)?; + + Ok(Self { + address, + entity_uuid, + callsign, + vehicle_type: vehicle_type.clamp(0, 255) as u8, + lat_deg, + lon_deg, + alt_msl_m, + rel_alt_m, + heading_deg, + groundspeed_mps, + roll_deg, + pitch_deg, + yaw_deg, + flying: flying != 0, + gimbal_roll_deg, + gimbal_pitch_deg, + gimbal_yaw_deg, + video_uri, + hfov_deg, + vfov_deg, + image_lat_deg, + image_lon_deg, + image_alt_msl_m, + }) + } +} diff --git a/src/uas/send.rs b/src/uas/send.rs new file mode 100644 index 0000000..15b39ee --- /dev/null +++ b/src/uas/send.rs @@ -0,0 +1,158 @@ +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::endpoint::socket_for_send; +use super::identity::{map_vehicle_type, should_send_video_stream_information, 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, +}; +use super::payload::{UasSystemPayload, UasTelemetryPayload}; + +fn sending_socket(ctx: &Context, error_prefix: &str) -> Result { + if let Some(socket) = socket_for_send() { + return Ok(socket); + } + + 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()); + info!("{} failed to bind UDP socket: {}", error_prefix, error); + Err("Failed to bind MAVLink mock socket") + } + } +} + +pub fn send_uas_telemetry(ctx: Context, payload: UasTelemetryPayload) -> &'static str { + info!( + "MAVLink mock send requested to {} sysid={} compid={} lat={} lon={} alt_msl={} rel_alt={} heading={} speed={} flying={}", + payload.address, + payload.system_id, + payload.component_id, + payload.lat_deg, + payload.lon_deg, + payload.alt_msl_m, + payload.rel_alt_m, + payload.heading_deg, + payload.groundspeed_mps, + payload.flying + ); + + let socket = match sending_socket(&ctx, "MAVLink mock") { + Ok(socket) => socket, + Err(message) => return message, + }; + + let packets = [ + heartbeat_packet(&payload), + gps_raw_int_packet(&payload), + global_position_int_packet(&payload), + attitude_packet(&payload), + vfr_hud_packet(&payload), + ]; + + 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); + return "Failed to send MAVLink mock telemetry"; + } + } + + 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 vehicle_type = map_vehicle_type(payload.vehicle_type); + + info!( + "MAVLink system send requested to {} entity_uuid={} sysid={} callsign={} lat={} lon={} alt_msl={} rel_alt={} heading={} gimbal_pitch={} gimbal_yaw={} video_uri={}", + payload.address, + payload.entity_uuid, + system_id, + payload.callsign, + payload.lat_deg, + payload.lon_deg, + payload.alt_msl_m, + payload.rel_alt_m, + payload.heading_deg, + payload.gimbal_pitch_deg, + payload.gimbal_yaw_deg, + payload.video_uri + ); + + let socket = match sending_socket(&ctx, "MAVLink system") { + Ok(socket) => socket, + Err(message) => return message, + }; + + let autopilot_payload = UasTelemetryPayload { + address: payload.address.clone(), + system_id, + component_id: AUTOPILOT_COMPONENT_ID, + vehicle_type, + 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, + groundspeed_mps: payload.groundspeed_mps, + roll_deg: payload.roll_deg, + pitch_deg: payload.pitch_deg, + yaw_deg: payload.yaw_deg, + flying: payload.flying, + }; + + let mut packets = vec![ + heartbeat_packet(&autopilot_payload), + gps_raw_int_packet(&autopilot_payload), + global_position_int_packet(&autopilot_payload), + attitude_packet(&autopilot_payload), + 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), + 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), + ]; + + if should_send_video_stream_information(&payload.video_uri) { + packets.push(video_stream_information_packet( + system_id, + &payload.callsign, + &payload.video_uri, + payload.hfov_deg, + )); + } else if !payload.video_uri.trim().is_empty() { + info!( + "Skipping VIDEO_STREAM_INFORMATION for sysid={} because URI is not a supported stream URI: {}", + system_id, payload.video_uri + ); + } + + 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); + return "Failed to send MAVLink system telemetry"; + } + } + + info!( + "MAVLink system sent {} packets to {} for sysid={} (camera comp={}, gimbal comp={})", + packets.len(), + payload.address, + system_id, + CAMERA_COMPONENT_ID, + GIMBAL_COMPONENT_ID + ); + "Sent MAVLink system telemetry" +}