mirror of https://github.com/ArduPilot/ardupilot
AP_InertialSensor: don't update disabled notches
This commit is contained in:
parent
5949abb858
commit
afe0c02835
|
@ -877,6 +877,9 @@ AP_InertialSensor::init(uint16_t loop_rate)
|
|||
// dynamically, the calculated value is always some multiple of the configured center frequency, so start with the
|
||||
// configured value
|
||||
for (auto ¬ch : harmonic_notches) {
|
||||
if (!notch.params.enabled()) {
|
||||
continue;
|
||||
}
|
||||
notch.calculated_notch_freq_hz[0] = notch.params.center_freq_hz();
|
||||
notch.num_calculated_notch_frequencies = 1;
|
||||
notch.num_dynamic_notches = 1;
|
||||
|
@ -1635,7 +1638,9 @@ void AP_InertialSensor::update(void)
|
|||
const bool converging = AP_HAL::millis() < HAL_INS_CONVERGANCE_MS;
|
||||
const float gyro_rate = _gyro_raw_sample_rates[i];
|
||||
for (auto ¬ch : harmonic_notches) {
|
||||
notch.update_params(i, converging, gyro_rate);
|
||||
if (notch.params.enabled()) {
|
||||
notch.update_params(i, converging, gyro_rate);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
Loading…
Reference in New Issue