diff --git a/addons/main/XEH_postInit.sqf b/addons/main/XEH_postInit.sqf index 96f1ff1..9000eaa 100644 --- a/addons/main/XEH_postInit.sqf +++ b/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); diff --git a/addons/uav/functions/fnc_handleMavlinkCallback.sqf b/addons/uav/functions/fnc_handleMavlinkCallback.sqf index 20f95ba..df96320 100644 --- a/addons/uav/functions/fnc_handleMavlinkCallback.sqf +++ b/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]]]]; diff --git a/addons/uav/functions/fnc_updateMavlinkBroadcast.sqf b/addons/uav/functions/fnc_updateMavlinkBroadcast.sqf index 1cf18fa..1617dbc 100644 --- a/addons/uav/functions/fnc_updateMavlinkBroadcast.sqf +++ b/addons/uav/functions/fnc_updateMavlinkBroadcast.sqf @@ -40,6 +40,11 @@ private _mavlinkAddress = player getVariable [QEGVAR(client,mavlink_address), "" if (_mavlinkAddress isEqualTo "") exitWith {}; private _pos = [_uav] call EFUNC(client,extractClientPosition); +private _relAlt = ((getPosATL _uav) select 2) max 0; +if (isNil {_uav getVariable "armatak_uas_home_atl"}) then { + _uav setVariable ["armatak_uas_home_atl", getPosATL _uav, true]; + _uav setVariable ["armatak_uas_home_geo", [_pos select 1, _pos select 2, (_pos select 3) - _relAlt], true]; +}; private _uuid = [_uav] call armatak_fnc_extract_uuid; private _callsign = [_uav] call armatak_fnc_extract_marker_callsign; private _videoUri = [_uav] call FUNC(resolveVideoUri); @@ -48,8 +53,15 @@ private _up = vectorUp _uav; private _yaw = getDir _uav; private _pitch = asin (((_dir select 2) max -1) min 1); private _roll = asin (((_up select 0) max -1) min 1); -private _relAlt = ((getPosATL _uav) select 2) max 0; private _uavType = if (_uav isKindOf "Plane") then {1} else {[2, 3] select (_uav isKindOf "Helicopter")}; +private _armed = _uav getVariable ["armatak_uas_armed", isEngineOn _uav]; +if !(isEngineOn _uav) then { + _armed = false; + _uav setVariable ["armatak_uas_armed", false, true]; +}; +private _groundSpeed = abs (_pos select 6); +private _landed = (_relAlt <= 1.5) && {_groundSpeed <= 0.5}; +private _batteryRemaining = round ((((fuel _uav) max 0) min 1) * 100); private _gimbalRoll = 0; private _gimbalPitch = _pitch; @@ -92,7 +104,8 @@ private _systemPayload = [ _roll, _pitch, _yaw, - parseNumber isEngineOn _uav, + parseNumber _armed, + parseNumber _landed, _gimbalRoll, _gimbalPitch, _gimbalYaw, @@ -102,7 +115,8 @@ private _systemPayload = [ _imageLat, _imageLon, _imageAlt, - parseNumber _hasTurretCamera + parseNumber _hasTurretCamera, + _batteryRemaining ]; "armatak" callExtension ["uas:send_uas_system", [_systemPayload]]; diff --git a/src/uas/callbacks.rs b/src/uas/callbacks.rs index 36a9ce0..dc50813 100644 --- a/src/uas/callbacks.rs +++ b/src/uas/callbacks.rs @@ -8,11 +8,29 @@ use super::identity::should_send_video_stream_information; use super::packets::{ autopilot_version_packet, camera_fov_status_packet_for_component, camera_information_packet_for_component, command_ack_packet, gimbal_manager_information_packet, - home_position_packet, mount_orientation_packet_for_component, mount_status_packet, + home_position_packet, mission_ack_packet, mission_request_int_packet, + mount_orientation_packet_for_component, mount_status_packet, video_stream_information_packet_for_component, video_stream_status_packet_for_component, }; -use super::state::{latest_system, set_active_camera}; +use super::state::{latest_system, set_active_camera, set_home}; use log::info; +use std::collections::HashMap; +use std::sync::{Mutex, OnceLock}; + +#[derive(Clone, Copy)] +struct MissionUploadState { + count: u16, + next_seq: u16, + mission_type: u8, + gcs_system: u8, + gcs_component: u8, +} + +static MISSION_UPLOADS: OnceLock>> = OnceLock::new(); + +fn mission_uploads() -> &'static Mutex> { + 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 { + 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 { payload .get(offset..offset + 2) @@ -221,7 +257,7 @@ pub(crate) fn mavlink_callback_event(bytes: &[u8], source: &str) -> Option Option= 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> { 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> { 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> { 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> { 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> { )] } +fn mission_count_response_packets( + gcs_system: u8, + gcs_component: u8, + payload: &[u8], +) -> Vec> { + 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> { + 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> { + 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> { + 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> { 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![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 => { diff --git a/src/uas/packets.rs b/src/uas/packets.rs index 3a3c1d7..bcea257 100644 --- a/src/uas/packets.rs +++ b/src/uas/packets.rs @@ -49,6 +49,37 @@ pub(crate) fn command_ack_packet( build_v2_packet(system_id, component_id, 77, &msg, 143) } +pub(crate) fn mission_request_int_packet( + system_id: u8, + component_id: u8, + target_system: u8, + target_component: u8, + seq: u16, + mission_type: u8, +) -> Vec { + 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 { + 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 { 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 { build_v1_packet(payload.system_id, payload.component_id, 74, &msg, 20) } -pub(crate) fn system_status_packet(system_id: u8) -> Vec { +pub(crate) fn system_status_packet(system_id: u8, battery_remaining_pct: i8) -> Vec { let fields = [ FieldSpec { ty: "uint32_t", @@ -203,12 +234,12 @@ pub(crate) fn system_status_packet(system_id: u8) -> Vec { 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 { +pub(crate) fn extended_sys_state_packet(system_id: u8, landed: bool) -> Vec { let fields = [ FieldSpec { ty: "uint8_t", @@ -225,10 +256,10 @@ pub(crate) fn extended_sys_state_packet(system_id: u8, flying: bool) -> Vec 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)