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/main/functions/extract_data/fn_extract_marker_callsign.sqf

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

addons/main/functions/extract_data/fn_extract_uas_camera_data.sqf

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

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 = _armValue >= 1;
+				private _doArm = (["param1", 0] call _number) >= 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];
-				"armatak" callExtension ["log", [["info", format ["Applied MAVLINK COMMAND_LONG %1 (%2) to UAV %3", _command, _commandName, _uav]]]];
+				systemChat format ["ATAK %1 %2", ["DISARM", "ARM"] select _doArm, _callsign];
+				"armatak" callExtension ["log", [["info", format ["Applied MAVLINK ARM=%1 to UAV %2", _doArm, _uav]]]];
 			};
 			case 22: {
-				_uav engineOn true;
-				_uav setFuel ((fuel _uav) max 0.1);
-				_uav flyInHeight 75;
-
-				if (_uav isKindOf "Helicopter" || {_uav isKindOf "VTOL_Base_F"} || {_uav isKindOf "Quadbike_01_F"}) then {
-					private _velocity = velocityModelSpace _uav;
-					_uav setVelocityModelSpace [
-						_velocity select 0,
-						(_velocity select 1) max 15,
-						((_velocity select 2) max 0) + 8
-					];
+				private _alt = (["param7", 75] call _number) max 10;
+				private _pos = getPosATL _uav;
+				_pos set [2, _alt];
+				[_uav, _pos, "MOVE", 80, 25] call _commandMove;
+				_uav setVelocityModelSpace [0, 15, 8];
+				systemChat format ["ATAK TAKEOFF %1m %2", round _alt, _callsign];
+			};
+			case 21: {
+				[9] call _applyMode;
+			};
+			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_resolveVideoUri.sqf

@@ -2,7 +2,7 @@
 
 params [["_uav", objNull, [objNull]]];
 
-private _defaultVideoUri = "rtsp://irontak.com:554/fpv";
+private _defaultVideoUri = "rtsp://undefined:554/fpv";
 private _activelyControlledUav = if (!isNull player) then {getConnectedUAV player} else {objNull};
 
 private _normalize = {
@@ -20,15 +20,19 @@ private _normalize = {
 	format ["rtp://%1", _url]
 };
 
-if (!isNull _uav && {_activelyControlledUav isEqualTo _uav}) then {
+if (!isNull _uav) then {
 	private _objectVideoUrl = [_uav] call armatak_fnc_extract_marker_video_url;
 	private _normalizedObjectVideoUrl = [_objectVideoUrl] call _normalize;
 	if (_normalizedObjectVideoUrl isNotEqualTo "") exitWith {
 		_normalizedObjectVideoUrl
 	};
-};
 
-if (!isNull _uav) exitWith {
+	private _activeSessionVideoUrl = player getVariable [QEGVAR(client,video_feed_url), ""];
+	private _normalizedActiveSessionVideoUrl = [_activeSessionVideoUrl] call _normalize;
+	if (_normalizedActiveSessionVideoUrl isNotEqualTo "") exitWith {
+		_normalizedActiveSessionVideoUrl
+	};
+
 	_defaultVideoUri
 };
 

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

src/uas/callbacks.rs

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

src/uas/constants.rs

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

src/uas/endpoint.rs

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

src/uas/identity.rs

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

src/uas/mod.rs

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

src/uas/packets.rs

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

src/uas/payload.rs

@@ -33,6 +33,7 @@ pub struct UasSystemPayload {
     pub pitch_deg: f32,
     pub yaw_deg: f32,
     pub flying: bool,
+    pub landed: bool,
     pub gimbal_roll_deg: f32,
     pub gimbal_pitch_deg: f32,
     pub gimbal_yaw_deg: f32,
@@ -42,6 +43,8 @@ pub struct UasSystemPayload {
     pub image_lat_deg: f64,
     pub image_lon_deg: f64,
     pub image_alt_msl_m: f32,
+    pub has_turret_camera: bool,
+    pub battery_remaining_pct: i8,
 }
 
 impl FromArma for UasTelemetryPayload {
@@ -114,6 +117,7 @@ impl FromArma for UasSystemPayload {
             pitch_deg,
             yaw_deg,
             flying,
+            landed,
             gimbal_roll_deg,
             gimbal_pitch_deg,
             gimbal_yaw_deg,
@@ -123,6 +127,8 @@ impl FromArma for UasSystemPayload {
             image_lat_deg,
             image_lon_deg,
             image_alt_msl_m,
+            has_turret_camera,
+            battery_remaining_pct,
         ) = <(
             String,
             String,
@@ -138,6 +144,7 @@ impl FromArma for UasSystemPayload {
             f32,
             f32,
             i32,
+            i32,
             f32,
             f32,
             f32,
@@ -147,6 +154,8 @@ impl FromArma for UasSystemPayload {
             f64,
             f64,
             f32,
+            i32,
+            i32,
         )>::from_arma(data)?;
 
         Ok(Self {
@@ -164,6 +173,7 @@ impl FromArma for UasSystemPayload {
             pitch_deg,
             yaw_deg,
             flying: flying != 0,
+            landed: landed != 0,
             gimbal_roll_deg,
             gimbal_pitch_deg,
             gimbal_yaw_deg,
@@ -173,6 +183,8 @@ impl FromArma for UasSystemPayload {
             image_lat_deg,
             image_lon_deg,
             image_alt_msl_m,
+            has_turret_camera: has_turret_camera != 0,
+            battery_remaining_pct: battery_remaining_pct.clamp(0, 100) as i8,
         })
     }
 }

src/uas/send.rs

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

src/uas/state.rs

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