valmojr/armatak
1
0
Fork 0
mirror of https://github.com/valmojr/armatak.git synced 2026-06-13 14:13:29 +00:00
Code Issues Packages Projects Releases Wiki Activity

Added UAS Tool active actions from ATAK

Browse Source
This commit is contained in:
Valmo Trindade
2026-05-10 01:19:39 -03:00
parent 80320c0f2c
commit 9b05bcccc2
5 changed files with 798 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
addons/main/XEH_postInit.sqf
View File

@@ -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);

359
addons/uav/functions/fnc_handleMavlinkCallback.sqf
View File

@@ -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]]]];

20
addons/uav/functions/fnc_updateMavlinkBroadcast.sqf
View File

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

404
src/uas/callbacks.rs
View File

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

43
src/uas/packets.rs
View File

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