Added UAS Tool active actions from ATAK

src/uas/callbacks.rs

@@ -8,11 +8,29 @@ 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,
+    home_position_packet, mission_ack_packet, mission_request_int_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 super::state::{latest_system, set_active_camera, set_home};
 use log::info;
+use std::collections::HashMap;
+use std::sync::{Mutex, OnceLock};
+
+#[derive(Clone, Copy)]
+struct MissionUploadState {
+    count: u16,
+    next_seq: u16,
+    mission_type: u8,
+    gcs_system: u8,
+    gcs_component: u8,
+}
+
+static MISSION_UPLOADS: OnceLock<Mutex<HashMap<u8, MissionUploadState>>> = OnceLock::new();
+
+fn mission_uploads() -> &'static Mutex<HashMap<u8, MissionUploadState>> {
+    MISSION_UPLOADS.get_or_init(|| Mutex::new(HashMap::new()))
+}
 
 pub(crate) fn hex_preview(bytes: &[u8], max_len: usize) -> String {
     bytes
@@ -25,14 +43,26 @@ pub(crate) fn hex_preview(bytes: &[u8], max_len: usize) -> String {
 
 pub(crate) fn mav_cmd_name(command_id: u16) -> &'static str {
     match command_id {
+        16 => "NAV_WAYPOINT",
+        17 => "NAV_LOITER_UNLIM",
+        20 => "NAV_RETURN_TO_LAUNCH",
+        21 => "NAV_LAND",
         22 => "NAV_TAKEOFF",
         176 => "DO_SET_MODE",
+        178 => "DO_CHANGE_SPEED",
+        179 => "DO_SET_HOME",
+        192 => "DO_REPOSITION",
+        205 => "DO_MOUNT_CONTROL",
+        43000 => "GUIDED_CHANGE_SPEED",
+        43001 => "GUIDED_CHANGE_ALTITUDE",
+        43002 => "GUIDED_CHANGE_HEADING",
         200 => "IMAGE_START_CAPTURE",
         201 => "IMAGE_STOP_CAPTURE",
         250 => "VIDEO_START_CAPTURE",
         251 => "VIDEO_STOP_CAPTURE",
         252 => "DO_CONTROL_VIDEO",
         400 => "COMPONENT_ARM_DISARM",
+        511 => "SET_MESSAGE_INTERVAL",
         512 => "REQUEST_MESSAGE",
         521 => "REQUEST_CAMERA_INFORMATION",
         2502 => "VIDEO_START_STREAMING",
@@ -43,6 +73,12 @@ pub(crate) fn mav_cmd_name(command_id: u16) -> &'static str {
     }
 }
 
+fn read_i32(payload: &[u8], offset: usize) -> Option<i32> {
+    payload
+        .get(offset..offset + 4)
+        .map(|bytes| i32::from_le_bytes([bytes[0], bytes[1], bytes[2], bytes[3]]))
+}
+
 fn read_i16(payload: &[u8], offset: usize) -> Option<i16> {
     payload
         .get(offset..offset + 2)
@@ -221,7 +257,7 @@ pub(crate) fn mavlink_callback_event(bytes: &[u8], source: &str) -> Option<Mavli
             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={};sysid={};compid={};command={};command_name={};target_system={};target_component={};frame={};current={};autocontinue={};param1={:.3};param2={:.3};param3={:.3};param4={:.3};x={};y={};z={:.3}",
                     source,
                     system_id,
                     component_id,
@@ -236,6 +272,181 @@ pub(crate) fn mavlink_callback_event(bytes: &[u8], source: &str) -> Option<Mavli
                     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_i32(payload, 16).unwrap_or(0),
+                    read_i32(payload, 20).unwrap_or(0),
+                    read_f32(payload, 24).unwrap_or(0.0),
+                ),
+            })
+        }
+        44 if payload.len() >= 4 => {
+            let count = read_u16(payload, 0)?;
+            let target_system = *payload.get(2)?;
+            let target_component = *payload.get(3)?;
+            let mission_type = payload.get(4).copied().unwrap_or(0);
+            Some(MavlinkCallbackEvent {
+                function: "MISSION_COUNT",
+                data: format!(
+                    "source={};sysid={};compid={};target_system={};target_component={};count={};mission_type={}",
+                    source, system_id, component_id, target_system, target_component, count, mission_type
+                ),
+            })
+        }
+        39 if payload.len() >= 37 => {
+            let seq = read_u16(payload, 28)?;
+            let command = read_u16(payload, 30)?;
+            let target_system = *payload.get(32)?;
+            let target_component = *payload.get(33)?;
+            let frame = *payload.get(34)?;
+            let current = *payload.get(35)?;
+            let autocontinue = *payload.get(36)?;
+            let mission_type = payload.get(37).copied().unwrap_or(0);
+            Some(MavlinkCallbackEvent {
+                function: "MISSION_ITEM",
+                data: format!(
+                    "source={};sysid={};compid={};seq={};command={};command_name={};target_system={};target_component={};frame={};current={};autocontinue={};mission_type={};param1={:.3};param2={:.3};param3={:.3};param4={:.3};lat={:.7};lon={:.7};alt={:.3}",
+                    source,
+                    system_id,
+                    component_id,
+                    seq,
+                    command,
+                    mav_cmd_name(command),
+                    target_system,
+                    target_component,
+                    frame,
+                    current,
+                    autocontinue,
+                    mission_type,
+                    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),
+                ),
+            })
+        }
+        73 if payload.len() >= 37 => {
+            let seq = read_u16(payload, 28)?;
+            let command = read_u16(payload, 30)?;
+            let target_system = *payload.get(32)?;
+            let target_component = *payload.get(33)?;
+            let frame = *payload.get(34)?;
+            let current = *payload.get(35)?;
+            let autocontinue = *payload.get(36)?;
+            let mission_type = payload.get(37).copied().unwrap_or(0);
+            Some(MavlinkCallbackEvent {
+                function: "MISSION_ITEM_INT",
+                data: format!(
+                    "source={};sysid={};compid={};seq={};command={};command_name={};target_system={};target_component={};frame={};current={};autocontinue={};mission_type={};param1={:.3};param2={:.3};param3={:.3};param4={:.3};x={};y={};z={:.3};lat={:.7};lon={:.7};alt={:.3}",
+                    source,
+                    system_id,
+                    component_id,
+                    seq,
+                    command,
+                    mav_cmd_name(command),
+                    target_system,
+                    target_component,
+                    frame,
+                    current,
+                    autocontinue,
+                    mission_type,
+                    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_i32(payload, 16).unwrap_or(0),
+                    read_i32(payload, 20).unwrap_or(0),
+                    read_f32(payload, 24).unwrap_or(0.0),
+                    read_i32(payload, 16).unwrap_or(0) as f64 / 1e7,
+                    read_i32(payload, 20).unwrap_or(0) as f64 / 1e7,
+                    read_f32(payload, 24).unwrap_or(0.0),
+                ),
+            })
+        }
+        45 if payload.len() >= 2 => {
+            let target_system = *payload.get(0)?;
+            let target_component = *payload.get(1)?;
+            let mission_type = payload.get(2).copied().unwrap_or(0);
+            Some(MavlinkCallbackEvent {
+                function: "MISSION_CLEAR_ALL",
+                data: format!(
+                    "source={};sysid={};compid={};target_system={};target_component={};mission_type={}",
+                    source, system_id, component_id, target_system, target_component, mission_type
+                ),
+            })
+        }
+        41 if payload.len() >= 4 => {
+            let seq = read_u16(payload, 0)?;
+            let target_system = *payload.get(2)?;
+            let target_component = *payload.get(3)?;
+            Some(MavlinkCallbackEvent {
+                function: "MISSION_SET_CURRENT",
+                data: format!(
+                    "source={};sysid={};compid={};target_system={};target_component={};seq={}",
+                    source, system_id, component_id, target_system, target_component, seq
+                ),
+            })
+        }
+        243 if payload.len() >= 53 => {
+            let lat_int = read_i32(payload, 0)?;
+            let lon_int = read_i32(payload, 4)?;
+            let alt_mm = read_i32(payload, 8)?;
+            let target_system = *payload.get(52)?;
+            Some(MavlinkCallbackEvent {
+                function: "SET_HOME_POSITION",
+                data: format!(
+                    "source={};sysid={};compid={};target_system={};lat={:.7};lon={:.7};alt={:.3}",
+                    source,
+                    system_id,
+                    component_id,
+                    target_system,
+                    lat_int as f64 / 1e7,
+                    lon_int as f64 / 1e7,
+                    alt_mm as f32 / 1000.0,
+                ),
+            })
+        }
+        86 if payload.len() >= 53 => {
+            let lat_int = read_i32(payload, 4)?;
+            let lon_int = read_i32(payload, 8)?;
+            let alt = read_f32(payload, 12)?;
+            let type_mask = read_u16(payload, 48)?;
+            let target_system = *payload.get(50)?;
+            let target_component = *payload.get(51)?;
+            let coordinate_frame = *payload.get(52)?;
+            Some(MavlinkCallbackEvent {
+                function: "SET_POSITION_TARGET_GLOBAL_INT",
+                data: format!(
+                    "source={};sysid={};compid={};target_system={};target_component={};coordinate_frame={};type_mask={};lat={:.7};lon={:.7};alt={:.3};vx={:.3};vy={:.3};vz={:.3};yaw={:.3}",
+                    source,
+                    system_id,
+                    component_id,
+                    target_system,
+                    target_component,
+                    coordinate_frame,
+                    type_mask,
+                    lat_int as f64 / 1e7,
+                    lon_int as f64 / 1e7,
+                    alt,
+                    read_f32(payload, 16).unwrap_or(0.0),
+                    read_f32(payload, 20).unwrap_or(0.0),
+                    read_f32(payload, 24).unwrap_or(0.0),
+                    read_f32(payload, 40).unwrap_or(0.0),
+                ),
+            })
+        }
+        11 if payload.len() >= 6 => {
+            let custom_mode = payload
+                .get(0..4)
+                .map(|bytes| u32::from_le_bytes([bytes[0], bytes[1], bytes[2], bytes[3]]))?;
+            let target_system = *payload.get(4)?;
+            let base_mode = *payload.get(5)?;
+            Some(MavlinkCallbackEvent {
+                function: "SET_MODE",
+                data: format!(
+                    "source={};sysid={};compid={};target_system={};base_mode={};custom_mode={}",
+                    source, system_id, component_id, target_system, base_mode, custom_mode
                 ),
             })
         }
