From 5a2e41c4e44823d84d800f7178a94d8d03abd6d0 Mon Sep 17 00:00:00 2001
From: Beniamino Pozzan <beniamino.pozzan@phd.unipd.it>
Date: Wed, 26 Oct 2022 20:50:23 -0700
Subject: [PATCH] microdds_client: add XRCE_DDS_KEY parameter

Multiple agents can connect to the same client

For sitl builds, if the intance number is different from zero,
  XRCE_DDS_KEY is set to the instance number and
  the microdds_client is automatically started
    with namespace
      px4_"instance_number"
    and udp port 8888
If the instance number is equal to zero
  XRCE_DDS_KEY is left untouched and
  the microdds_client is automatically started
    without namespace
    and udp port 8888

Signed-off-by: Beniamino Pozzan <beniamino.pozzan@phd.unipd.it>
---
 ROMFS/px4fmu_common/init.d-posix/rcS            |  8 +++++++-
 src/modules/microdds_client/microdds_client.cpp |  8 +++++++-
 src/modules/microdds_client/microdds_client.h   |  3 ++-
 src/modules/microdds_client/module.yaml         | 12 ++++++++++++
 4 files changed, 28 insertions(+), 3 deletions(-)

diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS
index daccb7bbab..43630694bb 100644
--- a/ROMFS/px4fmu_common/init.d-posix/rcS
+++ b/ROMFS/px4fmu_common/init.d-posix/rcS
@@ -260,7 +260,13 @@ fi
 navigator start
 
 # Try to start the microdds_client with UDP transport if module exists
-microdds_client start -t udp -p "$((px4_instance+8888))" -n "px4_${px4_instance}"
+if [ $px4_instance -eq 0 ]
+then
+	microdds_client start -t udp -p 8888
+else
+	param set XRCE_DDS_KEY ${px4_instance}
+	microdds_client start -t udp -p 8888 -n "px4_$px4_instance"
+fi
 
 if param greater -s MNT_MODE_IN -1
 then
diff --git a/src/modules/microdds_client/microdds_client.cpp b/src/modules/microdds_client/microdds_client.cpp
index 490d84af11..1c371da8cc 100644
--- a/src/modules/microdds_client/microdds_client.cpp
+++ b/src/modules/microdds_client/microdds_client.cpp
@@ -184,7 +184,13 @@ void MicroddsClient::run()
 
 		// Session
 		// The key identifier of the Client. All Clients connected to an Agent must have a different key.
-		const uint32_t key = 0xAAAABBBB;
+		const uint32_t key = (uint32_t)_param_xrce_key.get();
+
+		if (key == 0) {
+			PX4_ERR("session key must be different from zero");
+			return;
+		}
+
 		uxrSession session;
 		uxr_init_session(&session, _comm, key);
 
diff --git a/src/modules/microdds_client/microdds_client.h b/src/modules/microdds_client/microdds_client.h
index 7801ddb9d7..6403195c1a 100644
--- a/src/modules/microdds_client/microdds_client.h
+++ b/src/modules/microdds_client/microdds_client.h
@@ -92,7 +92,8 @@ private:
 	Timesync _timesync{};
 
 	DEFINE_PARAMETERS(
-		(ParamInt<px4::params::XRCE_DDS_DOM_ID>) _param_xrce_dds_dom_id
+		(ParamInt<px4::params::XRCE_DDS_DOM_ID>) _param_xrce_dds_dom_id,
+		(ParamInt<px4::params::XRCE_DDS_KEY>) _param_xrce_key
 	)
 };
 
diff --git a/src/modules/microdds_client/module.yaml b/src/modules/microdds_client/module.yaml
index 1c5bf77856..0b30eb2319 100644
--- a/src/modules/microdds_client/module.yaml
+++ b/src/modules/microdds_client/module.yaml
@@ -44,6 +44,18 @@ parameters:
             reboot_required: true
             default: 0
 
+        XRCE_DDS_KEY:
+            description:
+                short: XRCE DDS key
+                long: |
+                    XRCE DDS key, must be different from zero.
+                    In a single agent - multi client configuration, each client
+                    must have a unique session key.
+            category: System
+            type: int32
+            reboot_required: true
+            default: 1
+
         XRCE_DDS_UDP_PRT:
             description:
                 short: Micro DDS UDP Port