From 4c1816a9834e68c84374a29dbb6d999a3973c44e Mon Sep 17 00:00:00 2001
From: Andy Piper <github@andypiper.com>
Date: Thu, 18 Jul 2019 21:19:08 +0200
Subject: [PATCH] Copter: port betaflight in-flight fft analysis to arducopter
 and expose as a log message

add harmonic notch tracking mode
call AP_GyroFFT::update_freq_hover() from update_throttle_hover()
move gyrofft configuration and control to AP_Vehicle
move fft logging to fft library
---
 ArduCopter/Attitude.cpp                | 3 +++
 ArduCopter/Copter.cpp                  | 4 ++++
 ArduCopter/Log.cpp                     | 6 ++----
 ArduCopter/RC_Channel.cpp              | 2 +-
 ArduCopter/system.cpp                  | 7 +++++++
 libraries/Filter/HarmonicNotchFilter.h | 1 +
 6 files changed, 18 insertions(+), 5 deletions(-)

diff --git a/ArduCopter/Attitude.cpp b/ArduCopter/Attitude.cpp
index 1ec6fd815e..e819a73ff9 100644
--- a/ArduCopter/Attitude.cpp
+++ b/ArduCopter/Attitude.cpp
@@ -64,6 +64,9 @@ void Copter::update_throttle_hover()
         labs(ahrs.roll_sensor) < 500 && labs(ahrs.pitch_sensor) < 500) {
         // Can we set the time constant automatically
         motors->update_throttle_hover(0.01f);
+#if HAL_GYROFFT_ENABLED
+        gyro_fft.update_freq_hover(0.01f, motors->get_throttle_out());
+#endif
     }
 #endif
 }
diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp
index 130bc1df87..b4bb7f1d55 100644
--- a/ArduCopter/Copter.cpp
+++ b/ArduCopter/Copter.cpp
@@ -161,6 +161,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
     SCHED_TASK_CLASS(AP_Logger,      &copter.logger,           periodic_tasks, 400, 300),
 #endif
     SCHED_TASK_CLASS(AP_InertialSensor,    &copter.ins,                 periodic,       400,  50),
+
     SCHED_TASK_CLASS(AP_Scheduler,         &copter.scheduler,           update_logging, 0.1,  75),
 #if RPM_ENABLED == ENABLED
     SCHED_TASK(rpm_update,            40,    200),
@@ -575,6 +576,9 @@ void Copter::update_altitude()
 
     if (should_log(MASK_LOG_CTUN)) {
         Log_Write_Control_Tuning();
+#if HAL_GYROFFT_ENABLED
+        gyro_fft.write_log_messages();
+#endif
     }
 }
 
diff --git a/ArduCopter/Log.cpp b/ArduCopter/Log.cpp
index fcec22fd1e..49ced3795d 100644
--- a/ArduCopter/Log.cpp
+++ b/ArduCopter/Log.cpp
@@ -20,7 +20,6 @@ struct PACKED log_Control_Tuning {
     float    terr_alt;
     int16_t  target_climb_rate;
     int16_t  climb_rate;
-    float    dynamic_notch_freq;
 };
 
 // Write a control tuning packet
@@ -60,8 +59,7 @@ void Copter::Log_Write_Control_Tuning()
         rangefinder_alt     : surface_tracking.get_dist_for_logging(),
         terr_alt            : terr_alt,
         target_climb_rate   : target_climb_rate_cms,
-        climb_rate          : int16_t(inertial_nav.get_velocity_z()), // float -> int16_t
-        dynamic_notch_freq  : ins.get_gyro_dynamic_notch_center_freq_hz()
+        climb_rate          : int16_t(inertial_nav.get_velocity_z()) // float -> int16_t
     };
     logger.WriteBlock(&pkt, sizeof(pkt));
 }
@@ -456,7 +454,7 @@ const struct LogStructure Copter::log_structure[] = {
     { LOG_PARAMTUNE_MSG, sizeof(log_ParameterTuning),
       "PTUN", "QBfff",         "TimeUS,Param,TunVal,TunMin,TunMax", "s----", "F----" },
     { LOG_CONTROL_TUNING_MSG, sizeof(log_Control_Tuning),
-      "CTUN", "Qffffffefffhhf", "TimeUS,ThI,ABst,ThO,ThH,DAlt,Alt,BAlt,DSAlt,SAlt,TAlt,DCRt,CRt,N", "s----mmmmmmnnz", "F----00B000BB-" },
+      "CTUN", "Qffffffefffhh", "TimeUS,ThI,ABst,ThO,ThH,DAlt,Alt,BAlt,DSAlt,SAlt,TAlt,DCRt,CRt", "s----mmmmmmnn", "F----00B000BB" },
     { LOG_MOTBATT_MSG, sizeof(log_MotBatt),
       "MOTB", "Qffff",  "TimeUS,LiftMax,BatVolt,BatRes,ThLimit", "s-vw-", "F-00-" },
     { LOG_DATA_INT16_MSG, sizeof(log_Data_Int16t),         
diff --git a/ArduCopter/RC_Channel.cpp b/ArduCopter/RC_Channel.cpp
index d157d85be8..46b5a6d3cb 100644
--- a/ArduCopter/RC_Channel.cpp
+++ b/ArduCopter/RC_Channel.cpp
@@ -556,7 +556,7 @@ void RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const aux_sw
                 break;
             }
             break;
-
+            
     default:
         RC_Channel::do_aux_function(ch_option, ch_flag);
         break;
diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp
index fdbf16f86e..86d484fe42 100644
--- a/ArduCopter/system.cpp
+++ b/ArduCopter/system.cpp
@@ -133,6 +133,7 @@ void Copter::init_ardupilot()
     attitude_control->parameter_sanity_check();
     pos_control->set_dt(scheduler.get_loop_period_s());
 
+
     // init the optical flow sensor
     init_optflow();
 
@@ -291,6 +292,12 @@ void Copter::update_dynamic_notch()
         case HarmonicNotchDynamicMode::UpdateBLHeli: // BLHeli based tracking
             ins.update_harmonic_notch_freq_hz(MAX(ref_freq, AP_BLHeli::get_singleton()->get_average_motor_frequency_hz() * ref));
             break;
+#endif
+#if HAL_GYROFFT_ENABLED
+        case HarmonicNotchDynamicMode::UpdateGyroFFT: // FFT based tracking
+            // set the harmonic notch filter frequency scaled on measured frequency
+            ins.update_harmonic_notch_freq_hz(gyro_fft.get_weighted_noise_center_freq_hz());
+            break;
 #endif
         case HarmonicNotchDynamicMode::Fixed: // static
         default:
diff --git a/libraries/Filter/HarmonicNotchFilter.h b/libraries/Filter/HarmonicNotchFilter.h
index 7138bb0a39..3a8735d803 100644
--- a/libraries/Filter/HarmonicNotchFilter.h
+++ b/libraries/Filter/HarmonicNotchFilter.h
@@ -64,6 +64,7 @@ enum class HarmonicNotchDynamicMode {
     UpdateThrottle  = 1,
     UpdateRPM       = 2,
     UpdateBLHeli    = 3,
+    UpdateGyroFFT   = 4,
 };
 
 /*