diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index b8262548e9676..c2c0cb46a3117 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -986,9 +986,6 @@ AP_InertialSensor::init(uint16_t loop_rate) if (!notch.params.enabled() && !fft_enabled) { continue; } - for (uint8_t i = 0; i < ARRAY_SIZE(notch.calculated_notch_freq_hz); i++) { - notch.calculated_notch_freq_hz[i] = notch.params.center_freq_hz(); - } notch.num_calculated_notch_frequencies = 1; notch.num_dynamic_notches = 1; #if APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_ArduPlane) @@ -1015,15 +1012,13 @@ AP_InertialSensor::init(uint16_t loop_rate) sensors_used += _use(i); } - uint8_t num_filters = 0; + uint16_t num_filters = 0; for (auto ¬ch : harmonic_notches) { // calculate number of notches we might want to use for harmonic notch if (notch.params.enabled() || fft_enabled) { - const bool double_notch = notch.params.hasOption(HarmonicNotchFilterParams::Options::DoubleNotch); - const bool triple_notch = notch.params.hasOption(HarmonicNotchFilterParams::Options::TripleNotch); const bool all_sensors = notch.params.hasOption(HarmonicNotchFilterParams::Options::EnableOnAllIMUs); num_filters += __builtin_popcount(notch.params.harmonics()) - * notch.num_dynamic_notches * (double_notch ? 2 : triple_notch ? 3 : 1) + * notch.num_dynamic_notches * notch.params.num_composite_notches() * (all_sensors?sensors_used:1); } } @@ -1038,13 +1033,10 @@ AP_InertialSensor::init(uint16_t loop_rate) if (_use(i)) { for (auto ¬ch : harmonic_notches) { if (notch.params.enabled() || fft_enabled) { - const bool double_notch = notch.params.hasOption(HarmonicNotchFilterParams::Options::DoubleNotch); - const bool triple_notch = notch.params.hasOption(HarmonicNotchFilterParams::Options::TripleNotch); notch.filter[i].allocate_filters(notch.num_dynamic_notches, - notch.params.harmonics(), double_notch ? 2 : triple_notch ? 3 : 1); + notch.params.harmonics(), notch.params.num_composite_notches()); // initialise default settings, these will be subsequently changed in AP_InertialSensor_Backend::update_gyro() - notch.filter[i].init(_gyro_raw_sample_rates[i], notch.calculated_notch_freq_hz[0], - notch.params.bandwidth_hz(), notch.params.attenuation_dB()); + notch.filter[i].init(_gyro_raw_sample_rates[i], notch.params); } } } @@ -1835,25 +1827,21 @@ void AP_InertialSensor::_save_gyro_calibration() */ void AP_InertialSensor::HarmonicNotch::update_params(uint8_t instance, bool converging, float gyro_rate) { - const float center_freq = calculated_notch_freq_hz[0]; if (!is_equal(last_bandwidth_hz[instance], params.bandwidth_hz()) || !is_equal(last_attenuation_dB[instance], params.attenuation_dB()) || - (params.tracking_mode() == HarmonicNotchDynamicMode::Fixed && !is_equal(last_center_freq_hz[instance], center_freq)) || + (params.tracking_mode() == HarmonicNotchDynamicMode::Fixed && + !is_equal(last_center_freq_hz[instance], params.center_freq_hz())) || converging) { - filter[instance].init(gyro_rate, - center_freq, - params.bandwidth_hz(), - params.attenuation_dB()); - last_center_freq_hz[instance] = center_freq; + filter[instance].init(gyro_rate, params); + last_center_freq_hz[instance] = params.center_freq_hz(); last_bandwidth_hz[instance] = params.bandwidth_hz(); last_attenuation_dB[instance] = params.attenuation_dB(); } else if (params.tracking_mode() != HarmonicNotchDynamicMode::Fixed) { if (num_calculated_notch_frequencies > 1) { filter[instance].update(num_calculated_notch_frequencies, calculated_notch_freq_hz); } else { - filter[instance].update(center_freq); + filter[instance].update(calculated_notch_freq_hz[0]); } - last_center_freq_hz[instance] = center_freq; } } @@ -2297,23 +2285,23 @@ void AP_InertialSensor::acal_update() } #endif -// Update the harmonic notch frequency +/* + Update the harmonic notch frequency + + Note that zero is a valid value and will disable the notch unless + the TreatLowAsMin option is set +*/ void AP_InertialSensor::HarmonicNotch::update_freq_hz(float scaled_freq) { - // protect against zero as the scaled frequency - if (is_positive(scaled_freq)) { - calculated_notch_freq_hz[0] = scaled_freq; - } + calculated_notch_freq_hz[0] = scaled_freq; num_calculated_notch_frequencies = 1; } // Update the harmonic notch frequency void AP_InertialSensor::HarmonicNotch::update_frequencies_hz(uint8_t num_freqs, const float scaled_freq[]) { - // protect against zero as the scaled frequency + // note that we allow zero through, which will disable the notch for (uint8_t i = 0; i < num_freqs; i++) { - if (is_positive(scaled_freq[i])) { - calculated_notch_freq_hz[i] = scaled_freq[i]; - } + calculated_notch_freq_hz[i] = scaled_freq[i]; } // any uncalculated frequencies will float at the previous value or the initialized freq if none num_calculated_notch_frequencies = num_freqs; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp index 2318766a6720a..5c640a9f3096a 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp @@ -144,28 +144,29 @@ bool AP_InertialSensor::BatchSampler::Write_ISBD() const // @Description: Filter Tuning Message - per motor // @Field: TimeUS: microseconds since system startup // @Field: I: instance -// @Field: NDn: number of active dynamic harmonic notches -// @Field: NF1: dynamic harmonic notch centre frequency for motor 1 -// @Field: NF2: dynamic harmonic notch centre frequency for motor 2 -// @Field: NF3: dynamic harmonic notch centre frequency for motor 3 -// @Field: NF4: dynamic harmonic notch centre frequency for motor 4 -// @Field: NF5: dynamic harmonic notch centre frequency for motor 5 -// @Field: NF6: dynamic harmonic notch centre frequency for motor 6 -// @Field: NF7: dynamic harmonic notch centre frequency for motor 7 -// @Field: NF8: dynamic harmonic notch centre frequency for motor 8 -// @Field: NF9: dynamic harmonic notch centre frequency for motor 9 -// @Field: NF10: dynamic harmonic notch centre frequency for motor 10 -// @Field: NF11: dynamic harmonic notch centre frequency for motor 11 -// @Field: NF12: dynamic harmonic notch centre frequency for motor 12 +// @Field: NDn: number of active harmonic notches +// @Field: NF1: desired harmonic notch centre frequency for motor 1 +// @Field: NF2: desired harmonic notch centre frequency for motor 2 +// @Field: NF3: desired harmonic notch centre frequency for motor 3 +// @Field: NF4: desired harmonic notch centre frequency for motor 4 +// @Field: NF5: desired harmonic notch centre frequency for motor 5 +// @Field: NF6: desired harmonic notch centre frequency for motor 6 +// @Field: NF7: desired harmonic notch centre frequency for motor 7 +// @Field: NF8: desired harmonic notch centre frequency for motor 8 +// @Field: NF9: desired harmonic notch centre frequency for motor 9 +// @Field: NF10: desired harmonic notch centre frequency for motor 10 +// @Field: NF11: desired harmonic notch centre frequency for motor 11 +// @Field: NF12: desired harmonic notch centre frequency for motor 12 // @LoggerMessage: FTNS // @Description: Filter Tuning Message // @Field: TimeUS: microseconds since system startup // @Field: I: instance -// @Field: NF: dynamic harmonic notch centre frequency +// @Field: NF: desired harmonic notch centre frequency void AP_InertialSensor::write_notch_log_messages() const { + const uint64_t now_us = AP_HAL::micros64(); for (auto ¬ch : harmonic_notches) { const uint8_t i = ¬ch - &harmonic_notches[0]; if (!notch.params.enabled()) { @@ -176,7 +177,7 @@ void AP_InertialSensor::write_notch_log_messages() const // log per motor center frequencies AP::logger().WriteStreaming( "FTN", "TimeUS,I,NDn,NF1,NF2,NF3,NF4,NF5,NF6,NF7,NF8,NF9,NF10,NF11,NF12", "s#-zzzzzzzzzzzz", "F--------------", "QBBffffffffffff", - AP_HAL::micros64(), + now_us, i, notch.num_calculated_notch_frequencies, notches[0], notches[1], notches[2], notches[3], @@ -186,10 +187,15 @@ void AP_InertialSensor::write_notch_log_messages() const // log single center frequency AP::logger().WriteStreaming( "FTNS", "TimeUS,I,NF", "s#z", "F--", "QBf", - AP_HAL::micros64(), + now_us, i, notches[0]); } + + // ask the HarmonicNotchFilter object for primary gyro to + // log the actual notch centers + const uint8_t primary_gyro = AP::ahrs().get_primary_gyro_index(); + notch.filter[primary_gyro].log_notch_centers(i, now_us); } } diff --git a/libraries/AP_Logger/AP_Logger_Backend.cpp b/libraries/AP_Logger/AP_Logger_Backend.cpp index 8d45d8187e78c..9d0ce4ad09951 100644 --- a/libraries/AP_Logger/AP_Logger_Backend.cpp +++ b/libraries/AP_Logger/AP_Logger_Backend.cpp @@ -11,6 +11,7 @@ #include #include #include +#include #include "AP_Logger.h" #if HAL_LOGGER_FENCE_ENABLED @@ -574,6 +575,7 @@ bool AP_Logger_Backend::Write_VER() patch: fwver.patch, fw_type: fwver.fw_type, git_hash: fwver.fw_hash, + filter_version: AP_FILTER_VERSION, }; strncpy(pkt.fw_string, fwver.fw_string, ARRAY_SIZE(pkt.fw_string)-1); diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h index 6d4542540a61e..9e86ea834da81 100644 --- a/libraries/AP_Logger/LogStructure.h +++ b/libraries/AP_Logger/LogStructure.h @@ -660,6 +660,7 @@ struct PACKED log_VER { char fw_string[64]; uint16_t _APJ_BOARD_ID; uint8_t build_type; + uint8_t filter_version; }; @@ -1170,6 +1171,7 @@ struct PACKED log_VER { // @Field: FWS: Firmware version string // @Field: APJ: Board ID // @Field: BU: Build vehicle type +// @Field: FV: Filter version // @LoggerMessage: MOTB // @Description: Motor mixer information @@ -1300,7 +1302,7 @@ LOG_STRUCTURE_FROM_AIS \ { LOG_SCRIPTING_MSG, sizeof(log_Scripting), \ "SCR", "QNIii", "TimeUS,Name,Runtime,Total_mem,Run_mem", "s#sbb", "F-F--", true }, \ { LOG_VER_MSG, sizeof(log_VER), \ - "VER", "QBHBBBBIZHB", "TimeUS,BT,BST,Maj,Min,Pat,FWT,GH,FWS,APJ,BU", "s----------", "F----------", false }, \ + "VER", "QBHBBBBIZHBB", "TimeUS,BT,BST,Maj,Min,Pat,FWT,GH,FWS,APJ,BU,FV", "s-----------", "F-----------", false }, \ { LOG_MOTBATT_MSG, sizeof(log_MotBatt), \ "MOTB", "QfffffB", "TimeUS,LiftMax,BatVolt,ThLimit,ThrAvMx,ThrOut,FailFlags", "s------", "F------" , true } diff --git a/libraries/AP_Vehicle/AP_Vehicle.cpp b/libraries/AP_Vehicle/AP_Vehicle.cpp index ff42ea97a108b..ba72bf74c32f0 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.cpp +++ b/libraries/AP_Vehicle/AP_Vehicle.cpp @@ -750,7 +750,6 @@ void AP_Vehicle::update_throttle_notch(AP_InertialSensor::HarmonicNotch ¬ch) #if APM_BUILD_TYPE(APM_BUILD_ArduPlane)||APM_BUILD_COPTER_OR_HELI||APM_BUILD_TYPE(APM_BUILD_Rover) const float ref_freq = notch.params.center_freq_hz(); const float ref = notch.params.reference(); - const float min_ratio = notch.params.freq_min_ratio(); #if APM_BUILD_TYPE(APM_BUILD_ArduPlane)||APM_BUILD_COPTER_OR_HELI const AP_Motors* motors = AP::motors(); @@ -760,7 +759,7 @@ void AP_Vehicle::update_throttle_notch(AP_InertialSensor::HarmonicNotch ¬ch) const float motors_throttle = motors != nullptr ? abs(motors->get_throttle() / 100.0f) : 0; #endif - float throttle_freq = ref_freq * MAX(min_ratio, sqrtf(motors_throttle / ref)); + float throttle_freq = ref_freq * sqrtf(MAX(0,motors_throttle) / ref); notch.update_freq_hz(throttle_freq); #endif @@ -780,17 +779,6 @@ void AP_Vehicle::update_dynamic_notch(AP_InertialSensor::HarmonicNotch ¬ch) return; } -#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)||APM_BUILD_COPTER_OR_HELI - const AP_Motors* motors = AP::motors(); - if (motors != nullptr && motors->get_spool_state() == AP_Motors::SpoolState::SHUT_DOWN) { - notch.set_inactive(true); - } else { - notch.set_inactive(false); - } -#else // APM_BUILD_Rover: keep notch active - notch.set_inactive(false); -#endif - switch (notch.params.tracking_mode()) { case HarmonicNotchDynamicMode::UpdateThrottle: // throttle based tracking // set the harmonic notch filter frequency approximately scaled on motor rpm implied by throttle @@ -805,9 +793,9 @@ void AP_Vehicle::update_dynamic_notch(AP_InertialSensor::HarmonicNotch ¬ch) float rpm; if (rpm_sensor != nullptr && rpm_sensor->get_rpm(sensor, rpm)) { // set the harmonic notch filter frequency from the main rotor rpm - notch.update_freq_hz(MAX(ref_freq * notch.params.freq_min_ratio(), rpm * ref * (1.0/60))); + notch.update_freq_hz(rpm * ref * (1.0/60)); } else { - notch.update_freq_hz(ref_freq); + notch.update_freq_hz(0); } break; } @@ -819,18 +807,13 @@ void AP_Vehicle::update_dynamic_notch(AP_InertialSensor::HarmonicNotch ¬ch) float notches[INS_MAX_NOTCHES]; // ESC telemetry will return 0 for missing data, but only after 1s const uint8_t num_notches = AP::esc_telem().get_motor_frequencies_hz(INS_MAX_NOTCHES, notches); - for (uint8_t i = 0; i < num_notches; i++) { - if (!is_zero(notches[i])) { - notches[i] = MAX(ref_freq, notches[i]); - } - } if (num_notches > 0) { notch.update_frequencies_hz(num_notches, notches); } else { // throttle fallback update_throttle_notch(notch); } } else { - notch.update_freq_hz(MAX(ref_freq, AP::esc_telem().get_average_motor_frequency_hz() * ref)); + notch.update_freq_hz(AP::esc_telem().get_average_motor_frequency_hz() * ref); } break; #endif @@ -842,20 +825,14 @@ void AP_Vehicle::update_dynamic_notch(AP_InertialSensor::HarmonicNotch ¬ch) const uint8_t peaks = gyro_fft.get_weighted_noise_center_frequencies_hz(notch.num_dynamic_notches, notches); if (peaks > 0) { - for (uint8_t i = 0; i < peaks; i++) { - notches[i] = MAX(ref_freq, notches[i]); - } + notch.set_inactive(false); notch.update_frequencies_hz(peaks, notches); } else { // since FFT can be used post-filter it is better to disable the notch when there is no data notch.set_inactive(true); } } else { float center_freq = gyro_fft.get_weighted_noise_center_freq_hz(); - if (!is_zero(center_freq)) { - notch.update_freq_hz(MAX(ref_freq, center_freq)); - } else { // since FFT can be used post-filter it is better to disable the notch when there is no data - notch.set_inactive(true); - } + notch.update_freq_hz(center_freq); } break; #endif diff --git a/libraries/Filter/Filter.h b/libraries/Filter/Filter.h index 7f11ed4852b85..ed1f00b4489a3 100644 --- a/libraries/Filter/Filter.h +++ b/libraries/Filter/Filter.h @@ -9,3 +9,12 @@ #include "LowPassFilter.h" #include "ModeFilter.h" #include "Butter.h" + +/* + the filter version is logged in the VER message to assist the online + analysis tools, so they can display the right filter formulas for + this version of the code + This should be incremented on significant filtering changes + */ +#define AP_FILTER_VERSION 2 + diff --git a/libraries/Filter/HarmonicNotchFilter.cpp b/libraries/Filter/HarmonicNotchFilter.cpp index 5a35ea5b7416d..ed12108c2cd43 100644 --- a/libraries/Filter/HarmonicNotchFilter.cpp +++ b/libraries/Filter/HarmonicNotchFilter.cpp @@ -20,10 +20,7 @@ #include "HarmonicNotchFilter.h" #include -#include -#include -#include -#include +#include #define HNF_MAX_FILTERS HAL_HNF_MAX_FILTERS // must be even for double-notch filters @@ -34,6 +31,23 @@ #define NOTCH_DEBUG_LOGGING 0 #endif +#if NOTCH_DEBUG_LOGGING +#include +#include +#include +#include +#endif + +/* + cutoff proportion of sample rate above which we do not use the notch + */ +#define HARMONIC_NYQUIST_CUTOFF 0.48f + +/* + point at which the harmonic notch goes to zero attenuation + */ +#define NOTCHFILTER_ATTENUATION_CUTOFF 0.25 + // table of user settable parameters const AP_Param::GroupInfo HarmonicNotchFilterParams::var_info[] = { @@ -111,7 +125,7 @@ const AP_Param::GroupInfo HarmonicNotchFilterParams::var_info[] = { // @Param: OPTS // @DisplayName: Harmonic Notch Filter options // @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. - // @Bitmask: 0:Double notch,1:Multi-Source,2:Update at loop rate,3:EnableOnAllIMUs,4:Triple notch + // @Bitmask: 0:Double notch,1:Multi-Source,2:Update at loop rate,3:EnableOnAllIMUs,4:Triple notch, 5:Use min freq on RPM failure // @User: Advanced // @RebootRequired: True AP_GROUPINFO("OPTS", 8, HarmonicNotchFilterParams, _options, 0), @@ -141,8 +155,11 @@ HarmonicNotchFilter::~HarmonicNotchFilter() { the constraints are used to determine attenuation (A) and quality (Q) factors for the filter */ template -void HarmonicNotchFilter::init(float sample_freq_hz, float center_freq_hz, float bandwidth_hz, float attenuation_dB) +void HarmonicNotchFilter::init(float sample_freq_hz, HarmonicNotchFilterParams &_params) { + // keep a copy of the params object + params = &_params; + // sanity check the input if (_filters == nullptr || is_zero(sample_freq_hz) || isnan(sample_freq_hz)) { return; @@ -150,10 +167,22 @@ void HarmonicNotchFilter::init(float sample_freq_hz, float center_freq_hz, fl _sample_freq_hz = sample_freq_hz; - const float nyquist_limit = sample_freq_hz * 0.48f; + const float bandwidth_hz = params->bandwidth_hz(); + const float attenuation_dB = params->attenuation_dB(); + float center_freq_hz = params->center_freq_hz(); + + const float nyquist_limit = sample_freq_hz * HARMONIC_NYQUIST_CUTOFF; const float bandwidth_limit = bandwidth_hz * 0.52f; - // adjust the fundamental center frequency to be in the allowable range + + // remember the lowest frequency we will have a notch enabled at + _minimum_freq = center_freq_hz * params->freq_min_ratio(); + + /* + adjust the fundamental center frequency we use for the initial + calculation of A and Q to be in the allowable range + */ center_freq_hz = constrain_float(center_freq_hz, bandwidth_limit, nyquist_limit); + // Calculate spread required to achieve an equivalent single notch using two notches with Bandwidth/2 _notch_spread = bandwidth_hz / (32 * center_freq_hz); @@ -162,7 +191,6 @@ void HarmonicNotchFilter::init(float sample_freq_hz, float center_freq_hz, fl NotchFilter::calculate_A_and_Q(center_freq_hz, bandwidth_hz / _composite_notches, attenuation_dB, _A, _Q); _initialised = true; - update(center_freq_hz); } /* @@ -191,6 +219,10 @@ void HarmonicNotchFilter::allocate_filters(uint8_t num_notches, uint32_t harm template void HarmonicNotchFilter::expand_filter_count(uint16_t total_notches) { + if (total_notches <= _num_filters) { + // enough already + return; + } if (_alloc_has_failed) { // we've failed to allocate before, don't try again return; @@ -212,46 +244,92 @@ void HarmonicNotchFilter::expand_filter_count(uint16_t total_notches) } /* - update the underlying filters' center frequency using the current attenuation and quality - this function is cheaper than init() because A & Q do not need to be recalculated + set the center frequency of a single notch harmonic + + The spread_mul is the frequency multiplier from the spread of the + double or triple notch. The harmonic_mul is the multiplier for the + frequency for this harmonic */ template -void HarmonicNotchFilter::update(float center_freq_hz) +void HarmonicNotchFilter::set_center_frequency(uint16_t idx, float notch_center, float spread_mul, uint8_t harmonic_mul) { - if (!_initialised) { + const float nyquist_limit = _sample_freq_hz * HARMONIC_NYQUIST_CUTOFF; + auto ¬ch = _filters[idx]; + + // scale the notch with the harmonic multiplier + notch_center *= harmonic_mul; + + /* disable the filter if its center frequency is above the nyquist + frequency + + NOTE: should this be notch_center*spread_mul ? As it is here we + do sometimes run the upper notch in a double or triple notch at + higher than the nyquist. + */ + if (notch_center >= nyquist_limit) { + notch.disable(); return; } - // adjust the fundamental center frequency to be in the allowable range - const float nyquist_limit = _sample_freq_hz * 0.48f; - center_freq_hz = constrain_float(center_freq_hz, 0.0f, nyquist_limit); + // the minimum frequency for a harmonic is the base minimum + float harmonic_min_freq = _minimum_freq; + + // we can adjust the attenuation at low frequencies + float A = _A; + + // on some vehicles we want to treat zero or very low frequency as + // the minimum frequency, on others we want to disable the + // notch for low frequencies + const bool treat_low_freq_as_min = params->hasOption(HarmonicNotchFilterParams::Options::TreatLowAsMin); + + if (treat_low_freq_as_min) { + /* + when we are treating low as min we don't want to collapse + the harmonics down to low frequencies + */ + harmonic_min_freq *= harmonic_mul; + } else { + /* + disable if the notch is less than 25% of the min frequency + */ + const float disable_freq = harmonic_min_freq * NOTCHFILTER_ATTENUATION_CUTOFF; + if (notch_center < disable_freq) { + notch.disable(); + return; + } - _num_enabled_filters = 0; - // update all of the filters using the new center frequency and existing A & Q - for (uint8_t i = 0; i < HNF_MAX_HARMONICS && _num_enabled_filters < _num_filters; i++) { - if ((1U< 1) { - float notch_center_double; - // only enable the filter if its center frequency is below the nyquist frequency - notch_center_double = notch_center * (1.0 - _notch_spread); - if (notch_center_double < nyquist_limit) { - _filters[_num_enabled_filters++].init_with_A_and_Q(_sample_freq_hz, notch_center_double, _A, _Q); - } - // only enable the filter if its center frequency is below the nyquist frequency - notch_center_double = notch_center * (1.0 + _notch_spread); - if (notch_center_double < nyquist_limit) { - _filters[_num_enabled_filters++].init_with_A_and_Q(_sample_freq_hz, notch_center_double, _A, _Q); - } - } + if (notch_center < harmonic_min_freq) { + /* + scale the attenuation so that we fade out the notch as + we get further below the min frequency. The attenuation + factor A goes to 1.0 (which means no attenuation) + Scaling the attenuation in this way means we don't get a + glitch at the disable point + */ + A = linear_interpolate(A, 1.0, notch_center, harmonic_min_freq, disable_freq); } } + + // don't let the notch go below the min frequency + notch_center = MAX(notch_center, harmonic_min_freq); + + /* adjust notch center for spread for double and triple notch. + This adjustment is applied last to maintain the symmetry of the + double and triple notches + */ + notch_center *= spread_mul; + + notch.init_with_A_and_Q(_sample_freq_hz, notch_center, A, _Q); +} + +/* + update the underlying filters' center frequency using the current attenuation and quality + this function is cheaper than init() because A & Q do not need to be recalculated + */ +template +void HarmonicNotchFilter::update(float center_freq_hz) +{ + update(1, ¢er_freq_hz); } /* @@ -266,9 +344,9 @@ void HarmonicNotchFilter::update(uint8_t num_centers, const float center_freq } // adjust the frequencies to be in the allowable range - const float nyquist_limit = _sample_freq_hz * 0.48f; + const float nyquist_limit = _sample_freq_hz * HARMONIC_NYQUIST_CUTOFF; - uint16_t total_notches = num_centers * _num_harmonics * _composite_notches; + const uint16_t total_notches = num_centers * _num_harmonics * _composite_notches; if (total_notches > _num_filters) { // alloc realloc of filters expand_filter_count(total_notches); @@ -286,25 +364,14 @@ void HarmonicNotchFilter::update(uint8_t num_centers, const float center_freq continue; } - const float notch_center = constrain_float(center_freq_hz[center_n] * (harmonic_n+1), 0.0f, nyquist_limit); + const float notch_center = constrain_float(center_freq_hz[center_n], 0.0f, nyquist_limit); + const uint8_t harmonic_mul = (harmonic_n+1); if (_composite_notches != 2) { - // only enable the filter if its center frequency is below the nyquist frequency - if (notch_center < nyquist_limit) { - _filters[_num_enabled_filters++].init_with_A_and_Q(_sample_freq_hz, notch_center, _A, _Q); - } + set_center_frequency(_num_enabled_filters++, notch_center, 1.0, harmonic_mul); } if (_composite_notches > 1) { - float notch_center_double; - // only enable the filter if its center frequency is below the nyquist frequency - notch_center_double = notch_center * (1.0 - _notch_spread); - if (notch_center_double < nyquist_limit) { - _filters[_num_enabled_filters++].init_with_A_and_Q(_sample_freq_hz, notch_center_double, _A, _Q); - } - // only enable the filter if its center frequency is below the nyquist frequency - notch_center_double = notch_center * (1.0 + _notch_spread); - if (notch_center_double < nyquist_limit) { - _filters[_num_enabled_filters++].init_with_A_and_Q(_sample_freq_hz, notch_center_double, _A, _Q); - } + set_center_frequency(_num_enabled_filters++, notch_center, 1.0 - _notch_spread, harmonic_mul); + set_center_frequency(_num_enabled_filters++, notch_center, 1.0 + _notch_spread, harmonic_mul); } } } @@ -360,6 +427,84 @@ void HarmonicNotchFilter::reset() } } +#if HAL_LOGGING_ENABLED +// @LoggerMessage: FCN +// @Description: Filter Center Message - per motor +// @Field: TimeUS: microseconds since system startup +// @Field: I: instance +// @Field: NF: total number of active harmonic notches +// @Field: CF1: First harmonic centre frequency for motor 1 +// @Field: CF2: First harmonic centre frequency for motor 2 +// @Field: CF3: First harmonic centre frequency for motor 3 +// @Field: CF4: First harmonic centre frequency for motor 4 +// @Field: CF5: First harmonic centre frequency for motor 5 +// @Field: CF6: First harmonic centre frequency for motor 6 +// @Field: HF1: Second harmonic centre frequency for motor 1 +// @Field: HF2: Second harmonic centre frequency for motor 2 +// @Field: HF3: Second harmonic centre frequency for motor 3 +// @Field: HF4: Second harmonic centre frequency for motor 4 +// @Field: HF5: Second harmonic centre frequency for motor 5 +// @Field: HF6: Second harmonic centre frequency for motor 6 + +// @LoggerMessage: FCNS +// @Description: Filter Center Message +// @Field: TimeUS: microseconds since system startup +// @Field: I: instance +// @Field: CF: notch centre frequency +// @Field: HF: 2nd harmonic frequency + +/* + log center frequencies of 1st and 2nd harmonic of a harmonic notch + instance for up to 6 frequency sources + + the instance number passed in corresponds to the harmonic notch + instance in AP_InertialSensor + */ +template +void HarmonicNotchFilter::log_notch_centers(uint8_t instance, uint64_t now_us) const +{ + /* + for composite notches we only log the first entry. For triple + and single notch this is the center. For double notch it is the + lower frequency + */ + const uint16_t filters_per_source = _composite_notches * _num_harmonics; + if (_num_filters == 0 || filters_per_source == 0) { + return; + } + const uint8_t num_sources = MIN(6, _num_filters / filters_per_source); + float centers[6] {}; + float first_harmonic[6] {}; + + for (uint8_t i=0; i 1) { + AP::logger().WriteStreaming( + "FCN", "TimeUS,I,NF,CF1,CF2,CF3,CF4,CF5,CF6,HF1,HF2,HF3,HF4,HF5,HF6", "s#-zzzzzzzzzzzz", "F--------------", "QBHffffffffffff", + now_us, + instance, + _num_filters, + centers[0], centers[1], centers[2], centers[3], centers[4], centers[5], + first_harmonic[0], first_harmonic[1], first_harmonic[2], first_harmonic[3], first_harmonic[4], first_harmonic[5]); + } else { + // log single center frequency + AP::logger().WriteStreaming( + "FCNS", "TimeUS,I,CF,HF", "s#zz", "F---", "QBff", + now_us, + instance, + centers[0], + first_harmonic[0]); + } +} +#endif // HAL_LOGGING_ENABLED + /* create parameters for the harmonic notch filter and initialise defaults */ diff --git a/libraries/Filter/HarmonicNotchFilter.h b/libraries/Filter/HarmonicNotchFilter.h index 898857b82e58e..3a5704d67d6cc 100644 --- a/libraries/Filter/HarmonicNotchFilter.h +++ b/libraries/Filter/HarmonicNotchFilter.h @@ -21,6 +21,8 @@ #define HNF_MAX_HARMONICS 16 +class HarmonicNotchFilterParams; + /* a filter that manages a set of notch filters targetted at a fundamental center frequency and multiples of that fundamental frequency @@ -34,16 +36,29 @@ class HarmonicNotchFilter { // expand filter bank with new filters void expand_filter_count(uint16_t total_notches); // initialize the underlying filters using the provided filter parameters - void init(float sample_freq_hz, float center_freq_hz, float bandwidth_hz, float attenuation_dB); + void init(float sample_freq_hz, HarmonicNotchFilterParams ¶ms); // update the underlying filters' center frequencies using center_freq_hz as the fundamental void update(float center_freq_hz); - // update all o fthe underlying center frequencies individually + // update all of the underlying center frequencies individually void update(uint8_t num_centers, const float center_freq_hz[]); + + /* + set center frequency of one notch. + spread_mul is a scale factor for spreading of double or triple notch + harmonic_mul is the multiplier for harmonics, 1 is for the fundamental + */ + void set_center_frequency(uint16_t idx, float center_freq_hz, float spread_mul, uint8_t harmonic_mul); + // apply a sample to each of the underlying filters in turn T apply(const T &sample); // reset each of the underlying filters void reset(); + /* + log notch center frequencies and first harmonic + */ + void log_notch_centers(uint8_t instance, uint64_t now_us) const; + private: // underlying bank of notch filters NotchFilter* _filters; @@ -69,6 +84,12 @@ class HarmonicNotchFilter { // have we failed to expand filters? bool _alloc_has_failed; + + // minimum frequency (from INS_HNTCH_FREQ * INS_HNTCH_FM_RAT) + float _minimum_freq; + + // pointer to params object for this filter + HarmonicNotchFilterParams *params; }; // Harmonic notch update mode @@ -92,6 +113,7 @@ class HarmonicNotchFilterParams : public NotchFilterParams { LoopRateUpdate = 1<<2, EnableOnAllIMUs = 1<<3, TripleNotch = 1<<4, + TreatLowAsMin = 1<<5, }; HarmonicNotchFilterParams(void); @@ -100,17 +122,26 @@ class HarmonicNotchFilterParams : public NotchFilterParams { // set the fundamental center frequency of the harmonic notch void set_center_freq_hz(float center_freq) { _center_freq_hz.set(center_freq); } + // set the bandwidth of the harmonic notch void set_bandwidth_hz(float bandwidth_hz) { _bandwidth_hz.set(bandwidth_hz); } + + // set the attenuation of the harmonic notch + void set_attenuation(float attenuation_dB) { _attenuation_dB.set(attenuation_dB); } + // harmonics enabled on the harmonic notch uint32_t harmonics(void) const { return _harmonics; } + // set the harmonics value void set_harmonics(uint32_t hmncs) { _harmonics.set(hmncs); } + // has the user set the harmonics value void set_default_harmonics(uint32_t hmncs) { _harmonics.set_default(hmncs); } + // reference value of the harmonic notch float reference(void) const { return _reference; } void set_reference(float ref) { _reference.set(ref); } + // notch options bool hasOption(Options option) const { return _options & uint16_t(option); } // notch dynamic tracking mode @@ -123,9 +154,17 @@ class HarmonicNotchFilterParams : public NotchFilterParams { } void set_freq_min_ratio(float ratio) { _freq_min_ratio.set(ratio); } + // set options flags + void set_options(uint16_t options) { _options.set(options); } + // save parameters void save_params(); + // return the number of composite notches given the options + uint8_t num_composite_notches(void) const { + return hasOption(Options::DoubleNotch) ? 2 : hasOption(Options::TripleNotch) ? 3: 1; + } + private: // configured notch harmonics AP_Int32 _harmonics; diff --git a/libraries/Filter/NotchFilter.cpp b/libraries/Filter/NotchFilter.cpp index fa2c024721be9..dbbec6f80a802 100644 --- a/libraries/Filter/NotchFilter.cpp +++ b/libraries/Filter/NotchFilter.cpp @@ -59,7 +59,10 @@ template void NotchFilter::init_with_A_and_Q(float sample_freq_hz, float center_freq_hz, float A, float Q) { // don't update if no updates required - if (initialised && is_equal(center_freq_hz, _center_freq_hz) && is_equal(sample_freq_hz, _sample_freq_hz)) { + if (initialised && + is_equal(center_freq_hz, _center_freq_hz) && + is_equal(sample_freq_hz, _sample_freq_hz) && + is_equal(A, _A)) { return; } @@ -91,6 +94,7 @@ void NotchFilter::init_with_A_and_Q(float sample_freq_hz, float center_freq_h _center_freq_hz = new_center_freq; _sample_freq_hz = sample_freq_hz; + _A = A; initialised = true; } else { // leave center_freq_hz at last value diff --git a/libraries/Filter/NotchFilter.h b/libraries/Filter/NotchFilter.h index 624929208e155..9f2e49c66acb6 100644 --- a/libraries/Filter/NotchFilter.h +++ b/libraries/Filter/NotchFilter.h @@ -44,11 +44,20 @@ class NotchFilter { // calculate attenuation and quality from provided center frequency and bandwidth static void calculate_A_and_Q(float center_freq_hz, float bandwidth_hz, float attenuation_dB, float& A, float& Q); + void disable(void) { + initialised = false; + } + + // return the frequency to log for the notch + float logging_frequency(void) const { + return initialised?_center_freq_hz:0; + } + protected: bool initialised, need_reset; float b0, b1, b2, a1, a2; - float _center_freq_hz, _sample_freq_hz; + float _center_freq_hz, _sample_freq_hz, _A; T ntchsig1, ntchsig2, signal2, signal1; }; diff --git a/libraries/Filter/tests/plot_harmonictest2.gnu b/libraries/Filter/tests/plot_harmonictest2.gnu new file mode 100644 index 0000000000000..2df39eb16c5b1 --- /dev/null +++ b/libraries/Filter/tests/plot_harmonictest2.gnu @@ -0,0 +1,11 @@ +#!/usr/bin/gnuplot -persist +set y2tics 0,10 +set ytics nomirror +set style data linespoints +set key autotitle +set datafile separator "," +set key autotitle columnhead +set xlabel "Freq(Hz)" +set ylabel "Attenuation(dB)" +#set ylabel2 "PhaseLag(deg)" +plot "harmonicnotch_test2.csv" using 1:2 axis x1y1, "harmonicnotch_test2.csv" using 1:3 axis x1y2 diff --git a/libraries/Filter/tests/plot_harmonictest3.gnu b/libraries/Filter/tests/plot_harmonictest3.gnu new file mode 100644 index 0000000000000..d2f7cc337a865 --- /dev/null +++ b/libraries/Filter/tests/plot_harmonictest3.gnu @@ -0,0 +1,12 @@ +#!/usr/bin/gnuplot -persist +set y2tics 0,10 +set ytics nomirror +set style data linespoints +set key autotitle +set datafile separator "," +set key autotitle columnhead +set xlabel "Freq(Hz)" +set ylabel "Attenuation(dB)" +#set ylabel2 "PhaseLag(deg)" +plot "harmonicnotch_test3.csv" using 1:2 axis x1y1, "harmonicnotch_test3.csv" using 1:3 axis x1y2, "harmonicnotch_test3.csv" using 1:4 axis x1y1, "harmonicnotch_test3.csv" using 1:5 axis x1y2 + diff --git a/libraries/Filter/tests/plot_harmonictest4.gnu b/libraries/Filter/tests/plot_harmonictest4.gnu new file mode 100644 index 0000000000000..cfc8b5117e8e3 --- /dev/null +++ b/libraries/Filter/tests/plot_harmonictest4.gnu @@ -0,0 +1,12 @@ +#!/usr/bin/gnuplot -persist +set y2tics 0,10 +set ytics nomirror +set style data linespoints +set key autotitle +set datafile separator "," +set key autotitle columnhead +set xlabel "Freq(Hz)" +set ylabel "Attenuation(dB)" +set key left bottom +plot "harmonicnotch_test4.csv" using 1:2, "harmonicnotch_test4.csv" using 1:3, "harmonicnotch_test4.csv" using 1:4, "harmonicnotch_test4.csv" using 1:5, "harmonicnotch_test4.csv" using 1:6, "harmonicnotch_test4.csv" using 1:7, "harmonicnotch_test4.csv" using 1:8 + diff --git a/libraries/Filter/tests/test_notchfilter.cpp b/libraries/Filter/tests/test_notchfilter.cpp index 65390a9b841d6..a13c9aeae161a 100644 --- a/libraries/Filter/tests/test_notchfilter.cpp +++ b/libraries/Filter/tests/test_notchfilter.cpp @@ -6,6 +6,11 @@ const AP_HAL::HAL& hal = AP_HAL::get_HAL(); +static double ratio_to_dB(double ratio) +{ + return 10*log(ratio); +} + /* test if a reset of a notch filter results in no glitch in the following samples with constant input @@ -74,7 +79,6 @@ TEST(NotchFilterTest, HarmonicNotchTest) { const uint8_t num_test_freq = 150; const uint8_t harmonics = 15; - const uint8_t num_harmonics = __builtin_popcount(harmonics); const float base_freq = 46; const float bandwidth = base_freq/2; const float attenuation_dB = 60; @@ -86,7 +90,6 @@ TEST(NotchFilterTest, HarmonicNotchTest) const float test_amplitude = 0.7; const double dt = 1.0 / rate_hz; - bool double_notch = true; HarmonicNotchFilter filters[num_test_freq][chained_filters] {}; struct { double in; @@ -105,8 +108,16 @@ TEST(NotchFilterTest, HarmonicNotchTest) for (uint8_t i=0; i filter {}; + struct { + double last_in; + double last_out; + double v_max; + uint32_t last_crossing; + uint32_t total_lag_samples; + uint32_t lag_count; + float get_lag_degrees(const float freq) const { + const float lag_avg = total_lag_samples/float(lag_count); + return (360.0 * lag_avg * freq) / rate_hz; + } + } integral {}; + + auto &f = filter; + + + HarmonicNotchFilterParams notch_params {}; + notch_params.set_options(options); + notch_params.set_attenuation(attenuation_dB); + notch_params.set_bandwidth_hz(bandwidth); + notch_params.set_center_freq_hz(base_freq); + notch_params.set_freq_min_ratio(1.0); + f.allocate_filters(1, harmonics, notch_params.num_composite_notches()); + f.init(rate_hz, notch_params); + f.update(source_freq); + + for (uint32_t s=0; s= samples/10) { + integral.v_max = MAX(integral.v_max, v); + } + if (sample >= 0 && integral.last_in < 0) { + integral.last_crossing = s; + } + if (v >= 0 && integral.last_out < 0 && integral.last_crossing != 0) { + integral.total_lag_samples += s - integral.last_crossing; + integral.lag_count++; + } + integral.last_in = sample; + integral.last_out = v; + f.update(source_freq); + } + phase_lag = integral.get_lag_degrees(test_freq); + out_attenuation_dB = ratio_to_dB(integral.v_max/test_amplitude); +} + +/* + test the test_one_filter function + */ +TEST(NotchFilterTest, HarmonicNotchTest2) +{ + const float min_freq = 1.0; + const float max_freq = 200; + const uint16_t steps = 2000; + + const char *csv_file = "harmonicnotch_test2.csv"; + FILE *f = fopen(csv_file, "w"); + fprintf(f, "Freq(Hz),Attenuation(dB),Lag(deg)\n"); + + for (uint16_t i=0; i