Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: ensure that notches get updated while converging

Browse Source
This commit is contained in:
Andy Piper 2024-05-02 15:06:27 +01:00 committed by Andrew Tridgell
parent 1fe7f6b099
commit f78eb58fb4
2 changed files with 3 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -1850,7 +1850,9 @@ void AP_InertialSensor::HarmonicNotch::update_params(uint8_t instance, bool conv
last_center_freq_hz[instance] = params.center_freq_hz(); last_center_freq_hz[instance] = params.center_freq_hz();
last_bandwidth_hz[instance] = params.bandwidth_hz(); last_bandwidth_hz[instance] = params.bandwidth_hz();
last_attenuation_dB[instance] = params.attenuation_dB(); last_attenuation_dB[instance] = params.attenuation_dB();
} else if (params.tracking_mode() != HarmonicNotchDynamicMode::Fixed) { }
if (params.tracking_mode() != HarmonicNotchDynamicMode::Fixed) {
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 {

6
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -449,12 +449,6 @@ public:
float calculated_notch_freq_hz[INS_MAX_NOTCHES]; float calculated_notch_freq_hz[INS_MAX_NOTCHES];
uint8_t num_calculated_notch_frequencies; uint8_t num_calculated_notch_frequencies;
// Update the harmonic notch frequency
void update_notch_freq_hz(float scaled_freq);
// Update the harmonic notch frequencies
void update_notch_frequencies_hz(uint8_t num_freqs, const float scaled_freq[]);
// runtime update of notch parameters // runtime update of notch parameters
void update_params(uint8_t instance, bool converging, float gyro_rate); void update_params(uint8_t instance, bool converging, float gyro_rate);