From bfc3a5a74971ae13674ab9e007b01cbe291f410d Mon Sep 17 00:00:00 2001
From: Andy Piper <github@andypiper.com>
Date: Sun, 6 Mar 2022 13:51:31 +0100
Subject: [PATCH] RC_Channel: add in-flight FFT notch tuning function

initialize FFT tune
---
 libraries/RC_Channel/RC_Channel.cpp | 26 ++++++++++++++++++++++++++
 libraries/RC_Channel/RC_Channel.h   |  4 +++-
 2 files changed, 29 insertions(+), 1 deletion(-)

diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp
index 31738a25d7..ba740519a7 100644
--- a/libraries/RC_Channel/RC_Channel.cpp
+++ b/libraries/RC_Channel/RC_Channel.cpp
@@ -519,6 +519,7 @@ void RC_Channel::init_aux_function(const aux_func_t ch_option, const AuxSwitchPo
     case AUX_FUNC::RUNCAM_OSD_CONTROL:
     case AUX_FUNC::SPRAYER:
     case AUX_FUNC::DISABLE_AIRSPEED_USE:
+    case AUX_FUNC::FFT_NOTCH_TUNE:
 #if HAL_MOUNT_ENABLED
     case AUX_FUNC::RETRACT_MOUNT:
 #endif
@@ -579,6 +580,7 @@ const RC_Channel::LookupTable RC_Channel::lookuptable[] = {
     { AUX_FUNC::EMERGENCY_LANDING_EN, "Emergency Landing"},
     { AUX_FUNC::WEATHER_VANE_ENABLE, "Weathervane"},
     { AUX_FUNC::TURBINE_START, "Turbine Start"},
+    { AUX_FUNC::FFT_NOTCH_TUNE, "FFT Notch Tuning"},
 };
 
 /* lookup the announcement for switch change */
@@ -900,6 +902,26 @@ void RC_Channel::do_aux_function_mission_reset(const AuxSwitchPos ch_flag)
     mission->reset();
 }
 
+void RC_Channel::do_aux_function_fft_notch_tune(const AuxSwitchPos ch_flag)
+{
+#if HAL_GYROFFT_ENABLED
+    AP_GyroFFT *fft = AP::fft();
+    if (fft == nullptr) {
+        return;
+    }
+
+    switch (ch_flag) {
+        case AuxSwitchPos::HIGH:
+            fft->start_notch_tune();
+            break;
+        case AuxSwitchPos::MIDDLE:
+        case AuxSwitchPos::LOW:
+            fft->stop_notch_tune();
+            break;
+    }
+#endif
+}
+
 bool RC_Channel::run_aux_function(aux_func_t ch_option, AuxSwitchPos pos, AuxFuncTriggerSource source)
 {
     const bool ret = do_aux_function(ch_option, pos);
@@ -988,6 +1010,10 @@ bool RC_Channel::do_aux_function(const aux_func_t ch_option, const AuxSwitchPos
         do_aux_function_avoid_adsb(ch_flag);
         break;
 
+    case AUX_FUNC::FFT_NOTCH_TUNE:
+        do_aux_function_fft_notch_tune(ch_flag);
+        break;
+
 #if HAL_GENERATOR_ENABLED
     case AUX_FUNC::GENERATOR:
         do_aux_function_generator(ch_flag);
diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h
index 5da864f7ec..cc46031126 100644
--- a/libraries/RC_Channel/RC_Channel.h
+++ b/libraries/RC_Channel/RC_Channel.h
@@ -224,7 +224,8 @@ public:
         OPTFLOW_CAL =        158, // optical flow calibration
         FORCEFLYING =        159, // enable or disable land detection for GPS based manual modes preventing land detection and maintainting set_throttle_mix_max
         WEATHER_VANE_ENABLE = 160, // enable/disable weathervaning
-        TURBINE_START =       161, // initialize turbine start sequence
+        TURBINE_START =      161, // initialize turbine start sequence
+        FFT_NOTCH_TUNE =     162, // FFT notch tuning function
 
         // inputs from 200 will eventually used to replace RCMAP
         ROLL =               201, // roll input
@@ -315,6 +316,7 @@ protected:
     void do_aux_function_relay(uint8_t relay, bool val);
     void do_aux_function_sprayer(const AuxSwitchPos ch_flag);
     void do_aux_function_generator(const AuxSwitchPos ch_flag);
+    void do_aux_function_fft_notch_tune(const AuxSwitchPos ch_flag);
 
     typedef int8_t modeswitch_pos_t;
     virtual void mode_switch_changed(modeswitch_pos_t new_pos) {