Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-13 19:40:31 +03:00
Code Issues Wiki Activity

Dimmable RPM harmonics (#12838)

Browse source
This commit is contained in:
Jan Post 2023-10-20 19:30:30 +02:00 • committed by GitHub
parent fe0e0cb5b8
commit ef34a91aa2
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
6 changed files with 48 additions and 23 deletions
Download patch file Download diff file Expand all files Collapse all files

3
src/main/blackbox/blackbox.c
View file

@ -1518,6 +1518,9 @@ static bool blackboxWriteSysinfo(void)
#endif
#ifdef USE_RPM_FILTER
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RPM_FILTER_HARMONICS, "%d", rpmFilterConfig()->rpm_filter_harmonics);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RPM_FILTER_WEIGHTS, "%d,%d,%d", rpmFilterConfig()->rpm_filter_weights[0],
rpmFilterConfig()->rpm_filter_weights[1],
rpmFilterConfig()->rpm_filter_weights[2]);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RPM_FILTER_Q, "%d", rpmFilterConfig()->rpm_filter_q);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RPM_FILTER_MIN_HZ, "%d", rpmFilterConfig()->rpm_filter_min_hz);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RPM_FILTER_FADE_RANGE_HZ, "%d", rpmFilterConfig()->rpm_filter_fade_range_hz);

1
src/main/cli/settings.c
View file

@ -1721,6 +1721,7 @@ const clivalue_t valueTable[] = {
#ifdef USE_RPM_FILTER
{ PARAM_NAME_RPM_FILTER_HARMONICS, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 3 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, rpm_filter_harmonics) },
{ PARAM_NAME_RPM_FILTER_WEIGHTS, VAR_UINT8 | MASTER_VALUE | MODE_ARRAY, .config.array.length = RPM_FILTER_HARMONICS_MAX, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, rpm_filter_weights) },
{ PARAM_NAME_RPM_FILTER_Q, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 250, 3000 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, rpm_filter_q) },
{ PARAM_NAME_RPM_FILTER_MIN_HZ, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 30, 200 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, rpm_filter_min_hz) },
{ PARAM_NAME_RPM_FILTER_FADE_RANGE_HZ, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, rpm_filter_fade_range_hz) },

1
src/main/fc/parameter_names.h
View file

@ -116,6 +116,7 @@
#define PARAM_NAME_SIMPLIFIED_GYRO_FILTER_MULTIPLIER "simplified_gyro_filter_multiplier"
#define PARAM_NAME_DEBUG_MODE "debug_mode"
#define PARAM_NAME_RPM_FILTER_HARMONICS "rpm_filter_harmonics"
#define PARAM_NAME_RPM_FILTER_WEIGHTS "rpm_filter_weights"
#define PARAM_NAME_RPM_FILTER_Q "rpm_filter_q"
#define PARAM_NAME_RPM_FILTER_MIN_HZ "rpm_filter_min_hz"
#define PARAM_NAME_RPM_FILTER_FADE_RANGE_HZ "rpm_filter_fade_range_hz"

58
src/main/flight/rpm_filter.c
View file

