Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-17 21:35:44 +03:00
Code Issues Wiki Activity

Change RPM filter minHz limit

Browse source 
- Changed lower limit of minHz from 50 to 30
- Added fadeRangeHz to blackbox header
- Renamed variables to fit BF coding convention
- Renamed CLI commands respectively
This commit is contained in:
KarateBrot 2021-09-10 13:21:51 +02:00
parent a2bbd31c9c
commit 3ee3b54ad6
5 changed files with 43 additions and 40 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -1430,10 +1430,11 @@ static bool blackboxWriteSysinfo(void)
BLACKBOX_PRINT_HEADER_LINE("motor_poles", "%d", motorConfig()->motorPoleCount); BLACKBOX_PRINT_HEADER_LINE("motor_poles", "%d", motorConfig()->motorPoleCount);
#endif #endif
#ifdef USE_RPM_FILTER #ifdef USE_RPM_FILTER
BLACKBOX_PRINT_HEADER_LINE("gyro_rpm_notch_harmonics", "%d", rpmFilterConfig()->gyro_rpm_notch_harmonics); BLACKBOX_PRINT_HEADER_LINE("rpm_filter_harmonics", "%d", rpmFilterConfig()->rpm_filter_harmonics);
BLACKBOX_PRINT_HEADER_LINE("gyro_rpm_notch_q", "%d", rpmFilterConfig()->gyro_rpm_notch_q); BLACKBOX_PRINT_HEADER_LINE("rpm_filter_q", "%d", rpmFilterConfig()->rpm_filter_q);
BLACKBOX_PRINT_HEADER_LINE("gyro_rpm_notch_min", "%d", rpmFilterConfig()->gyro_rpm_notch_min); BLACKBOX_PRINT_HEADER_LINE("rpm_filter_min_hz", "%d", rpmFilterConfig()->rpm_filter_min_hz);
BLACKBOX_PRINT_HEADER_LINE("rpm_notch_lpf", "%d", rpmFilterConfig()->rpm_lpf); BLACKBOX_PRINT_HEADER_LINE("rpm_filter_fade_range_hz", "%d", rpmFilterConfig()->rpm_filter_fade_range_hz);
BLACKBOX_PRINT_HEADER_LINE("rpm_filter_lpf_hz", "%d", rpmFilterConfig()->rpm_filter_lpf_hz);
#endif #endif
#if defined(USE_ACC) #if defined(USE_ACC)
BLACKBOX_PRINT_HEADER_LINE("acc_lpf_hz", "%d", (int)(accelerometerConfig()->acc_lpf_hz * 100.0f)); BLACKBOX_PRINT_HEADER_LINE("acc_lpf_hz", "%d", (int)(accelerometerConfig()->acc_lpf_hz * 100.0f));

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

@ -1656,11 +1656,11 @@ const clivalue_t valueTable[] = {
#endif #endif
#ifdef USE_RPM_FILTER #ifdef USE_RPM_FILTER
{ "gyro_rpm_notch_harmonics", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 3 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, gyro_rpm_notch_harmonics) }, { "rpm_filter_harmonics", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 3 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, rpm_filter_harmonics) },
{ "gyro_rpm_notch_q", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 250, 3000 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, gyro_rpm_notch_q) }, { "rpm_filter_q", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 250, 3000 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, rpm_filter_q) },
{ "gyro_rpm_notch_min", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 50, 200 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, gyro_rpm_notch_min) }, { "rpm_filter_min_hz", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 30, 200 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, rpm_filter_min_hz) },
{ "gyro_rpm_notch_fade_range_hz", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, gyro_rpm_notch_fade_range_hz) }, { "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) },
{ "rpm_notch_lpf", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 100, 500 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, rpm_lpf) }, { "rpm_filter_lpf_hz", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 100, 500 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, rpm_filter_lpf_hz) },
#endif #endif
#ifdef USE_RX_FLYSKY #ifdef USE_RX_FLYSKY

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

