From 41e1ee60237d0aa414ca15e13316133e5ba89477 Mon Sep 17 00:00:00 2001
From: Pedro-Roque <padr@kth.se>
Date: Wed, 11 Mar 2026 15:58:47 -0700
Subject: [PATCH] feat: ensure safe DDS interface by default

---
 src/modules/uxrce_dds_client/dds_topics.h.em      |  4 ++++
 src/modules/uxrce_dds_client/module.yaml          | 11 +++++++++++
 src/modules/uxrce_dds_client/uxrce_dds_client.cpp | 12 ++++++++++++
 src/modules/uxrce_dds_client/uxrce_dds_client.h   |  7 ++++++-
 4 files changed, 33 insertions(+), 1 deletion(-)

diff --git a/src/modules/uxrce_dds_client/dds_topics.h.em b/src/modules/uxrce_dds_client/dds_topics.h.em
index 763cbb2990..279ff4edbe 100644
--- a/src/modules/uxrce_dds_client/dds_topics.h.em
+++ b/src/modules/uxrce_dds_client/dds_topics.h.em
@@ -180,8 +180,10 @@ struct RcvTopicsPubs {
 @[    end for]@
 
 	uint32_t num_payload_received{};
+	bool _allow_publishing{false};
 
 	bool init(uxrSession *session, uxrStreamId reliable_out_stream_id, uxrStreamId reliable_in_stream_id, uxrStreamId best_effort_in_stream_id, uxrObjectId participant_id, const char *client_namespace);
+	void allow_publishing(bool enabled) { _allow_publishing = enabled; }
 };
 
 static void on_topic_update(uxrSession *session, uxrObjectId object_id, uint16_t request_id, uxrStreamId stream_id,
@@ -191,6 +193,8 @@ static void on_topic_update(uxrSession *session, uxrObjectId object_id, uint16_t
 	const int64_t time_offset_us = session->time_offset / 1000; // ns -> us
 	pubs->num_payload_received += length;
 
+	if(!pubs->_allow_publishing) return;
+
 	switch (object_id.id) {
 @[    for idx, sub in enumerate(subscriptions)]@
 	case @(idx)+ (65535U / 32U) + 1: {
diff --git a/src/modules/uxrce_dds_client/module.yaml b/src/modules/uxrce_dds_client/module.yaml
index 5988a5a92c..518f378de9 100644
--- a/src/modules/uxrce_dds_client/module.yaml
+++ b/src/modules/uxrce_dds_client/module.yaml
@@ -153,3 +153,14 @@ parameters:
             category: System
             reboot_required: true
             default: 0
+
+        UXRCE_DDS_SAFE:
+            description:
+                short: Enables offboard safety protection
+                long: |
+                    If disable, allows offboard passthrough
+                    even in non-offboard modes.
+            type: boolean
+            category: System
+            reboot_required: true
+            default: 1
diff --git a/src/modules/uxrce_dds_client/uxrce_dds_client.cpp b/src/modules/uxrce_dds_client/uxrce_dds_client.cpp
index b773bc0e5e..f3400e59d9 100644
--- a/src/modules/uxrce_dds_client/uxrce_dds_client.cpp
+++ b/src/modules/uxrce_dds_client/uxrce_dds_client.cpp
@@ -374,6 +374,8 @@ bool UxrceddsClient::setupSession(uxrSession *session)
 	}
 
 	_connected = true;
+
+	_safe_dds_mode = _param_uxrce_dds_safe.get();
 	return true;
 }
 
@@ -651,6 +653,16 @@ void UxrceddsClient::run()
 
 			int bytes_available = 0;
 
+			// Update vehicle status to check for offboard mode
+			vehicle_status_s vehicle_status{};
+			_vehicle_status_sub.copy(&vehicle_status);
+			_offboard_mode_enabled = (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD);
+
+			// Allow publish from DDS to uORB if:
+			// - _param_uxrce_dds_safe is false , regardless of offboard mode
+			// - _param_uxrce_dds_safe is true AND offboard mode is enabled
+			_pubs->allow_publishing(!_safe_dds_mode || (_safe_dds_mode && _offboard_mode_enabled));
+
 			if (ioctl(_fd, FIONREAD, (unsigned long)&bytes_available) == OK) {
 				if (bytes_available > 10) {
 					orb_poll_timeout_ms = 0;
diff --git a/src/modules/uxrce_dds_client/uxrce_dds_client.h b/src/modules/uxrce_dds_client/uxrce_dds_client.h
index a7c1a5a13d..503e216b9f 100644
--- a/src/modules/uxrce_dds_client/uxrce_dds_client.h
+++ b/src/modules/uxrce_dds_client/uxrce_dds_client.h
@@ -41,6 +41,7 @@
 #include <uORB/topics/message_format_request.h>
 #include <uORB/topics/message_format_response.h>
 #include <uORB/Subscription.hpp>
+#include <uORB/topics/vehicle_status.h>
 
 #include <lib/timesync/Timesync.hpp>
 
@@ -138,6 +139,7 @@ private:
 
 	uORB::Publication<message_format_response_s> _message_format_response_pub{ORB_ID(message_format_response)};
 	uORB::Subscription _message_format_request_sub{ORB_ID(message_format_request)};
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
 
 	/** Synchronizes the system clock if the time is off by more than 5 seconds */
 	void syncSystemClock(uxrSession *session);
@@ -202,6 +204,8 @@ private:
 	bool _connected{false};
 	bool _session_created{false};
 	bool _timesync_converged{false};
+	bool _offboard_mode_enabled{false};
+	bool _safe_dds_mode{true};
 
 	Timesync _timesync{timesync_status_s::SOURCE_PROTOCOL_DDS};
 
@@ -216,6 +220,7 @@ private:
 		(ParamInt<px4::params::UXRCE_DDS_SYNCT>) _param_uxrce_dds_synct,
 		(ParamInt<px4::params::UXRCE_DDS_TX_TO>) _param_uxrce_dds_tx_to,
 		(ParamInt<px4::params::UXRCE_DDS_RX_TO>) _param_uxrce_dds_rx_to,
-		(ParamInt<px4::params::UXRCE_DDS_FLCTRL>) _param_uxrce_dds_flctrl
+		(ParamInt<px4::params::UXRCE_DDS_FLCTRL>) _param_uxrce_dds_flctrl,
+		(ParamInt<px4::params::UXRCE_DDS_SAFE>) _param_uxrce_dds_safe
 	)
 };