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

AP_InertialSensor: fixed the last notch values to be per-instance 

thanks to Andy for noticing this
This commit is contained in:
Andrew Tridgell 2022-04-17 17:03:46 +10:00
parent e0bb717231
commit dff0e5ecc1
2 changed files with 10 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -1593,23 +1593,23 @@ 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, params.bandwidth_hz()) ||
!is_equal(last_attenuation_dB, params.attenuation_dB()) ||
if (!is_equal(last_bandwidth_hz[instance], params.bandwidth_hz()) ||
!is_equal(last_attenuation_dB[instance], params.attenuation_dB()) ||
converging) {
filter[instance].init(gyro_rate,
center_freq,
params.bandwidth_hz(),
params.attenuation_dB());
last_center_freq_hz = center_freq;
last_bandwidth_hz = params.bandwidth_hz();
last_attenuation_dB = params.attenuation_dB();
} else if (!is_equal(last_center_freq_hz, center_freq)) {
last_center_freq_hz[instance] = center_freq;
last_bandwidth_hz[instance] = params.bandwidth_hz();
last_attenuation_dB[instance] = params.attenuation_dB();
} else if (!is_equal(last_center_freq_hz[instance], center_freq)) {
if (num_calculated_notch_frequencies > 1) {
filter[instance].update(num_calculated_notch_frequencies, calculated_notch_freq_hz);
} else {
filter[instance].update(center_freq);
}
last_center_freq_hz = center_freq;
last_center_freq_hz[instance] = center_freq;
}
}

6
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -446,9 +446,9 @@ public:
private:
// support for updating harmonic filter at runtime
float last_center_freq_hz;
float last_bandwidth_hz;
float last_attenuation_dB;
float last_center_freq_hz[INS_MAX_INSTANCES];
float last_bandwidth_hz[INS_MAX_INSTANCES];
float last_attenuation_dB[INS_MAX_INSTANCES];
} harmonic_notches[HAL_INS_NUM_HARMONIC_NOTCH_FILTERS];
private: