From 1e3237cd105ffd4a245b2dd0b8c7c36c43926f83 Mon Sep 17 00:00:00 2001
From: Daniel Mesham <daniel@auterion.com>
Date: Thu, 2 May 2024 12:07:00 +0200
Subject: [PATCH] WIP: allow selecting the RC override mode with a parameter

* Options are Position and Postion Slow.
---
 src/modules/commander/Commander.cpp          | 24 +++++++++++++++++++-
 src/modules/commander/Commander.hpp          |  8 +++++++
 src/modules/commander/commander_params.c     | 12 ++++++++++
 src/modules/commander/failsafe/framework.cpp |  4 +++-
 4 files changed, 46 insertions(+), 2 deletions(-)

diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp
index 444f5b0c8d..375d862425 100644
--- a/src/modules/commander/Commander.cpp
+++ b/src/modules/commander/Commander.cpp
@@ -2916,7 +2916,11 @@ void Commander::manualControlCheck()
 				if (override_enabled) {
 					// If no failsafe is active, directly change the mode, otherwise pass the request to the failsafe state machine
 					if (_failsafe.selectedAction() <= FailsafeBase::Action::Warn) {
-						if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_POSCTL, ModeChangeSource::User, true)) {
+						// TODO: determine the mode to use
+						const int32_t rc_override_mode = getRcOverrideMode();
+						PX4_INFO("RC override to mode %d", rc_override_mode);
+
+						if (_user_mode_intention.change(rc_override_mode, ModeChangeSource::User, true)) {
 							tune_positive(true);
 							mavlink_log_info(&_mavlink_log_pub, "Pilot took over using sticks\t");
 							events::send(events::ID("commander_rc_override"), events::Log::Info, "Pilot took over using sticks");
@@ -2967,6 +2971,24 @@ void Commander::send_parachute_command()
 	set_tune_override(tune_control_s::TUNE_ID_PARACHUTE_RELEASE);
 }
 
+uint8_t Commander::getRcOverrideMode() const
+{
+	const int32_t rc_override_mode = _param_com_rc_ov_mode.get();
+
+	switch (rc_override_mode) {
+	case static_cast<int32_t>(RcOverrideModes::POSITION):
+		return vehicle_status_s::NAVIGATION_STATE_POSCTL;
+
+	case static_cast<int32_t>(RcOverrideModes::POSITION_SLOW):
+		return vehicle_status_s::NAVIGATION_STATE_POSITION_SLOW;
+
+	default:
+		PX4_WARN("Unexpected value for COM_RC_OV_MODE: %d. Using Position mode.", rc_override_mode);
+	}
+
+	return vehicle_status_s::NAVIGATION_STATE_POSCTL;
+}
+
 int Commander::print_usage(const char *reason)
 {
 	if (reason) {
diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp
index d477585c03..910137e3e8 100644
--- a/src/modules/commander/Commander.hpp
+++ b/src/modules/commander/Commander.hpp
@@ -200,6 +200,8 @@ private:
 
 	void modeManagementUpdate();
 
+	uint8_t getRcOverrideMode() const;
+
 	enum class PrearmedMode {
 		DISABLED = 0,
 		SAFETY_BUTTON = 1,
@@ -211,6 +213,11 @@ private:
 		OFFBOARD_MODE_BIT = (1 << 1),
 	};
 
+	enum class RcOverrideModes : int32_t {
+		POSITION = 0,
+		POSITION_SLOW = 1
+	};
+
 	/* Decouple update interval and hysteresis counters, all depends on intervals */
 	static constexpr uint64_t COMMANDER_MONITORING_INTERVAL{10_ms};
 
@@ -346,6 +353,7 @@ private:
 		(ParamFloat<px4::params::COM_OBC_LOSS_T>)   _param_com_obc_loss_t,
 		(ParamInt<px4::params::COM_PREARM_MODE>)    _param_com_prearm_mode,
 		(ParamInt<px4::params::COM_RC_OVERRIDE>)    _param_com_rc_override,
+		(ParamInt<px4::params::COM_RC_OV_MODE>)     _param_com_rc_ov_mode,
 		(ParamInt<px4::params::COM_FLIGHT_UUID>)    _param_flight_uuid,
 		(ParamInt<px4::params::COM_TAKEOFF_ACT>)    _param_takeoff_finished_action,
 		(ParamFloat<px4::params::COM_CPU_MAX>)      _param_com_cpu_max
diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c
index 8f2c3d51ce..2060d11f3c 100644
--- a/src/modules/commander/commander_params.c
+++ b/src/modules/commander/commander_params.c
@@ -483,6 +483,18 @@ PARAM_DEFINE_INT32(COM_RC_OVERRIDE, 1);
  */
 PARAM_DEFINE_FLOAT(COM_RC_STICK_OV, 30.0f);
 
+/**
+ * RC stick override mode
+ *
+ * When an RC override occurs (see COM_RC_OVERRIDE), switch to the selected mode, if possible.
+ *
+ * @value 0 Position
+ * @value 1 Position Slow
+ *
+ * @group Commander
+ */
+PARAM_DEFINE_INT32(COM_RC_OV_MODE, 0);
+
 /**
  * Require valid mission to arm
  *
diff --git a/src/modules/commander/failsafe/framework.cpp b/src/modules/commander/failsafe/framework.cpp
index 9f88f85f3e..b718b3f586 100644
--- a/src/modules/commander/failsafe/framework.cpp
+++ b/src/modules/commander/failsafe/framework.cpp
@@ -485,6 +485,8 @@ void FailsafeBase::getSelectedAction(const State &state, const failsafe_flags_s
 	if (actionAllowsUserTakeover(selected_action) && takeover_allowed) {
 		if (!_user_takeover_active && rc_sticks_takeover_request) {
 			// TODO: if the user intended mode is a stick-controlled mode, switch back to that instead
+			PX4_WARN("Setting POSCTL");
+			// TODO: get configured mode here
 			returned_state.updated_user_intended_mode = vehicle_status_s::NAVIGATION_STATE_POSCTL;
 		}
 
@@ -493,7 +495,7 @@ void FailsafeBase::getSelectedAction(const State &state, const failsafe_flags_s
 		returned_state.delayed_action = Action::None;
 
 		if (!_user_takeover_active) {
-			PX4_DEBUG("Activating user takeover");
+			PX4_WARN("Activating user takeover");
 #ifndef EMSCRIPTEN_BUILD
 			events::send(events::ID("failsafe_rc_override"), events::Log::Info, "Pilot took over using sticks");
 #endif // EMSCRIPTEN_BUILD