From ea3af39773b74b2d7e0cdc6d4fbe59b36504b06f Mon Sep 17 00:00:00 2001
From: Andy Piper <github@andypiper.com>
Date: Sun, 7 Feb 2021 13:20:55 +0000
Subject: [PATCH] RC_Channel: add support for 6-position switch and use it for
 VTX power

---
 libraries/RC_Channel/RC_Channel.cpp | 34 ++++++++++++++++++++++-------
 libraries/RC_Channel/RC_Channel.h   |  2 ++
 2 files changed, 28 insertions(+), 8 deletions(-)

diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp
index 3b180589d7..9b4864e8d7 100644
--- a/libraries/RC_Channel/RC_Channel.cpp
+++ b/libraries/RC_Channel/RC_Channel.cpp
@@ -44,6 +44,7 @@ extern const AP_HAL::HAL& hal;
 #include <AP_VisualOdom/AP_VisualOdom.h>
 #include <AP_AHRS/AP_AHRS.h>
 #include <AP_Mount/AP_Mount.h>
+#include <AP_RCTelemetry/AP_VideoTX.h>
 
 #define SWITCH_DEBOUNCE_TIME_MS  200
 
@@ -408,15 +409,15 @@ void RC_Channel::reset_mode_switch()
     read_mode_switch();
 }
 
-void RC_Channel::read_mode_switch()
+// read a 6 position switch
+bool RC_Channel::read_6pos_switch(int8_t& position)
 {
-    // calculate position of flight mode switch
+    // calculate position of 6 pos switch
     const uint16_t pulsewidth = get_radio_in();
     if (pulsewidth <= 900 || pulsewidth >= 2200) {
-        return;  // This is an error condition
+        return false;  // This is an error condition
     }
 
-    modeswitch_pos_t position;
     if      (pulsewidth < 1231) position = 0;
     else if (pulsewidth < 1361) position = 1;
     else if (pulsewidth < 1491) position = 2;
@@ -424,12 +425,20 @@ void RC_Channel::read_mode_switch()
     else if (pulsewidth < 1750) position = 4;
     else position = 5;
 
-    if (!debounce_completed((int8_t)position)) {
-        return;
+    if (!debounce_completed(position)) {
+        return false;
     }
 
-    // set flight mode and simple mode setting
-    mode_switch_changed(position);
+    return true;
+}
+
+void RC_Channel::read_mode_switch()
+{
+    int8_t position;
+    if (read_6pos_switch(position)) {
+        // set flight mode and simple mode setting
+        mode_switch_changed(modeswitch_pos_t(position));
+    }
 }
 
 bool RC_Channel::debounce_completed(int8_t position) 
@@ -493,6 +502,7 @@ void RC_Channel::init_aux_function(const aux_func_t ch_option, const AuxSwitchPo
     case AUX_FUNC::SCRIPTING_6:
     case AUX_FUNC::SCRIPTING_7:
     case AUX_FUNC::SCRIPTING_8:
+    case AUX_FUNC::VTX_POWER:
         break;
     case AUX_FUNC::AVOID_ADSB:
     case AUX_FUNC::AVOID_PROXIMITY:
@@ -589,7 +599,15 @@ bool RC_Channel::read_aux()
         // may wish to add special cases for other "AUXSW" things
         // here e.g. RCMAP_ROLL etc once they become options
         return false;
+    } else if (_option == AUX_FUNC::VTX_POWER) {
+        int8_t position;
+        if (read_6pos_switch(position)) {
+            AP::vtx().change_power(position);
+            return true;
+        }
+        return false;
     }
+
     AuxSwitchPos new_position;
     if (!read_3pos_switch(new_position)) {
         return false;
diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h
index 1e32d91620..e3ef06442b 100644
--- a/libraries/RC_Channel/RC_Channel.h
+++ b/libraries/RC_Channel/RC_Channel.h
@@ -192,6 +192,7 @@ public:
         ARSPD_CALIBRATE=      91, // calibrate airspeed ratio 
         FBWA =                92, // Fly-By-Wire-A
         RELOCATE_MISSION =    93, // used in separate branch MISSION_RELATIVE
+        VTX_POWER =           94, // VTX power level
 
         // entries from 100 onwards are expected to be developer
         // options used for testing
@@ -233,6 +234,7 @@ public:
     };
 
     bool read_3pos_switch(AuxSwitchPos &ret) const WARN_IF_UNUSED;
+    bool read_6pos_switch(int8_t& position) WARN_IF_UNUSED;
     AuxSwitchPos get_aux_switch_pos() const;
 
     virtual void do_aux_function(aux_func_t ch_option, AuxSwitchPos);