Added initial Laser Ranger Finder simulator on port 17211 (as default) emulate digital pointer for in game laser designators

2026-06-13 13:03:29 +00:00 · 2026-05-14 18:53:34 -03:00
parent 720f9da2df
commit d4dfd80cdf
11 changed files with 310 additions and 15 deletions
--- a/addons/client/XEH_PREP.hpp
+++ b/addons/client/XEH_PREP.hpp
@@ -1,3 +1,4 @@
 PREP(convertClientLocation);
 PREP(extractClientPosition);
+PREP(sendLaserRangeFinder);
 PREP(startUDPSocket);
--- 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);
--- 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";
    };
--- a/addons/client/functions/fnc_sendLaserRangeFinder.sqf
+++ 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];
+};
--- 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;
--- 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", []];
--- 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),
        }
    }
 }
--- a/src/cot/lrf.rs
+++ 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<LaserRangeFinderPayload, FromArmaError> {
+        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<LaserRangeFinderClearPayload, FromArmaError> {
+        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)
+}
--- 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;
--- 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(
--- a/src/udp_socket.rs
+++ b/src/udp_socket.rs
@@ -93,6 +93,8 @@ impl UdpClient {

 lazy_static! {
    static ref UDP_CLIENT: Arc<Mutex<Option<UdpClient>>> = Arc::new(Mutex::new(None));
+    static ref LRF_CLIENT: Arc<Mutex<Option<UdpClient>>> = Arc::new(Mutex::new(None));
+    static ref COT_CLIENT: Arc<Mutex<Option<UdpClient>>> = 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<Mutex<Option<UdpClient>>>,
+    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<UdpCommand>, Receiver<UdpCommand>) = 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<UdpCommand>, Receiver<UdpCommand>) = 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"
 }