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 :
16 : #ifndef HAL_DEBUG_BUILD
17 : #define AP_INLINE_VECTOR_OPS
18 : #pragma GCC optimize("O2")
19 : #endif
20 :
21 : #include "HarmonicNotchFilter.h"
22 : #include <GCS_MAVLink/GCS.h>
23 : #include <AP_Logger/AP_Logger.h>
24 : #include <AP_Vehicle/AP_Vehicle_Type.h>
25 :
26 : /*
27 : optional logging for SITL only of all notch frequencies
28 : */
29 : #ifndef NOTCH_DEBUG_LOGGING
30 : #define NOTCH_DEBUG_LOGGING 0
31 : #endif
32 :
33 : #if NOTCH_DEBUG_LOGGING
34 : #include <fcntl.h>
35 : #include <sys/stat.h>
36 : #include <sys/types.h>
37 : #include <stdio.h>
38 : #endif
39 :
40 : /*
41 : cutoff proportion of sample rate above which we do not use the notch
42 : */
43 : #define HARMONIC_NYQUIST_CUTOFF 0.48f
44 :
45 : /*
46 : point at which the harmonic notch goes to zero attenuation
47 : */
48 : #define NOTCHFILTER_ATTENUATION_CUTOFF 0.25
49 :
50 : #if APM_BUILD_TYPE(APM_BUILD_Heli)
51 : // We cannot use throttle based notch on helis
52 : #define NOTCHFILTER_DEFAULT_MODE float(HarmonicNotchDynamicMode::Fixed) // fixed
53 : #else
54 : #define NOTCHFILTER_DEFAULT_MODE float(HarmonicNotchDynamicMode::UpdateThrottle) // throttle based
55 : #endif
56 :
57 :
58 : // table of user settable parameters
59 : const AP_Param::GroupInfo HarmonicNotchFilterParams::var_info[] = {
60 :
61 : // @Param: ENABLE
62 : // @DisplayName: Harmonic Notch Filter enable
63 : // @Description: Harmonic Notch Filter enable
64 : // @Values: 0:Disabled,1:Enabled
65 : // @User: Advanced
66 : AP_GROUPINFO_FLAGS("ENABLE", 1, HarmonicNotchFilterParams, _enable, 0, AP_PARAM_FLAG_ENABLE),
67 :
68 : // @Param: FREQ
69 : // @DisplayName: Harmonic Notch Filter base frequency
70 : // @Description: Harmonic Notch Filter base center frequency in Hz. This is the center frequency for static notches, the center frequency for Throttle based notches at the reference thrust value, and the minimum limit of center frequency variation for all other notch types. This should always be set lower than half the backend gyro rate (which is typically 1Khz).
71 : // @Range: 10 495
72 : // @Units: Hz
73 : // @User: Advanced
74 : AP_GROUPINFO("FREQ", 2, HarmonicNotchFilterParams, _center_freq_hz, 80),
75 :
76 : // @Param: BW
77 : // @DisplayName: Harmonic Notch Filter bandwidth
78 : // @Description: Harmonic Notch Filter bandwidth in Hz. This is typically set to half the base frequency. The ratio of base frequency to bandwidth determines the notch quality factor and is fixed across harmonics.
79 : // @Range: 5 250
80 : // @Units: Hz
81 : // @User: Advanced
82 : AP_GROUPINFO("BW", 3, HarmonicNotchFilterParams, _bandwidth_hz, 40),
83 :
84 : // @Param: ATT
85 : // @DisplayName: Harmonic Notch Filter attenuation
86 : // @Description: Harmonic Notch Filter attenuation in dB. Values greater than 40dB will typically produce a hard notch rather than a modest attenuation of motor noise.
87 : // @Range: 5 50
88 : // @Units: dB
89 : // @User: Advanced
90 : AP_GROUPINFO("ATT", 4, HarmonicNotchFilterParams, _attenuation_dB, 40),
91 :
92 : // @Param: HMNCS
93 : // @DisplayName: Harmonic Notch Filter harmonics
94 : // @Description: Bitmask of harmonic frequencies to apply Harmonic Notch Filter to. This option takes effect on the next reboot. A value of 0 disables this filter. The first harmonic refers to the base frequency.
95 : // @Bitmask: 0: 1st harmonic
96 : // @Bitmask: 1: 2nd harmonic
97 : // @Bitmask: 2: 3rd harmonic
98 : // @Bitmask: 3: 4th harmonic
99 : // @Bitmask: 4: 5th harmonic
100 : // @Bitmask: 5: 6th harmonic
101 : // @Bitmask: 6: 7th harmonic
102 : // @Bitmask: 7: 8th harmonic
103 : // @Bitmask: 8: 9th harmonic
104 : // @Bitmask: 9: 10th harmonic
105 : // @Bitmask: 10: 11th harmonic
106 : // @Bitmask: 11: 12th harmonic
107 : // @Bitmask: 12: 13th harmonic
108 : // @Bitmask: 13: 14th harmonic
109 : // @Bitmask: 14: 15th harmonic
110 : // @Bitmask: 15: 16th harmonic
111 : // @User: Advanced
112 : // @RebootRequired: True
113 : AP_GROUPINFO("HMNCS", 5, HarmonicNotchFilterParams, _harmonics, 3),
114 :
115 : // @Param: REF
116 : // @DisplayName: Harmonic Notch Filter reference value
117 : // @Description: A reference value of zero disables dynamic updates on the Harmonic Notch Filter and a positive value enables dynamic updates on the Harmonic Notch Filter. For throttle-based scaling, this parameter is the reference value associated with the specified frequency to facilitate frequency scaling of the Harmonic Notch Filter. For RPM and ESC telemetry based tracking, this parameter is set to 1 to enable the Harmonic Notch Filter using the RPM sensor or ESC telemetry set to measure rotor speed. The sensor data is converted to Hz automatically for use in the Harmonic Notch Filter. This reference value may also be used to scale the sensor data, if required. For example, rpm sensor data is required to measure heli motor RPM. Therefore the reference value can be used to scale the RPM sensor to the rotor RPM.
118 : // @User: Advanced
119 : // @Range: 0.0 1.0
120 : // @RebootRequired: True
121 : AP_GROUPINFO("REF", 6, HarmonicNotchFilterParams, _reference, 0),
122 :
123 : // @Param: MODE
124 : // @DisplayName: Harmonic Notch Filter dynamic frequency tracking mode
125 : // @Description: Harmonic Notch Filter dynamic frequency tracking mode. Dynamic updates can be throttle, RPM sensor, ESC telemetry or dynamic FFT based. Throttle-based harmonic notch cannot be used on fixed wing only planes. It can for Copters, QuadPlane(while in VTOL modes), and Rovers.
126 : // @Range: 0 5
127 : // @Values: 0:Fixed,1:Throttle,2:RPM Sensor,3:ESC Telemetry,4:Dynamic FFT,5:Second RPM Sensor
128 : // @User: Advanced
129 : AP_GROUPINFO("MODE", 7, HarmonicNotchFilterParams, _tracking_mode, NOTCHFILTER_DEFAULT_MODE),
130 :
131 : // @Param: OPTS
132 : // @DisplayName: Harmonic Notch Filter options
133 : // @Description: Harmonic Notch Filter options. Triple and double-notches can provide deeper attenuation across a wider bandwidth with reduced latency than single notches and are suitable for larger aircraft. Multi-Source attaches a harmonic notch to each detected noise frequency instead of simply being multiples of the base frequency, in the case of FFT it will attach notches to each of three detected noise peaks, in the case of ESC it will attach notches to each of four motor RPM values. Loop rate update changes the notch center frequency at the scheduler loop rate rather than at the default of 200Hz. If both double and triple notches are specified only double notches will take effect.
134 : // @Bitmask: 0:Double notch,1:Multi-Source,2:Update at loop rate,3:EnableOnAllIMUs,4:Triple notch, 5:Use min freq on RPM source failure, 6:Quintuple notch
135 : // @User: Advanced
136 : // @RebootRequired: True
137 : AP_GROUPINFO("OPTS", 8, HarmonicNotchFilterParams, _options, 0),
138 :
139 : // @Param: FM_RAT
140 : // @DisplayName: Throttle notch min frequency ratio
141 : // @Description: The minimum ratio below the configured frequency to take throttle based notch filters when flying at a throttle level below the reference throttle. Note that lower frequency notch filters will have more phase lag. If you want throttle based notch filtering to be effective at a throttle up to 30% below the configured notch frequency then set this parameter to 0.7. The default of 1.0 means the notch will not go below the frequency in the FREQ parameter.
142 : // @Range: 0.1 1.0
143 : // @User: Advanced
144 : AP_GROUPINFO("FM_RAT", 9, HarmonicNotchFilterParams, _freq_min_ratio, 1.0),
145 :
146 : AP_GROUPEND
147 : };
148 :
149 : /*
150 : destroy all of the associated notch filters
151 : */
152 : template <class T>
153 31324 : HarmonicNotchFilter<T>::~HarmonicNotchFilter() {
154 31324 : delete[] _filters;
155 31324 : _num_filters = 0;
156 31324 : _num_enabled_filters = 0;
157 31324 : }
158 :
159 : /*
160 : initialise the associated filters with the provided shaping constraints
161 : the constraints are used to determine attenuation (A) and quality (Q) factors for the filter
162 : */
163 : template <class T>
164 281129 : void HarmonicNotchFilter<T>::init(float sample_freq_hz, HarmonicNotchFilterParams &_params)
165 : {
166 : // keep a copy of the params object
167 281129 : params = &_params;
168 :
169 : // sanity check the input
170 281129 : if (_filters == nullptr || is_zero(sample_freq_hz) || isnan(sample_freq_hz)) {
171 10740 : return;
172 : }
173 :
174 270387 : _sample_freq_hz = sample_freq_hz;
175 :
176 270387 : const float bandwidth_hz = params->bandwidth_hz();
177 270385 : const float attenuation_dB = params->attenuation_dB();
178 270388 : float center_freq_hz = params->center_freq_hz();
179 :
180 270389 : const float nyquist_limit = sample_freq_hz * HARMONIC_NYQUIST_CUTOFF;
181 270389 : const float bandwidth_limit = bandwidth_hz * 0.52f;
182 :
183 : // remember the lowest frequency we will have a notch enabled at
184 270389 : _minimum_freq = center_freq_hz * params->freq_min_ratio();
185 :
186 : /*
187 : adjust the fundamental center frequency we use for the initial
188 : calculation of A and Q to be in the allowable range
189 : */
190 270387 : center_freq_hz = constrain_float(center_freq_hz, bandwidth_limit, nyquist_limit);
191 :
192 : // Calculate spread required to achieve an equivalent single notch using two notches with Bandwidth/2
193 270385 : _notch_spread = bandwidth_hz / (32 * center_freq_hz);
194 :
195 : // position the individual notches so that the attenuation is no worse than a single notch
196 : // calculate attenuation and quality from the shaping constraints
197 270385 : NotchFilter<T>::calculate_A_and_Q(center_freq_hz, bandwidth_hz / _composite_notches, attenuation_dB, _A, _Q);
198 :
199 270383 : _initialised = true;
200 :
201 : // ensure static notches are allocated and working
202 270383 : if (params->tracking_mode() == HarmonicNotchDynamicMode::Fixed) {
203 28706 : update(center_freq_hz);
204 : }
205 : }
206 :
207 : /*
208 : calculate the number of notch filters needed for the given config,
209 : clamped to HAL_HNF_MAX_FILTERS and rounded down to a whole number of composite
210 : groups so update() never writes past the end of the filter array
211 : */
212 : template <class T>
213 1241942825 : uint16_t HarmonicNotchFilter<T>::notch_count(uint8_t num_sources, uint8_t num_harmonics, uint8_t composite_notches) const
214 : {
215 1241942825 : if (num_sources == 0 || num_harmonics == 0 || composite_notches == 0) {
216 0 : return 0;
217 : }
218 :
219 : // This relies on integer division to round the result down
220 1241942825 : const uint16_t max_notches = HAL_HNF_MAX_FILTERS / composite_notches;
221 1241942825 : const uint16_t notches = MIN(num_sources * num_harmonics, max_notches);
222 1241942825 : return notches * composite_notches;
223 : }
224 :
225 : /*
226 : allocate a collection of, at most HAL_HNF_MAX_FILTERS, notch filters to be managed by this harmonic notch filter
227 : */
228 : template <class T>
229 27350 : void HarmonicNotchFilter<T>::allocate_filters(uint8_t num_notches, uint32_t harmonics, uint8_t composite_notches)
230 : {
231 27350 : _composite_notches = composite_notches;
232 27350 : _num_harmonics = __builtin_popcount(harmonics);
233 27350 : _num_filters = notch_count(num_notches, _num_harmonics, _composite_notches);
234 27350 : _harmonics = harmonics;
235 :
236 27350 : if (_num_filters > 0) {
237 27962 : _filters = NEW_NOTHROW NotchFilter<T>[_num_filters];
238 27350 : if (_filters == nullptr) {
239 0 : GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Failed to allocate %u bytes for notch filter", (unsigned int)(_num_filters * sizeof(NotchFilter<T>)));
240 0 : _num_filters = 0;
241 : }
242 : }
243 27350 : }
244 :
245 : /*
246 : expand the number of filters at runtime, allowing for RPM sources such as lua scripts
247 : */
248 : template <class T>
249 12 : void HarmonicNotchFilter<T>::expand_filter_count(uint16_t total_notches)
250 : {
251 12 : if (total_notches <= _num_filters) {
252 : // enough already
253 0 : return;
254 : }
255 12 : if (_alloc_has_failed) {
256 : // we've failed to allocate before, don't try again
257 0 : return;
258 : }
259 : /*
260 : note that we rely on the semaphore in
261 : AP_InertialSensor_Backend.cpp to make this thread safe
262 : */
263 108 : auto filters = NEW_NOTHROW NotchFilter<T>[total_notches];
264 12 : if (filters == nullptr) {
265 0 : _alloc_has_failed = true;
266 0 : return;
267 : }
268 12 : memcpy(filters, _filters, sizeof(filters[0])*_num_filters);
269 12 : auto _old_filters = _filters;
270 12 : _filters = filters;
271 12 : _num_filters = total_notches;
272 12 : delete[] _old_filters;
273 : }
274 :
275 : /*
276 : set the center frequency of a single notch harmonic
277 :
278 : The spread_mul is the frequency multiplier from the spread of the
279 : double or triple notch. The harmonic_mul is the multiplier for the
280 : frequency for this harmonic
281 : */
282 : template <class T>
283 4356698760 : void HarmonicNotchFilter<T>::set_center_frequency(uint16_t idx, float notch_center, float spread_mul, uint8_t harmonic_mul)
284 : {
285 4356698760 : const float nyquist_limit = _sample_freq_hz * HARMONIC_NYQUIST_CUTOFF;
286 4356698760 : auto ¬ch = _filters[idx];
287 :
288 : // scale the notch with the harmonic multiplier
289 4356698760 : notch_center *= harmonic_mul;
290 :
291 : /* disable the filter if its center frequency is above the nyquist
292 : frequency
293 :
294 : NOTE: should this be notch_center*spread_mul ? As it is here we
295 : do sometimes run the upper notch in a double or triple notch at
296 : higher than the nyquist.
297 : */
298 4356698760 : if (notch_center >= nyquist_limit) {
299 3377966 : notch.disable();
300 510241330 : return;
301 : }
302 :
303 : // the minimum frequency for a harmonic is the base minimum
304 4353320794 : float harmonic_min_freq = _minimum_freq;
305 :
306 : // we can adjust the attenuation at low frequencies
307 4353320794 : float A = _A;
308 :
309 : // on some vehicles we want to treat zero or very low frequency as
310 : // the minimum frequency, on others we want to disable the
311 : // notch for low frequencies
312 4353320794 : const bool treat_low_freq_as_min = params->hasOption(HarmonicNotchFilterParams::Options::TreatLowAsMin);
313 :
314 4353320864 : if (treat_low_freq_as_min) {
315 : /*
316 : when we are treating low as min we don't want to collapse
317 : the harmonics down to low frequencies
318 : */
319 100021200 : harmonic_min_freq *= harmonic_mul;
320 : } else {
321 : /*
322 : disable if the notch is less than 25% of the min frequency
323 : */
324 4253299664 : const float disable_freq = harmonic_min_freq * NOTCHFILTER_ATTENUATION_CUTOFF;
325 4253299664 : if (notch_center < disable_freq) {
326 506863360 : notch.disable();
327 506863368 : return;
328 : }
329 :
330 3746436304 : if (notch_center < harmonic_min_freq) {
331 : /*
332 : scale the attenuation so that we fade out the notch as
333 : we get further below the min frequency. The attenuation
334 : factor A goes to 1.0 (which means no attenuation)
335 : Scaling the attenuation in this way means we don't get a
336 : glitch at the disable point
337 : */
338 942446316 : A = linear_interpolate(A, 1.0, notch_center, harmonic_min_freq, disable_freq);
339 : }
340 : }
341 :
342 : // don't let the notch go below the min frequency
343 3846457504 : notch_center = MAX(notch_center, harmonic_min_freq);
344 :
345 : /* adjust notch center for spread for double and triple notch.
346 : This adjustment is applied last to maintain the symmetry of the
347 : double and triple notches
348 : */
349 3846457514 : notch_center *= spread_mul;
350 :
351 3846457514 : notch.init_with_A_and_Q(_sample_freq_hz, notch_center, A, _Q);
352 : }
353 :
354 : /*
355 : update the underlying filters' center frequency using the current attenuation and quality
356 : this function is cheaper than init() because A & Q do not need to be recalculated
357 : */
358 : template <class T>
359 1240930558 : void HarmonicNotchFilter<T>::update(float center_freq_hz)
360 : {
361 1240930558 : update(1, ¢er_freq_hz);
362 1240930566 : }
363 :
364 : /*
365 : update the underlying filters' center frequency using the current attenuation and quality
366 : this function is cheaper than init() because A & Q do not need to be recalculated
367 : */
368 : template <class T>
369 1241928947 : void HarmonicNotchFilter<T>::update(uint8_t num_centers, const float center_freq_hz[])
370 : {
371 1241928947 : if (!_initialised) {
372 13472 : return;
373 : }
374 :
375 : // adjust the frequencies to be in the allowable range
376 1241915475 : const float nyquist_limit = _sample_freq_hz * HARMONIC_NYQUIST_CUTOFF;
377 :
378 1241915475 : const uint16_t total_notches = notch_count(num_centers, _num_harmonics, _composite_notches);
379 1241915475 : if (total_notches > _num_filters) {
380 : // alloc realloc of filters
381 12 : expand_filter_count(total_notches);
382 : }
383 :
384 1241915471 : _num_enabled_filters = 0;
385 :
386 : // update all of the filters using the new center frequencies and existing A & Q
387 4795758480 : for (uint16_t i = 0; i < num_centers * HNF_MAX_HARMONICS && _num_enabled_filters < _num_filters; i++) {
388 3553843162 : const uint8_t harmonic_n = i / num_centers;
389 3553843162 : const uint8_t center_n = i % num_centers;
390 : // the filters are ordered by center and then harmonic so
391 : // f1h1, f2h1, f3h1, f4h1, f1h2, f2h2, etc
392 3553843162 : if (!((1U<<harmonic_n) & _harmonics)) {
393 4930769 : continue;
394 : }
395 :
396 3548912393 : const float notch_center = constrain_float(center_freq_hz[center_n], 0.0f, nyquist_limit);
397 3548912392 : const uint8_t harmonic_mul = (harmonic_n+1);
398 3548912392 : if (_composite_notches != 2) {
399 3547912164 : set_center_frequency(_num_enabled_filters++, notch_center, 1.0, harmonic_mul);
400 : }
401 3548912379 : if (_composite_notches > 1) {
402 403779842 : set_center_frequency(_num_enabled_filters++, notch_center, 1.0 - _notch_spread, harmonic_mul);
403 403779842 : set_center_frequency(_num_enabled_filters++, notch_center, 1.0 + _notch_spread, harmonic_mul);
404 : }
405 3548912360 : if (_composite_notches > 3) {
406 613474 : set_center_frequency(_num_enabled_filters++, notch_center, 1.0 - 2 * _notch_spread, harmonic_mul);
407 613474 : set_center_frequency(_num_enabled_filters++, notch_center, 1.0 + 2 * _notch_spread, harmonic_mul);
408 : }
409 : }
410 : }
411 :
412 : /*
413 : apply a sample to each of the underlying filters in turn and return the output
414 : */
415 : template <class T>
416 1362311788 : T HarmonicNotchFilter<T>::apply(const T &sample)
417 : {
418 1362311788 : if (!_initialised) {
419 25677 : return sample;
420 : }
421 :
422 : #if NOTCH_DEBUG_LOGGING
423 : static int dfd = -1;
424 : if (dfd == -1) {
425 : dfd = ::open("notch.txt", O_WRONLY|O_CREAT|O_TRUNC, 0644);
426 : }
427 : #endif
428 :
429 1362286111 : T output = sample;
430 6681970785 : for (uint16_t i = 0; i < _num_enabled_filters; i++) {
431 : #if NOTCH_DEBUG_LOGGING
432 : if (!_filters[i].initialised) {
433 : ::dprintf(dfd, "------- ");
434 : } else {
435 : ::dprintf(dfd, "%.4f ", _filters[i]._center_freq_hz);
436 : }
437 : #endif
438 5319684674 : output = _filters[i].apply(output);
439 : }
440 : #if NOTCH_DEBUG_LOGGING
441 : if (_num_enabled_filters > 0) {
442 : ::dprintf(dfd, "\n");
443 : }
444 : #endif
445 1362286111 : return output;
446 : }
447 :
448 : /*
449 : reset all of the underlying filters
450 : */
451 : template <class T>
452 1997829 : void HarmonicNotchFilter<T>::reset()
453 : {
454 1997829 : if (!_initialised) {
455 19515 : return;
456 : }
457 :
458 19259517 : for (uint16_t i = 0; i < _num_filters; i++) {
459 17281203 : _filters[i].reset();
460 : }
461 : }
462 :
463 : #if HAL_LOGGING_ENABLED
464 : // @LoggerMessage: FCN
465 : // @Description: Filter Center Message - per motor
466 : // @Field: TimeUS: microseconds since system startup
467 : // @Field: I: instance
468 : // @Field: NF: total number of active harmonic notches
469 : // @Field: CF1: First harmonic centre frequency for motor 1
470 : // @Field: CF2: First harmonic centre frequency for motor 2
471 : // @Field: CF3: First harmonic centre frequency for motor 3
472 : // @Field: CF4: First harmonic centre frequency for motor 4
473 : // @Field: CF5: First harmonic centre frequency for motor 5
474 : // @Field: CF6: First harmonic centre frequency for motor 6
475 : // @Field: HF1: Second harmonic centre frequency for motor 1
476 : // @Field: HF2: Second harmonic centre frequency for motor 2
477 : // @Field: HF3: Second harmonic centre frequency for motor 3
478 : // @Field: HF4: Second harmonic centre frequency for motor 4
479 : // @Field: HF5: Second harmonic centre frequency for motor 5
480 : // @Field: HF6: Second harmonic centre frequency for motor 6
481 :
482 : // @LoggerMessage: FCNS
483 : // @Description: Filter Center Message
484 : // @Field: TimeUS: microseconds since system startup
485 : // @Field: I: instance
486 : // @Field: CF: notch centre frequency
487 : // @Field: HF: 2nd harmonic frequency
488 :
489 : /*
490 : log center frequencies of 1st and 2nd harmonic of a harmonic notch
491 : instance for up to 6 frequency sources
492 :
493 : the instance number passed in corresponds to the harmonic notch
494 : instance in AP_InertialSensor
495 : */
496 : template <class T>
497 23862 : void HarmonicNotchFilter<T>::log_notch_centers(uint8_t instance, uint64_t now_us) const
498 : {
499 : /*
500 : for composite notches we only log the first entry. For triple
501 : and single notch this is the center. For double notch it is the
502 : lower frequency
503 : */
504 23862 : const uint16_t filters_per_source = _composite_notches * _num_harmonics;
505 23862 : if (_num_filters == 0 || filters_per_source == 0) {
506 46 : return;
507 : }
508 23816 : const uint8_t num_sources = MIN(6, _num_filters / filters_per_source);
509 23816 : float centers[6] {};
510 23816 : float first_harmonic[6] {};
511 :
512 : // filters are ordered: f1h1, f2h1, ..., fNh1, f1h2, f2h2, ..., fNh2, ...
513 23816 : const uint16_t second_harmonic_offset = num_sources * _composite_notches;
514 79624 : for (uint8_t i=0; i<num_sources; i++) {
515 55808 : centers[i] = _filters[i*_composite_notches].logging_frequency();
516 55808 : const uint16_t h2_idx = second_harmonic_offset + i*_composite_notches;
517 55808 : if (h2_idx < _num_filters) {
518 34068 : first_harmonic[i] = _filters[h2_idx].logging_frequency();
519 : }
520 : }
521 :
522 23816 : if (num_sources > 1) {
523 10942 : AP::logger().WriteStreaming(
524 : "FCN", "TimeUS,I,NF,CF1,CF2,CF3,CF4,CF5,CF6,HF1,HF2,HF3,HF4,HF5,HF6", "s#-zzzzzzzzzzzz", "F--------------", "QBHffffffffffff",
525 : now_us,
526 : instance,
527 10942 : _num_filters,
528 10942 : centers[0], centers[1], centers[2], centers[3], centers[4], centers[5],
529 10942 : first_harmonic[0], first_harmonic[1], first_harmonic[2], first_harmonic[3], first_harmonic[4], first_harmonic[5]);
530 : } else {
531 : // log single center frequency
532 12874 : AP::logger().WriteStreaming(
533 : "FCNS", "TimeUS,I,CF,HF", "s#zz", "F---", "QBff",
534 : now_us,
535 : instance,
536 12874 : centers[0],
537 12874 : first_harmonic[0]);
538 : }
539 : }
540 : #endif // HAL_LOGGING_ENABLED
541 :
542 : /*
543 : create parameters for the harmonic notch filter and initialise defaults
544 : */
545 33496 : HarmonicNotchFilterParams::HarmonicNotchFilterParams(void)
546 : {
547 33496 : AP_Param::setup_object_defaults(this, var_info);
548 33496 : }
549 :
550 6264 : void HarmonicNotchFilterParams::init()
551 : {
552 : // PARAMETER_CONVERSION - Added: Apr-2024 for ArduPilot-4.5
553 6264 : _harmonics.convert_bitmask_parameter_width(AP_PARAM_INT8);
554 6264 : }
555 :
556 : /*
557 : save changed parameters
558 : */
559 2 : void HarmonicNotchFilterParams::save_params()
560 : {
561 2 : _enable.save();
562 2 : _center_freq_hz.save();
563 2 : _bandwidth_hz.save();
564 2 : _attenuation_dB.save();
565 2 : _harmonics.save();
566 2 : _reference.save();
567 2 : _freq_min_ratio.save();
568 2 : }
569 :
570 :
571 : // return the number of composite notches given the options
572 27424 : uint8_t HarmonicNotchFilterParams::num_composite_notches(void) const
573 : {
574 27424 : if (hasOption(Options::DoubleNotch)) {
575 2409 : return 2;
576 : }
577 25015 : if (hasOption(Options::TripleNotch)) {
578 4012 : return 3;
579 : }
580 21003 : if (hasOption(Options::QuintupleNotch)) {
581 5 : return 5;
582 : }
583 20998 : return 1;
584 : }
585 :
586 :
587 : /*
588 : instantiate template classes
589 : */
590 : template class HarmonicNotchFilter<Vector3f>;
591 : template class HarmonicNotchFilter<float>;
592 :
|
