Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Filtering: re-work harmonic notch filter freq clamping and disable #25442

Merged
merged 16 commits into from
Mar 15, 2024
Merged

Filtering: re-work harmonic notch filter freq clamping and disable #25442

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
16 commits
Select commit Hold shift + click to select a range
22f2b3f
Filter: rework harmonic notch
tridge Nov 2, 2023
d24c2be
AP_Vehicle: removed the clamping of notch filters at vehicle level
tridge Nov 2, 2023
210b2a1
AP_InertialSensor: removed zero checks and clamping on notch filters
tridge Nov 2, 2023
1559dcd
Filter: fixed test suite
tridge Nov 2, 2023
ffc6590
Filter: fixed filter expansion for other than motors based RPM sources
tridge Nov 6, 2023
5706eab
AP_InertialSensor: fixed num_filters to be uint16_t
tridge Nov 6, 2023
4b3780e
Filter: adjust attenuation properly at low frequencies
tridge Nov 6, 2023
210ea26
Filter: added test for attenuation adjustment
tridge Jan 29, 2024
f2c9f5a
AP_InertialSensor: use num_composite_notches()
tridge Jan 21, 2024
f38f154
AP_Logger: added VER.FV for filter version
tridge Jan 30, 2024
b4a493f
AP_InertialSensor: added comments
tridge Jan 30, 2024
088e1d3
Filter: added logging of notch centers and 1st harmonic
tridge Feb 19, 2024
d6a9627
AP_InertialSensor: call logging function for notch centers and harmonics
tridge Feb 19, 2024
07cd9f8
AP_InertialSensor: adjust logging descriptions
tridge Mar 11, 2024
4a921e1
Filter: log up to 6 harmonics
tridge Mar 11, 2024
f45fd83
Filter: use a define for nyquist cutoff
tridge Mar 11, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
48 changes: 18 additions & 30 deletions libraries/AP_InertialSensor/AP_InertialSensor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -986,9 +986,6 @@ AP_InertialSensor::init(uint16_t loop_rate)
if (!notch.params.enabled() && !fft_enabled) {
continue;
}
for (uint8_t i = 0; i < ARRAY_SIZE(notch.calculated_notch_freq_hz); i++) {
notch.calculated_notch_freq_hz[i] = notch.params.center_freq_hz();
}
notch.num_calculated_notch_frequencies = 1;
notch.num_dynamic_notches = 1;
#if APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_ArduPlane)
Expand All @@ -1015,15 +1012,13 @@ AP_InertialSensor::init(uint16_t loop_rate)
sensors_used += _use(i);
}

uint8_t num_filters = 0;
uint16_t num_filters = 0;
for (auto &notch : harmonic_notches) {
// calculate number of notches we might want to use for harmonic notch
if (notch.params.enabled() || fft_enabled) {
const bool double_notch = notch.params.hasOption(HarmonicNotchFilterParams::Options::DoubleNotch);
const bool triple_notch = notch.params.hasOption(HarmonicNotchFilterParams::Options::TripleNotch);
const bool all_sensors = notch.params.hasOption(HarmonicNotchFilterParams::Options::EnableOnAllIMUs);
num_filters += __builtin_popcount(notch.params.harmonics())
* notch.num_dynamic_notches * (double_notch ? 2 : triple_notch ? 3 : 1)
* notch.num_dynamic_notches * notch.params.num_composite_notches()
* (all_sensors?sensors_used:1);
}
}
Expand All @@ -1038,13 +1033,10 @@ AP_InertialSensor::init(uint16_t loop_rate)
if (_use(i)) {
for (auto &notch : harmonic_notches) {
if (notch.params.enabled() || fft_enabled) {
const bool double_notch = notch.params.hasOption(HarmonicNotchFilterParams::Options::DoubleNotch);
const bool triple_notch = notch.params.hasOption(HarmonicNotchFilterParams::Options::TripleNotch);
notch.filter[i].allocate_filters(notch.num_dynamic_notches,
notch.params.harmonics(), double_notch ? 2 : triple_notch ? 3 : 1);
notch.params.harmonics(), notch.params.num_composite_notches());
// initialise default settings, these will be subsequently changed in AP_InertialSensor_Backend::update_gyro()
notch.filter[i].init(_gyro_raw_sample_rates[i], notch.calculated_notch_freq_hz[0],
notch.params.bandwidth_hz(), notch.params.attenuation_dB());
notch.filter[i].init(_gyro_raw_sample_rates[i], notch.params);
}
}
}
Expand Down Expand Up @@ -1835,25 +1827,21 @@ void AP_InertialSensor::_save_gyro_calibration()
*/
void AP_InertialSensor::HarmonicNotch::update_params(uint8_t instance, bool converging, float gyro_rate)
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Maybe this is the existing behavior, but this looks like I could not change bandwidth or attenuation at runtime for any of the moving notches?

