From a0a2bdaea51b351cba9899095e74c6c4a1b42360 Mon Sep 17 00:00:00 2001
From: Bertug Dilman <92028947+bdilman@users.noreply.github.com>
Date: Tue, 28 Jan 2025 14:33:15 +0100
Subject: [PATCH] commander: COM_MODE_ARM_CHK parameter to allow mode
 registration while armed (#24249)

---
 ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator     |  3 +++
 .../HealthAndArmingChecks/checks/externalChecks.hpp   |  6 +++++-
 src/modules/commander/ModeManagement.cpp              |  9 +++------
 src/modules/commander/commander_params.c              | 11 +++++++++++
 4 files changed, 22 insertions(+), 7 deletions(-)

diff --git a/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator b/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator
index 90e86aa6880e..fd0954744e47 100644
--- a/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator
+++ b/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator
@@ -4,6 +4,9 @@
 # Simulator IMU data provided at 250 Hz
 param set-default IMU_INTEG_RATE 250
 
+# For simulation, allow registering modes while armed for developer convenience
+param set-default COM_MODE_ARM_CHK 1
+
 if [ "$PX4_SIMULATOR" = "sihsim" ] || [ "$(param show -q SYS_AUTOSTART)" -eq "0" ]; then
 
 	echo "INFO  [init] SIH simulator"
diff --git a/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.hpp b/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.hpp
index 7129e4620361..bbba4180008c 100644
--- a/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.hpp
+++ b/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.hpp
@@ -38,6 +38,7 @@
 #include <uORB/topics/arming_check_reply.h>
 #include <uORB/Subscription.hpp>
 #include <uORB/Publication.hpp>
+#include <px4_platform_common/module_params.h>
 
 static_assert((1ull << arming_check_reply_s::HEALTH_COMPONENT_INDEX_AVOIDANCE) == (uint64_t)
 	      health_component_t::avoidance, "enum definition missmatch");
@@ -66,7 +67,7 @@ class ExternalChecks : public HealthAndArmingCheckBase
 	void update();
 
 	bool isUnresponsive(int registration_id);
-
+	bool allowUpdateWhileArmed() const { return _param_com_mode_arm_chk.get(); }
 private:
 	static constexpr hrt_abstime REQUEST_TIMEOUT = 50_ms;
 	static constexpr hrt_abstime UPDATE_INTERVAL = 300_ms;
@@ -109,4 +110,7 @@ class ExternalChecks : public HealthAndArmingCheckBase
 	uORB::Subscription _arming_check_reply_sub{ORB_ID(arming_check_reply)};
 
 	uORB::Publication<arming_check_request_s> _arming_check_request_pub{ORB_ID(arming_check_request)};
+	DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
+					(ParamBool<px4::params::COM_MODE_ARM_CHK>) _param_com_mode_arm_chk
+				       );
 };
diff --git a/src/modules/commander/ModeManagement.cpp b/src/modules/commander/ModeManagement.cpp
index 14ba7ba28fa3..4430a0f56305 100644
--- a/src/modules/commander/ModeManagement.cpp
+++ b/src/modules/commander/ModeManagement.cpp
@@ -370,11 +370,7 @@ void ModeManagement::update(bool armed, uint8_t user_intended_nav_state, bool fa
 	_failsafe_action_active = failsafe_action_active;
 	_external_checks.update();
 
-	bool allow_update_while_armed = false;
-#if defined(CONFIG_ARCH_BOARD_PX4_SITL)
-	// For simulation, allow registering modes while armed for developer convenience
-	allow_update_while_armed = true;
-#endif
+	bool allow_update_while_armed = _external_checks.allowUpdateWhileArmed();
 
 	if (armed && !allow_update_while_armed) {
 		// Reject registration requests
@@ -408,7 +404,8 @@ void ModeManagement::update(bool armed, uint8_t user_intended_nav_state, bool fa
 			}
 		}
 
-		// As we're disarmed we can use the user intended mode, as no failsafe will be active
+		// As we're disarmed we can use the user intended mode, as no failsafe will be active.
+		// Note that this might not be true if COM_MODE_ARM_CHK is set
 		checkNewRegistrations(update_request);
 		checkUnregistrations(user_intended_nav_state, update_request);
 	}
diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c
index 48b0cce5221a..37dcbdc9a9c7 100644
--- a/src/modules/commander/commander_params.c
+++ b/src/modules/commander/commander_params.c
@@ -1018,3 +1018,14 @@ PARAM_DEFINE_FLOAT(COM_THROW_SPEED, 5);
  * @increment 1
  */
 PARAM_DEFINE_INT32(COM_FLTT_LOW_ACT, 3);
+
+/**
+ * Allow external mode registration while armed.
+ *
+ * By default disabled for safety reasons
+ *
+ * @group Commander
+ * @boolean
+ *
+ */
+PARAM_DEFINE_INT32(COM_MODE_ARM_CHK, 0);