diff --git a/libraries/AP_GyroFFT/AP_GyroFFT.cpp b/libraries/AP_GyroFFT/AP_GyroFFT.cpp index 815fac0df9..c1469fcb38 100644 --- a/libraries/AP_GyroFFT/AP_GyroFFT.cpp +++ b/libraries/AP_GyroFFT/AP_GyroFFT.cpp @@ -336,7 +336,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); @@ -486,10 +486,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 30a26eaae8..3767c0cbcb 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 @@ -211,6 +211,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;