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

Merge bfff81889c into 9458ac043a

Browse source
This commit is contained in:
MikeNomatter 2025-07-09 17:04:52 +03:00 • committed by GitHub
commit 64b4ee4866
No known key found for this signature in database
GPG key ID: B5690EEEBB952194
7 changed files with 41 additions and 37 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -1700,7 +1700,6 @@ static bool blackboxWriteSysinfo(void)
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_MOTOR_POLES, "%d", motorConfig()->motorPoleCount);
#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]);

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

@ -1936,7 +1936,6 @@ const clivalue_t valueTable[] = {
#endif
#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) },

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

@ -150,7 +150,6 @@
#define PARAM_NAME_SIMPLIFIED_GYRO_FILTER "simplified_gyro_filter"
#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"

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

@ -46,7 +46,7 @@
typedef struct rpmFilter_s {
int numHarmonics;
int activeHarmonicsCount;
float weights[RPM_FILTER_HARMONICS_MAX];
float minHz;
float maxHz;
@ -70,46 +70,48 @@ void rpmFilterInit(const rpmFilterConfig_t *config, const timeUs_t looptimeUs)
{
motorIndex = 0;
harmonicIndex = 0;
rpmFilter.numHarmonics = 0; // disable RPM Filtering
rpmFilter.activeHarmonicsCount = 0; // disable RPM Filtering
// if bidirectional DShot is not available
if (!useDshotTelemetry) {
return;
}
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);
if (rpmFilter.weights[n] > 0.0f) {
rpmFilter.activeHarmonicsCount++;
}
}
// if RPM Filtering is configured to be off
if (!config->rpm_filter_harmonics) {
if (!isRpmFilterEnabled()) {
return;
}
// if we get to this point, enable and init RPM filtering
rpmFilter.numHarmonics = config->rpm_filter_harmonics;
rpmFilter.minHz = config->rpm_filter_min_hz;
rpmFilter.maxHz = 0.48f * 1e6f / looptimeUs; // don't go quite to nyquist to avoid oscillations
rpmFilter.fadeRangeHz = config->rpm_filter_fade_range_hz;
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++) {
biquadFilterInit(&rpmFilter.notch[axis][motor][i], rpmFilter.minHz * i, rpmFilter.looptimeUs, rpmFilter.q, FILTER_NOTCH, 0.0f);
for (int i = 0; i < RPM_FILTER_HARMONICS_MAX; i++) {
biquadFilterInit(&rpmFilter.notch[axis][motor][i], rpmFilter.minHz * (i + 1), rpmFilter.looptimeUs, rpmFilter.q, FILTER_NOTCH, rpmFilter.weights[i]);
}
}
}
const float loopIterationsPerUpdate = RPM_FILTER_DURATION_S / (looptimeUs * 1e-6f);
const float numNotchesPerAxis = getMotorCount() * rpmFilter.numHarmonics;
const float numNotchesPerAxis = getMotorCount() * rpmFilter.activeHarmonicsCount;
notchUpdatesPerIteration = ceilf(numNotchesPerAxis / loopIterationsPerUpdate); // round to ceiling
}
FAST_CODE_NOINLINE void rpmFilterUpdate(void)
{
if (!useDshotTelemetry) {
if (!isRpmFilterEnabled()) {
return;
}
@ -161,34 +163,31 @@ FAST_CODE_NOINLINE void rpmFilterUpdate(void)
}
// cycle through all notches on ROLL (takes RPM_FILTER_DURATION_S at max.)
harmonicIndex = (harmonicIndex + 1) % rpmFilter.numHarmonics;
if (harmonicIndex == 0) {
motorIndex = (motorIndex + 1) % getMotorCount();
//not sure if it correct
motorIndex++;
if (motorIndex >= getMotorCount()) {
// All motors processed for this harmonic, move to next harmonic
motorIndex = 0;
harmonicIndex = (harmonicIndex + 1) % RPM_FILTER_HARMONICS_MAX;
}
}
}
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);
for (int i = 0; i < RPM_FILTER_HARMONICS_MAX; i++) {
if (rpmFilter.weights[i] > 0.0f) {
for (int motor = 0; motor < getMotorCount(); motor++) {
value = biquadFilterApplyDF1Weighted(&rpmFilter.notch[axis][motor][i], value);
}
}
}
return value;
}
bool isRpmFilterEnabled(void)
{
return rpmFilter.numHarmonics > 0;
return rpmFilter.activeHarmonicsCount > 0;
}
#endif // USE_RPM_FILTER

20
src/main/msp/msp.c
View file

@ -1911,10 +1911,15 @@ case MSP_NAME:
sbufWriteU16(dst, 0);
#endif
#if defined(USE_RPM_FILTER)
sbufWriteU8(dst, rpmFilterConfig()->rpm_filter_harmonics);
for (int i = 0; i < RPM_FILTER_HARMONICS_MAX; i++) {
// Added in MSP API 1.46
sbufWriteU8(dst, rpmFilterConfig()->rpm_filter_weights[i]);
}
sbufWriteU8(dst, rpmFilterConfig()->rpm_filter_min_hz);
#else
sbufWriteU8(dst, 0);
for (int i = 0; i < RPM_FILTER_HARMONICS_MAX; i++) {
sbufWriteU8(dst, 0);
}
sbufWriteU8(dst, 0);
#endif
#if defined(USE_DYN_NOTCH_FILTER)
@ -3107,7 +3112,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
sbufReadU16(src);
#endif
}
if (sbufBytesRemaining(src) >= 8) {
if (sbufBytesRemaining(src) >= 10) {
// Added in MSP API 1.42
#if defined(USE_DYN_NOTCH_FILTER)
sbufReadU8(src); // DEPRECATED 1.43: dyn_notch_range
@ -3121,10 +3126,15 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
sbufReadU16(src);
#endif
#if defined(USE_RPM_FILTER)
rpmFilterConfigMutable()->rpm_filter_harmonics = sbufReadU8(src);
for (int i = 0; i < RPM_FILTER_HARMONICS_MAX; i++) {
// Added in MSP API 1.46
rpmFilterConfigMutable()->rpm_filter_weights[i] = sbufReadU8(src);
}
rpmFilterConfigMutable()->rpm_filter_min_hz = sbufReadU8(src);
#else
sbufReadU8(src);
for (int i = 0; i < RPM_FILTER_HARMONICS_MAX; i++) {
sbufReadU8(src);
}
sbufReadU8(src);
#endif
}

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

@ -30,7 +30,6 @@
PG_REGISTER_WITH_RESET_TEMPLATE(rpmFilterConfig_t, rpmFilterConfig, PG_RPM_FILTER_CONFIG, 6);
PG_RESET_TEMPLATE(rpmFilterConfig_t, rpmFilterConfig,
.rpm_filter_harmonics = 3,
.rpm_filter_min_hz = 100,
.rpm_filter_fade_range_hz = 50,
.rpm_filter_q = 500,

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

@ -28,7 +28,6 @@
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