From d4dfd80cdf9929446eaf94ee650ccac4aeabb8c8 Mon Sep 17 00:00:00 2001 From: Valmo Trindade Date: Thu, 14 May 2026 18:53:34 -0300 Subject: [PATCH] Added initial Laser Ranger Finder simulator on port 17211 (as default) emulate digital pointer for in game laser designators --- addons/client/XEH_PREP.hpp | 1 + addons/client/XEH_postInit.sqf | 1 + addons/client/dialog.hpp | 26 +++- .../functions/fnc_sendLaserRangeFinder.sqf | 52 +++++++ .../client/functions/fnc_startUDPSocket.sqf | 24 ++- addons/main/XEH_postInit.sqf | 11 +- src/cot/digital_pointer.rs | 2 +- src/cot/lrf.rs | 59 ++++++++ src/cot/mod.rs | 1 + src/lib.rs | 9 ++ src/udp_socket.rs | 139 +++++++++++++++++- 11 files changed, 310 insertions(+), 15 deletions(-) create mode 100644 addons/client/functions/fnc_sendLaserRangeFinder.sqf create mode 100644 src/cot/lrf.rs diff --git a/addons/client/XEH_PREP.hpp b/addons/client/XEH_PREP.hpp index e9fa3c7..1d7b27b 100644 --- a/addons/client/XEH_PREP.hpp +++ b/addons/client/XEH_PREP.hpp @@ -1,3 +1,4 @@ PREP(convertClientLocation); PREP(extractClientPosition); +PREP(sendLaserRangeFinder); PREP(startUDPSocket); diff --git a/addons/client/XEH_postInit.sqf b/addons/client/XEH_postInit.sqf index 2480d8c..64e7a5b 100644 --- a/addons/client/XEH_postInit.sqf +++ b/addons/client/XEH_postInit.sqf @@ -6,3 +6,4 @@ _local_address = "armatak" callExtension ["local_ip", []] select 0; SETVAR(player,GVAR(localAddress),_local_address); SETVAR(player,GVAR(eudConnected),false); +SETVAR(player,GVAR(lrfEnabled),false); diff --git a/addons/client/dialog.hpp b/addons/client/dialog.hpp index e424bcf..459e542 100644 --- a/addons/client/dialog.hpp +++ b/addons/client/dialog.hpp @@ -12,7 +12,7 @@ class armatak_udp_socket_start_dialog { x = "0.386562 * safezoneW + safezoneX"; y = "0.357 * safezoneH + safezoneY"; w = "0.216563 * safezoneW"; - h = "0.418 * safezoneH"; + h = "0.495 * safezoneH"; colorBackground[]={0,0,0,0.45}; }; }; @@ -48,12 +48,22 @@ class armatak_udp_socket_start_dialog { idc = 16969; text = ""; x = "0.391719 * safezoneW + safezoneX"; - y = "0.632 * safezoneH + safezoneY"; + y = "0.709 * safezoneH + safezoneY"; w = "0.20625 * safezoneW"; h = "0.044 * safezoneH"; colorBackground[]={0,0,0,0.5}; tooltip = "Optional shared feed URL. If empty, the UAV 3DEN URL is used first, then a local RTP fallback."; }; + class armatak_gui_module_udp_socket_dialog_lrf_port_edit: RscEdit { + idc = 16971; + text = "17211"; + x = "0.391719 * safezoneW + safezoneX"; + y = "0.632 * safezoneH + safezoneY"; + w = "0.20625 * safezoneW"; + h = "0.044 * safezoneH"; + colorBackground[]={0,0,0,0.5}; + tooltip = "ATAK local Laser Range Finder UDP input. Leave empty to disable."; + }; class armatak_gui_module_udp_socket_dialog_address_text: RscText { idc = 16963; text = "EUD's Address"; @@ -82,6 +92,14 @@ class armatak_udp_socket_start_dialog { idc = 16970; text = "Video Feed URL (Optional)"; x = "0.391719 * safezoneW + safezoneX"; + y = "0.676 * safezoneH + safezoneY"; + w = "0.20625 * safezoneW"; + h = "0.033 * safezoneH"; + }; + class armatak_gui_module_udp_socket_dialog_lrf_port_text: RscText { + idc = 16972; + text = "Laser Range Finder Port"; + x = "0.391719 * safezoneW + safezoneX"; y = "0.599 * safezoneH + safezoneY"; w = "0.20625 * safezoneW"; h = "0.033 * safezoneH"; @@ -91,7 +109,7 @@ class armatak_udp_socket_start_dialog { text = "Cancel"; action = "closeDialog 2;"; x = "0.551563 * safezoneW + safezoneX"; - y = "0.709 * safezoneH + safezoneY"; + y = "0.786 * safezoneH + safezoneY"; w = "0.0464063 * safezoneW"; h = "0.055 * safezoneH"; }; @@ -100,7 +118,7 @@ class armatak_udp_socket_start_dialog { text = "Ok"; action = QUOTE(call FUNC(startUDPSocket)); x = "0.5 * safezoneW + safezoneX"; - y = "0.709 * safezoneH + safezoneY"; + y = "0.786 * safezoneH + safezoneY"; w = "0.0464063 * safezoneW"; h = "0.055 * safezoneH"; }; diff --git a/addons/client/functions/fnc_sendLaserRangeFinder.sqf b/addons/client/functions/fnc_sendLaserRangeFinder.sqf new file mode 100644 index 0000000..9bef112 --- /dev/null +++ b/addons/client/functions/fnc_sendLaserRangeFinder.sqf @@ -0,0 +1,52 @@ +#include "..\script_component.hpp" + +params ["_unit"]; + +private _lrfEnabled = player getVariable [QGVAR(lrfEnabled), false]; + +private _uid = format ["%1.LRF", _unit call armatak_fnc_extract_uuid]; +private _laserTarget = laserTarget _unit; + +if (!isNull _laserTarget) exitWith { + private _originASL = getPosASL _unit; + private _targetASL = getPosASL _laserTarget; + private _delta = _targetASL vectorDiff _originASL; + + private _dx = _delta select 0; + private _dy = _delta select 1; + private _dz = _delta select 2; + private _horizontalDistance = sqrt ((_dx * _dx) + (_dy * _dy)); + private _slantDistance = (_originASL vectorDistance _targetASL) max 1; + private _azimuth = (((_dx atan2 _dy) + 360) mod 360); + private _elevation = _dz atan2 (_horizontalDistance max 0.001); + private _lastTargetASL = player getVariable [QGVAR(lrfLastTargetASL), []]; + private _lastSentAt = player getVariable [QGVAR(lrfLastSentAt), -1000]; + private _targetMoved = _lastTargetASL isEqualTo [] || {(_lastTargetASL vectorDistance _targetASL) > 5}; + private _sendCooldownElapsed = (time - _lastSentAt) >= 2.5; + + player setVariable [QGVAR(lrfWasActive), true]; + player setVariable [QGVAR(lrfLostAt), -1]; + player setVariable [QGVAR(lrfClearSent), false]; + + if (_lrfEnabled && {_targetMoved} && {_sendCooldownElapsed}) then { + "armatak" callExtension ["udp_socket:send_lrf", [[_uid, _slantDistance, _azimuth, _elevation]]]; + player setVariable [QGVAR(lrfLastTargetASL), _targetASL]; + player setVariable [QGVAR(lrfLastSentAt), time]; + }; +}; + +if !(player getVariable [QGVAR(lrfWasActive), false]) exitWith {}; + +private _lostAt = player getVariable [QGVAR(lrfLostAt), -1]; +if (_lostAt < 0) then { + player setVariable [QGVAR(lrfLostAt), time]; +}; + +private _clearSent = player getVariable [QGVAR(lrfClearSent), false]; +if (_lrfEnabled && {!_clearSent} && {(time - (player getVariable [QGVAR(lrfLostAt), time])) >= 6}) then { + "armatak" callExtension ["udp_socket:clear_lrf", [_uid]]; + player setVariable [QGVAR(lrfWasActive), false]; + player setVariable [QGVAR(lrfClearSent), true]; + player setVariable [QGVAR(lrfLastTargetASL), []]; + player setVariable [QGVAR(lrfLastSentAt), -1000]; +}; diff --git a/addons/client/functions/fnc_startUDPSocket.sqf b/addons/client/functions/fnc_startUDPSocket.sqf index 79def4c..0f397bc 100644 --- a/addons/client/functions/fnc_startUDPSocket.sqf +++ b/addons/client/functions/fnc_startUDPSocket.sqf @@ -14,24 +14,38 @@ disableSerialization; private _eud_address = ctrlText 16961; private _gnss_port = ctrlText 16962; private _mavlink_port = ctrlText 16967; +private _lrf_port = ctrlText 16971; private _video_feed_url = ctrlText 16969; private _udp_socket_fulladdress = _eud_address + ":" + _gnss_port; private _mavlink_address = _eud_address + ":" + _mavlink_port; +private _lrf_port_trimmed = trim _lrf_port; +private _lrf_enabled = _lrf_port_trimmed isNotEqualTo ""; +private _lrf_address = _eud_address + ":" + _lrf_port_trimmed; player setVariable [QGVAR(udp_socket_address), _udp_socket_fulladdress]; player setVariable [QGVAR(mavlink_address), _mavlink_address]; +player setVariable [QGVAR(lrf_address), _lrf_address]; +player setVariable [QGVAR(lrfEnabled), _lrf_enabled]; +player setVariable [QGVAR(lrfWasActive), false]; +player setVariable [QGVAR(lrfLostAt), -1]; +player setVariable [QGVAR(lrfClearSent), false]; +player setVariable [QGVAR(lrfLastTargetASL), []]; +player setVariable [QGVAR(lrfLastSentAt), -1000]; player setVariable [QGVAR(video_feed_url), trim _video_feed_url]; -player setVariable [QGVAR(eudConnected), true]; +player setVariable [QGVAR(eudConnected), true, true]; private _advertised_video_uri = [objNull] call EFUNC(uav,resolveVideoUri); "armatak" callExtension ["udp_socket:start", [_udp_socket_fulladdress]]; "armatak" callExtension ["uas:start_endpoint", [parseNumber _mavlink_port]]; +if (_lrf_enabled) then { + "armatak" callExtension ["udp_socket:start_lrf", [_lrf_address]]; +}; private _mdnsInstanceName = format ["ArmaTAK-%1", name player]; "armatak" callExtension ["mdns:start_uas_advertisement", [_mdnsInstanceName, parseNumber _mavlink_port, _advertised_video_uri]]; -"armatak" callExtension ["log", [["info", format ["Client UDP socket started for %1, MAVLink target set to %2 and advertised video URI set to %3", _udp_socket_fulladdress, _mavlink_address, _advertised_video_uri]]]]; +"armatak" callExtension ["log", [["info", format ["Client UDP socket started for %1, MAVLink target set to %2, LRF target set to %3 and advertised video URI set to %4. Digital pointer uses ATAK LRF when enabled.", _udp_socket_fulladdress, _mavlink_address, _lrf_address, _advertised_video_uri]]]]; call EFUNC(uav,startMavlinkBroadcast); @@ -41,5 +55,11 @@ call EFUNC(uav,startMavlinkBroadcast); "armatak" callExtension ["udp_socket:send_gps_cot", [player call FUNC(extractClientPosition)]]; }, 0.5, []] call CBA_fnc_addPerFrameHandler; +[{ + if !(player getVariable [QGVAR(eudConnected), false]) exitWith {}; + + [player] call FUNC(sendLaserRangeFinder); +}, 0.25, []] call CBA_fnc_addPerFrameHandler; + deleteVehicle _logic; closeDialog 1; diff --git a/addons/main/XEH_postInit.sqf b/addons/main/XEH_postInit.sqf index 9000eaa..bd7242e 100644 --- a/addons/main/XEH_postInit.sqf +++ b/addons/main/XEH_postInit.sqf @@ -9,10 +9,11 @@ addMissionEventHandler ["ExtensionCallback", { switch (_function) do { case "EUD Connected": { - SETVAR(player,EGVAR(client,eudConnected),true); + player setVariable [QEGVAR(client,eudConnected), true, true]; }; case "EUD Disconnected": { - SETVAR(player,EGVAR(client,eudConnected),false); + player setVariable [QEGVAR(client,eudConnected), false, true]; + SETVAR(player,EGVAR(client,lrfEnabled),false); call EFUNC(uav,stopMavlinkBroadcast); "armatak" callExtension ["uas:stop_endpoint", []]; "armatak" callExtension ["mdns:stop", []]; @@ -27,14 +28,16 @@ addMissionEventHandler ["ExtensionCallback", { [_function, "error", _name] call FUNC(notify); if (_function == "UDP Socket is not running") then { - SETVAR(player,EGVAR(client,eudConnected),false); + player setVariable [QEGVAR(client,eudConnected), false, true]; + SETVAR(player,EGVAR(client,lrfEnabled),false); call EFUNC(uav,stopMavlinkBroadcast); "armatak" callExtension ["uas:stop_endpoint", []]; "armatak" callExtension ["mdns:stop", []]; }; if (_function == "failed to bind UDP socket") then { - SETVAR(player,EGVAR(client,eudConnected),false); + player setVariable [QEGVAR(client,eudConnected), false, true]; + SETVAR(player,EGVAR(client,lrfEnabled),false); call EFUNC(uav,stopMavlinkBroadcast); "armatak" callExtension ["uas:stop_endpoint", []]; "armatak" callExtension ["mdns:stop", []]; diff --git a/src/cot/digital_pointer.rs b/src/cot/digital_pointer.rs index 3ad7580..d70a6c8 100644 --- a/src/cot/digital_pointer.rs +++ b/src/cot/digital_pointer.rs @@ -41,7 +41,7 @@ impl DigitalPointerPayload { link_uid: Some(self.link_uid.clone()), remarker: None, video_url: None, - stale_seconds: None, + stale_seconds: Some(7), } } } diff --git a/src/cot/lrf.rs b/src/cot/lrf.rs new file mode 100644 index 0000000..f791047 --- /dev/null +++ b/src/cot/lrf.rs @@ -0,0 +1,59 @@ +use arma_rs::{FromArma, FromArmaError}; +use chrono::{SecondsFormat, Utc}; + +pub struct LaserRangeFinderPayload { + pub uid: String, + pub distance_meters: f64, + pub azimuth_degrees: f64, + pub elevation_degrees: f64, +} + +impl FromArma for LaserRangeFinderPayload { + fn from_arma(data: String) -> Result { + let (uid, distance_meters, azimuth_degrees, elevation_degrees) = + <(String, f64, f64, f64)>::from_arma(data)?; + Ok(Self { + uid, + distance_meters, + azimuth_degrees, + elevation_degrees, + }) + } +} + +impl LaserRangeFinderPayload { + pub fn to_lrf_message(&self) -> String { + let timestamp = Utc::now().to_rfc3339_opts(SecondsFormat::Millis, true); + + format!( + "1,{},{},{:.2},{:.2},{:.2}", + self.uid, + timestamp, + self.distance_meters.max(0.0), + normalize_degrees(self.azimuth_degrees), + self.elevation_degrees + ) + } +} + +pub struct LaserRangeFinderClearPayload { + pub uid: String, +} + +impl FromArma for LaserRangeFinderClearPayload { + fn from_arma(data: String) -> Result { + let uid = String::from_arma(data)?; + Ok(Self { uid }) + } +} + +impl LaserRangeFinderClearPayload { + pub fn to_lrf_message(&self) -> String { + let timestamp = Utc::now().to_rfc3339_opts(SecondsFormat::Millis, true); + format!("1,{},{},RANGE_ERROR", self.uid, timestamp) + } +} + +fn normalize_degrees(degrees: f64) -> f64 { + degrees.rem_euclid(360.0) +} diff --git a/src/cot/mod.rs b/src/cot/mod.rs index 76a7c55..be3db16 100644 --- a/src/cot/mod.rs +++ b/src/cot/mod.rs @@ -4,6 +4,7 @@ pub mod digital_pointer; pub mod draws; pub mod eud; pub mod gps; +pub mod lrf; pub mod message; pub mod nato; pub mod report_marker; diff --git a/src/lib.rs b/src/lib.rs index d341285..ae82e3b 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -59,8 +59,17 @@ pub fn init() -> Extension { "udp_socket", Group::new() .command("start", udp_socket::start) + .command("start_lrf", udp_socket::start_lrf) + .command("start_cot", udp_socket::start_cot) .command("send_payload", udp_socket::send_payload) .command("send_gps_cot", udp_socket::send_gps_cot) + .command("send_eud_cot", udp_socket::send_eud_cot) + .command("send_lrf", udp_socket::send_lrf) + .command("clear_lrf", udp_socket::clear_lrf) + .command( + "send_digital_pointer_cot", + udp_socket::send_digital_pointer_cot, + ) .command("stop", udp_socket::stop), ) .group( diff --git a/src/udp_socket.rs b/src/udp_socket.rs index ea0c812..e3fa296 100644 --- a/src/udp_socket.rs +++ b/src/udp_socket.rs @@ -93,6 +93,8 @@ impl UdpClient { lazy_static! { static ref UDP_CLIENT: Arc>> = Arc::new(Mutex::new(None)); + static ref LRF_CLIENT: Arc>> = Arc::new(Mutex::new(None)); + static ref COT_CLIENT: Arc>> = Arc::new(Mutex::new(None)); } pub fn start(ctx: Context, address: String) -> &'static str { @@ -125,13 +127,22 @@ pub fn start(ctx: Context, address: String) -> &'static str { "Starting UDP Client" } -pub fn send_payload(ctx: Context, payload: String) -> &'static str { - if let Some(ref client) = *UDP_CLIENT.lock().unwrap() { +fn send_with_client( + client_slot: &Arc>>, + ctx: Context, + payload: String, + missing_message: &'static str, +) { + if let Some(ref client) = *client_slot.lock().unwrap() { client.send_payload(ctx, payload); } else { - let _ = ctx.callback_null("UDP SOCKET ERROR", "UDP Socket is not running"); - info!("UDP send requested while socket was not running"); + let _ = ctx.callback_null("UDP SOCKET ERROR", missing_message); + info!("UDP send requested while target socket was not running"); } +} + +pub fn send_payload(ctx: Context, payload: String) -> &'static str { + send_with_client(&UDP_CLIENT, ctx, payload, "UDP Socket is not running"); "Sending payload to UDP server" } @@ -146,6 +157,116 @@ pub fn send_gps_cot( "Sending GPS Cursor Over Time to UDP server" } +pub fn send_eud_cot( + ctx: Context, + cursor_over_time: cot::gps::ExternalPositionPayload, +) -> &'static str { + send_with_client( + &COT_CLIENT, + ctx, + cursor_over_time.to_cot().convert_to_xml(), + "CoT UDP Socket is not running", + ); + + "Sending EUD Cursor Over Time to CoT UDP server" +} + +pub fn start_lrf(ctx: Context, address: String) -> &'static str { + info!("LRF UDP socket start requested for {}", address); + + let (tx, rx): (Sender, Receiver) = mpsc::channel(); + + let client = UdpClient { + tx, + address: address.clone(), + }; + + { + let mut client_guard = LRF_CLIENT.lock().unwrap(); + if let Some(ref existing_client) = *client_guard { + info!( + "Stopping previous LRF UDP client {} before starting {}", + existing_client.address, address + ); + existing_client.stop(); + } + *client_guard = Some(UdpClient { + tx: client.tx.clone(), + address: client.address.clone(), + }); + } + + client.start(address, rx, ctx); + + "Starting LRF UDP Client" +} + +pub fn start_cot(ctx: Context, address: String) -> &'static str { + info!("CoT UDP socket start requested for {}", address); + + let (tx, rx): (Sender, Receiver) = mpsc::channel(); + + let client = UdpClient { + tx, + address: address.clone(), + }; + + { + let mut client_guard = COT_CLIENT.lock().unwrap(); + if let Some(ref existing_client) = *client_guard { + info!( + "Stopping previous CoT UDP client {} before starting {}", + existing_client.address, address + ); + existing_client.stop(); + } + *client_guard = Some(UdpClient { + tx: client.tx.clone(), + address: client.address.clone(), + }); + } + + client.start(address, rx, ctx); + + "Starting CoT UDP Client" +} + +pub fn send_lrf(ctx: Context, payload: cot::lrf::LaserRangeFinderPayload) -> &'static str { + send_with_client( + &LRF_CLIENT, + ctx, + payload.to_lrf_message(), + "LRF UDP Socket is not running", + ); + + "Sending Laser Range Finder payload to UDP server" +} + +pub fn clear_lrf(ctx: Context, payload: cot::lrf::LaserRangeFinderClearPayload) -> &'static str { + send_with_client( + &LRF_CLIENT, + ctx, + payload.to_lrf_message(), + "LRF UDP Socket is not running", + ); + + "Clearing Laser Range Finder payload on UDP server" +} + +pub fn send_digital_pointer_cot( + ctx: Context, + payload: cot::digital_pointer::DigitalPointerPayload, +) -> &'static str { + send_with_client( + &COT_CLIENT, + ctx, + payload.to_cot().convert_to_xml(), + "CoT UDP Socket is not running", + ); + + "Sending Digital Pointer CoT to UDP server" +} + pub fn stop(ctx: Context) -> &'static str { if let Some(ref client) = *UDP_CLIENT.lock().unwrap() { info!("UDP socket stop requested for {}", client.address); @@ -156,5 +277,15 @@ pub fn stop(ctx: Context) -> &'static str { info!("UDP stop requested while socket was not running"); } + if let Some(ref client) = *LRF_CLIENT.lock().unwrap() { + info!("LRF UDP socket stop requested for {}", client.address); + client.stop(); + } + + if let Some(ref client) = *COT_CLIENT.lock().unwrap() { + info!("CoT UDP socket stop requested for {}", client.address); + client.stop(); + } + "Stopping UDP Client" }