Why not call a init for param change for all types? Then just call update if moving.

Should we also track the enable param? What if I turn on or off a notch at run time?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think that is existing behaviour?

{
const float center_freq = calculated_notch_freq_hz[0];
if (!is_equal(last_bandwidth_hz[instance], params.bandwidth_hz()) ||
!is_equal(last_attenuation_dB[instance], params.attenuation_dB()) ||
(params.tracking_mode() == HarmonicNotchDynamicMode::Fixed && !is_equal(last_center_freq_hz[instance], center_freq)) ||
(params.tracking_mode() == HarmonicNotchDynamicMode::Fixed &&
!is_equal(last_center_freq_hz[instance], params.center_freq_hz())) ||
converging) {
filter[instance].init(gyro_rate,
center_freq,
params.bandwidth_hz(),
params.attenuation_dB());
last_center_freq_hz[instance] = center_freq;
filter[instance].init(gyro_rate, params);
last_center_freq_hz[instance] = params.center_freq_hz();
last_bandwidth_hz[instance] = params.bandwidth_hz();
last_attenuation_dB[instance] = params.attenuation_dB();
} else if (params.tracking_mode() != HarmonicNotchDynamicMode::Fixed) {
if (num_calculated_notch_frequencies > 1) {
filter[instance].update(num_calculated_notch_frequencies, calculated_notch_freq_hz);
} else {
filter[instance].update(center_freq);
filter[instance].update(calculated_notch_freq_hz[0]);
}
last_center_freq_hz[instance] = center_freq;
}
}

Expand Down Expand Up @@ -2297,23 +2285,23 @@ void AP_InertialSensor::acal_update()
}
#endif

// Update the harmonic notch frequency
/*
Update the harmonic notch frequency

Note that zero is a valid value and will disable the notch unless
the TreatLowAsMin option is set
*/
void AP_InertialSensor::HarmonicNotch::update_freq_hz(float scaled_freq)
{
// protect against zero as the scaled frequency
if (is_positive(scaled_freq)) {
calculated_notch_freq_hz[0] = scaled_freq;
}
calculated_notch_freq_hz[0] = scaled_freq;
num_calculated_notch_frequencies = 1;
}