@ -30,6 +30,7 @@
#include "common/filter.h" #include "common/filter.h"
#include "common/maths.h" #include "common/maths.h"
#include "common/time.h"
#include "drivers/dshot.h" #include "drivers/dshot.h"
@ -59,7 +60,7 @@ typedef struct rpmNotchFilter_s {
float maxHz; float maxHz;
float fadeRangeHz; float fadeRangeHz;
float q; float q;
uint32_t looptime; timeUs_t looptimeUs;
biquadFilter_t notch[XYZ_AXIS_COUNT][MAX_SUPPORTED_MOTORS][RPM_FILTER_MAXHARMONICS]; biquadFilter_t notch[XYZ_AXIS_COUNT][MAX_SUPPORTED_MOTORS][RPM_FILTER_MAXHARMONICS];
@ -87,28 +88,28 @@ PG_REGISTER_WITH_RESET_FN(rpmFilterConfig_t, rpmFilterConfig, PG_RPM_FILTER_CONF
void pgResetFn_rpmFilterConfig(rpmFilterConfig_t *config) void pgResetFn_rpmFilterConfig(rpmFilterConfig_t *config)
{ {
config->gyro_rpm_notch_harmonics = 3; config->rpm_filter_harmonics = 3;
config->gyro_rpm_notch_min = 100; config->rpm_filter_min_hz = 100;
config->gyro_rpm_notch_fade_range_hz = 50; config->rpm_filter_fade_range_hz = 50;
config->gyro_rpm_notch_q = 500; config->rpm_filter_q = 500;
config->rpm_lpf = 150; config->rpm_filter_lpf_hz = 150;
} }
static void rpmNotchFilterInit(rpmNotchFilter_t *filter, const rpmFilterConfig_t *config, const uint32_t looptime) static void rpmNotchFilterInit(rpmNotchFilter_t *filter, const rpmFilterConfig_t *config, const timeUs_t looptimeUs)
{ {
filter->harmonics = config->gyro_rpm_notch_harmonics; filter->harmonics = config->rpm_filter_harmonics;
filter->minHz = config->gyro_rpm_notch_min; filter->minHz = config->rpm_filter_min_hz;
filter->maxHz = 0.48f * 1e6f / looptime; // don't go quite to nyquist to avoid oscillations filter->maxHz = 0.48f * 1e6f / looptimeUs; // don't go quite to nyquist to avoid oscillations
filter->fadeRangeHz = config->gyro_rpm_notch_fade_range_hz; filter->fadeRangeHz = config->rpm_filter_fade_range_hz;
filter->q = config->gyro_rpm_notch_q / 100.0f; filter->q = config->rpm_filter_q / 100.0f;
filter->looptime = looptime; filter->looptimeUs = looptimeUs;
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
for (int motor = 0; motor < getMotorCount(); motor++) { for (int motor = 0; motor < getMotorCount(); motor++) {
for (int i = 0; i < filter->harmonics; i++) { for (int i = 0; i < filter->harmonics; i++) {
biquadFilterInit( biquadFilterInit(
&filter->notch[axis][motor][i], filter->minHz * i, filter->looptime, filter->q, FILTER_NOTCH, 0.0f); &filter->notch[axis][motor][i], filter->minHz * i, filter->looptimeUs, filter->q, FILTER_NOTCH, 0.0f);
} }
} }
} }
@ -126,7 +127,7 @@ void rpmFilterInit(const rpmFilterConfig_t *config)
} }
pidLooptime = gyro.targetLooptime; pidLooptime = gyro.targetLooptime;
if (config->gyro_rpm_notch_harmonics) { if (config->rpm_filter_harmonics) {
gyroFilter = &filters[numberRpmNotchFilters++]; gyroFilter = &filters[numberRpmNotchFilters++];
rpmNotchFilterInit(gyroFilter, config, pidLooptime); rpmNotchFilterInit(gyroFilter, config, pidLooptime);
} else { } else {
@ -134,7 +135,7 @@ void rpmFilterInit(const rpmFilterConfig_t *config)
} }
for (int i = 0; i < getMotorCount(); i++) { for (int i = 0; i < getMotorCount(); i++) {
pt1FilterInit(&rpmFilters[i], pt1FilterGain(config->rpm_lpf, pidLooptime * 1e-6f)); pt1FilterInit(&rpmFilters[i], pt1FilterGain(config->rpm_filter_lpf_hz, pidLooptime * 1e-6f));
} }
erpmToHz = ERPM_PER_LSB / SECONDS_PER_MINUTE / (motorConfig()->motorPoleCount / 2.0f); erpmToHz = ERPM_PER_LSB / SECONDS_PER_MINUTE / (motorConfig()->motorPoleCount / 2.0f);
@ -163,7 +164,7 @@ float rpmFilterGyro(const int axis, float value)
return applyFilter(gyroFilter, axis, value); return applyFilter(gyroFilter, axis, value);
} }
FAST_CODE_NOINLINE void rpmFilterUpdate() FAST_CODE_NOINLINE void rpmFilterUpdate(void)
{ {
if (gyroFilter == NULL) { if (gyroFilter == NULL) {
return; return;
@ -195,7 +196,7 @@ FAST_CODE_NOINLINE void rpmFilterUpdate()
} }
biquadFilterUpdate( biquadFilterUpdate(
template, frequency, currentFilter->looptime, currentFilter->q, FILTER_NOTCH, weight); template, frequency, currentFilter->looptimeUs, currentFilter->q, FILTER_NOTCH, weight);
for (int axis = 1; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 1; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilter_t *clone = &currentFilter->notch[axis][currentMotor][currentHarmonic]; biquadFilter_t *clone = &currentFilter->notch[axis][currentMotor][currentHarmonic];
@ -223,10 +224,10 @@ FAST_CODE_NOINLINE void rpmFilterUpdate()
bool isRpmFilterEnabled(void) bool isRpmFilterEnabled(void)
{ {
return (motorConfig()->dev.useDshotTelemetry && rpmFilterConfig()->gyro_rpm_notch_harmonics); return (motorConfig()->dev.useDshotTelemetry && rpmFilterConfig()->rpm_filter_harmonics);
} }
float rpmMinMotorFrequency() float rpmMinMotorFrequency(void)
{ {
if (minMotorFrequency == 0.0f) { if (minMotorFrequency == 0.0f) {
minMotorFrequency = 10000.0f; minMotorFrequency = 10000.0f;

15
src/main/flight/rpm_filter.h
View file

@ -25,18 +25,19 @@
typedef struct rpmFilterConfig_s typedef struct rpmFilterConfig_s
{ {
uint8_t gyro_rpm_notch_harmonics; // how many harmonics should be covered with notches? 0 means filter off uint8_t rpm_filter_harmonics; // how many harmonics should be covered with notches? 0 means filter off
uint8_t gyro_rpm_notch_min; // minimum frequency of the notches uint8_t rpm_filter_min_hz; // minimum frequency of the notches
uint16_t gyro_rpm_notch_fade_range_hz; // range in which to gradually turn off notches down to minHz uint16_t rpm_filter_fade_range_hz; // range in which to gradually turn off notches down to minHz
uint16_t gyro_rpm_notch_q; // q of the notches uint16_t rpm_filter_q; // q of the notches
uint16_t rpm_filter_lpf_hz; // the cutoff of the lpf on reported motor rpm
uint16_t rpm_lpf; // the cutoff of the lpf on reported motor rpm
} rpmFilterConfig_t; } rpmFilterConfig_t;
PG_DECLARE(rpmFilterConfig_t, rpmFilterConfig); PG_DECLARE(rpmFilterConfig_t, rpmFilterConfig);
void rpmFilterInit(const rpmFilterConfig_t *config); void rpmFilterInit(const rpmFilterConfig_t *config);
float rpmFilterGyro(const int axis, float value); float rpmFilterGyro(const int axis, float value);
void rpmFilterUpdate(); void rpmFilterUpdate(void);
bool isRpmFilterEnabled(void); bool isRpmFilterEnabled(void);
float rpmMinMotorFrequency(); float rpmMinMotorFrequency(void);

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

@ -1792,8 +1792,8 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
sbufWriteU16(dst, 0); sbufWriteU16(dst, 0);
#endif #endif
#if defined(USE_RPM_FILTER) #if defined(USE_RPM_FILTER)
sbufWriteU8(dst, rpmFilterConfig()->gyro_rpm_notch_harmonics); sbufWriteU8(dst, rpmFilterConfig()->rpm_filter_harmonics);
sbufWriteU8(dst, rpmFilterConfig()->gyro_rpm_notch_min); sbufWriteU8(dst, rpmFilterConfig()->rpm_filter_min_hz);
#else #else
sbufWriteU8(dst, 0); sbufWriteU8(dst, 0);
sbufWriteU8(dst, 0); sbufWriteU8(dst, 0);
@ -2675,8 +2675,8 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
sbufReadU16(src); sbufReadU16(src);
#endif #endif
#if defined(USE_RPM_FILTER) #if defined(USE_RPM_FILTER)
rpmFilterConfigMutable()->gyro_rpm_notch_harmonics = sbufReadU8(src); rpmFilterConfigMutable()->rpm_filter_harmonics = sbufReadU8(src);
rpmFilterConfigMutable()->gyro_rpm_notch_min = sbufReadU8(src); rpmFilterConfigMutable()->rpm_filter_min_hz = sbufReadU8(src);
#else #else
sbufReadU8(src); sbufReadU8(src);
sbufReadU8(src); sbufReadU8(src);