Added UAS Tool active actions from ATAK

addons/main/XEH_postInit.sqf

@@ -61,6 +61,14 @@ addMissionEventHandler ["ExtensionCallback", {
 				case "COMMAND_LONG";
 				case "COMMAND_INT";
 				case "COMMAND_ACK";
+				case "MISSION_COUNT";
+				case "MISSION_ITEM";
+				case "MISSION_ITEM_INT";
+				case "MISSION_CLEAR_ALL";
+				case "MISSION_SET_CURRENT";
+				case "SET_HOME_POSITION";
+				case "SET_MODE";
+				case "SET_POSITION_TARGET_GLOBAL_INT";
 				case "MANUAL_CONTROL": {
 					"armatak" callExtension ["log", [["info", format ["MAVLINK UDP CALLBACK %1 %2", _function, _data]]]];
 					[_function, _data] call EFUNC(uav,handleMavlinkCallback);

addons/uav/functions/fnc_handleMavlinkCallback.sqf

@@ -6,42 +6,281 @@ if (!hasInterface) exitWith {};
 
 private _payload = [_data] call FUNC(parseMavlinkCallbackData);
 private _uav = getConnectedUAV player;
+if (isNull _uav) then {
+	_uav = player getVariable [QGVAR(broadcastingUav), objNull];
+};
+
 if (isNull _uav) exitWith {
 	"armatak" callExtension ["log", [["warn", format ["Ignoring MAVLINK UDP callback %1 because no UAV is connected: %2", _function, _data]]]];
 };
 
-private _command = parseNumber (_payload getOrDefault ["command", "-1"]);
+private _number = {
+	params ["_key", ["_default", 0]];
+	private _raw = _payload getOrDefault [_key, str _default];
+	private _value = parseNumber _raw;
+	if (!finite _value) exitWith {_default};
+	_value
+};
+
+private _uavGroup = {
+	params ["_vehicle"];
+	private _crew = crew _vehicle;
+	if (_crew isEqualTo []) exitWith {grpNull};
+	group (_crew select 0)
+};
+
+private _clearWaypoints = {
+	params ["_group"];
+	if (isNull _group) exitWith {};
+	for "_i" from ((count waypoints _group) - 1) to 0 step -1 do {
+		deleteWaypoint [_group, _i];
+	};
+};
+
+private _clearUavRoute = {
+	private _group = [_uav] call _uavGroup;
+	if (isNull _group) exitWith {false};
+	[_group] call _clearWaypoints;
+	_uav setVariable ["armatak_uas_mission_items", [], true];
+	true
+};
+
+private _geoToAtl = {
+	params ["_vehicle", "_lat", "_lon", ["_alt", -1]];
+
+	private _current = [_vehicle] call EFUNC(client,extractClientPosition);
+	private _currentLat = _current select 1;
+	private _currentLon = _current select 2;
+	private _currentAtl = getPosATL _vehicle;
+
+	private _northM = (_lat - _currentLat) * 111320;
+	private _eastM = (_lon - _currentLon) * (111320 * (cos _currentLat));
+
+	[
+		(_currentAtl select 0) + _eastM,
+		(_currentAtl select 1) + _northM,
+		if (_alt >= 0) then {_alt} else {_currentAtl select 2}
+	]
+};
+
+private _commandMove = {
+	params ["_vehicle", "_positionAtl", ["_type", "MOVE"], ["_radius", 80], ["_completion", 50]];
+
+	private _group = [_vehicle] call _uavGroup;
+	if (isNull _group) exitWith {false};
+
+	[_group] call _clearWaypoints;
+
+	_vehicle engineOn true;
+	_vehicle setVariable ["armatak_uas_armed", true, true];
+	_vehicle setFuel ((fuel _vehicle) max 0.1);
+	_vehicle flyInHeight ((_positionAtl select 2) max 10);
+	_vehicle doMove _positionAtl;
+
+	private _wp = _group addWaypoint [_positionAtl, 0];
+	_wp setWaypointType _type;
+	_wp setWaypointBehaviour "CARELESS";
+	_wp setWaypointCombatMode "BLUE";
+	_wp setWaypointSpeed "NORMAL";
+	_wp setWaypointCompletionRadius _completion;
+
+	if (_type == "LOITER") then {
+		_wp setWaypointLoiterRadius (_radius max 25);
+		_wp setWaypointLoiterType "CIRCLE_L";
+	};
+
+	true
+};
+
+private _appendMissionWaypoint = {
+	params ["_vehicle", "_positionAtl", "_command", "_seq", ["_radius", 80]];
+
+	private _group = [_vehicle] call _uavGroup;
+	if (isNull _group) exitWith {false};
+
+	private _type = switch (_command) do {
+		case 17;
+		case 18;
+		case 19;
+		case 31: {"LOITER"};
+		case 21: {"MOVE"};
+		default {"MOVE"};
+	};
+
+	private _wp = _group addWaypoint [_positionAtl, 0];
+	_wp setWaypointType _type;
+	_wp setWaypointBehaviour "CARELESS";
+	_wp setWaypointCombatMode "BLUE";
+	_wp setWaypointSpeed "NORMAL";
+	_wp setWaypointCompletionRadius 35;
+
+	if (_type == "LOITER") then {
+		_wp setWaypointLoiterRadius (_radius max 25);
+		_wp setWaypointLoiterType "CIRCLE_L";
+	};
+	if (_command == 21) then {
+		_wp setWaypointStatements ["true", "(vehicle this) land 'LAND'"];
+	};
+
+	private _items = _vehicle getVariable ["armatak_uas_mission_items", []];
+	_items pushBack [_seq, _command, _positionAtl];
+	_vehicle setVariable ["armatak_uas_mission_items", _items, true];
+
+	true
+};
+
 private _commandName = _payload getOrDefault ["command_name", "UNKNOWN"];
+private _command = [_payload getOrDefault ["command", "-1"]] call BIS_fnc_parseNumber;
+private _callsign = [_uav] call armatak_fnc_extract_marker_callsign;
+
+private _applySpeed = {
+	params ["_speed"];
+	if (_speed <= 0) exitWith {false};
+	_uav limitSpeed _speed;
+	systemChat format ["ATAK SPEED %1m/s %2", round _speed, _callsign];
+	true
+};
+
+private _applyMode = {
+	params ["_mode"];
+
+	switch (_mode) do {
+		case 4: {
+			_uav engineOn true;
+			_uav setVariable ["armatak_uas_armed", true, true];
+			_uav setFuel ((fuel _uav) max 0.1);
+			systemChat format ["ATAK GUIDED %1", _callsign];
+		};
+		case 5: {
+			private _pos = getPosATL _uav;
+			[_uav, _pos, "LOITER", 80, 25] call _commandMove;
+			systemChat format ["ATAK LOITER %1", _callsign];
+		};
+		case 6;
+		case 21;
+		case 27: {
+			private _home = _uav getVariable ["armatak_uas_home_atl", getPosATL _uav];
+			_home set [2, ((_home select 2) max 60)];
+			[_uav, _home, "MOVE", 80, 60] call _commandMove;
+			systemChat format ["ATAK RTL %1", _callsign];
+		};
+		case 9: {
+			private _pos = getPosATL _uav;
+			_pos set [2, 0];
+			[_uav, _pos, "MOVE", 30, 20] call _commandMove;
+			_uav flyInHeight 0;
+			systemChat format ["ATAK LAND %1", _callsign];
+		};
+		default {
+			"armatak" callExtension ["log", [["info", format ["Unhandled MAVLINK mode %1 for UAV %2", _mode, _uav]]]];
+		};
+	};
+};
+
+private _setHomeFromGeo = {
+	params ["_lat", "_lon", "_alt"];
+	if (_lat == 0 && {_lon == 0}) exitWith {false};
+	private _homeAtl = [_uav, _lat, _lon, _alt] call _geoToAtl;
+	_uav setVariable ["armatak_uas_home_atl", _homeAtl, true];
+	_uav setVariable ["armatak_uas_home_geo", [_lat, _lon, _alt], true];
+	systemChat format ["ATAK HOME %1", _callsign];
+	true
+};
 
 switch (_function) do {
 	case "COMMAND_LONG": {
 		switch (_command) do {
+			case 176: {
+				private _mode = ["param2", -1] call _number;
+				if (_mode < 0) then {
+					_mode = ["param1", -1] call _number;
+				};
+				[_mode] call _applyMode;
+			};
+			case 178: {
+				private _speed = ["param2", -1] call _number;
+				if (_speed <= 0) then {
+					_speed = ["param1", -1] call _number;
+				};
+				[_speed] call _applySpeed;
+			};
+			case 179: {
+				private _useCurrent = (["param1", 0] call _number) >= 1;
+				if (_useCurrent) then {
+					private _pos = [_uav] call EFUNC(client,extractClientPosition);
+					private _relAlt = ((getPosATL _uav) select 2) max 0;
+					private _homeAtl = getPosATL _uav;
+					_uav setVariable ["armatak_uas_home_atl", _homeAtl, true];
+					_uav setVariable ["armatak_uas_home_geo", [_pos select 1, _pos select 2, (_pos select 3) - _relAlt], true];
+					systemChat format ["ATAK HOME %1", _callsign];
+				} else {
+					[["param5", 0] call _number, ["param6", 0] call _number, ["param7", 0] call _number] call _setHomeFromGeo;
+				};
+			};
 			case 400: {
-				private _armValue = parseNumber (_payload getOrDefault ["param1", "0"]);
+				private _doArm = (["param1", 0] call _number) >= 1;
-				private _doArm = _armValue >= 1;
 				_uav engineOn _doArm;
+				_uav setVariable ["armatak_uas_armed", _doArm, true];
 				if (_doArm) then {
 					_uav setFuel ((fuel _uav) max 0.1);
 				};
-				systemChat format ["ATAK %1 %2", ["DISARM", "ARM"] select _doArm, [_uav] call armatak_fnc_extract_marker_callsign];
+				systemChat format ["ATAK %1 %2", ["DISARM", "ARM"] select _doArm, _callsign];
-				"armatak" callExtension ["log", [["info", format ["Applied MAVLINK COMMAND_LONG %1 (%2) to UAV %3", _command, _commandName, _uav]]]];
+				"armatak" callExtension ["log", [["info", format ["Applied MAVLINK ARM=%1 to UAV %2", _doArm, _uav]]]];
 			};
 			case 22: {
-				_uav engineOn true;
+				private _alt = (["param7", 75] call _number) max 10;
-				_uav setFuel ((fuel _uav) max 0.1);
+				private _pos = getPosATL _uav;
-				_uav flyInHeight 75;
+				_pos set [2, _alt];
-
+				[_uav, _pos, "MOVE", 80, 25] call _commandMove;
-				if (_uav isKindOf "Helicopter" || {_uav isKindOf "VTOL_Base_F"} || {_uav isKindOf "Quadbike_01_F"}) then {
+				_uav setVelocityModelSpace [0, 15, 8];
-					private _velocity = velocityModelSpace _uav;
+				systemChat format ["ATAK TAKEOFF %1m %2", round _alt, _callsign];
-					_uav setVelocityModelSpace [
+			};
-						_velocity select 0,
+			case 21: {
-						(_velocity select 1) max 15,
+				[9] call _applyMode;
-						((_velocity select 2) max 0) + 8
+			};
-					];
+			case 20: {
+				[6] call _applyMode;
+			};
+			case 16: {
+				private _lat = ["param5", 0] call _number;
+				private _lon = ["param6", 0] call _number;
+				private _alt = ["param7", -1] call _number;
+				private _pos = [_uav, _lat, _lon, _alt] call _geoToAtl;
+				[_uav, _pos, "MOVE", 80, 50] call _commandMove;
+				systemChat format ["ATAK MOVE %1", _callsign];
+			};
+			case 17: {
+				private _lat = ["param5", 0] call _number;
+				private _lon = ["param6", 0] call _number;
+				private _alt = ["param7", -1] call _number;
+				private _radius = abs (["param3", 80] call _number);
+				private _pos = [_uav, _lat, _lon, _alt] call _geoToAtl;
+				[_uav, _pos, "LOITER", _radius, 30] call _commandMove;
+				systemChat format ["ATAK LOITER %1", _callsign];
+			};
+			case 43000: {
+				private _speed = ["param2", -1] call _number;
+				if (_speed <= 0) then {
+					_speed = ["param1", -1] call _number;
+				};
+				[_speed] call _applySpeed;
+			};
+			case 43001: {
+				private _alt = ["param1", -1] call _number;
+				if (_alt < 0) then {
+					_alt = ["param7", -1] call _number;
+				};
+				private _pos = getPosATL _uav;
+				_pos set [2, _alt max 10];
+				[_uav, _pos, "MOVE", 80, 25] call _commandMove;
+				systemChat format ["ATAK ALT %1m %2", round (_pos select 2), _callsign];
+			};
+			case 43002: {
+				private _heading = ["param1", -1] call _number;
+				if (_heading >= 0) then {
+					_uav setDir _heading;
+					systemChat format ["ATAK HDG %1 %2", round _heading, _callsign];
 				};
-
-				systemChat format ["ATAK TAKEOFF %1", [_uav] call armatak_fnc_extract_marker_callsign];
-				"armatak" callExtension ["log", [["info", format ["Applied MAVLINK TAKEOFF to UAV %1", _uav]]]];
 			};
 			default {
 				"armatak" callExtension ["log", [["info", format ["Unhandled MAVLINK COMMAND_LONG %1 (%2): %3", _command, _commandName, _data]]]];
@@ -49,7 +288,87 @@ switch (_function) do {
 		};
 	};
 	case "COMMAND_INT": {
-		"armatak" callExtension ["log", [["info", format ["Received MAVLINK COMMAND_INT %1 (%2): %3", _command, _commandName, _data]]]];
+		private _lat = (["x", 0] call _number) / 1e7;
+		private _lon = (["y", 0] call _number) / 1e7;
+		private _alt = ["z", -1] call _number;
+
+		switch (_command) do {
+			case 16: {
+				private _pos = [_uav, _lat, _lon, _alt] call _geoToAtl;
+				[_uav, _pos, "MOVE", 80, 50] call _commandMove;
+				systemChat format ["ATAK MOVE %1", _callsign];
+			};
+			case 17;
+			case 192: {
+				private _radius = abs (["param3", 80] call _number);
+				private _direction = ["CIRCLE_L", "CIRCLE_R"] select ((["param4", 0] call _number) < 0);
+				private _pos = [_uav, _lat, _lon, _alt] call _geoToAtl;
+				private _type = ["MOVE", "LOITER"] select (_radius > 1);
+				[_uav, _pos, _type, _radius, 30] call _commandMove;
+				if (_type == "LOITER") then {
+					private _group = [_uav] call _uavGroup;
+					private _waypoints = waypoints _group;
+					if (_waypoints isNotEqualTo []) then {
+						(_waypoints select -1) setWaypointLoiterType _direction;
+					};
+				};
+				systemChat format ["ATAK %1 %2", _type, _callsign];
+			};
+			default {
+				"armatak" callExtension ["log", [["info", format ["Unhandled MAVLINK COMMAND_INT %1 (%2): %3", _command, _commandName, _data]]]];
+			};
+		};
+	};
+	case "MISSION_COUNT": {
+		private _count = ["count", 0] call _number;
+		[] call _clearUavRoute;
+		systemChat format ["ATAK ROUTE %1 pts %2", round _count, _callsign];
+		"armatak" callExtension ["log", [["info", format ["Receiving MAVLINK mission count=%1 for UAV %2", _count, _uav]]]];
+	};
+	case "MISSION_CLEAR_ALL": {
+		[] call _clearUavRoute;
+		systemChat format ["ATAK ROUTE CLEAR %1", _callsign];
+	};
+	case "MISSION_SET_CURRENT": {
+		private _seq = ["seq", 0] call _number;
+		"armatak" callExtension ["log", [["info", format ["MAVLINK mission set current seq=%1 for UAV %2", _seq, _uav]]]];
+	};
+	case "MISSION_ITEM";
+	case "MISSION_ITEM_INT": {
+		private _seq = ["seq", 0] call _number;
+		private _missionCommand = ["command", -1] call _number;
+		private _lat = ["lat", 0] call _number;
+		private _lon = ["lon", 0] call _number;
+		private _alt = ["alt", -1] call _number;
+
+		if (_lat == 0 && {_lon == 0}) exitWith {
+			"armatak" callExtension ["log", [["warn", format ["Ignoring MAVLINK mission item at zero coordinate: %1", _data]]]];
+		};
+
+		private _pos = [_uav, _lat, _lon, _alt] call _geoToAtl;
+		private _radius = abs (["param3", 80] call _number);
+		[_uav, _pos, _missionCommand, _seq, _radius] call _appendMissionWaypoint;
+		_uav engineOn true;
+		_uav setVariable ["armatak_uas_armed", true, true];
+		_uav setFuel ((fuel _uav) max 0.1);
+		_uav flyInHeight ((_pos select 2) max 10);
+		systemChat format ["ATAK ROUTE WP %1 %2", round _seq, _callsign];
+		"armatak" callExtension ["log", [["info", format ["Added MAVLINK mission item seq=%1 command=%2 posATL=%3 for UAV %4", _seq, _missionCommand, _pos, _uav]]]];
+	};
+	case "SET_HOME_POSITION": {
+		[["lat", 0] call _number, ["lon", 0] call _number, ["alt", 0] call _number] call _setHomeFromGeo;
+	};
+	case "SET_POSITION_TARGET_GLOBAL_INT": {
+		private _lat = ["lat", 0] call _number;
+		private _lon = ["lon", 0] call _number;
+		private _alt = ["alt", -1] call _number;
+		private _pos = [_uav, _lat, _lon, _alt] call _geoToAtl;
+		[_uav, _pos, "MOVE", 80, 40] call _commandMove;
+		systemChat format ["ATAK GUIDED MOVE %1", _callsign];
+	};
+	case "SET_MODE": {
+		private _mode = ["custom_mode", -1] call _number;
+		[_mode] call _applyMode;
 	};
 	case "COMMAND_ACK": {
 		"armatak" callExtension ["log", [["info", format ["Received MAVLINK COMMAND_ACK %1 (%2): %3", _command, _commandName, _data]]]];

addons/uav/functions/fnc_updateMavlinkBroadcast.sqf

@@ -40,6 +40,11 @@ private _mavlinkAddress = player getVariable [QEGVAR(client,mavlink_address), ""
 if (_mavlinkAddress isEqualTo "") exitWith {};
 
 private _pos = [_uav] call EFUNC(client,extractClientPosition);
+private _relAlt = ((getPosATL _uav) select 2) max 0;
+if (isNil {_uav getVariable "armatak_uas_home_atl"}) then {
+	_uav setVariable ["armatak_uas_home_atl", getPosATL _uav, true];
+	_uav setVariable ["armatak_uas_home_geo", [_pos select 1, _pos select 2, (_pos select 3) - _relAlt], true];
+};
 private _uuid = [_uav] call armatak_fnc_extract_uuid;
 private _callsign = [_uav] call armatak_fnc_extract_marker_callsign;
 private _videoUri = [_uav] call FUNC(resolveVideoUri);
@@ -48,8 +53,15 @@ private _up = vectorUp _uav;
 private _yaw = getDir _uav;
 private _pitch = asin (((_dir select 2) max -1) min 1);
 private _roll = asin (((_up select 0) max -1) min 1);
-private _relAlt = ((getPosATL _uav) select 2) max 0;
 private _uavType = if (_uav isKindOf "Plane") then {1} else {[2, 3] select (_uav isKindOf "Helicopter")};
+private _armed = _uav getVariable ["armatak_uas_armed", isEngineOn _uav];
+if !(isEngineOn _uav) then {
+	_armed = false;
+	_uav setVariable ["armatak_uas_armed", false, true];
+};
+private _groundSpeed = abs (_pos select 6);
+private _landed = (_relAlt <= 1.5) && {_groundSpeed <= 0.5};
+private _batteryRemaining = round ((((fuel _uav) max 0) min 1) * 100);
 
 private _gimbalRoll = 0;
 private _gimbalPitch = _pitch;
@@ -92,7 +104,8 @@ private _systemPayload = [
 	_roll,
 	_pitch,
 	_yaw,
-	parseNumber isEngineOn _uav,
+	parseNumber _armed,
+	parseNumber _landed,
 	_gimbalRoll,
 	_gimbalPitch,
 	_gimbalYaw,
@@ -102,7 +115,8 @@ private _systemPayload = [
 	_imageLat,
 	_imageLon,
 	_imageAlt,
-	parseNumber _hasTurretCamera
+	parseNumber _hasTurretCamera,
+	_batteryRemaining
 ];
 
 "armatak" callExtension ["uas:send_uas_system", [_systemPayload]];

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.home_lat_deg,
-            system.lon_deg,
+            system.home_lon_deg,
-            system.alt_msl_m - system.rel_alt_m,
+            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 {
+    msg.push(if landed {
-        MAV_LANDED_STATE_IN_AIR
-    } else {
         MAV_LANDED_STATE_ON_GROUND
+    } else {
+        MAV_LANDED_STATE_IN_AIR
     });
 
     build_v2_packet(system_id, AUTOPILOT_COMPONENT_ID, 245, &msg, crc_extra)