// Update the harmonic notch frequency
void AP_InertialSensor::HarmonicNotch::update_frequencies_hz(uint8_t num_freqs, const float scaled_freq[]) {
// protect against zero as the scaled frequency
// note that we allow zero through, which will disable the notch
for (uint8_t i = 0; i < num_freqs; i++) {
if (is_positive(scaled_freq[i])) {
calculated_notch_freq_hz[i] = scaled_freq[i];
}
calculated_notch_freq_hz[i] = scaled_freq[i];
}
// any uncalculated frequencies will float at the previous value or the initialized freq if none
num_calculated_notch_frequencies = num_freqs;
Expand Down
38 changes: 22 additions & 16 deletions libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -144,28 +144,29 @@ bool AP_InertialSensor::BatchSampler::Write_ISBD() const
// @Description: Filter Tuning Message - per motor
// @Field: TimeUS: microseconds since system startup
// @Field: I: instance
// @Field: NDn: number of active dynamic harmonic notches
// @Field: NF1: dynamic harmonic notch centre frequency for motor 1
// @Field: NF2: dynamic harmonic notch centre frequency for motor 2
// @Field: NF3: dynamic harmonic notch centre frequency for motor 3
// @Field: NF4: dynamic harmonic notch centre frequency for motor 4
// @Field: NF5: dynamic harmonic notch centre frequency for motor 5
// @Field: NF6: dynamic harmonic notch centre frequency for motor 6
// @Field: NF7: dynamic harmonic notch centre frequency for motor 7
// @Field: NF8: dynamic harmonic notch centre frequency for motor 8
// @Field: NF9: dynamic harmonic notch centre frequency for motor 9
// @Field: NF10: dynamic harmonic notch centre frequency for motor 10
// @Field: NF11: dynamic harmonic notch centre frequency for motor 11
// @Field: NF12: dynamic harmonic notch centre frequency for motor 12
// @Field: NDn: number of active harmonic notches
// @Field: NF1: desired harmonic notch centre frequency for motor 1
// @Field: NF2: desired harmonic notch centre frequency for motor 2
// @Field: NF3: desired harmonic notch centre frequency for motor 3
// @Field: NF4: desired harmonic notch centre frequency for motor 4
// @Field: NF5: desired harmonic notch centre frequency for motor 5
// @Field: NF6: desired harmonic notch centre frequency for motor 6
// @Field: NF7: desired harmonic notch centre frequency for motor 7
// @Field: NF8: desired harmonic notch centre frequency for motor 8
// @Field: NF9: desired harmonic notch centre frequency for motor 9
// @Field: NF10: desired harmonic notch centre frequency for motor 10
// @Field: NF11: desired harmonic notch centre frequency for motor 11
// @Field: NF12: desired harmonic notch centre frequency for motor 12

// @LoggerMessage: FTNS
// @Description: Filter Tuning Message
// @Field: TimeUS: microseconds since system startup
// @Field: I: instance
// @Field: NF: dynamic harmonic notch centre frequency
// @Field: NF: desired harmonic notch centre frequency

void AP_InertialSensor::write_notch_log_messages() const
{
const uint64_t now_us = AP_HAL::micros64();
for (auto &notch : harmonic_notches) {
const uint8_t i = &notch - &harmonic_notches[0];
if (!notch.params.enabled()) {
Expand All @@ -176,7 +177,7 @@ void AP_InertialSensor::write_notch_log_messages() const
// log per motor center frequencies
AP::logger().WriteStreaming(
"FTN", "TimeUS,I,NDn,NF1,NF2,NF3,NF4,NF5,NF6,NF7,NF8,NF9,NF10,NF11,NF12", "s#-zzzzzzzzzzzz", "F--------------", "QBBffffffffffff",
AP_HAL::micros64(),
now_us,
i,
notch.num_calculated_notch_frequencies,
notches[0], notches[1], notches[2], notches[3],
Expand All @@ -186,10 +187,15 @@ void AP_InertialSensor::write_notch_log_messages() const
// log single center frequency
AP::logger().WriteStreaming(
"FTNS", "TimeUS,I,NF", "s#z", "F--", "QBf",
AP_HAL::micros64(),
now_us,
i,
notches[0]);
}

// ask the HarmonicNotchFilter object for primary gyro to
// log the actual notch centers
const uint8_t primary_gyro = AP::ahrs().get_primary_gyro_index();
notch.filter[primary_gyro].log_notch_centers(i, now_us);
}
}

Expand Down
2 changes: 2 additions & 0 deletions libraries/AP_Logger/AP_Logger_Backend.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,7 @@
#include <AP_Scheduler/AP_Scheduler.h>
#include <AP_Rally/AP_Rally.h>
#include <AP_Vehicle/AP_Vehicle_Type.h>
#include <Filter/Filter.h>
#include "AP_Logger.h"

#if HAL_LOGGER_FENCE_ENABLED
Expand Down Expand Up @@ -574,6 +575,7 @@ bool AP_Logger_Backend::Write_VER()
patch: fwver.patch,
fw_type: fwver.fw_type,
git_hash: fwver.fw_hash,
filter_version: AP_FILTER_VERSION,
};
strncpy(pkt.fw_string, fwver.fw_string, ARRAY_SIZE(pkt.fw_string)-1);

Expand Down
4 changes: 3 additions & 1 deletion libraries/AP_Logger/LogStructure.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -660,6 +660,7 @@ struct PACKED log_VER {
char fw_string[64];
uint16_t _APJ_BOARD_ID;
uint8_t build_type;
uint8_t filter_version;
};


Expand Down Expand Up @@ -1170,6 +1171,7 @@ struct PACKED log_VER {
// @Field: FWS: Firmware version string
// @Field: APJ: Board ID
// @Field: BU: Build vehicle type
// @Field: FV: Filter version

// @LoggerMessage: MOTB
// @Description: Motor mixer information
Expand Down Expand Up @@ -1300,7 +1302,7 @@ LOG_STRUCTURE_FROM_AIS \
{ LOG_SCRIPTING_MSG, sizeof(log_Scripting), \
"SCR", "QNIii", "TimeUS,Name,Runtime,Total_mem,Run_mem", "s#sbb", "F-F--", true }, \
{ LOG_VER_MSG, sizeof(log_VER), \
"VER", "QBHBBBBIZHB", "TimeUS,BT,BST,Maj,Min,Pat,FWT,GH,FWS,APJ,BU", "s----------", "F----------", false }, \
"VER", "QBHBBBBIZHBB", "TimeUS,BT,BST,Maj,Min,Pat,FWT,GH,FWS,APJ,BU,FV", "s-----------", "F-----------", false }, \
{ LOG_MOTBATT_MSG, sizeof(log_MotBatt), \
"MOTB", "QfffffB", "TimeUS,LiftMax,BatVolt,ThLimit,ThrAvMx,ThrOut,FailFlags", "s------", "F------" , true }

Expand Down
35 changes: 6 additions & 29 deletions libraries/AP_Vehicle/AP_Vehicle.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -750,7 +750,6 @@ void AP_Vehicle::update_throttle_notch(AP_InertialSensor::HarmonicNotch &notch)
#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)||APM_BUILD_COPTER_OR_HELI||APM_BUILD_TYPE(APM_BUILD_Rover)
const float ref_freq = notch.params.center_freq_hz();
const float ref = notch.params.reference();
const float min_ratio = notch.params.freq_min_ratio();

#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)||APM_BUILD_COPTER_OR_HELI
const AP_Motors* motors = AP::motors();
Expand All @@ -760,7 +759,7 @@ void AP_Vehicle::update_throttle_notch(AP_InertialSensor::HarmonicNotch &notch)
const float motors_throttle = motors != nullptr ? abs(motors->get_throttle() / 100.0f) : 0;
#endif

float throttle_freq = ref_freq * MAX(min_ratio, sqrtf(motors_throttle / ref));
float throttle_freq = ref_freq * sqrtf(MAX(0,motors_throttle) / ref);

notch.update_freq_hz(throttle_freq);
#endif
Expand All @@ -780,17 +779,6 @@ void AP_Vehicle::update_dynamic_notch(AP_InertialSensor::HarmonicNotch &notch)
return;
}

