From 12a32cf6c9aaaf6e88ae11babdd2784f5074c058 Mon Sep 17 00:00:00 2001
From: Michel Pastor <shellixyz@gmail.com>
Date: Thu, 28 Dec 2023 17:24:54 +0100
Subject: [PATCH] Plane: multiplex parm set selection with tune selector
 channel

---
 ArduPlane/RC_Channel.cpp          | 16 ----------------
 libraries/AP_Tuning/AP_Tuning.cpp | 29 ++++++++++++++++++++++++++++-
 libraries/AP_Tuning/AP_Tuning.h   |  2 ++
 libraries/RC_Channel/RC_Channel.h |  2 --
 4 files changed, 30 insertions(+), 19 deletions(-)

diff --git a/ArduPlane/RC_Channel.cpp b/ArduPlane/RC_Channel.cpp
index ac3728df83..4fa6df1489 100644
--- a/ArduPlane/RC_Channel.cpp
+++ b/ArduPlane/RC_Channel.cpp
@@ -186,7 +186,6 @@ void RC_Channel_Plane::init_aux_function(const RC_Channel::aux_func_t ch_option,
     case AUX_FUNC::SERVOS_AUTO_TRIM:
     case AUX_FUNC::EMERGENCY_LANDING_EN:
     case AUX_FUNC::FW_AUTOTUNE:
-    case AUX_FUNC::TUNE_PARAM_SELECT:
         break;
 
     case AUX_FUNC::SOARING:
@@ -436,21 +435,6 @@ bool RC_Channel_Plane::do_aux_function(const aux_func_t ch_option, const AuxSwit
         do_aux_function_armdisarm(ch_flag);
         break;
 
-    case AUX_FUNC::TUNE_PARAM_SELECT: {
-        switch (ch_flag) {
-            case AuxSwitchPos::LOW:
-                plane.tuning.set_current_parmset(plane.tuning.get_parmset1());
-                break;
-            case AuxSwitchPos::MIDDLE:
-                plane.tuning.set_current_parmset(plane.tuning.get_parmset2());
-                break;
-            case AuxSwitchPos::HIGH:
-                plane.tuning.set_current_parmset(plane.tuning.get_parmset3());
-                break;
-        }
-        break;
-    }
-
     default:
         return RC_Channel::do_aux_function(ch_option, ch_flag);
     }
diff --git a/libraries/AP_Tuning/AP_Tuning.cpp b/libraries/AP_Tuning/AP_Tuning.cpp
index 87d46a4494..8f1698eea9 100644
--- a/libraries/AP_Tuning/AP_Tuning.cpp
+++ b/libraries/AP_Tuning/AP_Tuning.cpp
@@ -77,6 +77,33 @@ void AP_Tuning::check_selector_switch(void)
         return;
     }
     uint16_t selector_in = selchan->get_radio_in();
+
+    int8_t parmset_switch_pos = -1;
+    if (selector_in < 1200) {
+        parmset_switch_pos = 0;
+    } else if (selector_in < 1400) {
+        parmset_switch_pos = 1;
+    } else if (selector_in < 1600) {
+        parmset_switch_pos = 2;
+    }
+
+    if (parmset_switch_pos > -1 && parmset_switch_pos != prev_parmset_switch_pos) {
+        int16_t new_parmset;
+        switch (parmset_switch_pos) {
+            case 0:
+                new_parmset = get_parmset1();
+                break;
+            case 1:
+                new_parmset = get_parmset2();
+                break;
+            case 2:
+                new_parmset = get_parmset3();
+                break;
+        }
+        set_current_parmset(new_parmset);
+        prev_parmset_switch_pos = parmset_switch_pos;
+    }
+
     if (selector_in >= RC_Channel::AUX_PWM_TRIGGER_HIGH) {
         // high selector
         if (selector_start_ms == 0) {
@@ -92,7 +119,7 @@ void AP_Tuning::check_selector_switch(void)
             changed = false;
             need_revert = 0;
         }
-    } else if (selector_in <= RC_Channel::AUX_PWM_TRIGGER_LOW) {
+    } else if (selector_in < RC_Channel::AUX_PWM_TRIGGER_HIGH) {
         // low selector
         if (selector_start_ms != 0) {
             uint32_t hold_time = AP_HAL::millis() - selector_start_ms;
diff --git a/libraries/AP_Tuning/AP_Tuning.h b/libraries/AP_Tuning/AP_Tuning.h
index f475eb4ed0..12765eacbd 100644
--- a/libraries/AP_Tuning/AP_Tuning.h
+++ b/libraries/AP_Tuning/AP_Tuning.h
@@ -75,6 +75,8 @@ class AP_Tuning
     // the parameter we are tuning
     uint8_t current_parm;
 
+    int8_t prev_parmset_switch_pos = -1;
+
     // current index into the parameter set
     uint8_t current_parm_index;
 
diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h
index c375d9a2b3..44c142578f 100644
--- a/libraries/RC_Channel/RC_Channel.h
+++ b/libraries/RC_Channel/RC_Channel.h
@@ -260,8 +260,6 @@ class RC_Channel {
         MOUNT2_PITCH =       216, // mount3 pitch input
         MOUNT2_YAW =         217, // mount4 yaw input
 
-        TUNE_PARAM_SELECT =  250,
-
         // inputs 248-249 are reserved for the Skybrush fork at
         // https://github.com/skybrush-io/ardupilot