@ -44,7 +44,6 @@
#include "rpm_filter.h"
#define RPM_FILTER_HARMONICS_MAX 3
#define RPM_FILTER_DURATION_S 0.001f // Maximum duration allowed to update all RPM notches once
#define SECONDS_PER_MINUTE 60.0f
#define ERPM_PER_LSB 100.0f
@ -53,6 +52,7 @@
typedef struct rpmFilter_s {
int numHarmonics;
float weights[RPM_FILTER_HARMONICS_MAX];
float minHz;
float maxHz;
float fadeRangeHz;
@ -109,6 +109,10 @@ void rpmFilterInit(const rpmFilterConfig_t *config, const timeUs_t looptimeUs)
rpmFilter.q = config->rpm_filter_q / 100.0f;
rpmFilter.looptimeUs = looptimeUs;
for (int n = 0; n < RPM_FILTER_HARMONICS_MAX; n++) {
rpmFilter.weights[n] = constrainf(config->rpm_filter_weights[n] / 100.0f, 0.0f, 1.0f);
}
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
for (int motor = 0; motor < getMotorCount(); motor++) {
for (int i = 0; i < rpmFilter.numHarmonics; i++) {
@ -145,30 +149,37 @@ FAST_CODE_NOINLINE void rpmFilterUpdate(void)
// update RPM notches
for (int i = 0; i < notchUpdatesPerIteration; i++) {
// select current notch on ROLL
biquadFilter_t *template = &rpmFilter.notch[0][motorIndex][harmonicIndex];
// Only bother updating notches which can have an effect on filtered output
if (rpmFilter.weights[harmonicIndex] > 0.0f) {
const float frequencyHz = constrainf((harmonicIndex + 1) * motorFrequencyHz[motorIndex], rpmFilter.minHz, rpmFilter.maxHz);
const float marginHz = frequencyHz - rpmFilter.minHz;
// select current notch on ROLL
biquadFilter_t *template = &rpmFilter.notch[0][motorIndex][harmonicIndex];
// fade out notch when approaching minHz (turn it off)
float weight = 1.0f;
if (marginHz < rpmFilter.fadeRangeHz) {
weight = marginHz / rpmFilter.fadeRangeHz;
}
const float frequencyHz = constrainf((harmonicIndex + 1) * motorFrequencyHz[motorIndex], rpmFilter.minHz, rpmFilter.maxHz);
const float marginHz = frequencyHz - rpmFilter.minHz;
float weight = 1.0f;
// update notch
biquadFilterUpdate(template, frequencyHz, rpmFilter.looptimeUs, rpmFilter.q, FILTER_NOTCH, weight);
// fade out notch when approaching minHz (turn it off)
if (marginHz < rpmFilter.fadeRangeHz) {
weight *= marginHz / rpmFilter.fadeRangeHz;
}
// copy notch properties to corresponding notches on PITCH and YAW
for (int axis = 1; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilter_t *dest = &rpmFilter.notch[axis][motorIndex][harmonicIndex];
dest->b0 = template->b0;
dest->b1 = template->b1;
dest->b2 = template->b2;
dest->a1 = template->a1;
dest->a2 = template->a2;
dest->weight = template->weight;
// attenuate notches per harmonics group
weight *= rpmFilter.weights[harmonicIndex];
// update notch
biquadFilterUpdate(template, frequencyHz, rpmFilter.looptimeUs, rpmFilter.q, FILTER_NOTCH, weight);
// copy notch properties to corresponding notches on PITCH and YAW
for (int axis = 1; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilter_t *dest = &rpmFilter.notch[axis][motorIndex][harmonicIndex];
dest->b0 = template->b0;
dest->b1 = template->b1;
dest->b2 = template->b2;
dest->a1 = template->a1;
dest->a2 = template->a2;
dest->weight = template->weight;
}
}
// cycle through all notches on ROLL (takes RPM_FILTER_DURATION_S at max.)
@ -184,6 +195,11 @@ FAST_CODE float rpmFilterApply(const int axis, float value)
// Iterate over all notches on axis and apply each one to value.
// Order of application doesn't matter because biquads are linear time-invariant filters.
for (int i = 0; i < rpmFilter.numHarmonics; i++) {
if (rpmFilter.weights[i] <= 0.0f) {
continue; // skip harmonics which have no effect on filtered output
}
for (int motor = 0; motor < getMotorCount(); motor++) {
value = biquadFilterApplyDF1Weighted(&rpmFilter.notch[axis][motor][i], value);
}

3
src/main/pg/rpm_filter.c
View file

@ -34,7 +34,8 @@ PG_RESET_TEMPLATE(rpmFilterConfig_t, rpmFilterConfig,
.rpm_filter_min_hz = 100,
.rpm_filter_fade_range_hz = 50,
.rpm_filter_q = 500,
.rpm_filter_lpf_hz = 150
.rpm_filter_lpf_hz = 150,
.rpm_filter_weights = { 100, 100, 100 },
);
#endif // USE_RPM_FILTER

3
src/main/pg/rpm_filter.h
View file

@ -24,9 +24,12 @@
#include "pg/pg.h"
#define RPM_FILTER_HARMONICS_MAX 3
typedef struct rpmFilterConfig_s
{
uint8_t rpm_filter_harmonics; // how many harmonics should be covered with notches? 0 means filter off
uint8_t rpm_filter_weights[RPM_FILTER_HARMONICS_MAX]; // effect or "weight" (0% - 100%) of each RPM filter harmonic
uint8_t rpm_filter_min_hz; // minimum frequency of the notches
uint16_t rpm_filter_fade_range_hz; // range in which to gradually turn off notches down to minHz
uint16_t rpm_filter_q; // q of the notches