From 6613d4da3d192ec712a62ccdcedeee85c62ef76e Mon Sep 17 00:00:00 2001
From: Andy Piper <github@andypiper.com>
Date: Mon, 30 Dec 2019 18:15:48 +0000
Subject: [PATCH] AP_Vehicle: add FFT configuration and initialization

add arming checks to validate FFT performance
allow gyros to be sampled at either the fastloop rate or gyro rate.
add gyro and parameter update loops for GyroFFT
add GYRO_FFT aux function
save FFT results on disarm
---
 libraries/AP_Vehicle/AP_Vehicle.cpp | 15 ++++++++++++++-
 libraries/AP_Vehicle/AP_Vehicle.h   |  4 ++++
 2 files changed, 18 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_Vehicle/AP_Vehicle.cpp b/libraries/AP_Vehicle/AP_Vehicle.cpp
index 8bbbd33b82..8ae78f4217 100644
--- a/libraries/AP_Vehicle/AP_Vehicle.cpp
+++ b/libraries/AP_Vehicle/AP_Vehicle.cpp
@@ -14,6 +14,11 @@ const AP_Param::GroupInfo AP_Vehicle::var_info[] = {
     AP_SUBGROUPINFO(runcam, "CAM_RC_", 1, AP_Vehicle, AP_RunCam),
 #endif
 
+#if HAL_GYROFFT_ENABLED
+    // @Group: FFT_
+    // @Path: ../AP_GyroFFT/AP_GyroFFT.cpp
+    AP_SUBGROUPINFO(gyro_fft, "FFT_",  2, AP_Vehicle, AP_GyroFFT),
+#endif
     AP_GROUPEND
 };
 
@@ -55,6 +60,10 @@ void AP_Vehicle::setup()
 
     // init_ardupilot is where the vehicle does most of its initialisation.
     init_ardupilot();
+    // gyro FFT needs to be initialized really late
+#if HAL_GYROFFT_ENABLED
+    gyro_fft.init(AP::scheduler().get_loop_period_us());
+#endif
 }
 
 void AP_Vehicle::loop()
@@ -70,7 +79,11 @@ void AP_Vehicle::loop()
  */
 const AP_Scheduler::Task AP_Vehicle::scheduler_tasks[] = {
 #if HAL_RUNCAM_ENABLED
-    SCHED_TASK_CLASS(AP_RunCam,    &vehicle.runcam,              update,          50,  50),
+    SCHED_TASK_CLASS(AP_RunCam,    &vehicle.runcam,         update,                   50, 50),
+#endif
+#if HAL_GYROFFT_ENABLED
+    SCHED_TASK_CLASS(AP_GyroFFT,   &vehicle.gyro_fft,       sample_gyros,      LOOP_RATE, 50),
+    SCHED_TASK_CLASS(AP_GyroFFT,   &vehicle.gyro_fft,       update_parameters,         1, 50),
 #endif
 };
 
diff --git a/libraries/AP_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h
index eab2e0962a..550085bf81 100644
--- a/libraries/AP_Vehicle/AP_Vehicle.h
+++ b/libraries/AP_Vehicle/AP_Vehicle.h
@@ -38,6 +38,7 @@
 #include <AP_Camera/AP_RunCam.h>
 #include <AP_Hott_Telem/AP_Hott_Telem.h>
 #include <AP_ESC_Telem/AP_ESC_Telem.h>
+#include <AP_GyroFFT/AP_GyroFFT.h>
 
 class AP_Vehicle : public AP_HAL::HAL::Callbacks {
 
@@ -196,6 +197,9 @@ protected:
     AP_RSSI rssi;
 #if HAL_RUNCAM_ENABLED
     AP_RunCam runcam;
+#endif
+#if HAL_GYROFFT_ENABLED
+    AP_GyroFFT gyro_fft;
 #endif
     AP_SerialManager serial_manager;