diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index ed98f5fab2fe5..53c3dff3135cf 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -950,7 +950,6 @@ AP_InertialSensor::init(uint16_t loop_rate) if (!notch.params.enabled() && !fft_enabled) { continue; } - notch.calculated_notch_freq_hz[0] = 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) @@ -977,15 +976,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); } } @@ -1000,13 +997,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); } } } @@ -1702,25 +1696,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; } } @@ -2152,23 +2142,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_Logger/AP_Logger_Backend.cpp b/libraries/AP_Logger/AP_Logger_Backend.cpp index c054f44955000..b5d5e4ca531e4 100644 --- a/libraries/AP_Logger/AP_Logger_Backend.cpp +++ b/libraries/AP_Logger/AP_Logger_Backend.cpp @@ -7,6 +7,12 @@ #include #include #include +#include +#include "AP_Logger.h" + +#if HAL_LOGGER_FENCE_ENABLED + #include +#endif extern const AP_HAL::HAL& hal; @@ -562,6 +568,7 @@ bool AP_Logger_Backend::Write_VER() fw_type: fwver.fw_type, git_hash: fwver.fw_hash, build_type: fwver.vehicle_type, + 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 d44aa7156bbc4..c893283ced68d 100644 --- a/libraries/AP_Logger/LogStructure.h +++ b/libraries/AP_Logger/LogStructure.h @@ -685,6 +685,7 @@ struct PACKED log_VER { char fw_string[64]; uint16_t _APJ_BOARD_ID; uint8_t build_type; + uint8_t filter_version; }; @@ -1345,7 +1346,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 309d76d42b01c..35c9ae040f503 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.cpp +++ b/libraries/AP_Vehicle/AP_Vehicle.cpp @@ -512,7 +512,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(); @@ -522,7 +521,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 @@ -542,17 +541,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 @@ -567,9 +555,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, 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; } @@ -581,18 +569,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 @@ -604,20 +587,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 f5e58f492596a..873379958b46b 100644 --- a/libraries/Filter/HarmonicNotchFilter.cpp +++ b/libraries/Filter/HarmonicNotchFilter.cpp @@ -20,10 +20,27 @@ #include "HarmonicNotchFilter.h" #include +#include +#include +#include +#include #define HNF_MAX_FILTERS HAL_HNF_MAX_FILTERS // must be even for double-notch filters #define HNF_MAX_HARMONICS 8 +/* + optional logging for SITL only of all notch frequencies + */ +#ifndef NOTCH_DEBUG_LOGGING +#define NOTCH_DEBUG_LOGGING 0 +#endif + +/* + 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[] = { @@ -84,8 +101,8 @@ 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. Dynamic harmonics 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:Dynamic harmonic,2:Update at loop rate,3:EnableOnAllIMUs,4:Triple notch + // @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, 5:Use min freq on RPM failure // @User: Advanced // @RebootRequired: True AP_GROUPINFO("OPTS", 8, HarmonicNotchFilterParams, _options, 0), @@ -115,8 +132,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; @@ -124,10 +144,22 @@ void HarmonicNotchFilter::init(float sample_freq_hz, float center_freq_hz, fl _sample_freq_hz = sample_freq_hz; + 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 * 0.48f; 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); @@ -136,7 +168,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); } /* @@ -163,10 +194,9 @@ void HarmonicNotchFilter::allocate_filters(uint8_t num_notches, uint8_t harmo expand the number of filters at runtime, allowing for RPM sources such as lua scripts */ template -void HarmonicNotchFilter::expand_filter_count(uint8_t num_notches) +void HarmonicNotchFilter::expand_filter_count(uint16_t total_notches) { - uint8_t num_filters = _num_harmonics * num_notches * _composite_notches; - if (num_filters <= _num_filters) { + if (total_notches <= _num_filters) { // enough already return; } @@ -178,7 +208,7 @@ void HarmonicNotchFilter::expand_filter_count(uint8_t num_notches) note that we rely on the semaphore in AP_InertialSensor_Backend.cpp to make this thread safe */ - auto filters = new NotchFilter[num_filters]; + auto filters = new NotchFilter[total_notches]; if (filters == nullptr) { _alloc_has_failed = true; return; @@ -186,51 +216,97 @@ void HarmonicNotchFilter::expand_filter_count(uint8_t num_notches) memcpy(filters, _filters, sizeof(filters[0])*_num_filters); auto _old_filters = _filters; _filters = filters; - _num_filters = num_filters; + _num_filters = total_notches; delete[] _old_filters; } /* - 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 * 0.48f; + 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, 1.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); } /* @@ -247,15 +323,16 @@ 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; - if (num_centers > _num_filters) { + const uint16_t total_notches = num_centers * _num_harmonics * _composite_notches; + if (total_notches > _num_filters) { // alloc realloc of filters - expand_filter_count(num_centers); + expand_filter_count(total_notches); } _num_enabled_filters = 0; // update all of the filters using the new center frequencies and existing A & Q - for (uint8_t i = 0; i < num_centers * HNF_MAX_HARMONICS && _num_enabled_filters < _num_filters; i++) { + for (uint16_t i = 0; i < num_centers * HNF_MAX_HARMONICS && _num_enabled_filters < _num_filters; i++) { const uint8_t harmonic_n = i / num_centers; const uint8_t center_n = i % num_centers; // the filters are ordered by center and then harmonic so @@ -264,25 +341,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), 1.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); } } } @@ -297,10 +363,29 @@ T HarmonicNotchFilter::apply(const T &sample) return sample; } +#if NOTCH_DEBUG_LOGGING + static int dfd = -1; + if (dfd == -1) { + dfd = ::open("notch.txt", O_WRONLY|O_CREAT|O_TRUNC, 0644); + } +#endif + T output = sample; - for (uint8_t i = 0; i < _num_enabled_filters; i++) { + for (uint16_t i = 0; i < _num_enabled_filters; i++) { +#if NOTCH_DEBUG_LOGGING + if (!_filters[i].initialised) { + ::dprintf(dfd, "------- "); + } else { + ::dprintf(dfd, "%.4f ", _filters[i]._center_freq_hz); + } +#endif output = _filters[i].apply(output); } +#if NOTCH_DEBUG_LOGGING + if (_num_enabled_filters > 0) { + ::dprintf(dfd, "\n"); + } +#endif return output; } @@ -314,7 +399,7 @@ void HarmonicNotchFilter::reset() return; } - for (uint8_t i = 0; i < _num_filters; i++) { + for (uint16_t i = 0; i < _num_filters; i++) { _filters[i].reset(); } } diff --git a/libraries/Filter/HarmonicNotchFilter.h b/libraries/Filter/HarmonicNotchFilter.h index 064365f0b2771..e2fcc8148c61f 100644 --- a/libraries/Filter/HarmonicNotchFilter.h +++ b/libraries/Filter/HarmonicNotchFilter.h @@ -21,6 +21,8 @@ #define HNF_MAX_HARMONICS 8 +class HarmonicNotchFilterParams; + /* a filter that manages a set of notch filters targetted at a fundamental center frequency and multiples of that fundamental frequency @@ -32,13 +34,21 @@ class HarmonicNotchFilter { // allocate a bank of notch filters for this harmonic notch filter void allocate_filters(uint8_t num_notches, uint8_t harmonics, uint8_t composite_notches); // expand filter bank with new filters - void expand_filter_count(uint8_t num_notches); + 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 @@ -60,15 +70,21 @@ class HarmonicNotchFilter { // number of notches that make up a composite notch uint8_t _composite_notches; // number of allocated filters - uint8_t _num_filters; + uint16_t _num_filters; // pre-calculated number of harmonics uint8_t _num_harmonics; // number of enabled filters - uint8_t _num_enabled_filters; + uint16_t _num_enabled_filters; bool _initialised; // 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,22 +108,32 @@ class HarmonicNotchFilterParams : public NotchFilterParams { LoopRateUpdate = 1<<2, EnableOnAllIMUs = 1<<3, TripleNotch = 1<<4, + TreatLowAsMin = 1<<5, }; HarmonicNotchFilterParams(void); // 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 - uint8_t harmonics(void) const { return _harmonics; } + uint32_t harmonics(void) const { return _harmonics; } + // set the harmonics value - void set_harmonics(uint8_t hmncs) { _harmonics.set(hmncs); } + void set_harmonics(uint32_t hmncs) { _harmonics.set(hmncs); } + // has the user set the harmonics value - void set_default_harmonics(uint8_t hmncs) { _harmonics.set_default(hmncs); } + 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 @@ -120,9 +146,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_Int8 _harmonics; diff --git a/libraries/Filter/NotchFilter.cpp b/libraries/Filter/NotchFilter.cpp index 538eb02deef75..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; } @@ -71,17 +74,27 @@ void NotchFilter::init_with_A_and_Q(float sample_freq_hz, float center_freq_h _center_freq_hz * NOTCH_MAX_SLEW_UPPER); } - if ((new_center_freq > 2.0) && (new_center_freq < 0.5 * sample_freq_hz) && (Q > 0.0)) { + if (is_positive(new_center_freq) && (new_center_freq < 0.5 * sample_freq_hz) && (Q > 0.0)) { float omega = 2.0 * M_PI * new_center_freq / sample_freq_hz; float alpha = sinf(omega) / (2 * Q); b0 = 1.0 + alpha*sq(A); b1 = -2.0 * cosf(omega); b2 = 1.0 - alpha*sq(A); - a0_inv = 1.0/(1.0 + alpha); a1 = b1; a2 = 1.0 - alpha; + + const float a0_inv = 1.0/(1.0 + alpha); + + // Pre-multiply to save runtime calc + b0 *= a0_inv; + b1 *= a0_inv; + b2 *= a0_inv; + a1 *= a0_inv; + a2 *= a0_inv; + _center_freq_hz = new_center_freq; _sample_freq_hz = sample_freq_hz; + _A = A; initialised = true; } else { // leave center_freq_hz at last value @@ -100,16 +113,17 @@ T NotchFilter::apply(const T &sample) // sample as output and update delayed samples signal1 = sample; signal2 = sample; - ntchsig = sample; ntchsig1 = sample; ntchsig2 = sample; need_reset = false; return sample; } + + T output = sample*b0 + ntchsig1*b1 + ntchsig2*b2 - signal1*a1 - signal2*a2; + ntchsig2 = ntchsig1; - ntchsig1 = ntchsig; - ntchsig = sample; - T output = (ntchsig*b0 + ntchsig1*b1 + ntchsig2*b2 - signal1*a1 - signal2*a2) * a0_inv; + ntchsig1 = sample; + signal2 = signal1; signal1 = output; return output; diff --git a/libraries/Filter/NotchFilter.h b/libraries/Filter/NotchFilter.h index 53bacc8e7419f..c397f6b40ccf8 100644 --- a/libraries/Filter/NotchFilter.h +++ b/libraries/Filter/NotchFilter.h @@ -26,24 +26,34 @@ #include +template +class HarmonicNotchFilter; + template class NotchFilter { public: + friend class HarmonicNotchFilter; // set parameters void init(float sample_freq_hz, float center_freq_hz, float bandwidth_hz, float attenuation_dB); void init_with_A_and_Q(float sample_freq_hz, float center_freq_hz, float A, float Q); T apply(const T &sample); void reset(); + float center_freq_hz() const { return _center_freq_hz; } + float sample_freq_hz() const { return _sample_freq_hz; } // 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); -private: + void disable(void) { + initialised = false; + } + +protected: bool initialised, need_reset; - float b0, b1, b2, a1, a2, a0_inv; - float _center_freq_hz, _sample_freq_hz; - T ntchsig, ntchsig1, ntchsig2, signal2, signal1; + float b0, b1, b2, a1, a2; + 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 4cc0c0dbf6c87..79d2325fa5460 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