Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 :
15 : Code by Andy Piper with help from betaflight
16 : */
17 :
18 : #include "AP_GyroFFT.h"
19 :
20 : #if HAL_GYROFFT_ENABLED
21 :
22 : #include <GCS_MAVLink/GCS.h>
23 : #include <AP_Logger/AP_Logger.h>
24 : #include <Filter/HarmonicNotchFilter.h>
25 : #include <AP_BoardConfig/AP_BoardConfig.h>
26 : #include <AP_Arming/AP_Arming.h>
27 : #include <AP_Vehicle/AP_Vehicle.h>
28 : #if APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_ArduPlane)
29 : #include <AP_Motors/AP_Motors.h>
30 : #endif
31 : #include <stdio.h>
32 :
33 : extern const AP_HAL::HAL& hal;
34 :
35 : #ifndef FFT_DEFAULT_WINDOW_SIZE
36 : // the H7 can cope with a longer length and these boards generally have BMI088 which needs a longer length
37 : #if defined(STM32H7)
38 : #define FFT_DEFAULT_WINDOW_SIZE 64
39 : #else
40 : #define FFT_DEFAULT_WINDOW_SIZE 32
41 : #endif
42 : #endif
43 : #ifndef FFT_DEFAULT_WINDOW_OVERLAP
44 : #if defined(STM32H7)
45 : #define FFT_DEFAULT_WINDOW_OVERLAP 0.75f
46 : #else
47 : #define FFT_DEFAULT_WINDOW_OVERLAP 0.5f
48 : #endif
49 : #endif
50 : #define FFT_THR_REF_DEFAULT 0.35f // the estimated throttle reference, 0 ~ 1
51 : #define FFT_SNR_DEFAULT 25.0f // a higher SNR is safer and this works quite well on a Pixracer
52 : #define FFT_SNR_PFILT_DEFAULT 10.0f // post-filter there is much less noise so default should be lower
53 : #define FFT_STACK_SIZE 1024
54 : #define FFT_MIN_SAMPLES_PER_FRAME 16
55 : #define FFT_HARMONIC_FIT_DEFAULT 10
56 : #define FFT_HARMONIC_FIT_FILTER_HZ 15.0f
57 : #define FFT_HARMONIC_FIT_MULT 50.0f
58 : #define FFT_HARMONIC_FIT_TRACK_ROLL 4
59 : #define FFT_HARMONIC_FIT_TRACK_PITCH 5
60 :
61 : // table of user settable parameters
62 : const AP_Param::GroupInfo AP_GyroFFT::var_info[] = {
63 :
64 : // @Param: ENABLE
65 : // @DisplayName: Enable
66 : // @Description: Enable Gyro FFT analyser
67 : // @Values: 0:Disabled,1:Enabled
68 : // @User: Advanced
69 : // @RebootRequired: True
70 : AP_GROUPINFO_FLAGS("ENABLE", 1, AP_GyroFFT, _enable, 0, AP_PARAM_FLAG_ENABLE),
71 :
72 : // @Param: MINHZ
73 : // @DisplayName: Minimum Frequency
74 : // @Description: Lower bound of FFT frequency detection in Hz. On larger vehicles the minimum motor frequency is likely to be significantly lower than for smaller vehicles.
75 : // @Range: 20 400
76 : // @Units: Hz
77 : // @User: Advanced
78 : AP_GROUPINFO("MINHZ", 2, AP_GyroFFT, _fft_min_hz, 50),
79 :
80 : // @Param: MAXHZ
81 : // @DisplayName: Maximum Frequency
82 : // @Description: Upper bound of FFT frequency detection in Hz. On smaller vehicles the maximum motor frequency is likely to be significantly higher than for larger vehicles.
83 : // @Range: 20 495
84 : // @Units: Hz
85 : // @User: Advanced
86 : AP_GROUPINFO("MAXHZ", 3, AP_GyroFFT, _fft_max_hz, 450),
87 :
88 : // @Param: SAMPLE_MODE
89 : // @DisplayName: Sample Mode
90 : // @Description: Sampling mode (and therefore rate). 0: Gyro rate sampling, 1: Fast loop rate sampling, 2: Fast loop rate / 2 sampling, 3: Fast loop rate / 3 sampling. Takes effect on reboot.
91 : // @Range: 0 4
92 : // @User: Advanced
93 : // @RebootRequired: True
94 : AP_GROUPINFO("SAMPLE_MODE", 4, AP_GyroFFT, _sample_mode, 0),
95 :
96 : // @Param: WINDOW_SIZE
97 : // @DisplayName: FFT window size
98 : // @Description: Size of window to be used in FFT calculations. Takes effect on reboot. Must be a power of 2 and between 32 and 512. Larger windows give greater frequency resolution but poorer time resolution, consume more CPU time and may not be appropriate for all vehicles. Time and frequency resolution are given by the sample-rate / window-size. Windows of 256 are only really recommended for F7 class boards, windows of 512 or more H7 class.
99 : // @Range: 32 1024
100 : // @User: Advanced
101 : // @RebootRequired: True
102 : AP_GROUPINFO("WINDOW_SIZE", 5, AP_GyroFFT, _window_size, FFT_DEFAULT_WINDOW_SIZE),
103 :
104 : // @Param: WINDOW_OLAP
105 : // @DisplayName: FFT window overlap
106 : // @Description: Percentage of window to be overlapped before another frame is process. Takes effect on reboot. A good default is 50% overlap. Higher overlap results in more processed frames but not necessarily more temporal resolution. Lower overlap results in lost information at the frame edges.
107 : // @Range: 0 0.9
108 : // @User: Advanced
109 : // @RebootRequired: True
110 : AP_GROUPINFO("WINDOW_OLAP", 6, AP_GyroFFT, _window_overlap, FFT_DEFAULT_WINDOW_OVERLAP),
111 :
112 : // @Param: FREQ_HOVER
113 : // @DisplayName: FFT learned hover frequency
114 : // @Description: The learned hover noise frequency
115 : // @Range: 0 250
116 : // @User: Advanced
117 : AP_GROUPINFO("FREQ_HOVER", 7, AP_GyroFFT, _freq_hover_hz, 80.0f),
118 :
119 : // @Param: THR_REF
120 : // @DisplayName: FFT learned thrust reference
121 : // @Description: FFT learned thrust reference for the hover frequency and FFT minimum frequency.
122 : // @Range: 0.01 0.9
123 : // @User: Advanced
124 : AP_GROUPINFO("THR_REF", 8, AP_GyroFFT, _throttle_ref, FFT_THR_REF_DEFAULT),
125 :
126 : // @Param: SNR_REF
127 : // @DisplayName: FFT SNR reference threshold
128 : // @Description: FFT SNR reference threshold in dB at which a signal is determined to be present.
129 : // @Range: 0.0 100.0
130 : // @User: Advanced
131 : AP_GROUPINFO("SNR_REF", 9, AP_GyroFFT, _snr_threshold_db, FFT_SNR_DEFAULT),
132 :
133 : // @Param: ATT_REF
134 : // @DisplayName: FFT attenuation for bandwidth calculation
135 : // @Description: FFT attenuation level in dB for bandwidth calculation and peak detection. The bandwidth is calculated by comparing peak power output with the attenuated version. The default of 15 has shown to be a good compromise in both simulations and real flight.
136 : // @Range: 0 100
137 : // @User: Advanced
138 : AP_GROUPINFO("ATT_REF", 10, AP_GyroFFT, _attenuation_power_db, 15),
139 :
140 : // @Param: BW_HOVER
141 : // @DisplayName: FFT learned bandwidth at hover
142 : // @Description: FFT learned bandwidth at hover for the attenuation frequencies.
143 : // @Range: 0 200
144 : // @User: Advanced
145 : AP_GROUPINFO("BW_HOVER", 11, AP_GyroFFT, _bandwidth_hover_hz, 20),
146 :
147 : // @Param: HMNC_FIT
148 : // @DisplayName: FFT harmonic fit frequency threshold
149 : // @Description: FFT harmonic fit frequency threshold percentage at which a signal of the appropriate frequency is determined to be the harmonic of another. Signals that have a harmonic relationship that varies at most by this percentage are considered harmonics of each other for the purpose of selecting the harmonic notch frequency. If a match is found then the lower frequency harmonic is always used as the basis for the dynamic harmonic notch. A value of zero completely disables harmonic matching.
150 : // @Range: 0 100
151 : // @User: Advanced
152 : // @RebootRequired: True
153 : AP_GROUPINFO("HMNC_FIT", 12, AP_GyroFFT, _harmonic_fit, FFT_HARMONIC_FIT_DEFAULT),
154 :
155 : // @Param: HMNC_PEAK
156 : // @DisplayName: FFT harmonic peak target
157 : // @Description: The FFT harmonic peak target that should be returned by FTN1.PkAvg. The resulting value will be used by the harmonic notch if configured to track the FFT frequency. By default the appropriate peak is auto-detected based on the harmonic fit between peaks and the energy-weighted average frequency on roll on pitch is used. Setting this to 1 will always target the highest energy peak. Setting this to 2 will target the highest energy peak that is lower in frequency than the highest energy peak. Setting this to 3 will target the highest energy peak that is higher in frequency than the highest energy peak. Setting this to 4 will target the highest energy peak on the roll axis only and only the roll frequency will be used (some vehicles have a much more pronounced peak on roll). Setting this to 5 will target the highest energy peak on the pitch axis only and only the pitch frequency will be used (some vehicles have a much more pronounced peak on roll).
158 : // @Values: 0:Auto,1:Center Frequency,2:Lower-Shoulder Frequency,3:Upper-Shoulder Frequency,4:Roll-Axis,5:Pitch-Axis
159 : // @User: Advanced
160 : AP_GROUPINFO("HMNC_PEAK", 13, AP_GyroFFT, _harmonic_peak, 0),
161 :
162 : // @Param: NUM_FRAMES
163 : // @DisplayName: FFT output frames to retain and average
164 : // @Description: Number of output frequency frames to retain and average in order to calculate final frequencies. Averaging output frames can drastically reduce noise and jitter at the cost of latency as long as the input is stable. The default is to perform no averaging. For rapidly changing frequencies (e.g. smaller aircraft) fewer frames should be averaged.
165 : // @Range: 0 8
166 : // @User: Advanced
167 : // @RebootRequired: True
168 : AP_GROUPINFO("NUM_FRAMES", 14, AP_GyroFFT, _num_frames, 0),
169 :
170 : // @Param: OPTIONS
171 : // @DisplayName: FFT options
172 : // @Description: FFT configuration options. Values: 1:Apply the FFT *after* the filter bank,2:Check noise at the motor frequencies using ESC data as a reference
173 : // @Bitmask: 0:Enable post-filter FFT,1:Check motor noise
174 : // @User: Advanced
175 : // @RebootRequired: True
176 : AP_GROUPINFO("OPTIONS", 15, AP_GyroFFT, _options, 0),
177 :
178 : AP_GROUPEND
179 : };
180 :
181 : // The FFT splits the frequency domain into an number of bins
182 : // A sampling frequency of 1000 and max frequency (Nyquist) of 500 at a window size of 32 gives 16 frequency bins each 31.25Hz wide
183 : // The first bin is used to store the DC and Nyquist values combined.
184 : // Eg [DC/Nyquist], [16,47), [47,78), [78,109) etc
185 : // For a loop rate of 800Hz, 16 bins each 25Hz wide
186 : // Eg X[0]=[DC/Nyquist], X[1]=[12,37), X[2]=[37,62), X[3]=[62,87), X[4]=[87,112)
187 : // So middle frequency is X[n] * 25 and the range is X[n] * 25 - 12 < f < X[n] * 25 + 12
188 :
189 : const extern AP_HAL::HAL& hal;
190 :
191 33536 : AP_GyroFFT::AP_GyroFFT()
192 : {
193 2096 : _thread_state._noise_needs_calibration = 0x07; // all axes need calibration
194 2096 : AP_Param::setup_object_defaults(this, var_info);
195 :
196 2096 : if (_singleton != nullptr) {
197 0 : AP_HAL::panic("AP_GyroFFT must be singleton");
198 : }
199 2096 : _singleton = this;
200 2096 : }
201 :
202 : // initialize the FFT parameters and engine
203 2086 : __INITFUNC__ void AP_GyroFFT::init(uint16_t loop_rate_hz)
204 : {
205 : // if FFT analysis is not enabled we don't want to allocate any of the associated resources
206 2086 : if (!_enable) {
207 2066 : return;
208 : }
209 :
210 20 : _ins = &AP::ins();
211 :
212 : // sanity check
213 20 : if (_ins->get_raw_gyro_rate_hz() == 0) {
214 0 : AP_HAL::panic("AP_GyroFFT must be initialized after AP_InertialSensor");
215 : }
216 :
217 : // check that we support the window size requested and it is a power of 2
218 20 : _window_size.set(1 << lrintf(log2f(_window_size.get())));
219 : #if defined(STM32H7) || CONFIG_HAL_BOARD == HAL_BOARD_LINUX || CONFIG_HAL_BOARD == HAL_BOARD_SITL
220 20 : _window_size.set(constrain_int16(_window_size, 32, 512));
221 : #else
222 : _window_size.set(constrain_int16(_window_size, 32, 256));
223 : #endif
224 : // number of samples needed before a new frame can be processed
225 20 : _window_overlap.set(constrain_float(_window_overlap, 0.0f, 0.9f));
226 20 : _samples_per_frame = (1.0f - _window_overlap) * _window_size;
227 : // if we allow too small a number of samples per frame the output rate gets very high
228 : // this is particularly a problem on IMUs with higher sample rates (e.g. BMI088)
229 : // 16 gives a maximum output rate of 2Khz / 16 = 125Hz per axis or 375Hz in aggregate
230 20 : _samples_per_frame = MAX(FFT_MIN_SAMPLES_PER_FRAME, 1 << lrintf(log2f(_samples_per_frame)));
231 20 : if (_num_frames > 0) {
232 4 : _num_frames.set(constrain_int16(_num_frames, 2, AP_HAL::DSP::MAX_SLIDING_WINDOW_SIZE));
233 : }
234 :
235 : // check that we have enough memory for the window size requested
236 : // INS: XYZ_AXIS_COUNT * INS_MAX_INSTANCES * _window_size, DSP: 3 * _window_size, FFT: XYZ_AXIS_COUNT + 3 * _window_size
237 20 : const uint32_t allocation_count = (XYZ_AXIS_COUNT * INS_MAX_INSTANCES + 3 + XYZ_AXIS_COUNT + 3 + _num_frames) * sizeof(float);
238 20 : if (allocation_count * FFT_DEFAULT_WINDOW_SIZE > hal.util->available_memory() / 2) {
239 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "AP_GyroFFT: disabled, required %u bytes", (unsigned int)allocation_count * FFT_DEFAULT_WINDOW_SIZE);
240 0 : return;
241 20 : } else if (allocation_count * _window_size > hal.util->available_memory() / 2) {
242 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "AP_GyroFFT: req alloc %u bytes (free=%u)", (unsigned int)allocation_count * _window_size, (unsigned int)hal.util->available_memory());
243 0 : _window_size.set(FFT_DEFAULT_WINDOW_SIZE);
244 : }
245 : // save any changes that were made
246 20 : _window_size.save();
247 :
248 : // determine the FFT sample rate based on the gyro rate, loop rate and configuration
249 20 : if (_sample_mode == 0) {
250 19 : _fft_sampling_rate_hz = _ins->get_raw_gyro_rate_hz();
251 : } else {
252 1 : _fft_sampling_rate_hz = loop_rate_hz / _sample_mode;
253 4 : for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
254 3 : if (!_downsampled_gyro_data[axis].set_size(_window_size + _samples_per_frame)) {
255 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Failed to allocate window for AP_GyroFFT");
256 0 : return;
257 : }
258 : }
259 : }
260 20 : _current_sample_mode = _sample_mode & 0x07; // mask matches previous 3 bit storage and param range
261 :
262 1492 : _ref_energy = NEW_NOTHROW Vector3f[_window_size];
263 20 : if (_ref_energy == nullptr) {
264 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Failed to allocate window for AP_GyroFFT");
265 0 : return;
266 : }
267 :
268 : // make the gyro window match the window size plus a buffer to cope with the backend
269 : // getting too far ahead.
270 20 : if (!_ins->set_gyro_window_size(_window_size + _samples_per_frame)) {
271 0 : return;
272 : }
273 :
274 : #if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
275 : // check for harmonics across all harmonic notch filters
276 : // note that we only allow one harmonic notch filter linked to the FFT code
277 20 : uint32_t harmonics = 0;
278 20 : uint8_t num_notches = 0;
279 80 : for (auto ¬ch : _ins->harmonic_notches) {
280 60 : if (notch.params.enabled()) {
281 9 : harmonics |= notch.params.harmonics();
282 9 : num_notches = MAX(num_notches, notch.num_dynamic_notches);
283 : }
284 : }
285 20 : if (harmonics == 0) {
286 : // this allows use of FFT to find peaks with all notch filters disabled
287 13 : harmonics = 3;
288 : }
289 : // count the number of active harmonics or dynamic notchs
290 20 : _tracked_peaks = constrain_int16(MAX(__builtin_popcount(harmonics),
291 : num_notches), 1, FrequencyPeak::MAX_TRACKED_PEAKS);
292 :
293 : // calculate harmonic multiplier. this assumes the harmonics configured on the
294 : // harmonic notch reflect the multiples of the fundamental harmonic that should be tracked
295 20 : if (_harmonic_fit > 0) {
296 20 : uint8_t first_harmonic = 0;
297 100 : for (uint8_t i = 0; i < HNF_MAX_HARMONICS; i++) {
298 96 : if (harmonics & (1<<i)) {
299 36 : if (first_harmonic == 0) {
300 20 : first_harmonic = i + 1;
301 : } else {
302 16 : _harmonic_multiplier = float(i + 1) / first_harmonic;
303 16 : break;
304 : }
305 : }
306 : }
307 : // if no harmonic specified then select a simple 2x multiple
308 20 : if (is_zero(_harmonic_multiplier)) {
309 4 : _harmonic_multiplier = 2.0f;
310 : }
311 : }
312 : #endif // AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
313 :
314 : // initialise the HAL DSP subsystem
315 20 : _state = hal.dsp->fft_init(_window_size, _fft_sampling_rate_hz, _num_frames);
316 20 : if (_state == nullptr) {
317 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Failed to initialize DSP engine");
318 0 : return;
319 : }
320 :
321 : // per-axis frame time
322 20 : _frame_time_ms = _samples_per_frame * 1000 / _fft_sampling_rate_hz;
323 : // The update rate for the output, defaults are 1Khz / (1 - 0.5) * 32 == 62hz
324 20 : const float output_rate = static_cast<float>(_fft_sampling_rate_hz) / static_cast<float>(_samples_per_frame);
325 : // establish suitable defaults for the detected values
326 80 : for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
327 60 : _thread_state._center_freq_hz[axis] = _fft_min_hz;
328 :
329 240 : for (uint8_t peak = 0; peak < FrequencyPeak::MAX_TRACKED_PEAKS; peak++) {
330 180 : _thread_state._center_freq_hz_filtered[axis][peak] = _fft_min_hz;
331 : }
332 : // number of cycles to average over, two complete windows to be sure
333 60 : _noise_calibration_cycles[axis] = (_window_size / _samples_per_frame) * 2;
334 : // harmonic frequency fit should change relatively slowly
335 60 : _harmonic_fit_filter[axis].set_cutoff_frequency(output_rate, MIN(output_rate * 0.48f, FFT_HARMONIC_FIT_FILTER_HZ));
336 : }
337 :
338 : // configure a filter for frequency, bandwidth and energy for each of the three tracked noise peaks
339 : // filter more aggressively post-filter since the noise is harder to detect
340 20 : const float scale_factor = using_post_filter_samples() ? 0.1f : 1.0f;
341 80 : for (uint8_t peak = 0; peak < FrequencyPeak::MAX_TRACKED_PEAKS; peak++) {
342 : // calculate low-pass filter characteristics based on window size and overlap
343 60 : _center_freq_filter[peak].set_cutoff_frequency(output_rate, output_rate * 0.48f * scale_factor);
344 : // the bin energy jumps around a lot so requires more filtering
345 60 : _center_freq_energy_filter[peak].set_cutoff_frequency(output_rate, output_rate * 0.25f * scale_factor);
346 : // smooth the bandwidth output more aggressively
347 60 : _center_bandwidth_filter[peak].set_cutoff_frequency(output_rate, output_rate * 0.25f * scale_factor);
348 : }
349 :
350 : // turn down the SNR threshold if examining post-filter
351 20 : if (using_post_filter_samples()) {
352 2 : _snr_threshold_db.set_default(FFT_SNR_PFILT_DEFAULT);
353 : }
354 :
355 : // the number of cycles required to have a proper noise reference
356 20 : _noise_cycles = (_window_size / _samples_per_frame) * XYZ_AXIS_COUNT;
357 :
358 : // finally we are done
359 20 : _initialized = true;
360 20 : update_parameters(true);
361 : // start running FFTs
362 20 : if (start_update_thread()) {
363 20 : set_analysis_enabled(true);
364 : }
365 : }
366 :
367 : // sample the gyros either by using a gyro window sampled at the gyro rate or making individual samples
368 : // called from fast_loop thread - this function does not take out a semaphore to avoid waiting on the FFT thread
369 41686253 : void AP_GyroFFT::sample_gyros()
370 : {
371 41686253 : if (!analysis_enabled()) {
372 40883858 : return;
373 : }
374 :
375 : // update counters for gyro window
376 802395 : if (_current_sample_mode > 0) {
377 : // for loop rate sampling accumulate and average gyro samples
378 31554 : _oversampled_gyro_accum += _ins->get_gyro_for_fft();
379 31554 : _oversampled_gyro_count++;
380 :
381 31554 : if ((_oversampled_gyro_count % _current_sample_mode) == 0) {
382 : // calculate mean value of accumulated samples
383 31554 : Vector3f sample = _oversampled_gyro_accum / _current_sample_mode;
384 : // fast sampling means that the raw gyro values have already been averaged over 8 samples
385 31554 : _downsampled_gyro_data[0].push(sample.x);
386 31554 : _downsampled_gyro_data[1].push(sample.y);
387 31554 : _downsampled_gyro_data[2].push(sample.z);
388 :
389 31554 : _oversampled_gyro_accum.zero();
390 31554 : _oversampled_gyro_count = 0;
391 : }
392 : }
393 : }
394 :
395 : // update the state as as required
396 : // called from main thread at 400Hz - anything that requires atomic access to IMU data needs to be done here
397 41684408 : void AP_GyroFFT::update()
398 : {
399 41684408 : if (!analysis_enabled()) {
400 40882053 : return;
401 : }
402 :
403 802355 : WITH_SEMAPHORE(_sem);
404 :
405 802355 : _config._analysis_enabled = _analysis_enabled;
406 802355 : _global_state = _thread_state;
407 :
408 : // calculate health based on being 5 frames behind, SITL needs longer
409 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
410 802355 : const uint32_t output_delay = _frame_time_ms * FFT_MAX_MISSED_UPDATES * 2;
411 : #else
412 : const uint32_t output_delay = _frame_time_ms * FFT_MAX_MISSED_UPDATES;
413 : #endif
414 802355 : uint32_t now = AP_HAL::millis();
415 802355 : _rpy_health.x = (now - _global_state._health_ms.x <= output_delay);
416 802355 : _rpy_health.y = (now - _global_state._health_ms.y <= output_delay);
417 802355 : _rpy_health.z = (now - _global_state._health_ms.z <= output_delay);
418 :
419 802355 : _health = _global_state._health;
420 802355 : if (!_rpy_health.x) {
421 206812 : _health.x = 0;
422 : }
423 802355 : if (!_rpy_health.y) {
424 208601 : _health.y = 0;
425 : }
426 802355 : if (!_rpy_health.z) {
427 208288 : _health.z = 0;
428 : }
429 802355 : }
430 :
431 : // analyse gyro data using FFT, returns number of samples still held
432 : // called from FFT thread
433 535336241 : uint16_t AP_GyroFFT::run_cycle()
434 : {
435 535336241 : if (!analysis_enabled()) {
436 18 : return 0;
437 : }
438 :
439 535336223 : if (!_sem.take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
440 0 : return 0;
441 : }
442 :
443 : // do we have enough samples for another pass?
444 535336223 : if (!start_analysis()) {
445 535057667 : uint16_t new_sample_count = get_available_samples(_update_axis);
446 535057667 : _sem.give();
447 535057667 : return new_sample_count;
448 : }
449 :
450 : // take a copy of the config inside the semaphore
451 278556 : EngineConfig config = _config;
452 :
453 278556 : _sem.give();
454 :
455 278556 : uint32_t now = AP_HAL::micros();
456 :
457 : // get the appropriate gyro buffer
458 278556 : FloatBuffer& gyro_buffer = (_sample_mode == 0 ?_ins->get_raw_gyro_window(_update_axis) : _downsampled_gyro_data[_update_axis]);
459 : // if we have many more samples than the window size then we are struggling to
460 : // stay ahead of the gyro loop so drop samples so that this cycle will use all available samples
461 278556 : if (gyro_buffer.available() > uint32_t(_state->_window_size + uint16_t(_samples_per_frame >> 1))) { // half the frame size is a heuristic
462 281 : gyro_buffer.advance(gyro_buffer.available() - _state->_window_size);
463 : }
464 : // let's go!
465 278556 : hal.dsp->fft_start(_state, gyro_buffer, _samples_per_frame);
466 :
467 : // calculate FFT and update filters outside the semaphore
468 278556 : uint16_t bin_max = hal.dsp->fft_analyse(_state, config._fft_start_bin, config._fft_end_bin, config._attenuation_cutoff);
469 :
470 : // something has been detected, update the peak frequency and associated metrics
471 278556 : update_ref_energy(bin_max);
472 278556 : calculate_noise(false, config);
473 :
474 : // record how we are doing
475 278556 : _thread_state._last_output_us[_update_axis] = AP_HAL::micros();
476 278556 : _output_cycle_micros = _thread_state._last_output_us[_update_axis] - now;
477 :
478 : #if AP_SIM_ENABLED && HAL_LOGGING_ENABLED
479 : // extra logging when running simulations
480 : // @LoggerMessage: FTN3
481 : // @Description: Additional FFT Noise Frequency Peak
482 : // @Field: TimeUS: microseconds since system startup
483 : // @Field: Id: update axis
484 : // @Field: Pk1: Peak 1 frequency
485 : // @Field: Pk2: Peak 2 frequency
486 : // @Field: Pk3: Peak 3 Frequency
487 : // @Field: Bw1: Peak 1 noise bandwidth
488 : // @Field: Bw2: Peak 2 noise bandwidth
489 : // @Field: Bw3: Peak 3 noise bandwidth
490 : // @Field: En1: Peak 1 Maximum energy
491 : // @Field: En2: Peak 2 Maximum energy
492 : // @Field: En3: Peak 3 Maximum energy
493 557112 : AP::logger().WriteStreaming(
494 : "FTN3",
495 : "TimeUS,Id,Pk1,Pk2,Pk3,Bw1,Bw2,Bw3,En1,En2,En3",
496 : "s#zzzzzz---",
497 : "F----------",
498 : "QBfffffffff",
499 : AP_HAL::micros64(),
500 278556 : _update_axis,
501 278556 : _state->_peak_data[0]._freq_hz,
502 278556 : _state->_peak_data[1]._freq_hz,
503 278556 : _state->_peak_data[2]._freq_hz,
504 278556 : _state->_peak_data[0]._noise_width_hz,
505 278556 : _state->_peak_data[1]._noise_width_hz,
506 278556 : _state->_peak_data[2]._noise_width_hz,
507 278556 : _state->_freq_bins[_state->_peak_data[0]._bin],
508 278556 : _state->_freq_bins[_state->_peak_data[1]._bin],
509 278556 : _state->_freq_bins[_state->_peak_data[2]._bin]);
510 : #endif
511 :
512 : // move onto the next axis
513 278556 : _update_axis = (_update_axis + 1) % XYZ_AXIS_COUNT;
514 :
515 : // ready to receive another frame, because lock contention is so expensive we don't lock
516 : // around this flag but rather rely on the semaphore at the beginning of the loop to
517 : // ensure eventual visibility to the main loop
518 278556 : _thread_state._analysis_started = false;
519 :
520 : // samples remaining in the next axis
521 278556 : return get_available_samples(_update_axis);
522 : }
523 :
524 : // whether analysis can be run again or not
525 : // called from FFT thread with the semaphore held
526 535336223 : bool AP_GyroFFT::start_analysis() {
527 535336223 : if (_thread_state._analysis_started) {
528 0 : return false;
529 : }
530 : // don't run any more gyro cycles once noise is calibrated and the self-test is running
531 535336223 : if (!_thread_state._noise_needs_calibration && !_calibrated) {
532 535054761 : return false;
533 : }
534 :
535 281462 : if (get_available_samples(_update_axis) >= _state->_window_size) {
536 278556 : _thread_state._analysis_started = true;
537 278556 : return true;
538 : }
539 2906 : return false;
540 : }
541 :
542 : // update calculated values of dynamic parameters - runs at 1Hz
543 149237 : void AP_GyroFFT::update_parameters(bool force)
544 : {
545 149237 : if (!_initialized && !force) {
546 148903 : return;
547 : }
548 : #if AP_ARMING_ENABLED
549 : // lock contention is very costly, so don't allow configuration
550 : // updates while flying
551 2091 : if (AP::arming().is_armed() && !force) {
552 1757 : return;
553 : }
554 : #endif
555 :
556 334 : WITH_SEMAPHORE(_sem);
557 :
558 : // don't allow MAXHZ to go to Nyquist
559 334 : _fft_max_hz.set(MIN(_fft_max_hz, _fft_sampling_rate_hz * 0.48));
560 334 : _config._snr_threshold_db = _snr_threshold_db;
561 334 : _config._fft_min_hz = _fft_min_hz;
562 334 : _config._fft_max_hz = _fft_max_hz;
563 : // determine the start FFT bin for all frequency detection
564 334 : _config._fft_start_bin = MAX(floorf(_fft_min_hz.get() / _state->_bin_resolution), 1);
565 : // determine the endt FFT bin for all frequency detection
566 334 : _config._fft_end_bin = MIN(ceilf(_fft_max_hz.get() / _state->_bin_resolution), _state->_bin_count);
567 : // actual attenuation from the db value
568 334 : _config._attenuation_cutoff = powf(10.0f, -_attenuation_power_db * 0.1f);
569 334 : }
570 :
571 : // thread for processing gyro data via FFT
572 535336241 : void AP_GyroFFT::update_thread(void)
573 : {
574 : while (true) {
575 535336241 : uint16_t remaining_samples = run_cycle();
576 : // this is to stop us burning CPU while waiting for samples, the reduction by _samples_per_frame is a heuristic to prevent waiting too long
577 : // and missing frames (easy to see in SITL because the noise will keep calibrating)
578 : // we always delay by at least 1us to give logging a chance to run at the same priority
579 535336241 : uint32_t delay = constrain_int32((int16_t)_state->_window_size - (int16_t)remaining_samples, 0, _samples_per_frame)
580 535336241 : * 1e6 / _fft_sampling_rate_hz;
581 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
582 : // in SITL the gyros do not run in a different thread
583 535336241 : if (delay > 0) {
584 133955 : hal.scheduler->delay_microseconds(delay);
585 : }
586 : #else
587 : hal.scheduler->delay_microseconds(MAX(delay, 1U));
588 : #endif
589 535336221 : }
590 : }
591 :
592 : // start the update thread
593 20 : bool AP_GyroFFT::start_update_thread(void)
594 : {
595 20 : WITH_SEMAPHORE(_sem);
596 :
597 20 : if (_thread_created) {
598 0 : return true;
599 : }
600 :
601 20 : if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_GyroFFT::update_thread, void), "apm_fft", FFT_STACK_SIZE, AP_HAL::Scheduler::PRIORITY_IO, 0)) {
602 0 : AP_HAL::panic("Failed to start AP_GyroFFT update thread");
603 : return false;
604 : }
605 :
606 20 : _thread_created = true;
607 20 : return true;
608 20 : }
609 :
610 : // self-test the FFT analyser - can only be done while samples are not being taken
611 : // called from main thread
612 45825 : bool AP_GyroFFT::pre_arm_check(char *failure_msg, const uint8_t failure_msg_len)
613 : {
614 45825 : if (!analysis_enabled()) {
615 45674 : return true;
616 : }
617 :
618 : // already calibrated
619 151 : if (_calibrated) {
620 131 : return true;
621 : }
622 :
623 : // analysis is started in the main thread, don't trample on in-flight analysis
624 20 : if (_global_state._analysis_started) {
625 0 : hal.util->snprintf(failure_msg, failure_msg_len, "FFT still analyzing");
626 0 : return false;
627 : }
628 :
629 : // still calibrating noise so not ready
630 20 : if (_global_state._noise_needs_calibration) {
631 0 : hal.util->snprintf(failure_msg, failure_msg_len, "FFT calibrating noise");
632 0 : return false;
633 : }
634 :
635 : // make sure the frequency maximum is below Nyquist
636 20 : if (_fft_max_hz > _fft_sampling_rate_hz * 0.5f) {
637 0 : hal.util->snprintf(failure_msg, failure_msg_len, "FFT config MAXHZ %dHz > %dHz", _fft_max_hz.get(), _fft_sampling_rate_hz / 2);
638 0 : return false;
639 : }
640 :
641 : // check for sane frequency resolution - for 1k backends with length 32 this will be 32Hz
642 20 : if (_state->_bin_resolution > 50.0f) {
643 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "FFT: resolution is %.1fHz, increase length", _state->_bin_resolution);
644 0 : return true; // a low resolution is not fatal
645 : }
646 : #if 0 // these calculations do not result in a long enough expected delay
647 : // in order to test all frequencies we need to endure a long pause for higher windows lengths
648 : // see timings AP_HAL_ChibiOS/DSP.cpp for per step timings on different hardware
649 : #if defined(STM32H7)
650 : const uint32_t cycle_time = (16 * (1 << (_window_size / 32)) + 5) * (_config._fft_end_bin - _config._fft_start_bin + 1) * 2 / 1000; // H7
651 : #else
652 : const uint32_t cycle_time = (29 * (1 << (_window_size / 32)) + 5) * (_config._fft_end_bin - _config._fft_start_bin + 1) * 2 / 1000; // F4
653 : #endif
654 : #endif
655 20 : EXPECT_DELAY_MS(2000); // tested on an H7 at 1024 window
656 :
657 20 : float max_divergence = self_test_bin_frequencies();
658 : // for longer FFT lengths the resolution gets below 1Hz
659 20 : if (max_divergence > MAX(_state->_bin_resolution * 0.5f, 1)) {
660 0 : hal.util->snprintf(failure_msg, failure_msg_len, "FFT self-test failed, max error %fHz", max_divergence);
661 : }
662 :
663 20 : _calibrated = max_divergence <= MAX(_state->_bin_resolution * 0.5f, 1);
664 :
665 20 : if (_calibrated) {
666 : // provide the user with some useful information about what they have configured
667 20 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "FFT: calibrated %.1fKHz/%.1fHz/%.1fHz", _fft_sampling_rate_hz * 0.001f,
668 : _state->_bin_resolution * 0.5, 1000.0f * XYZ_AXIS_COUNT / _frame_time_ms);
669 : }
670 :
671 20 : return _calibrated;
672 20 : }
673 :
674 : // we may have disabled the FFT arming check, in which case make sure the engine can still run
675 1241 : bool AP_GyroFFT::prepare_for_arming()
676 : {
677 1241 : _calibrated = true;
678 1241 : return true;
679 : }
680 :
681 : // update the hover frequency input filter. should be called at 100hz when in a stable hover
682 : // called from main thread
683 1489615 : void AP_GyroFFT::update_freq_hover(float dt, float throttle_out)
684 : {
685 1489615 : if (!analysis_enabled()) {
686 1434479 : return;
687 : }
688 :
689 : // throttle averaging for average fft calculation
690 55136 : if (is_zero(_avg_throttle_out)) {
691 18 : _avg_throttle_out = throttle_out;
692 : } else {
693 55118 : _avg_throttle_out = constrain_float(_avg_throttle_out + (dt / (10.0f + dt)) * (throttle_out - _avg_throttle_out), 0.01f, 0.9f);
694 : }
695 :
696 : // we have chosen to constrain the hover frequency to be within the range reachable by the third order expo polynomial.
697 55136 : _freq_hover_hz.set(constrain_float(_freq_hover_hz + (dt / (10.0f + dt)) * (get_weighted_noise_center_freq_hz() - _freq_hover_hz), _fft_min_hz, _fft_max_hz));
698 55136 : _bandwidth_hover_hz.set(constrain_float(_bandwidth_hover_hz + (dt / (10.0f + dt)) * (get_weighted_noise_center_bandwidth_hz() - _bandwidth_hover_hz), 0, _fft_max_hz * 0.5f));
699 55136 : _throttle_ref.set(constrain_float(_throttle_ref + (dt / (10.0f + dt)) * (throttle_out * sq((float)_fft_min_hz.get() / _freq_hover_hz.get()) - _throttle_ref), 0.01f, 0.9f));
700 : }
701 :
702 : // save parameters as part of disarming
703 : // called from main thread
704 1103 : void AP_GyroFFT::save_params_on_disarm()
705 : {
706 1103 : if (!analysis_enabled()) {
707 1080 : return;
708 : }
709 :
710 23 : _freq_hover_hz.save();
711 23 : _bandwidth_hover_hz.save();
712 23 : _throttle_ref.save();
713 : }
714 :
715 : // notch tuning
716 2 : void AP_GyroFFT::start_notch_tune()
717 : {
718 2 : if (!analysis_enabled()) {
719 0 : return;
720 : }
721 :
722 2 : if (!hal.dsp->fft_start_average(_state)) {
723 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "FFT: Unable to start FFT averaging");
724 : }
725 : // throttle averaging for average fft calculation
726 2 : _avg_throttle_out = 0.0f;
727 : #if APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_ArduPlane)
728 2 : AP_Motors* motors = AP::motors();
729 2 : if (motors != nullptr) {
730 2 : _avg_throttle_out = motors->get_throttle_hover();
731 : }
732 : #endif
733 : }
734 :
735 : // calculate the frequency to be used for the harmonic notch
736 3 : float AP_GyroFFT::calculate_notch_frequency(float* freqs, uint16_t numpeaks, float harmonic_fit, uint8_t& harmonics)
737 : {
738 3 : float harmonic = freqs[0];
739 3 : harmonics = 1;
740 :
741 9 : for (uint8_t i = 1; i < numpeaks; i++) {
742 6 : if (freqs[i] < harmonic) {
743 32 : for (uint8_t x = 2; x <=HNF_MAX_HARMONICS; x++) {
744 30 : if (is_harmonic_of(harmonic, freqs[i], x, harmonic_fit)) {
745 2 : harmonic = freqs[i];
746 : }
747 : }
748 : }
749 : }
750 : // select the harmonics that were matched
751 12 : for (uint8_t i = 0; i < numpeaks; i++) {
752 153 : for (uint8_t x = 1; x <=HNF_MAX_HARMONICS; x++) {
753 144 : if (is_harmonic_of(freqs[i], harmonic, x, harmonic_fit)) {
754 8 : harmonics |= 1<<(x - 1);
755 : }
756 : }
757 : }
758 3 : return harmonic;
759 : }
760 :
761 6 : void AP_GyroFFT::stop_notch_tune()
762 : {
763 6 : if (!analysis_enabled()) {
764 4 : return;
765 : }
766 :
767 4 : float freqs[FrequencyPeak::MAX_TRACKED_PEAKS] {};
768 :
769 4 : uint16_t numpeaks = hal.dsp->fft_stop_average(_state, _config._fft_start_bin, _config._fft_end_bin, freqs);
770 :
771 4 : if (numpeaks == 0) {
772 2 : return;
773 : }
774 :
775 : uint8_t harmonics;
776 2 : float harmonic = calculate_notch_frequency(freqs, numpeaks, _harmonic_fit, harmonics);
777 :
778 2 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "FFT: Found peaks at %.1f/%.1f/%.1fHz", freqs[0], freqs[1], freqs[2]);
779 2 : GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "FFT: Selected %.1fHz", harmonic);
780 :
781 : // if we don't have a throttle value then all bets are off
782 2 : if (is_zero(_avg_throttle_out) || is_zero(harmonic)) {
783 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "FFT: Unable to calculate notch: need stable hover");
784 0 : AP_Notify::events.autotune_failed = true;
785 0 : return;
786 : }
787 :
788 : #if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
789 2 : if (!_ins->setup_throttle_gyro_harmonic_notch(harmonic, (float)_fft_min_hz.get(), _avg_throttle_out, harmonics)) {
790 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "FFT: Unable to set throttle notch with %.1fHz/%.2f",
791 : harmonic, _avg_throttle_out);
792 0 : AP_Notify::events.autotune_failed = true;
793 : // save results to FFT slots
794 0 : _throttle_ref.set(_avg_throttle_out);
795 0 : _freq_hover_hz.set(harmonic);
796 : } else {
797 2 : GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "FFT: Notch frequency %.1fHz and ref %.2f selected", harmonic, _avg_throttle_out);
798 2 : AP_Notify::events.autotune_complete = true;
799 : }
800 : #endif // AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
801 : }
802 :
803 : // return the noise peak that is being tracked
804 : // called from main thread
805 191009 : AP_GyroFFT::FrequencyPeak AP_GyroFFT::get_tracked_noise_peak() const
806 : {
807 : // if the user has specified a specific axis to track then use that
808 191009 : if (_harmonic_peak > FrequencyPeak::MAX_TRACKED_PEAKS) {
809 0 : switch (_harmonic_peak) {
810 0 : case FFT_HARMONIC_FIT_TRACK_ROLL:
811 0 : if (_global_state._harmonic_fit.x < _harmonic_fit) {
812 0 : return FrequencyPeak(_global_state._tracked_peak.x);
813 : }
814 0 : break;
815 0 : case FFT_HARMONIC_FIT_TRACK_PITCH:
816 0 : if (_global_state._harmonic_fit.y < _harmonic_fit) {
817 0 : return FrequencyPeak(_global_state._tracked_peak.y);
818 : }
819 0 : break;
820 0 : default:
821 0 : break;
822 : }
823 0 : return FrequencyPeak::CENTER;
824 : }
825 : // if the user has specified a specific peak to track then use that
826 191009 : if (_harmonic_peak > 0) {
827 0 : return FrequencyPeak(constrain_int16(_harmonic_peak - 1, FrequencyPeak::CENTER, FrequencyPeak::UPPER_SHOULDER));
828 : }
829 :
830 : // required fit of 10% is fairly conservative when testing in SITL, testing shows that it's safer to
831 : // require both tracked axes to fit - biasing towards the highest energy peak
832 191009 : if (_global_state._harmonic_fit.x < _harmonic_fit && _global_state._harmonic_fit.y < _harmonic_fit) {
833 29952 : return FrequencyPeak(_global_state._tracked_peak.x);
834 : }
835 :
836 161057 : return FrequencyPeak::CENTER;
837 : }
838 :
839 : // weighted center frequency
840 170588 : float AP_GyroFFT::get_weighted_freq_hz(FrequencyPeak peak) const
841 : {
842 170588 : const Vector3f& energy = get_center_freq_energy(peak);
843 170588 : const Vector3f& freq = get_noise_center_freq_hz(peak);
844 :
845 170588 : if (!energy.is_nan() && !is_zero(energy.x) && !is_zero(energy.y)) {
846 170246 : return (freq.x * energy.x + freq.y * energy.y) / (energy.x + energy.y);
847 : } else {
848 342 : return (freq.x + freq.y) * 0.5f;
849 : }
850 : }
851 :
852 : // return an average center frequency weighted by bin energy
853 : // called from main thread
854 130919 : float AP_GyroFFT::get_weighted_noise_center_freq_hz() const
855 : {
856 130919 : if (!analysis_enabled()) {
857 259 : return _fft_min_hz;
858 : }
859 :
860 130660 : if (_health.is_zero()) {
861 : #if APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_ArduPlane)
862 : // if we are post-filter sampling then throttle estimate will be useless
863 17643 : if (using_post_filter_samples()) {
864 8179 : return 0.0f;
865 : }
866 9464 : AP_Motors* motors = AP::motors();
867 9464 : if (motors != nullptr && !is_zero(_throttle_ref)) {
868 : // FFT is not healthy, fallback to FFT's throttle-based estimate
869 9464 : return constrain_float(_fft_min_hz * MAX(1.0f, sqrtf(motors->get_throttle_out() / _throttle_ref)), _fft_min_hz, _fft_max_hz);
870 : }
871 : #endif
872 : }
873 :
874 113017 : const FrequencyPeak peak = get_tracked_noise_peak();
875 : // pitch was good or required, roll was not, use pitch only
876 113017 : if (!_health.x || _harmonic_peak == FFT_HARMONIC_FIT_TRACK_PITCH) {
877 234 : return get_noise_center_freq_hz(peak).y; // Y-axis
878 : }
879 : // roll was good or required, pitch was not, use roll only
880 112783 : if (!_health.y || _harmonic_peak == FFT_HARMONIC_FIT_TRACK_ROLL) {
881 341 : return get_noise_center_freq_hz(peak).x; // X-axis
882 : }
883 :
884 112442 : return get_weighted_freq_hz(peak);
885 : }
886 :
887 : // return all the center frequencies weighted by bin energy
888 : // called from main thread
889 44424 : uint8_t AP_GyroFFT::get_weighted_noise_center_frequencies_hz(uint8_t num_freqs, float* freqs) const
890 : {
891 44424 : if (!analysis_enabled()) {
892 199 : freqs[0] = _fft_min_hz;
893 199 : return 1;
894 : }
895 :
896 44225 : if (_health.is_zero()) {
897 : #if APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_ArduPlane)
898 : // if we are post-filter sampling then throttle estimate will be useless
899 23181 : if (using_post_filter_samples()) {
900 18133 : return 0;
901 : }
902 5048 : AP_Motors* motors = AP::motors();
903 5048 : if (motors != nullptr) {
904 : // FFT is not healthy, fallback to FFT's throttle-based estimate
905 5048 : freqs[0] = constrain_float(_fft_min_hz * MAX(1.0f, sqrtf(motors->get_throttle_out() / _throttle_ref)), _fft_min_hz, _fft_max_hz);
906 5048 : return 1;
907 : }
908 : #endif
909 : }
910 :
911 : // pitch was good or required, roll was not, use pitch only
912 21044 : if (!_health.x || _harmonic_peak == FFT_HARMONIC_FIT_TRACK_PITCH) {
913 383 : const uint8_t tracked_peaks = MIN(_health.y, num_freqs);
914 1375 : for (uint8_t i = 0; i < tracked_peaks; i++) {
915 992 : freqs[i] = get_noise_center_freq_hz(FrequencyPeak(i)).y; // Y-axis
916 : }
917 383 : return tracked_peaks;
918 : }
919 : // roll was good or required, pitch was not, use roll only
920 20661 : if (!_health.y || _harmonic_peak == FFT_HARMONIC_FIT_TRACK_ROLL) {
921 729 : const uint8_t tracked_peaks = MIN(_health.x, num_freqs);
922 1790 : for (uint8_t i = 0; i < tracked_peaks; i++) {
923 1061 : freqs[i] = get_noise_center_freq_hz(FrequencyPeak(i)).x; // X-axis
924 : }
925 729 : return tracked_peaks;
926 : }
927 :
928 19932 : const uint8_t tracked_peaks = MIN(MAX(_health.x, _health.y), num_freqs);
929 78078 : for (uint8_t i = 0; i < tracked_peaks; i++) {
930 58146 : freqs[i] = get_weighted_freq_hz(FrequencyPeak(i));
931 : }
932 19932 : return tracked_peaks;
933 : }
934 :
935 : // return noise energy at the requested frequency
936 1202 : float AP_GyroFFT::has_noise_at_frequency_hz(float freq) const
937 : {
938 1202 : if (!analysis_enabled()) {
939 0 : return 0.0f;
940 : }
941 :
942 1202 : float max_energy = 0.0f;
943 :
944 : // check each axis of each peak to see if it contains the pass frequency
945 4808 : for (uint8_t i = 0; i < _tracked_peaks; i++) {
946 3606 : const Vector3f& noise = get_noise_center_freq_hz(FrequencyPeak(i));
947 3606 : const Vector3f& snr = get_noise_signal_to_noise_db(FrequencyPeak(i));
948 :
949 14424 : for (uint8_t j = 0; j < XYZ_AXIS_COUNT; j++) {
950 10818 : if (!_rpy_health[j]) {
951 5904 : continue;
952 : }
953 :
954 : // only check one bin either side of the frequency
955 4914 : if ((noise[j] - _state->_bin_resolution) < freq && (noise[j] + _state->_bin_resolution) > freq) {
956 619 : max_energy = MAX(snr[j], max_energy);
957 : }
958 : }
959 : }
960 1202 : return max_energy;
961 : }
962 :
963 77992 : float AP_GyroFFT::calculate_weighted_freq_hz(const Vector3f& energy, const Vector3f& freq) const
964 : {
965 : // there is generally a lot of high-energy, slightly lower frequency noise on yaw, however this
966 : // appears to be a second-order effect as only targetting pitch and roll (x & y) produces much cleaner output all round
967 77992 : if (!energy.is_nan() && !is_zero(energy.x) && !is_zero(energy.y)) {
968 77971 : return (freq.x * energy.x + freq.y * energy.y)
969 77971 : / (energy.x + energy.y);
970 : }
971 : else {
972 21 : return (freq.x + freq.y) * 0.5f;
973 : }
974 : }
975 :
976 : // @LoggerMessage: FTN1
977 : // @Description: FFT Filter Tuning
978 : // @Field: TimeUS: microseconds since system startup
979 : // @Field: PkAvg: peak noise frequency as an energy-weighted average of roll and pitch peak frequencies
980 : // @Field: BwAvg: bandwidth of weighted peak frequency where edges are determined by FFT_ATT_REF
981 : // @Field: SnX: signal-to-noise ratio on the roll axis
982 : // @Field: SnY: signal-to-noise ratio on the pitch axis
983 : // @Field: SnZ: signal-to-noise ratio on the yaw axis
984 : // @Field: FtX: harmonic fit on roll of the highest noise peak to the second highest noise peak
985 : // @Field: FtY: harmonic fit on pitch of the highest noise peak to the second highest noise peak
986 : // @Field: FtZ: harmonic fit on yaw of the highest noise peak to the second highest noise peak
987 : // @Field: FHX: FFT health, X-axis
988 : // @Field: FHY: FFT health, Y-axis
989 : // @Field: FHZ: FFT health, Z-axis
990 : // @Field: Tc: FFT cycle time
991 :
992 : #if HAL_LOGGING_ENABLED
993 :
994 : // log gyro fft messages
995 2211214 : void AP_GyroFFT::write_log_messages()
996 : {
997 2211214 : if (!analysis_enabled()) {
998 2188358 : return;
999 : }
1000 :
1001 45712 : AP::logger().WriteStreaming(
1002 : "FTN1",
1003 : "TimeUS,PkAvg,BwAvg,SnX,SnY,SnZ,FtX,FtY,FtZ,FHX,FHY,FHZ,Tc",
1004 : "szz---%%%---s",
1005 : "F-----------F",
1006 : "QffffffffBBBI",
1007 : AP_HAL::micros64(),
1008 22856 : get_weighted_noise_center_freq_hz(),
1009 22856 : get_weighted_noise_center_bandwidth_hz(),
1010 22856 : get_noise_signal_to_noise_db().x,
1011 22856 : get_noise_signal_to_noise_db().y,
1012 22856 : get_noise_signal_to_noise_db().z,
1013 22856 : get_raw_noise_harmonic_fit().x,
1014 22856 : get_raw_noise_harmonic_fit().y,
1015 22856 : get_raw_noise_harmonic_fit().z,
1016 22856 : _health.x, _health.y, _health.z, _output_cycle_micros);
1017 :
1018 22856 : log_noise_peak(0, FrequencyPeak::CENTER);
1019 22856 : if (_tracked_peaks> 1) {
1020 22856 : log_noise_peak(1, FrequencyPeak::LOWER_SHOULDER);
1021 22856 : log_noise_peak(2, FrequencyPeak::UPPER_SHOULDER);
1022 : }
1023 :
1024 : #if DEBUG_FFT
1025 : const uint32_t now = AP_HAL::millis();
1026 : // output at 1hz
1027 : if ((now - _last_output_ms) > 1000) {
1028 : // doing this from the update thread overflows the stack
1029 : WITH_SEMAPHORE(_sem);
1030 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "FFT: f:%.1f, fr:%.1f, b:%u, fd:%.1f",
1031 : _debug_state._center_freq_hz_filtered[FrequencyPeak::CENTER][_update_axis], _debug_state._center_freq_hz[_update_axis], _debug_max_bin, _debug_max_bin_freq);
1032 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "FFT: bw:%.1f, e:%.1f, r:%.1f, snr:%.1f",
1033 : _debug_state._center_bandwidth_hz_filtered[FrequencyPeak::CENTER][_update_axis], _debug_max_freq_bin, _ref_energy[_debug_max_bin][_update_axis], _debug_snr);
1034 : _last_output_ms = now;
1035 : }
1036 : #endif
1037 : }
1038 :
1039 : // @LoggerMessage: FTN2
1040 : // @Description: FFT Noise Frequency Peak
1041 : // @Field: TimeUS: microseconds since system startup
1042 : // @Field: Id: peak id where 0 is the centre peak, 1 is the lower shoulder and 2 is the upper shoulder
1043 : // @Field: PkX: noise frequency of the peak on roll
1044 : // @Field: PkY: noise frequency of the peak on pitch
1045 : // @Field: PkZ: noise frequency of the peak on yaw
1046 : // @Field: BwX: bandwidth of the peak frequency on roll where edges are determined by FFT_ATT_REF
1047 : // @Field: BwY: bandwidth of the peak frequency on pitch where edges are determined by FFT_ATT_REF
1048 : // @Field: BwZ: bandwidth of the peak frequency on yaw where edges are determined by FFT_ATT_REF
1049 : // @Field: SnX: signal-to-noise ratio on the roll axis
1050 : // @Field: SnY: signal-to-noise ratio on the pitch axis
1051 : // @Field: SnZ: signal-to-noise ratio on the yaw axis
1052 : // @Field: EnX: power spectral density bin energy of the peak on roll
1053 : // @Field: EnY: power spectral density bin energy of the peak on roll
1054 : // @Field: EnZ: power spectral density bin energy of the peak on roll
1055 :
1056 : // write a single log message
1057 68568 : void AP_GyroFFT::log_noise_peak(uint8_t id, FrequencyPeak peak) const
1058 : {
1059 137136 : AP::logger().WriteStreaming("FTN2", "TimeUS,Id,PkX,PkY,PkZ,BwX,BwY,BwZ,SnX,SnY,SnZ,EnX,EnY,EnZ", "s#zzzzzz------", "F-------------", "QBffffffffffff",
1060 : AP_HAL::micros64(),
1061 : id,
1062 68568 : get_noise_center_freq_hz(peak).x,
1063 68568 : get_noise_center_freq_hz(peak).y,
1064 68568 : get_noise_center_freq_hz(peak).z,
1065 68568 : get_noise_center_bandwidth_hz(peak).x,
1066 68568 : get_noise_center_bandwidth_hz(peak).y,
1067 68568 : get_noise_center_bandwidth_hz(peak).z,
1068 68568 : get_noise_signal_to_noise_db(peak).x,
1069 68568 : get_noise_signal_to_noise_db(peak).y,
1070 68568 : get_noise_signal_to_noise_db(peak).z,
1071 68568 : get_center_freq_energy(peak).x,
1072 68568 : get_center_freq_energy(peak).y,
1073 68568 : get_center_freq_energy(peak).z);
1074 68568 : }
1075 :
1076 : #endif
1077 :
1078 : // return an average noise bandwidth weighted by bin energy
1079 : // called from main thread
1080 77992 : float AP_GyroFFT::get_weighted_noise_center_bandwidth_hz() const
1081 : {
1082 77992 : if (!analysis_enabled()) {
1083 0 : return 0.0f;
1084 : }
1085 :
1086 77992 : const FrequencyPeak peak = get_tracked_noise_peak();
1087 :
1088 77992 : return calculate_weighted_freq_hz(get_center_freq_energy(peak), get_noise_center_bandwidth_hz(peak));
1089 : }
1090 :
1091 : // calculate noise frequencies from FFT data provided by the HAL subsystem
1092 : // called from FFT thread
1093 279734 : void AP_GyroFFT::calculate_noise(bool calibrating, const EngineConfig& config)
1094 : {
1095 : // calculate the SNR and center frequency energy
1096 279734 : float weighted_center_freq_hz = 0.0f;
1097 :
1098 279734 : uint8_t num_peaks = calculate_tracking_peaks(weighted_center_freq_hz, calibrating, config);
1099 :
1100 279734 : _thread_state._center_freq_bin[_update_axis] = _state->_peak_data[_thread_state._center_peak[_update_axis]]._bin;
1101 279734 : _thread_state._center_freq_hz[_update_axis] = weighted_center_freq_hz;
1102 : // record the last time we had a good signal on this axis
1103 279734 : if (num_peaks > 0) {
1104 241602 : _thread_state._health_ms[_update_axis] = AP_HAL::millis();
1105 : } else {
1106 38132 : _thread_state._health_ms[_update_axis] = 0;
1107 : }
1108 279734 : _thread_state._health[_update_axis] = num_peaks;
1109 279734 : FrequencyPeak tracked_peak = FrequencyPeak::CENTER;
1110 :
1111 : // record the tracked peak for harmonic fit, but only if we have more than one noise peak
1112 : // this checks filtered energies and so can allow energies to be closer together
1113 279734 : if (num_peaks > 1 && _tracked_peaks > 1 && !is_zero(get_tl_noise_center_freq_hz(FrequencyPeak::CENTER, _update_axis))) {
1114 228377 : if (get_tl_noise_center_freq_hz(FrequencyPeak::CENTER, _update_axis) > get_tl_noise_center_freq_hz(FrequencyPeak::LOWER_SHOULDER, _update_axis)
1115 : // ignore the fit if there is too big a discrepancy between the energies
1116 228377 : && get_tl_center_freq_energy(FrequencyPeak::CENTER, _update_axis) < get_tl_center_freq_energy(FrequencyPeak::LOWER_SHOULDER, _update_axis) * FFT_HARMONIC_FIT_MULT) {
1117 64703 : tracked_peak = FrequencyPeak::LOWER_SHOULDER;
1118 124158 : } else if (num_peaks > 2 && get_tl_noise_center_freq_hz(FrequencyPeak::CENTER, _update_axis) > get_tl_noise_center_freq_hz(FrequencyPeak::UPPER_SHOULDER, _update_axis)
1119 : // ignore the fit if there is too big a discrepancy between the energies
1120 287832 : && get_tl_center_freq_energy(FrequencyPeak::CENTER, _update_axis) < get_tl_center_freq_energy(FrequencyPeak::UPPER_SHOULDER, _update_axis) * FFT_HARMONIC_FIT_MULT) {
1121 15447 : tracked_peak = FrequencyPeak::UPPER_SHOULDER;
1122 : }
1123 : }
1124 :
1125 279734 : _thread_state._tracked_peak[_update_axis] = tracked_peak;
1126 :
1127 : // if targetting more than one harmonic then make sure we get the fundamental
1128 : // on larger copters the second harmonic often has more energy
1129 : // if the highest peak is above the second highest then check for harmonic fit
1130 : // comparisons are made using filter, normalised data
1131 279734 : if (_thread_state._tracked_peak[_update_axis] != FrequencyPeak::CENTER) {
1132 : // calculate the fit and filter at 10hz
1133 80150 : const float harmonic_fit = 100.0f * fabsf(get_tl_noise_center_freq_hz(FrequencyPeak::CENTER, _update_axis)
1134 80150 : - get_tl_noise_center_freq_hz(tracked_peak, _update_axis) * _harmonic_multiplier)
1135 80150 : / get_tl_noise_center_freq_hz(FrequencyPeak::CENTER, _update_axis);
1136 :
1137 : // calculate the fit and filter at 10hz
1138 80150 : if (isfinite(harmonic_fit)) {
1139 80150 : _thread_state._harmonic_fit[_update_axis] = _harmonic_fit_filter[_update_axis].apply(harmonic_fit);
1140 : }
1141 : } else {
1142 199584 : _thread_state._harmonic_fit[_update_axis] = 100.0f;
1143 : }
1144 : #if DEBUG_FFT
1145 : WITH_SEMAPHORE(_sem);
1146 : _debug_state = _thread_state;
1147 : _debug_max_freq_bin = _state->get_freq_bin(_state->_peak_data[FrequencyPeak::CENTER]._bin);
1148 : _debug_max_bin_freq = _state->_peak_data[FrequencyPeak::CENTER]._freq_hz;
1149 : _debug_snr = snr;
1150 : _debug_max_bin = _state->_peak_data[FrequencyPeak::CENTER]._bin;
1151 : #endif
1152 279734 : }
1153 :
1154 :
1155 : // calculate noise peaks based on the frequencies closest to the recent historical average, switching peaks around as necessary
1156 279734 : uint8_t AP_GyroFFT::calculate_tracking_peaks(float& weighted_center_freq_hz, bool calibrating, const EngineConfig& config)
1157 : {
1158 279734 : uint8_t num_peaks = 0;
1159 279734 : FrequencyData freqs(*this, config);
1160 :
1161 : // the noise peaks are returned by the HAL in decreasing order of magnitude, however each peak can temporarily
1162 : // switch places with another depending on a whole host of hardware and software factors
1163 : // thus we must be able to temporarily reassign the peaks so that the filtered values track
1164 : // a continuous frequency
1165 : DistanceMatrix distance_matrix;
1166 279734 : find_distance_matrix(distance_matrix, freqs, config);
1167 :
1168 279734 : FrequencyPeak center = find_closest_peak(FrequencyPeak::CENTER, distance_matrix);
1169 279734 : FrequencyPeak lower = find_closest_peak(FrequencyPeak::LOWER_SHOULDER, distance_matrix, 1 << center);
1170 279734 : FrequencyPeak upper = find_closest_peak(FrequencyPeak::UPPER_SHOULDER, distance_matrix, 1 << center | 1 << lower);
1171 :
1172 : // if we have had the maximum number of swapped cycles, force a full calculation
1173 279734 : if (calibrating || _distorted_cycles[_update_axis] == 0) {
1174 13030 : num_peaks = calculate_tracking_peaks(weighted_center_freq_hz, freqs, config);
1175 : #if DEBUG_FFT
1176 : printf("Skipped update, order would have been is %d/%.1f(%.1f) %d/%.1f(%.1f) %d/%.1f(%.1f) n = %d\n",
1177 : center, _state->_peak_data[center]._freq_hz, get_tl_noise_center_freq_hz(FrequencyPeak::CENTER, _update_axis),
1178 : lower, _state->_peak_data[lower]._freq_hz, get_tl_noise_center_freq_hz(FrequencyPeak::LOWER_SHOULDER, _update_axis),
1179 : upper, _state->_peak_data[upper]._freq_hz, get_tl_noise_center_freq_hz(FrequencyPeak::UPPER_SHOULDER, _update_axis), num_peaks);
1180 : #endif
1181 13030 : return num_peaks;
1182 : }
1183 :
1184 : // another peak is closer to what is currently considered the center frequency
1185 266704 : if (center != FrequencyPeak::CENTER || lower != FrequencyPeak::LOWER_SHOULDER || upper != FrequencyPeak::UPPER_SHOULDER) {
1186 61413 : if (lower != FrequencyPeak::NONE && calculate_filtered_noise(FrequencyPeak::LOWER_SHOULDER, lower, freqs, config)) {
1187 59679 : num_peaks++;
1188 : } else {
1189 1734 : lower = FrequencyPeak::NONE;
1190 : }
1191 61413 : if (upper != FrequencyPeak::NONE && calculate_filtered_noise(FrequencyPeak::UPPER_SHOULDER, upper, freqs, config)) {
1192 27316 : num_peaks++;
1193 : } else {
1194 34097 : upper = FrequencyPeak::NONE;
1195 : }
1196 61413 : if (center != FrequencyPeak::NONE && calculate_filtered_noise(FrequencyPeak::CENTER, center, freqs, config)) {
1197 61413 : num_peaks++;
1198 : } else {
1199 0 : center = FrequencyPeak::NONE;
1200 : }
1201 61413 : weighted_center_freq_hz = freqs.get_weighted_frequency(center);
1202 61413 : _thread_state._center_peak[_update_axis] = center;
1203 61413 : update_snr_values(freqs);
1204 : // if two adjacent peaks have simply swapped, we will allow this to continue indefinitely
1205 : // as there is no loss of fidelity
1206 61413 : if (!((center == FrequencyPeak::LOWER_SHOULDER && lower == FrequencyPeak::CENTER)
1207 6013 : || (center == FrequencyPeak::UPPER_SHOULDER && upper == FrequencyPeak::CENTER))) {
1208 53964 : _distorted_cycles[_update_axis]--;
1209 : }
1210 61413 : return num_peaks;
1211 : }
1212 :
1213 205291 : num_peaks = calculate_tracking_peaks(weighted_center_freq_hz, freqs, config);
1214 :
1215 205291 : return num_peaks;
1216 : }
1217 :
1218 : // calculate the noise and whether valid for each peak
1219 218321 : uint8_t AP_GyroFFT::calculate_tracking_peaks(float& weighted_center_freq_hz, const FrequencyData& freqs, const EngineConfig& config)
1220 : {
1221 218321 : uint8_t num_peaks = 0;
1222 218321 : if (calculate_filtered_noise(FrequencyPeak::LOWER_SHOULDER, FrequencyPeak::LOWER_SHOULDER, freqs, config)) {
1223 166908 : num_peaks++;
1224 : }
1225 218321 : if (calculate_filtered_noise(FrequencyPeak::UPPER_SHOULDER, FrequencyPeak::UPPER_SHOULDER, freqs, config)) {
1226 154839 : num_peaks++;
1227 : }
1228 218321 : if (calculate_filtered_noise(FrequencyPeak::CENTER, FrequencyPeak::CENTER, freqs, config)) {
1229 179913 : num_peaks++;
1230 : }
1231 : // record the number of cycles where something was tracked
1232 218321 : _distorted_cycles[_update_axis] = constrain_int16(_distorted_cycles[_update_axis] + 1, 0, FFT_MAX_MISSED_UPDATES);
1233 218321 : weighted_center_freq_hz = freqs.get_weighted_frequency(FrequencyPeak::CENTER);
1234 218321 : _thread_state._center_peak[_update_axis] = FrequencyPeak::CENTER;
1235 :
1236 218321 : update_snr_values(freqs);
1237 :
1238 218321 : return num_peaks;
1239 : }
1240 :
1241 : // calculate noise frequencies from FFT data provided by the HAL subsystem
1242 : // target_peak is the filtered record we want to apply the new fft data to, source peak is where the fft data is coming from
1243 : // called from FFT thread
1244 803749 : bool AP_GyroFFT::calculate_filtered_noise(FrequencyPeak target_peak, FrequencyPeak source_peak, const FrequencyData& freqs, const EngineConfig& config)
1245 : {
1246 803749 : if (source_peak > FrequencyPeak::MAX_TRACKED_PEAKS) {
1247 : // if we failed to find a signal, carry on using the previous readings
1248 0 : if (_missed_cycles[_update_axis][target_peak]++ < FFT_MAX_MISSED_UPDATES) {
1249 0 : return true; // the peak is synthetic
1250 : }
1251 0 : update_tl_center_freq_energy(target_peak, _update_axis, 0.0f);
1252 0 : update_tl_noise_center_bandwidth_hz(target_peak, _update_axis, _bandwidth_hover_hz);
1253 0 : update_tl_noise_center_freq_hz(target_peak, _update_axis, config._fft_min_hz);
1254 0 : return false;
1255 : }
1256 :
1257 803749 : AP_HAL::DSP::FrequencyPeakData* peak_data = &_state->_peak_data[source_peak];
1258 :
1259 803749 : const uint16_t nb = peak_data->_bin;
1260 :
1261 803749 : if (freqs.is_valid(FrequencyPeak(source_peak))) {
1262 : // total peak energy requires an integration, as an approximation use amplitude * noise width * 5/6
1263 601373 : update_tl_center_freq_energy(target_peak, _update_axis, _state->get_freq_bin(nb) * peak_data->_noise_width_hz * 0.8333f);
1264 601373 : update_tl_noise_center_bandwidth_hz(target_peak, _update_axis, peak_data->_noise_width_hz);
1265 601373 : update_tl_noise_center_freq_hz(target_peak, _update_axis, freqs.get_weighted_frequency(FrequencyPeak(source_peak)));
1266 601373 : _missed_cycles[_update_axis][target_peak] = 0;
1267 601373 : return true;
1268 : }
1269 :
1270 : // if we failed to find a signal, carry on using the previous readings
1271 202376 : if (_missed_cycles[_update_axis][target_peak]++ < FFT_MAX_MISSED_UPDATES) {
1272 48695 : return true; // the peak is synthetic
1273 : }
1274 :
1275 : // we failed to find a signal for more than FFT_MAX_MISSED_UPDATES cycles
1276 153681 : update_tl_center_freq_energy(target_peak, _update_axis, _state->get_freq_bin(nb) * peak_data->_noise_width_hz * 0.8333f); // use the actual energy detected rather than 0
1277 153681 : update_tl_noise_center_bandwidth_hz(target_peak, _update_axis, _bandwidth_hover_hz);
1278 153681 : update_tl_noise_center_freq_hz(target_peak, _update_axis, config._fft_min_hz);
1279 :
1280 153681 : return false;
1281 : }
1282 :
1283 279734 : void AP_GyroFFT::update_snr_values(const FrequencyData& freqs)
1284 : {
1285 279734 : _thread_state._center_freq_snr[FrequencyPeak::CENTER][_update_axis] = freqs.get_signal_to_noise(FrequencyPeak::CENTER);
1286 279734 : _thread_state._center_freq_snr[FrequencyPeak::LOWER_SHOULDER][_update_axis] = freqs.get_signal_to_noise(FrequencyPeak::LOWER_SHOULDER);
1287 279734 : _thread_state._center_freq_snr[FrequencyPeak::UPPER_SHOULDER][_update_axis] = freqs.get_signal_to_noise(FrequencyPeak::UPPER_SHOULDER);
1288 279734 : }
1289 :
1290 :
1291 : // filter values through a median sliding window followed by low pass filter
1292 : // this eliminates temporary spikes in the detected frequency that are either pure noise
1293 : // or a different peak that will erroneously bias the peak we are tracking
1294 2265162 : float AP_GyroFFT::MedianLowPassFilter3dFloat::apply(uint8_t axis, float sample)
1295 : {
1296 2265162 : _median_filter[axis].apply(sample);
1297 2265162 : const float a = _median_filter[axis].get_sample(0);
1298 2265162 : const float b = _median_filter[axis].get_sample(1);
1299 2265162 : const float c = _median_filter[axis].get_sample(2);
1300 2265162 : float median = MAX(MIN(a, b), MIN(MAX(a, b), c));
1301 2265162 : return _lowpass_filter[axis].apply(median);
1302 : }
1303 :
1304 : // initialize a FrequencyData structure with peak frequency information for use in the swapping algorithm
1305 279734 : AP_GyroFFT::FrequencyData::FrequencyData(const AP_GyroFFT& gyrofft, const EngineConfig& config)
1306 : {
1307 1118936 : for (uint8_t i = 0; i < FrequencyPeak::MAX_TRACKED_PEAKS; i++) {
1308 839202 : valid[i] = gyrofft.get_weighted_frequency(FrequencyPeak(i), frequency[i], snr[i], config);
1309 : }
1310 279734 : }
1311 :
1312 : // calculate noise frequencies from FFT data provided by the HAL subsystem
1313 839202 : bool AP_GyroFFT::get_weighted_frequency(FrequencyPeak peak, float& weighted_peak_freq_hz, float& snr, const EngineConfig& config) const
1314 : {
1315 839202 : AP_HAL::DSP::FrequencyPeakData* peak_data = &_state->_peak_data[peak];
1316 :
1317 839202 : const uint16_t bin = peak_data->_bin;
1318 :
1319 : // calculate the SNR and center frequency energy
1320 839202 : const float max_energy = MAX(1.0f, _state->get_freq_bin(bin));
1321 839202 : const float ref_energy = MAX(1.0f, _ref_energy[bin][_update_axis]);
1322 839202 : snr = 10.f * (log10f(max_energy) - log10f(ref_energy));
1323 :
1324 : // if the bin energy is above the noise threshold then we have a signal
1325 839202 : if (!_thread_state._noise_needs_calibration && isfinite(_state->get_freq_bin(bin)) && snr > config._snr_threshold_db) {
1326 632245 : weighted_peak_freq_hz = constrain_float(peak_data->_freq_hz, (float)config._fft_min_hz, (float)config._fft_max_hz);
1327 632245 : return true;
1328 : }
1329 :
1330 206957 : weighted_peak_freq_hz = (float)config._fft_min_hz;
1331 :
1332 206957 : return false;
1333 : }
1334 :
1335 : // calculate a matrix of distances between the current filtered estimates and instantaneous values from the current cycle
1336 279734 : void AP_GyroFFT::find_distance_matrix(DistanceMatrix& distance_matrix, const FrequencyData& freqs, const EngineConfig& config) const
1337 : {
1338 : float curr_freqs[FrequencyPeak::MAX_TRACKED_PEAKS];
1339 : // get the current frequency estimate for all peaks
1340 1118936 : for (uint8_t i = 0; i < FrequencyPeak::MAX_TRACKED_PEAKS; i++) {
1341 839202 : curr_freqs[i] = get_tl_noise_center_freq_hz(FrequencyPeak(i), _update_axis);
1342 : }
1343 : // calculate the matrix
1344 1118936 : for (uint8_t i = 0; i < FrequencyPeak::MAX_TRACKED_PEAKS; i++) {
1345 3356808 : for (uint8_t j = 0; j < FrequencyPeak::MAX_TRACKED_PEAKS; j++) {
1346 2517606 : distance_matrix[i][j] = fabsf((freqs.is_valid(FrequencyPeak(i)) ?
1347 2517606 : freqs.get_weighted_frequency(FrequencyPeak(i)) : FLT_MAX) - curr_freqs[j]);
1348 : }
1349 : }
1350 279734 : }
1351 :
1352 : // return the instantaneous peak that is closest to the target estimate peak
1353 839202 : AP_GyroFFT::FrequencyPeak AP_GyroFFT::find_closest_peak(const FrequencyPeak target, const DistanceMatrix& distance_matrix, uint8_t ignore) const
1354 : {
1355 : // find the closest peak to target
1356 839202 : uint8_t closest = target;
1357 3356808 : for (uint8_t i = 0; i < FrequencyPeak::MAX_TRACKED_PEAKS; i++) {
1358 2517606 : if (distance_matrix[i][target] < distance_matrix[closest][target] && (1 << i & ~ignore)) {
1359 111629 : closest = i;
1360 : }
1361 : }
1362 : // didn't find anything
1363 839202 : if (!(1<<closest & ~ignore)) {
1364 41347 : return FrequencyPeak::NONE;
1365 : }
1366 797855 : return FrequencyPeak(closest);
1367 : }
1368 :
1369 : // calculate noise baseline from FFT data provided by the HAL subsystem
1370 : // called from FFT thread
1371 278556 : void AP_GyroFFT::update_ref_energy(uint16_t max_bin)
1372 : {
1373 278556 : if (!_thread_state._noise_needs_calibration) {
1374 277980 : return;
1375 : }
1376 :
1377 : // according to https://www.tcd.ie/Physics/research/groups/magnetism/files/lectures/py5021/MagneticSensors3.pdf sensor noise is not necessarily gaussian
1378 : // determine a PS noise reference at each of the possible center frequencies
1379 576 : if (_noise_cycles == 0 && _noise_calibration_cycles[_update_axis] > 0) {
1380 15360 : for (uint16_t i = 1; i < _state->_bin_count; i++) {
1381 14976 : _ref_energy[i][_update_axis] += _state->get_freq_bin(i);
1382 : }
1383 384 : if (--_noise_calibration_cycles[_update_axis] == 0) {
1384 2208 : for (uint16_t i = 1; i < _state->_bin_count; i++) {
1385 2148 : const float cycles = (static_cast<float>(_window_size) / static_cast<float>(_samples_per_frame)) * 2;
1386 : // overall random noise is reduced by sqrt(N) when averaging periodigrams so adjust for that
1387 2148 : _ref_energy[i][_update_axis] = (_ref_energy[i][_update_axis] / cycles) * sqrtf(cycles);
1388 : }
1389 :
1390 60 : WITH_SEMAPHORE(_sem);
1391 60 : _thread_state._noise_needs_calibration &= ~(1 << _update_axis);
1392 60 : }
1393 384 : }
1394 192 : else if (_noise_cycles > 0) {
1395 192 : _noise_cycles--;
1396 : }
1397 : }
1398 :
1399 : // perform FFT analysis on the range of frequencies supported by the analyser
1400 : // called from main thread
1401 20 : float AP_GyroFFT::self_test_bin_frequencies()
1402 : {
1403 20 : if (_state->_window_size * sizeof(float) > hal.util->available_memory() / 2) {
1404 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "FFT: unable to run self-test, required %u bytes", (unsigned int)(_state->_window_size * sizeof(float)));
1405 0 : return 0.0f;
1406 : }
1407 :
1408 20 : FloatBuffer test_window(_state->_window_size);
1409 : // in the unlikely event we can't allocate a test window, skip the checks
1410 20 : if (test_window.get_size() == 0) {
1411 0 : return 0.0f;
1412 : }
1413 :
1414 20 : float max_divergence = 0;
1415 :
1416 609 : for (uint16_t bin = _config._fft_start_bin; bin <= _config._fft_end_bin; bin++) {
1417 : // the algorithm will only ever return values in this range
1418 589 : float frequency = constrain_float(bin * _state->_bin_resolution, _fft_min_hz, _fft_max_hz);
1419 589 : max_divergence = MAX(max_divergence, self_test(frequency, test_window)); // test bin centers
1420 589 : frequency = constrain_float(bin * _state->_bin_resolution - _state->_bin_resolution / 4, _fft_min_hz, _fft_max_hz);
1421 589 : max_divergence = MAX(max_divergence, self_test(frequency, test_window)); // test bin off-centers
1422 : }
1423 :
1424 20 : return max_divergence;
1425 20 : }
1426 :
1427 : // perform FFT analysis of a single sine wave at the selected frequency
1428 : // called from main thread
1429 1178 : float AP_GyroFFT::self_test(float frequency, FloatBuffer& test_window)
1430 : {
1431 1178 : test_window.clear();
1432 124954 : for(uint16_t i = 0; i < _state->_window_size; i++) {
1433 123776 : if (!test_window.push(sinf(2.0f * M_PI * frequency * i / _fft_sampling_rate_hz) * radians(20) * 2000)) {
1434 0 : AP_HAL::panic("Could not create FFT test window");
1435 : }
1436 : }
1437 :
1438 1178 : _update_axis = 0;
1439 :
1440 : // if using averaging we need to process _num_frames in order to not bias the result
1441 2690 : for (uint8_t i = 1; i < _num_frames; i++) {
1442 1512 : hal.dsp->fft_start(_state, test_window, 0);
1443 1512 : hal.dsp->fft_analyse(_state, _config._fft_start_bin, _config._fft_end_bin, _config._attenuation_cutoff);
1444 : }
1445 : // final cycle is the one we want
1446 1178 : hal.dsp->fft_start(_state, test_window, 0);
1447 1178 : uint16_t max_bin = hal.dsp->fft_analyse(_state, _config._fft_start_bin, _config._fft_end_bin, _config._attenuation_cutoff);
1448 :
1449 1178 : if (max_bin == 0) {
1450 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "FFT: self-test failed, failed to find frequency %.1f", frequency);
1451 : }
1452 :
1453 1178 : calculate_noise(true, _config);
1454 :
1455 1178 : float max_divergence = 0;
1456 : // make sure the selected frequencies are in the right bin
1457 1178 : max_divergence = MAX(max_divergence, fabsf(frequency - _thread_state._center_freq_hz[0]));
1458 1178 : if (_thread_state._center_freq_hz[0] < (frequency - MAX(_state->_bin_resolution * 0.5f, 1)) || _thread_state._center_freq_hz[0] > (frequency + MAX(_state->_bin_resolution * 0.5f, 1))) {
1459 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "FFT: self-test failed: wanted %.1f, had %.1f", frequency, _thread_state._center_freq_hz[0]);
1460 : }
1461 : #if DEBUG_FFT
1462 : else {
1463 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "FFT: self-test succeeded: wanted %.1f, had %.1f", frequency, _thread_state._center_freq_hz[0]);
1464 : }
1465 : #endif
1466 :
1467 1178 : return max_divergence;
1468 : }
1469 :
1470 : // singleton instance
1471 : AP_GyroFFT *AP_GyroFFT::_singleton;
1472 :
1473 : namespace AP {
1474 :
1475 50265 : AP_GyroFFT *fft()
1476 : {
1477 50265 : return AP_GyroFFT::get_singleton();
1478 : }
1479 :
1480 : }
1481 :
1482 : #endif // HAL_GYROFFT_ENABLED
|
