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

AP_InertialSensor: allow FFT window to be captured post-filter

Browse Source 
selectively apply filters twice to make sure that FFT is not tracking itself
clean-up notch disablement for non-DSP builds
use filter gyro data for FFT when analysing post-filter
always apply LPF after all of the notches
move to a phased application of window information for FFT
tap FFT gyro window at the appropriate place for downsampled FFTs
add accessor for FFT notch
document notch uses cases and flow
This commit is contained in:
Andy Piper 2022-09-22 15:35:48 +01:00 committed by Andrew Tridgell
parent 60dcb0423a
commit 90cdc1aa52
4 changed files with 88 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -846,6 +846,18 @@ bool AP_InertialSensor::set_gyro_window_size(uint16_t size) {
return true;
}
#if HAL_WITH_DSP
bool AP_InertialSensor::has_fft_notch() const
{
for (auto &notch : harmonic_notches) {
if (notch.params.enabled() && notch.params.tracking_mode() == HarmonicNotchDynamicMode::UpdateGyroFFT) {
return true;
}
}
return false;
}
#endif
void
AP_InertialSensor::init(uint16_t loop_rate)
{
@ -881,6 +893,33 @@ AP_InertialSensor::init(uint16_t loop_rate)
#if HAL_WITH_DSP
AP_GyroFFT* fft = AP::fft();
bool fft_enabled = fft != nullptr && fft->enabled();
if (fft_enabled) {
_post_filter_fft = fft->using_post_filter_samples();
}
// calculate the position that the FFT window needs to be applied
// Use cases:
// Gyro -> FFT window -> FFT Notch1/2 -> Non-FFT Notch2/1 -> LPF -> Filtered Gyro -- Phase 0
// Gyro -> FFT window -> Non-FFT Notch1/2 -> LPF -> Filtered Gyro -- Phase 0
// Gyro -> Non-FFT Notch1 -> Filtered FFT Window -> FFT Notch2 -> LPF -> Filtered Gyro -- Phase 1
// Gyro -> Non-FFT Notch1/2 -> Non-FFT Notch1/2 -> Filtered FFT Window -> LPF -> Filtered Gyro -- Phase 2
// Gyro -> Non-FFT Notch1/2 -> Filtered FFT Window -> LPF -> Filtered Gyro -- Phase 1
// Gyro -> Filtered FFT Window -> LPF -> Filtered Gyro -- Phase 0
// Gyro -> FFT window -> LPF -> Filtered Gyro -- Phase 0
// Gyro -> Notch1/2 -> LPF -> Filtered Gyro
if (_post_filter_fft) {
for (auto &notch : harmonic_notches) {
if (!notch.params.enabled()) {
continue;
}
// window must always come before any FFT notch
if (notch.params.tracking_mode() == HarmonicNotchDynamicMode::UpdateGyroFFT) {
break;
}
_fft_window_phase++;
}
}
#else
bool fft_enabled = false;
#endif

10
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -186,11 +186,12 @@ public:
// FFT support access
#if HAL_WITH_DSP
const Vector3f &get_raw_gyro(void) const { return _gyro_raw[_primary_gyro]; }
const Vector3f& get_gyro_for_fft(void) const { return _gyro_for_fft[_primary_gyro]; }
FloatBuffer& get_raw_gyro_window(uint8_t instance, uint8_t axis) { return _gyro_window[instance][axis]; }
FloatBuffer& get_raw_gyro_window(uint8_t axis) { return get_raw_gyro_window(_primary_gyro, axis); }
uint16_t get_raw_gyro_rate_hz() const { return get_raw_gyro_rate_hz(_primary_gyro); }
uint16_t get_raw_gyro_rate_hz(uint8_t instance) const { return _gyro_raw_sample_rates[_primary_gyro]; }
bool has_fft_notch() const;
#endif
bool set_gyro_window_size(uint16_t size);
// get accel offsets in m/s/s
@ -525,9 +526,14 @@ private:
Vector3f _gyro_filtered[INS_MAX_INSTANCES];
#if HAL_WITH_DSP
// Thread-safe public version of _last_raw_gyro
Vector3f _gyro_raw[INS_MAX_INSTANCES];
Vector3f _gyro_for_fft[INS_MAX_INSTANCES];
Vector3f _last_gyro_for_fft[INS_MAX_INSTANCES];
FloatBuffer _gyro_window[INS_MAX_INSTANCES][XYZ_AXIS_COUNT];
uint16_t _gyro_window_size;
// capture a gyro window after the filters
LowPassFilter2pVector3f _post_filter_gyro_filter[INS_MAX_INSTANCES];
bool _post_filter_fft;
uint8_t _fft_window_phase;
#endif
bool _new_accel_data[INS_MAX_INSTANCES];
bool _new_gyro_data[INS_MAX_INSTANCES];

62
libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
View File

@ -174,11 +174,38 @@ void AP_InertialSensor_Backend::_publish_gyro(uint8_t instance, const Vector3f &
_imu._delta_angle_acc_dt[instance] = 0;
}
void AP_InertialSensor_Backend::save_gyro_window(const uint8_t instance, const Vector3f &gyro, uint8_t phase)
{
#if HAL_WITH_DSP
// capture gyro window for FFT analysis
if (_imu._fft_window_phase == phase) {
if (_imu._gyro_window_size > 0) {
Vector3f scaled_gyro = gyro * _imu._gyro_raw_sampling_multiplier[instance];
// LPF always must come last to remove high-frequency shot noise, but the FFT still
// needs to see the same data so gets its own LPF at the tap point
if (_imu._post_filter_fft) {
scaled_gyro = _imu._post_filter_gyro_filter[instance].apply(scaled_gyro);
}
_imu._gyro_window[instance][0].push(scaled_gyro.x);
_imu._gyro_window[instance][1].push(scaled_gyro.y);
_imu._gyro_window[instance][2].push(scaled_gyro.z);
_imu._last_gyro_for_fft[instance] = scaled_gyro;
} else {
_imu._last_gyro_for_fft[instance] = gyro * _imu._gyro_raw_sampling_multiplier[instance];;
}
}
#endif
}
/*
apply harmonic notch and low pass gyro filters
*/
void AP_InertialSensor_Backend::apply_gyro_filters(const uint8_t instance, const Vector3f &gyro)
{
uint8_t filter_phase = 0;
save_gyro_window(instance, gyro, filter_phase++);
Vector3f gyro_filtered = gyro;
// apply the harmonic notch filters
@ -203,14 +230,18 @@ void AP_InertialSensor_Backend::apply_gyro_filters(const uint8_t instance, const
} else {
gyro_filtered = notch.filter[instance].apply(gyro_filtered);
}
save_gyro_window(instance, gyro_filtered, filter_phase++);
}
// apply the low pass filter last to attentuate any notch induced noise
// apply the low pass filter last to attenuate any notch induced noise
gyro_filtered = _imu._gyro_filter[instance].apply(gyro_filtered);
// if the filtering failed in any way then reset the filters and keep the old value
if (gyro_filtered.is_nan() || gyro_filtered.is_inf()) {
_imu._gyro_filter[instance].reset();
#if HAL_WITH_DSP
_imu._post_filter_gyro_filter[instance].reset();
#endif
for (auto &notch : _imu.harmonic_notches) {
notch.filter[instance].reset();
}
@ -300,17 +331,8 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
// save previous delta angle for coning correction
_imu._last_delta_angle[instance] = delta_angle;
_imu._last_raw_gyro[instance] = gyro;
#if HAL_WITH_DSP
// capture gyro window for FFT analysis
if (_imu._gyro_window_size > 0) {
const Vector3f& scaled_gyro = gyro * _imu._gyro_raw_sampling_multiplier[instance];
_imu._gyro_window[instance][0].push(scaled_gyro.x);
_imu._gyro_window[instance][1].push(scaled_gyro.y);
_imu._gyro_window[instance][2].push(scaled_gyro.z);
}
#endif
// apply gyro filters
// apply gyro filters and sample for FFT
apply_gyro_filters(instance, gyro);
_imu._new_gyro_data[instance] = true;
@ -402,17 +424,8 @@ void AP_InertialSensor_Backend::_notify_new_delta_angle(uint8_t instance, const
// save previous delta angle for coning correction
_imu._last_delta_angle[instance] = delta_angle;
_imu._last_raw_gyro[instance] = gyro;
#if HAL_WITH_DSP
// capture gyro window for FFT analysis
if (_imu._gyro_window_size > 0) {
const Vector3f& scaled_gyro = gyro * _imu._gyro_raw_sampling_multiplier[instance];
_imu._gyro_window[instance][0].push(scaled_gyro.x);
_imu._gyro_window[instance][1].push(scaled_gyro.y);
_imu._gyro_window[instance][2].push(scaled_gyro.z);
}
#endif
// apply gyro filters
// apply gyro filters and sample for FFT
apply_gyro_filters(instance, gyro);
_imu._new_gyro_data[instance] = true;
@ -711,9 +724,9 @@ void AP_InertialSensor_Backend::update_gyro(uint8_t instance) /* front end */
}
if (_imu._new_gyro_data[instance]) {
_publish_gyro(instance, _imu._gyro_filtered[instance]);
// copy the gyro samples from the backend to the frontend window
#if HAL_WITH_DSP
_imu._gyro_raw[instance] = _imu._last_raw_gyro[instance] * _imu._gyro_raw_sampling_multiplier[instance];
// copy the gyro samples from the backend to the frontend window for FFTs sampling at less than IMU rate
_imu._gyro_for_fft[instance] = _imu._last_gyro_for_fft[instance];
#endif
_imu._new_gyro_data[instance] = false;
}
@ -723,6 +736,9 @@ void AP_InertialSensor_Backend::update_gyro(uint8_t instance) /* front end */
if (_last_gyro_filter_hz != _gyro_filter_cutoff() || sensors_converging()) {
_imu._gyro_filter[instance].set_cutoff_frequency(gyro_rate, _gyro_filter_cutoff());
#if HAL_WITH_DSP
_imu._post_filter_gyro_filter[instance].set_cutoff_frequency(gyro_rate, _gyro_filter_cutoff());
#endif
_last_gyro_filter_hz = _gyro_filter_cutoff();
}

3
libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
View File

@ -142,8 +142,9 @@ protected:
// rotate gyro vector, offset and publish
void _publish_gyro(uint8_t instance, const Vector3f &gyro) __RAMFUNC__; /* front end */
// apply notch and lowpass gyro filters
// apply notch and lowpass gyro filters and sample for FFT
void apply_gyro_filters(const uint8_t instance, const Vector3f &gyro);
void save_gyro_window(const uint8_t instance, const Vector3f &gyro, uint8_t phase);
// this should be called every time a new gyro raw sample is
// available - be it published or not the sample is raw in the