mirror of https://github.com/ArduPilot/ardupilot
AP_InertialSensor: removed zero checks and clamping on notch filters
and pass params object down into HarmonicNotchFilter
This commit is contained in:
parent
e18983780f
commit
de8bec596e
|
@ -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)
|
||||
|
@ -1043,8 +1040,7 @@ AP_InertialSensor::init(uint16_t loop_rate)
|
|||
notch.filter[i].allocate_filters(notch.num_dynamic_notches,
|
||||
notch.params.harmonics(), double_notch ? 2 : triple_notch ? 3 : 1);
|
||||
// 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 +1831,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;
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -2300,10 +2292,7 @@ void AP_InertialSensor::acal_update()
|
|||
// Update the harmonic notch frequency
|
||||
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;
|
||||
}
|
||||
|
||||
|
@ -2311,9 +2300,7 @@ void AP_InertialSensor::HarmonicNotch::update_freq_hz(float scaled_freq)
|
|||
void AP_InertialSensor::HarmonicNotch::update_frequencies_hz(uint8_t num_freqs, const float scaled_freq[]) {
|
||||
// protect against zero as the scaled frequency
|
||||
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;
|
||||
|
|
Loading…
Reference in New Issue