Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -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 committed by Randy Mackay
parent 005de1ded2
commit be7f0afc87
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++) {
_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) {
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {

10
libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
View File

@ -724,10 +724,18 @@ void AP_InertialSensor_Backend::update_gyro(uint8_t instance) /* front end */
}
// possibly update filter frequency
const float gyro_rate = _gyro_raw_sample_rate(instance);
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();
}
for (auto &notch : _imu.harmonic_notches) {
if (notch.params.enabled()) {
notch.update_params(instance, sensors_converging(), gyro_rate);
}
}
}
/*