From 0835c432e34983339899d690c17b4634d02722a2 Mon Sep 17 00:00:00 2001
From: bresch <brescianimathieu@gmail.com>
Date: Mon, 22 May 2023 16:19:44 +0200
Subject: [PATCH] ekf2: select estimator instance manually using RC channel

---
 src/modules/ekf2/EKF2Selector.cpp       | 53 ++++++++++++++++++++++++-
 src/modules/ekf2/EKF2Selector.hpp       |  8 +++-
 src/modules/ekf2/ekf2_params_selector.c | 36 +++++++++++++++++
 3 files changed, 95 insertions(+), 2 deletions(-)

diff --git a/src/modules/ekf2/EKF2Selector.cpp b/src/modules/ekf2/EKF2Selector.cpp
index 99fb863eed..bac41f0df9 100644
--- a/src/modules/ekf2/EKF2Selector.cpp
+++ b/src/modules/ekf2/EKF2Selector.cpp
@@ -754,7 +754,16 @@ void EKF2Selector::Run()
 			}
 		}
 
-		if (!_instance[_selected_instance].healthy.get_state()) {
+		if (_param_ekf2_sel_rc_map.get() > 0) {
+			// Manual instance selection through RC
+			if (isRcOverride()) {
+				SelectInstance(_param_ekf2_sel_rc_inst.get());
+
+			} else {
+				SelectInstance(0);
+			}
+
+		} else if (!_instance[_selected_instance].healthy.get_state()) {
 			// prefer the best healthy instance using a different IMU
 			if (!SelectInstance(best_ekf_different_imu)) {
 				// otherwise switch to the healthy instance with best overall test ratio
@@ -807,6 +816,48 @@ void EKF2Selector::Run()
 	ScheduleDelayed(FILTER_UPDATE_PERIOD);
 }
 
+bool EKF2Selector::isRcOverride()
+{
+	manual_control_setpoint_s manual_control_setpoint{};
+	_manual_control_setpoint_sub.copy(&manual_control_setpoint);
+
+	float channel = 0;
+
+	switch (_param_ekf2_sel_rc_map.get()) {
+	case 0:
+		return false;
+
+	case 1:
+		channel = manual_control_setpoint.aux1;
+		break;
+
+	case 2:
+		channel = manual_control_setpoint.aux2;
+		break;
+
+	case 3:
+		channel = manual_control_setpoint.aux3;
+		break;
+
+	case 4:
+		channel = manual_control_setpoint.aux4;
+		break;
+
+	case 5:
+		channel = manual_control_setpoint.aux5;
+		break;
+
+	case 6:
+		channel = manual_control_setpoint.aux6;
+		break;
+
+	default:
+		return false;
+	}
+
+	return channel > .5f;
+}
+
 void EKF2Selector::PublishEstimatorSelectorStatus()
 {
 	estimator_selector_status_s selector_status{};
diff --git a/src/modules/ekf2/EKF2Selector.hpp b/src/modules/ekf2/EKF2Selector.hpp
index 6507233bd3..3d8a9cdaec 100644
--- a/src/modules/ekf2/EKF2Selector.hpp
+++ b/src/modules/ekf2/EKF2Selector.hpp
@@ -47,6 +47,7 @@
 #include <uORB/Publication.hpp>
 #include <uORB/topics/estimator_selector_status.h>
 #include <uORB/topics/estimator_status.h>
+#include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/sensor_selection.h>
 #include <uORB/topics/sensors_status_imu.h>
@@ -82,6 +83,8 @@ private:
 
 	void Run() override;
 
+	bool isRcOverride();
+
 	void PrintInstanceChange(const uint8_t old_instance, uint8_t new_instance);
 
 	void PublishEstimatorSelectorStatus();
@@ -228,6 +231,7 @@ private:
 	uint8_t _global_position_instance_prev{INVALID_INSTANCE};
 	uint8_t _odometry_instance_prev{INVALID_INSTANCE};
 
+	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 	uORB::Subscription _sensors_status_imu{ORB_ID(sensors_status_imu)};
 
@@ -245,7 +249,9 @@ private:
 		(ParamFloat<px4::params::EKF2_SEL_IMU_RAT>) _param_ekf2_sel_imu_angle_rate,
 		(ParamFloat<px4::params::EKF2_SEL_IMU_ANG>) _param_ekf2_sel_imu_angle,
 		(ParamFloat<px4::params::EKF2_SEL_IMU_ACC>) _param_ekf2_sel_imu_accel,
-		(ParamFloat<px4::params::EKF2_SEL_IMU_VEL>) _param_ekf2_sel_imu_velocity
+		(ParamFloat<px4::params::EKF2_SEL_IMU_VEL>) _param_ekf2_sel_imu_velocity,
+		(ParamInt<px4::params::EKF2_SEL_RC_MAP>) _param_ekf2_sel_rc_map,
+		(ParamInt<px4::params::EKF2_SEL_RC_INST>) _param_ekf2_sel_rc_inst
 	)
 };
 #endif // !EKF2SELECTOR_HPP
diff --git a/src/modules/ekf2/ekf2_params_selector.c b/src/modules/ekf2/ekf2_params_selector.c
index 43a630fe8f..a085199852 100644
--- a/src/modules/ekf2/ekf2_params_selector.c
+++ b/src/modules/ekf2/ekf2_params_selector.c
@@ -79,3 +79,39 @@ PARAM_DEFINE_FLOAT(EKF2_SEL_IMU_ACC, 1.0f);
  * @unit m/s
  */
 PARAM_DEFINE_FLOAT(EKF2_SEL_IMU_VEL, 2.0f);
+
+/**
+ * Manual instance selection channel
+ *
+ * Defines which RC_MAP_AUXn parameter maps the RC channel used to select a desired
+ * EKF2 instance.
+ * Use EKF2_SEL_RC_INST to define which instance is selected when this switch is active.
+ * When manual selection is active and the switch is off, instance 0 is selected.
+ * Manual selection overrides automatic instance switch.
+ *
+ * @value 0 Disable
+ * @value 1 Aux1
+ * @value 2 Aux2
+ * @value 3 Aux3
+ * @value 4 Aux4
+ * @value 5 Aux5
+ * @value 6 Aux6
+ * @min 0
+ * @max 6
+ * @group EKF2
+ */
+PARAM_DEFINE_INT32(EKF2_SEL_RC_MAP, 0);
+
+/**
+ * Manual instance selection
+ *
+ * Defines which estimator instance is used when manual switch is active.
+ * Use EKF2_SEL_RC_MAP to define which aux switch is used to trigger the switch.
+ * When manual selection is active and the switch is off, instance 0 is selected.
+ * Manual selection overrides automatic instance switch.
+ *
+ * @min 0
+ * @max 10
+ * @group EKF2
+ */
+PARAM_DEFINE_INT32(EKF2_SEL_RC_INST, 0);