mirror of
https://github.com/valmojr/armatak.git
synced 2026-06-13 20:43:30 +00:00
383 lines
12 KiB
Plaintext
383 lines
12 KiB
Plaintext
#include "..\script_component.hpp"
|
|
|
|
params ["_function", ["_data", "", [""]]];
|
|
|
|
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 _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 _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, _callsign];
|
|
"armatak" callExtension ["log", [["info", format ["Applied MAVLINK ARM=%1 to UAV %2", _doArm, _uav]]]];
|
|
};
|
|
case 22: {
|
|
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];
|
|
};
|
|
};
|
|
default {
|
|
"armatak" callExtension ["log", [["info", format ["Unhandled MAVLINK COMMAND_LONG %1 (%2): %3", _command, _commandName, _data]]]];
|
|
};
|
|
};
|
|
};
|
|
case "COMMAND_INT": {
|
|
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]]]];
|
|
};
|
|
case "MANUAL_CONTROL": {
|
|
"armatak" callExtension ["log", [["info", format ["Received MAVLINK MANUAL_CONTROL: %1", _data]]]];
|
|
};
|
|
default {
|
|
"armatak" callExtension ["log", [["info", format ["Unhandled MAVLINK UDP callback %1: %2", _function, _data]]]];
|
|
};
|
|
};
|