Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: don't update disabled notches

This commit is contained in:
Andrew Tridgell 2022-04-16 12:08:09 +10:00 committed by Randy Mackay
parent a15259d7e5
commit 5bdac5174c
1 changed files with 6 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -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 // dynamically, the calculated value is always some multiple of the configured center frequency, so start with the
// configured value // configured value
for (auto &notch : harmonic_notches) { for (auto &notch : harmonic_notches) {
if (!notch.params.enabled()) {
continue;
}
notch.calculated_notch_freq_hz[0] = notch.params.center_freq_hz(); notch.calculated_notch_freq_hz[0] = notch.params.center_freq_hz();
notch.num_calculated_notch_frequencies = 1; notch.num_calculated_notch_frequencies = 1;
notch.num_dynamic_notches = 1; notch.num_dynamic_notches = 1;
@ -1635,9 +1638,11 @@ void AP_InertialSensor::update(void)
const bool converging = AP_HAL::millis() < HAL_INS_CONVERGANCE_MS; const bool converging = AP_HAL::millis() < HAL_INS_CONVERGANCE_MS;
const float gyro_rate = _gyro_raw_sample_rates[i]; const float gyro_rate = _gyro_raw_sample_rates[i];
for (auto &notch : harmonic_notches) { for (auto &notch : harmonic_notches) {
if (notch.params.enabled()) {
notch.update_params(i, converging, gyro_rate); notch.update_params(i, converging, gyro_rate);
} }
} }
}
if (!_startup_error_counts_set) { if (!_startup_error_counts_set) {
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) { for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {