From 9c50f5ea08acd878b4eedb49588be4dc807e183b Mon Sep 17 00:00:00 2001
From: Julien Lecoeur <julien.lecoeur@gmail.com>
Date: Wed, 21 Aug 2019 20:38:08 +0200
Subject: [PATCH] Add parameter COM_PREARM_MODE

Condition to enter the prearmed state, an intermediate state between disarmed and armed
 * in which non-throttling actuators are active.
 *
 * @value 0 Disabled
 * @value 1 Safety button
 * @value 2 Always
---
 src/modules/commander/Commander.cpp      | 35 ++++++++++++++++++------
 src/modules/commander/Commander.hpp      | 10 ++++++-
 src/modules/commander/commander_params.c | 14 ++++++++++
 3 files changed, 49 insertions(+), 10 deletions(-)

diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp
index 2babca47c9..0d83f561dd 100644
--- a/src/modules/commander/Commander.cpp
+++ b/src/modules/commander/Commander.cpp
@@ -2333,18 +2333,35 @@ Commander::run()
 			status.timestamp = hrt_absolute_time();
 			_status_pub.publish(status);
 
-			/* set prearmed state if safety is off, or safety is not present and 5 seconds passed */
-			if (safety.safety_switch_available) {
+			switch ((PrearmedMode)_param_com_prearm_mode.get()) {
+			case PrearmedMode::DISABLED:
+				/* skip prearmed state  */
+				armed.prearmed = false;
+				break;
 
-				/* safety is off, go into prearmed */
-				armed.prearmed = safety.safety_off;
-
-			} else {
+			case PrearmedMode::ALWAYS:
 				/* safety is not present, go into prearmed
-				 * (all output drivers should be started / unlocked last in the boot process
-				 * when the rest of the system is fully initialized)
-				 */
+				* (all output drivers should be started / unlocked last in the boot process
+				* when the rest of the system is fully initialized)
+				*/
 				armed.prearmed = (hrt_elapsed_time(&commander_boot_timestamp) > 5_s);
+				break;
+
+			case PrearmedMode::SAFETY_BUTTON:
+				if (safety.safety_switch_available) {
+
+					/* safety is off, go into prearmed */
+					armed.prearmed = safety.safety_off;
+
+				} else {
+					/* safety is not present, do not go into prearmed */
+					armed.prearmed = false;
+				}
+				break;
+
+			default:
+				armed.prearmed = false;
+				break;
 			}
 
 			armed.timestamp = hrt_absolute_time();
diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp
index b4f8901dba..5fad5ce808 100644
--- a/src/modules/commander/Commander.hpp
+++ b/src/modules/commander/Commander.hpp
@@ -140,10 +140,18 @@ private:
 
 		(ParamFloat<px4::params::COM_OF_LOSS_T>) _param_com_of_loss_t,
 		(ParamInt<px4::params::COM_OBL_ACT>) _param_com_obl_act,
-		(ParamInt<px4::params::COM_OBL_RC_ACT>) _param_com_obl_rc_act
+		(ParamInt<px4::params::COM_OBL_RC_ACT>) _param_com_obl_rc_act,
+
+		(ParamInt<px4::params::COM_PREARM_MODE>) _param_com_prearm_mode
 
 	)
 
+	enum class PrearmedMode {
+		DISABLED = 0,
+		SAFETY_BUTTON = 1,
+		ALWAYS = 2
+	};
+
 	const int64_t POSVEL_PROBATION_MIN = 1_s;	/**< minimum probation duration (usec) */
 	const int64_t POSVEL_PROBATION_MAX = 100_s;	/**< maximum probation duration (usec) */
 
diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c
index c8656f7c56..4f884f4edc 100644
--- a/src/modules/commander/commander_params.c
+++ b/src/modules/commander/commander_params.c
@@ -953,3 +953,17 @@ PARAM_DEFINE_INT32(COM_FLT_PROFILE, 0);
  * @boolean
  */
 PARAM_DEFINE_INT32(COM_ARM_CHK_ESCS, 1);
+
+/**
+ * Condition to enter prearmed mode
+ *
+ * Condition to enter the prearmed state, an intermediate state between disarmed and armed
+ * in which non-throttling actuators are active.
+ *
+ * @value 0 Disabled
+ * @value 1 Safety button
+ * @value 2 Always
+ *
+ * @group Commander
+ */
+PARAM_DEFINE_INT32(COM_PREARM_MODE, 1);