@@ -264,11 +475,13 @@ pub(crate) fn mavlink_response_packets(bytes: &[u8]) -> Vec<Vec<u8>> {
         return Vec::new();
     }
 
-    let (msg_id, payload) = match bytes[0] {
+    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(&[]),
             )
         }
@@ -276,6 +489,8 @@ pub(crate) fn mavlink_response_packets(bytes: &[u8]) -> Vec<Vec<u8>> {
             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(&[]),
             )
         }
@@ -285,6 +500,12 @@ pub(crate) fn mavlink_response_packets(bytes: &[u8]) -> Vec<Vec<u8>> {
     match msg_id {
         76 if payload.len() >= 31 => command_long_response_packets(payload),
         75 if payload.len() >= 35 => command_int_response_packets(payload),
+        44 if payload.len() >= 4 => {
+            mission_count_response_packets(system_id, component_id, payload)
+        }
+        39 | 73 if payload.len() >= 37 => mission_item_response_packets(payload),
+        45 if payload.len() >= 2 => mission_clear_all_response_packets(payload),
+        243 if payload.len() >= 53 => set_home_position_response_packets(payload),
         _ => Vec::new(),
     }
 }
@@ -312,6 +533,26 @@ fn command_long_response_packets(payload: &[u8]) -> Vec<Vec<u8>> {
             let requested_message = read_f32(payload, 0).unwrap_or(0.0).round() as u32;
             packets.extend(requested_message_packets(target_system, requested_message));
         }
