From 9ea9c15c6a03fc0c253be1b1fadfbdaed64fc003 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 7 Jun 2021 16:13:14 +1000 Subject: [PATCH] AP_GyroFFT: pass GyroFFT loop rate in Hz rather than period in us It just calculates this anyway Also stops assigning a uint16_t value into a uint32_t value --- libraries/AP_GyroFFT/AP_GyroFFT.cpp | 3 +-- libraries/AP_GyroFFT/AP_GyroFFT.h | 2 +- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/libraries/AP_GyroFFT/AP_GyroFFT.cpp b/libraries/AP_GyroFFT/AP_GyroFFT.cpp index 7290069770..a5714bdee5 100644 --- a/libraries/AP_GyroFFT/AP_GyroFFT.cpp +++ b/libraries/AP_GyroFFT/AP_GyroFFT.cpp @@ -180,7 +180,7 @@ AP_GyroFFT::AP_GyroFFT() } // initialize the FFT parameters and engine -void AP_GyroFFT::init(uint32_t target_looptime_us) +void AP_GyroFFT::init(uint16_t loop_rate_hz) { // if FFT analysis is not enabled we don't want to allocate any of the associated resources if (!_enable) { @@ -226,7 +226,6 @@ void AP_GyroFFT::init(uint32_t target_looptime_us) if (_sample_mode == 0) { _fft_sampling_rate_hz = _ins->get_raw_gyro_rate_hz(); } else { - const uint16_t loop_rate_hz = 1000*1000UL / target_looptime_us; _fft_sampling_rate_hz = loop_rate_hz / _sample_mode; for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) { if (!_downsampled_gyro_data[axis].set_size(_window_size + _samples_per_frame)) { diff --git a/libraries/AP_GyroFFT/AP_GyroFFT.h b/libraries/AP_GyroFFT/AP_GyroFFT.h index 52fd07e2c7..c0d98322c6 100644 --- a/libraries/AP_GyroFFT/AP_GyroFFT.h +++ b/libraries/AP_GyroFFT/AP_GyroFFT.h @@ -47,7 +47,7 @@ public: AP_GyroFFT(const AP_GyroFFT &other) = delete; AP_GyroFFT &operator=(const AP_GyroFFT&) = delete; - void init(uint32_t target_looptime); + void init(uint16_t loop_rate_hz); // cycle through the FFT steps - runs in the FFT thread uint16_t run_cycle();