#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)||APM_BUILD_COPTER_OR_HELI
const AP_Motors* motors = AP::motors();
if (motors != nullptr && motors->get_spool_state() == AP_Motors::SpoolState::SHUT_DOWN) {
notch.set_inactive(true);
} else {
notch.set_inactive(false);
}
#else // APM_BUILD_Rover: keep notch active
notch.set_inactive(false);
#endif

switch (notch.params.tracking_mode()) {
case HarmonicNotchDynamicMode::UpdateThrottle: // throttle based tracking
// set the harmonic notch filter frequency approximately scaled on motor rpm implied by throttle
Expand All @@ -805,9 +793,9 @@ void AP_Vehicle::update_dynamic_notch(AP_InertialSensor::HarmonicNotch &notch)
float rpm;
if (rpm_sensor != nullptr && rpm_sensor->get_rpm(sensor, rpm)) {
// set the harmonic notch filter frequency from the main rotor rpm
notch.update_freq_hz(MAX(ref_freq * notch.params.freq_min_ratio(), rpm * ref * (1.0/60)));
notch.update_freq_hz(rpm * ref * (1.0/60));
} else {
notch.update_freq_hz(ref_freq);
notch.update_freq_hz(0);
}
break;
}
Expand All @@ -819,18 +807,13 @@ void AP_Vehicle::update_dynamic_notch(AP_InertialSensor::HarmonicNotch &notch)
float notches[INS_MAX_NOTCHES];
// ESC telemetry will return 0 for missing data, but only after 1s
const uint8_t num_notches = AP::esc_telem().get_motor_frequencies_hz(INS_MAX_NOTCHES, notches);
for (uint8_t i = 0; i < num_notches; i++) {
if (!is_zero(notches[i])) {
notches[i] = MAX(ref_freq, notches[i]);
}
}
if (num_notches > 0) {
notch.update_frequencies_hz(num_notches, notches);
} else { // throttle fallback
update_throttle_notch(notch);
}
} else {
notch.update_freq_hz(MAX(ref_freq, AP::esc_telem().get_average_motor_frequency_hz() * ref));
notch.update_freq_hz(AP::esc_telem().get_average_motor_frequency_hz() * ref);
}
break;
#endif
Expand All @@ -842,20 +825,14 @@ void AP_Vehicle::update_dynamic_notch(AP_InertialSensor::HarmonicNotch &notch)
const uint8_t peaks = gyro_fft.get_weighted_noise_center_frequencies_hz(notch.num_dynamic_notches, notches);