+        179 => {
+            if target_system != 0 {
+                let use_current = read_f32(payload, 0).unwrap_or(0.0) >= 1.0;
+                let lat = read_f32(payload, 16).unwrap_or(0.0) as f64;
+                let lon = read_f32(payload, 20).unwrap_or(0.0) as f64;
+                let alt = read_f32(payload, 24).unwrap_or(0.0);
+                if use_current {
+                    if let Some(system) = latest_system(target_system) {
+                        set_home(
+                            target_system,
+                            system.lat_deg,
+                            system.lon_deg,
+                            system.alt_msl_m - system.rel_alt_m,
+                        );
+                    }
+                } else if lat != 0.0 || lon != 0.0 {
+                    set_home(target_system, lat, lon, alt);
+                }
+            }
+        }
         521 => {
             if let Some(system) = latest_system(target_system) {
                 let camera_component =
@@ -399,6 +640,155 @@ fn command_int_response_packets(payload: &[u8]) -> Vec<Vec<u8>> {
     )]
 }
 
+fn mission_count_response_packets(
+    gcs_system: u8,
+    gcs_component: u8,
+    payload: &[u8],
+) -> Vec<Vec<u8>> {
+    let count = match read_u16(payload, 0) {
+        Some(count) => count,
+        None => return Vec::new(),
+    };
+    let target_system = *payload.get(2).unwrap_or(&0);
+    let target_component = *payload.get(3).unwrap_or(&AUTOPILOT_COMPONENT_ID);
+    let mission_type = payload.get(4).copied().unwrap_or(0);
+    if target_system == 0 {
+        return Vec::new();
+    }
+
+    let ack_component = if target_component == 0 {
+        AUTOPILOT_COMPONENT_ID
+    } else {
+        target_component
+    };
+
+    let Ok(mut uploads) = mission_uploads().lock() else {
+        return Vec::new();
+    };
+
+    if count == 0 {
+        uploads.remove(&target_system);
+        return vec![mission_ack_packet(
+            target_system,
+            ack_component,
+            gcs_system,
+            gcs_component,
+            mission_type,
+        )];
+    }
+
+    uploads.insert(
+        target_system,
+        MissionUploadState {
+            count,
+            next_seq: 0,
+            mission_type,
+            gcs_system,
+            gcs_component,
+        },
+    );
+
+    vec![mission_request_int_packet(
+        target_system,
+        ack_component,
+        gcs_system,
+        gcs_component,
+        0,
+        mission_type,
+    )]
+}
+
+fn mission_item_response_packets(payload: &[u8]) -> Vec<Vec<u8>> {
+    let seq = match read_u16(payload, 28) {
+        Some(seq) => seq,
+        None => return Vec::new(),
+    };
+    let target_system = *payload.get(32).unwrap_or(&0);
+    let target_component = *payload.get(33).unwrap_or(&AUTOPILOT_COMPONENT_ID);
+    if target_system == 0 {
+        return Vec::new();
+    }
+
+    let ack_component = if target_component == 0 {
+        AUTOPILOT_COMPONENT_ID
+    } else {
+        target_component
+    };
+
+    let Ok(mut uploads) = mission_uploads().lock() else {
+        return Vec::new();
+    };
+    let Some(state) = uploads.get_mut(&target_system) else {
+        return vec![mission_ack_packet(
+            target_system,
+            ack_component,
+            255,
+            190,
+            payload.get(37).copied().unwrap_or(0),
+        )];
+    };
+
+    state.next_seq = seq.saturating_add(1);
+    if state.next_seq < state.count {
+        vec![mission_request_int_packet(
+            target_system,
+            ack_component,
+            state.gcs_system,
+            state.gcs_component,
+            state.next_seq,
+            state.mission_type,
+        )]
+    } else {
+        let ack = mission_ack_packet(
+            target_system,
+            ack_component,
+            state.gcs_system,
+            state.gcs_component,
+            state.mission_type,
+        );
+        uploads.remove(&target_system);
+        vec![ack]
+    }
+}
+
+fn mission_clear_all_response_packets(payload: &[u8]) -> Vec<Vec<u8>> {
+    let target_system = *payload.get(0).unwrap_or(&0);
+    let target_component = *payload.get(1).unwrap_or(&AUTOPILOT_COMPONENT_ID);
+    if target_system == 0 {
+        return Vec::new();
+    }
+
+    if let Ok(mut uploads) = mission_uploads().lock() {
+        uploads.remove(&target_system);
+    }
+
+    vec![mission_ack_packet(
+        target_system,
+        if target_component == 0 {
+            AUTOPILOT_COMPONENT_ID
+        } else {
+            target_component
+        },
+        255,
+        190,
+        payload.get(2).copied().unwrap_or(0),
+    )]
+}
+
+fn set_home_position_response_packets(payload: &[u8]) -> Vec<Vec<u8>> {
+    let target_system = *payload.get(52).unwrap_or(&0);
+    if target_system == 0 {
+        return Vec::new();
+    }
+    let lat = read_i32(payload, 0).unwrap_or(0) as f64 / 1e7;
+    let lon = read_i32(payload, 4).unwrap_or(0) as f64 / 1e7;
+    let alt = read_i32(payload, 8).unwrap_or(0) as f32 / 1000.0;
+    if lat != 0.0 || lon != 0.0 {
+        set_home(target_system, lat, lon, alt);
+    }
+    Vec::new()
+}
+
 fn requested_message_packets(system_id: u8, requested_message: u32) -> Vec<Vec<u8>> {
     let Some(system) = latest_system(system_id) else {
         return Vec::new();
@@ -411,9 +801,9 @@ fn requested_message_packets(system_id: u8, requested_message: u32) -> Vec<Vec<u
         )],
         242 => vec![home_position_packet(
             system_id,
-            system.lat_deg,
-            system.lon_deg,
-            system.alt_msl_m - system.rel_alt_m,
+            system.home_lat_deg,
+            system.home_lon_deg,
+            system.home_alt_msl_m,
             system.heading_deg,
         )],
         259 => {

src/uas/packets.rs

@@ -49,6 +49,37 @@ pub(crate) fn command_ack_packet(
     build_v2_packet(system_id, component_id, 77, &msg, 143)
 }
 
+pub(crate) fn mission_request_int_packet(
+    system_id: u8,
+    component_id: u8,
+    target_system: u8,
+    target_component: u8,
+    seq: u16,
+    mission_type: u8,
+) -> Vec<u8> {
+    let mut msg = Vec::with_capacity(5);
+    msg.extend_from_slice(&seq.to_le_bytes());
+    msg.push(target_system);
+    msg.push(target_component);
+    msg.push(mission_type);
+    build_v2_packet(system_id, component_id, 51, &msg, 196)
+}
+
+pub(crate) fn mission_ack_packet(
+    system_id: u8,
+    component_id: u8,
+    target_system: u8,
+    target_component: u8,
+    mission_type: u8,
+) -> Vec<u8> {
+    let mut msg = Vec::with_capacity(4);
+    msg.push(target_system);
+    msg.push(target_component);
+    msg.push(0);
+    msg.push(mission_type);
+    build_v2_packet(system_id, component_id, 47, &msg, 153)
+}
+
 pub(crate) fn gps_raw_int_packet(payload: &UasTelemetryPayload) -> Vec<u8> {
     let time_usec = (Utc::now().timestamp_millis().max(0) as u64) * 1_000;
     let fix_type = if payload.flying { 3u8 } else { 2u8 };
@@ -119,7 +150,7 @@ pub(crate) fn vfr_hud_packet(payload: &UasTelemetryPayload) -> Vec<u8> {
     build_v1_packet(payload.system_id, payload.component_id, 74, &msg, 20)
 }
 
-pub(crate) fn system_status_packet(system_id: u8) -> Vec<u8> {
+pub(crate) fn system_status_packet(system_id: u8, battery_remaining_pct: i8) -> Vec<u8> {
     let fields = [
         FieldSpec {
             ty: "uint32_t",
@@ -203,12 +234,12 @@ pub(crate) fn system_status_packet(system_id: u8) -> Vec<u8> {
     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);
+    msg.push(battery_remaining_pct.clamp(0, 100) as u8);
 
     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<u8> {
+pub(crate) fn extended_sys_state_packet(system_id: u8, landed: bool) -> Vec<u8> {
     let fields = [
         FieldSpec {
             ty: "uint8_t",
@@ -225,10 +256,10 @@ pub(crate) fn extended_sys_state_packet(system_id: u8, flying: bool) -> Vec<u8>
 
     let mut msg = Vec::with_capacity(2);
     msg.push(MAV_LANDED_STATE_UNDEFINED);
-    msg.push(if flying {
-        MAV_LANDED_STATE_IN_AIR
-    } else {
+    msg.push(if landed {
         MAV_LANDED_STATE_ON_GROUND
+    } else {
+        MAV_LANDED_STATE_IN_AIR
     });
 
     build_v2_packet(system_id, AUTOPILOT_COMPONENT_ID, 245, &msg, crc_extra)