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

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

Browse Source 
thanks to Andy for noticing this
This commit is contained in:
Andrew Tridgell 2022-04-17 17:03:46 +10:00 committed by Randy Mackay
parent 97d42308a7
commit 9fba96c8d0
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) void AP_InertialSensor::HarmonicNotch::update_params(uint8_t instance, bool converging, float gyro_rate)
{ {
const float center_freq = calculated_notch_freq_hz[0]; const float center_freq = calculated_notch_freq_hz[0];
if (!is_equal(last_bandwidth_hz, params.bandwidth_hz()) || if (!is_equal(last_bandwidth_hz[instance], params.bandwidth_hz()) ||
!is_equal(last_attenuation_dB, params.attenuation_dB()) || !is_equal(last_attenuation_dB[instance], params.attenuation_dB()) ||
converging) { converging) {
filter[instance].init(gyro_rate, filter[instance].init(gyro_rate,
center_freq, center_freq,
params.bandwidth_hz(), params.bandwidth_hz(),
params.attenuation_dB()); params.attenuation_dB());
last_center_freq_hz = center_freq; last_center_freq_hz[instance] = center_freq;
last_bandwidth_hz = params.bandwidth_hz(); last_bandwidth_hz[instance] = params.bandwidth_hz();
last_attenuation_dB = params.attenuation_dB(); last_attenuation_dB[instance] = params.attenuation_dB();
} else if (!is_equal(last_center_freq_hz, center_freq)) { } else if (!is_equal(last_center_freq_hz[instance], center_freq)) {
if (num_calculated_notch_frequencies > 1) { if (num_calculated_notch_frequencies > 1) {
filter[instance].update(num_calculated_notch_frequencies, calculated_notch_freq_hz); filter[instance].update(num_calculated_notch_frequencies, calculated_notch_freq_hz);
} else { } else {
filter[instance].update(center_freq); 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

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