Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-12 19:03:58 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: add throttle-based notch setup function

Browse Source 
allocate harmonic notch filter if FFT is enabled
This commit is contained in:
Andy Piper 2022-03-06 15:00:30 +00:00 committed by Andrew Tridgell
parent 0fa0a27c77
commit e0b7e5f2a5
2 changed files with 42 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -13,6 +13,7 @@
#include <AP_AHRS/AP_AHRS.h> #include <AP_AHRS/AP_AHRS.h>
#include <AP_AHRS/AP_AHRS_View.h> #include <AP_AHRS/AP_AHRS_View.h>
#include <AP_ExternalAHRS/AP_ExternalAHRS.h> #include <AP_ExternalAHRS/AP_ExternalAHRS.h>
#include <AP_GyroFFT/AP_GyroFFT.h>
#if !APM_BUILD_TYPE(APM_BUILD_Rover) #if !APM_BUILD_TYPE(APM_BUILD_Rover)
#include <AP_Motors/AP_Motors_Class.h> #include <AP_Motors/AP_Motors_Class.h>
#endif #endif
@ -874,11 +875,17 @@ AP_InertialSensor::init(uint16_t loop_rate)
// initialise IMU batch logging // initialise IMU batch logging
batchsampler.init(); batchsampler.init();
#if HAL_WITH_DSP
AP_GyroFFT* fft = AP::fft();
bool fft_enabled = fft != nullptr && fft->enabled();
#else
bool fft_enabled = false;
#endif
// the center frequency of the harmonic notch is always taken from the calculated value so that it can be updated // the center frequency of the harmonic notch is always taken from the calculated value so that it can be updated
// dynamically, the calculated value is always some multiple of the configured center frequency, so start with the // dynamically, the calculated value is always some multiple of the configured center frequency, so start with the
// configured value // configured value
for (auto &notch : harmonic_notches) { for (auto &notch : harmonic_notches) {
if (!notch.params.enabled()) { if (!notch.params.enabled() && !fft_enabled) {
continue; continue;
} }
notch.calculated_notch_freq_hz[0] = notch.params.center_freq_hz(); notch.calculated_notch_freq_hz[0] = notch.params.center_freq_hz();
@ -911,7 +918,7 @@ AP_InertialSensor::init(uint16_t loop_rate)
uint8_t num_filters = 0; uint8_t num_filters = 0;
for (auto &notch : harmonic_notches) { for (auto &notch : harmonic_notches) {
// calculate number of notches we might want to use for harmonic notch // calculate number of notches we might want to use for harmonic notch
if (notch.params.enabled()) { if (notch.params.enabled() || fft_enabled) {
const bool double_notch = notch.params.hasOption(HarmonicNotchFilterParams::Options::DoubleNotch); const bool double_notch = notch.params.hasOption(HarmonicNotchFilterParams::Options::DoubleNotch);
num_filters += __builtin_popcount(notch.params.harmonics()) num_filters += __builtin_popcount(notch.params.harmonics())
* notch.num_dynamic_notches * (double_notch ? 2 : 1) * notch.num_dynamic_notches * (double_notch ? 2 : 1)
@ -928,7 +935,7 @@ AP_InertialSensor::init(uint16_t loop_rate)
// only allocate notches for IMUs in use // only allocate notches for IMUs in use
if (_use[i]) { if (_use[i]) {
for (auto &notch : harmonic_notches) { for (auto &notch : harmonic_notches) {
if (notch.params.enabled()) { if (notch.params.enabled() || fft_enabled) {
const bool double_notch = notch.params.hasOption(HarmonicNotchFilterParams::Options::DoubleNotch); const bool double_notch = notch.params.hasOption(HarmonicNotchFilterParams::Options::DoubleNotch);
notch.filter[i].allocate_filters(notch.num_dynamic_notches, notch.filter[i].allocate_filters(notch.num_dynamic_notches,
notch.params.harmonics(), double_notch); notch.params.harmonics(), double_notch);
@ -2067,6 +2074,25 @@ void AP_InertialSensor::HarmonicNotch::update_frequencies_hz(uint8_t num_freqs,
num_calculated_notch_frequencies = num_freqs; num_calculated_notch_frequencies = num_freqs;
} }
// setup the notch for throttle based tracking, called from FFT based tuning
bool AP_InertialSensor::setup_throttle_gyro_harmonic_notch(float center_freq_hz, float ref)
{
for (auto &notch : harmonic_notches) {
if (notch.params.tracking_mode() != HarmonicNotchDynamicMode::UpdateThrottle) {
continue;
}
notch.params.enable();
notch.params.set_center_freq_hz(center_freq_hz);
notch.params.set_reference(ref);
notch.params.set_bandwidth_hz(center_freq_hz / 2.0f);
notch.params.save_params();
// only enable the first notch
return true;
}
return false;
}
/* /*
set and save accelerometer bias along with trim calculation set and save accelerometer bias along with trim calculation
*/ */

3
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -253,6 +253,9 @@ public:
// get the accel filter rate in Hz // get the accel filter rate in Hz
uint16_t get_accel_filter_hz(void) const { return _accel_filter_cutoff; } uint16_t get_accel_filter_hz(void) const { return _accel_filter_cutoff; }
// setup the notch for throttle based tracking
bool setup_throttle_gyro_harmonic_notch(float center_freq_hz, float ref);
// write out harmonic notch log messages // write out harmonic notch log messages
void write_notch_log_messages() const; void write_notch_log_messages() const;