Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-21 23:33:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

Ap_Inertialsensor: by default only run harmonic notch on the active gyro

Browse Source 
this should save quite a lot of CPU. Only the active gyro impacts
vehicle flight
This commit is contained in:
Andrew Tridgell 2022-02-17 16:43:09 +11:00
parent 9e3a29bf86
commit a37f268dc6
2 changed files with 46 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

83
libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
View File

@ -174,6 +174,45 @@ void AP_InertialSensor_Backend::_publish_gyro(uint8_t instance, const Vector3f &
_imu._delta_angle_acc_dt[instance] = 0;
}
/*
apply harmonic notch and low pass gyro filters
*/
void AP_InertialSensor_Backend::apply_gyro_filters(const uint8_t instance, const Vector3f &gyro)
{
Vector3f gyro_filtered = gyro;
// apply the harmonic notch filters
for (auto &notch : _imu.harmonic_notches) {
if (!notch.params.enabled()) {
continue;
}
#ifndef HAL_BUILD_AP_PERIPH
// by default we only run the expensive notch filters on the
// currently active IMU we reset the inactive notch filters so
// that if we switch IMUs we're not left with old data
if (!notch.params.hasOption(HarmonicNotchFilterParams::Options::EnableOnAllIMUs) &&
instance != AP::ahrs().get_primary_gyro_index()) {
notch.filter[instance].reset();
continue;
}
#endif
gyro_filtered = notch.filter[instance].apply(gyro_filtered);
}
// apply the low pass filter last to attentuate 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();
for (auto &notch : _imu.harmonic_notches) {
notch.filter[instance].reset();
}
} else {
_imu._gyro_filtered[instance] = gyro_filtered;
}
}
void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
const Vector3f &gyro,
uint64_t sample_us)
@ -264,27 +303,9 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
_imu._gyro_window[instance][2].push(scaled_gyro.z);
}
#endif
Vector3f gyro_filtered = gyro;
// apply the harmonic notch filter
for (auto &notch : _imu.harmonic_notches) {
if (notch.params.enabled()) {
gyro_filtered = notch.filter[instance].apply(gyro_filtered);
}
}
// apply the low pass filter last to attentuate 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();
for (auto &notch : _imu.harmonic_notches) {
notch.filter[instance].reset();
}
} else {
_imu._gyro_filtered[instance] = gyro_filtered;
}
// apply gyro filters
apply_gyro_filters(instance, gyro);
_imu._new_gyro_data[instance] = true;
}
@ -383,27 +404,9 @@ void AP_InertialSensor_Backend::_notify_new_delta_angle(uint8_t instance, const
_imu._gyro_window[instance][2].push(scaled_gyro.z);
}
#endif
Vector3f gyro_filtered = gyro;
// apply the harmonic notch filters
for (auto &notch : _imu.harmonic_notches) {
if (notch.params.enabled()) {
gyro_filtered = notch.filter[instance].apply(gyro_filtered);
}
}
// apply the low pass filter last to attentuate 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();
for (auto &notch : _imu.harmonic_notches) {
notch.filter[instance].reset();
}
} else {
_imu._gyro_filtered[instance] = gyro_filtered;
}
// apply gyro filters
apply_gyro_filters(instance, gyro);
_imu._new_gyro_data[instance] = true;
}

3
libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
View File

@ -140,6 +140,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
void apply_gyro_filters(const uint8_t instance, const Vector3f &gyro);
// this should be called every time a new gyro raw sample is
// available - be it published or not the sample is raw in the
// sense that it's not filtered yet, but it must be rotated and