From 169f686e41bbc9b6cbf2cd508d3775d5a45031fe Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 28 Nov 2024 16:43:56 +1100 Subject: [PATCH 1/2] AP_InertialSensor: fixed Q calculation for notch filters when doing the init() we must use the reference frequency, not the current frequency. Using the current frequency leaves us with an incorrect value for Q. If the current frequency is below the reference frequency (which shouldn't happen in 4.5, but has been seen in 4.3) then the Q can be much too low and massive phase lag can happen. The vulnerability in 4.5.x is that the current frequency could be well above the reference frequency. For example, the user may be doing a motor test at 30s after boot, which is when we stop the sensors_converging() test, and thus is the last time we call init(). In that case we can end up with a Q which is much larger than the one that should come from INS_HNTCH_FREQ and INS_HNTCH_BW, and thus end up with a filter that produces a lot less attenuation than is desired, potentially leading to instability due to high noise. There are other scenarios that can cause this - for example a motor test of a fwd motor at 30s after boot, or a spurious FFT peak due to someone knocking the aircraft, or the vibration of a IC engine. --- .../AP_InertialSensor/AP_InertialSensor.cpp | 22 +++++++++++-------- 1 file changed, 13 insertions(+), 9 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index b0bc11aaca..7d1f67dc01 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -1059,8 +1059,10 @@ 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.center_freq_hz(), + notch.params.bandwidth_hz(), + notch.params.attenuation_dB()); } } } @@ -1783,25 +1785,27 @@ 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)) || + !is_equal(last_center_freq_hz[instance], params.center_freq_hz()) || converging) { filter[instance].init(gyro_rate, - center_freq, + params.center_freq_hz(), params.bandwidth_hz(), params.attenuation_dB()); - last_center_freq_hz[instance] = center_freq; + 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 (params.tracking_mode() != HarmonicNotchDynamicMode::Fixed) { + // we always call the update for non-fixed notches, even if we + // just called init() as the update may use a different + // frequency to the init 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; } } From 5321371c386358ec0ad8da07c2908faa3dda9f42 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 29 Nov 2024 13:35:51 +1100 Subject: [PATCH 2/2] AP_InertialSensor: stop sensors converging if motors arm if the user arms within 30s of startup then stop the re-init of the sensors. This can give less accurate frequency as the sample rate may not have settled yet, but it is better than doing init of the filters while the vehicle may be flying also fix a 32 bit millis wrap --- .../AP_InertialSensor/AP_InertialSensor_Backend.cpp | 9 +++++++++ libraries/AP_InertialSensor/AP_InertialSensor_Backend.h | 2 +- 2 files changed, 10 insertions(+), 1 deletion(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp index 85c9ed8f34..b2f0d6eed9 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp @@ -54,6 +54,15 @@ void AP_InertialSensor_Backend::_set_gyro_oversampling(uint8_t instance, uint8_t _imu._gyro_over_sampling[instance] = n; } +/* + while sensors are converging to get the true sample rate we re-init the notch filters. + stop doing this if the user arms + */ +bool AP_InertialSensor_Backend::sensors_converging() const +{ + return AP_HAL::millis64() < HAL_INS_CONVERGANCE_MS && !hal.util->get_soft_armed(); +} + /* update the sensor rate for FIFO sensors diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h index e51d9362b8..948424c975 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h @@ -212,7 +212,7 @@ class AP_InertialSensor_Backend void _update_sensor_rate(uint16_t &count, uint32_t &start_us, float &rate_hz) const __RAMFUNC__; // return true if the sensors are still converging and sampling rates could change significantly - bool sensors_converging() const { return AP_HAL::millis() < HAL_INS_CONVERGANCE_MS; } + bool sensors_converging() const; // set accelerometer max absolute offset for calibration void _set_accel_max_abs_offset(uint8_t instance, float offset);