Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-02 14:08:45 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: call notch param update with semaphore held

Browse Source
This commit is contained in:
Andrew Tridgell 2022-04-17 17:35:57 +10:00
parent bccca9ed2e
commit db14ba46dc
2 changed files with 9 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -1634,15 +1634,6 @@ void AP_InertialSensor::update(void)
for (uint8_t i=0; i<_backend_count; i++) { for (uint8_t i=0; i<_backend_count; i++) {
_backends[i]->update(); _backends[i]->update();
} }
for (uint8_t i=0; i<_gyro_count; i++) {
const bool converging = AP_HAL::millis() < HAL_INS_CONVERGANCE_MS;
const float gyro_rate = _gyro_raw_sample_rates[i];
for (auto &notch : harmonic_notches) {
if (notch.params.enabled()) {
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++) {

10
libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
View File

@ -716,10 +716,18 @@ void AP_InertialSensor_Backend::update_gyro(uint8_t instance) /* front end */
} }
// possibly update filter frequency // possibly update filter frequency
const float gyro_rate = _gyro_raw_sample_rate(instance);
if (_last_gyro_filter_hz != _gyro_filter_cutoff() || sensors_converging()) { if (_last_gyro_filter_hz != _gyro_filter_cutoff() || sensors_converging()) {
_imu._gyro_filter[instance].set_cutoff_frequency(_gyro_raw_sample_rate(instance), _gyro_filter_cutoff()); _imu._gyro_filter[instance].set_cutoff_frequency(gyro_rate, _gyro_filter_cutoff());
_last_gyro_filter_hz = _gyro_filter_cutoff(); _last_gyro_filter_hz = _gyro_filter_cutoff();
} }
for (auto &notch : _imu.harmonic_notches) {
if (notch.params.enabled()) {
notch.update_params(instance, sensors_converging(), gyro_rate);
}
}
} }
/* /*