if (peaks > 0) {
for (uint8_t i = 0; i < peaks; i++) {
notches[i] = MAX(ref_freq, notches[i]);
}
notch.set_inactive(false);
notch.update_frequencies_hz(peaks, notches);
} else { // since FFT can be used post-filter it is better to disable the notch when there is no data
notch.set_inactive(true);
}
} else {
float center_freq = gyro_fft.get_weighted_noise_center_freq_hz();
if (!is_zero(center_freq)) {
notch.update_freq_hz(MAX(ref_freq, center_freq));
} else { // since FFT can be used post-filter it is better to disable the notch when there is no data
notch.set_inactive(true);
}
notch.update_freq_hz(center_freq);
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think zero is special in the FFT engine here - we should check - so this code might be wrong to remove

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

can you check that? looks like zero could be returned if unhealthy.

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yes zero is a special value here. Unhealthy simply means that no peaks were detected, which is quite common in post-filter analysis hence why we need to switch off the notches. Same is actually true for the first if clause above it so we need to make sure we disable the notches that do not have peaks - or return a special value.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

using zero will switch off the notch, unless you have TreatLowAsMin which means the user wants to keep it running anyway. So this really should be equivalent to the old code

}
break;
#endif
Expand Down
9 changes: 9 additions & 0 deletions libraries/Filter/Filter.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,3 +9,12 @@
#include "LowPassFilter.h"
#include "ModeFilter.h"
#include "Butter.h"

/*
the filter version is logged in the VER message to assist the online
analysis tools, so they can display the right filter formulas for
this version of the code
This should be incremented on significant filtering changes
*/
#define AP_FILTER_VERSION 2

Loading
Loading