From 6f1d0a4e727d442258f8e3e01c8ffc49e31ad5a8 Mon Sep 17 00:00:00 2001
From: Andy Piper <github@andypiper.com>
Date: Wed, 8 Jun 2022 20:30:33 +0100
Subject: [PATCH] AP_GyroFFT: make sure the parameters are updated at least
 once on startup

---
 libraries/AP_GyroFFT/AP_GyroFFT.cpp | 6 +++---
 libraries/AP_GyroFFT/AP_GyroFFT.h   | 3 ++-
 2 files changed, 5 insertions(+), 4 deletions(-)

diff --git a/libraries/AP_GyroFFT/AP_GyroFFT.cpp b/libraries/AP_GyroFFT/AP_GyroFFT.cpp
index cc07c8859a..bf34353e2e 100644
--- a/libraries/AP_GyroFFT/AP_GyroFFT.cpp
+++ b/libraries/AP_GyroFFT/AP_GyroFFT.cpp
@@ -327,7 +327,7 @@ void AP_GyroFFT::init(uint16_t loop_rate_hz)
 
     // finally we are done
     _initialized = true;
-    update_parameters();
+    update_parameters(true);
     // start running FFTs
     if (start_update_thread()) {
         set_analysis_enabled(true);
@@ -477,10 +477,10 @@ bool AP_GyroFFT::start_analysis() {
 }
 
 // update calculated values of dynamic parameters - runs at 1Hz
-void AP_GyroFFT::update_parameters()
+void AP_GyroFFT::update_parameters(bool force)
 {
     // lock contention is very costly, so don't allow configuration updates while flying
-    if (!_initialized || AP::arming().is_armed()) {
+    if ((!_initialized || AP::arming().is_armed()) && !force) {
         return;
     }
 
diff --git a/libraries/AP_GyroFFT/AP_GyroFFT.h b/libraries/AP_GyroFFT/AP_GyroFFT.h
index c0d98322c6..637ffb0479 100644
--- a/libraries/AP_GyroFFT/AP_GyroFFT.h
+++ b/libraries/AP_GyroFFT/AP_GyroFFT.h
@@ -56,7 +56,7 @@ public:
     // update the engine state - runs at 400Hz
     void update();
     // update calculated values of dynamic parameters - runs at 1Hz
-    void update_parameters();
+    void update_parameters() { update_parameters(false); }
     // thread for processing gyro data via FFT
     void update_thread();
     // start the update thread
@@ -206,6 +206,7 @@ private:
     uint16_t get_available_samples(uint8_t axis) {
         return _sample_mode == 0 ?_ins->get_raw_gyro_window(axis).available() : _downsampled_gyro_data[axis].available();
     }
+    void update_parameters(bool force);
     // semaphore for access to shared FFT data
     HAL_Semaphore _sem;