rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Change RPM filter minHz limit 

- 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
Show Stats 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);
#endif
#ifdef USE_RPM_FILTER
BLACKBOX_PRINT_HEADER_LINE("gyro_rpm_notch_harmonics", "%d", rpmFilterConfig()->gyro_rpm_notch_harmonics);
BLACKBOX_PRINT_HEADER_LINE("gyro_rpm_notch_q", "%d", rpmFilterConfig()->gyro_rpm_notch_q);
BLACKBOX_PRINT_HEADER_LINE("gyro_rpm_notch_min", "%d", rpmFilterConfig()->gyro_rpm_notch_min);
BLACKBOX_PRINT_HEADER_LINE("rpm_notch_lpf", "%d", rpmFilterConfig()->rpm_lpf);
BLACKBOX_PRINT_HEADER_LINE("rpm_filter_harmonics", "%d", rpmFilterConfig()->rpm_filter_harmonics);
BLACKBOX_PRINT_HEADER_LINE("rpm_filter_q", "%d", rpmFilterConfig()->rpm_filter_q);
BLACKBOX_PRINT_HEADER_LINE("rpm_filter_min_hz", "%d", rpmFilterConfig()->rpm_filter_min_hz);
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
#if defined(USE_ACC)
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
#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) },
{ "gyro_rpm_notch_q", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 250, 3000 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, gyro_rpm_notch_q) },
{ "gyro_rpm_notch_min", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 50, 200 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, gyro_rpm_notch_min) },
{ "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_notch_lpf", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 100, 500 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, rpm_lpf) },
{ "rpm_filter_harmonics", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 3 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, rpm_filter_harmonics) },
{ "rpm_filter_q", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 250, 3000 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, rpm_filter_q) },
{ "rpm_filter_min_hz", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 30, 200 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, rpm_filter_min_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_filter_lpf_hz", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 100, 500 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, rpm_filter_lpf_hz) },
#endif
#ifdef USE_RX_FLYSKY

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

@ -30,6 +30,7 @@
#include "common/filter.h"
#include "common/maths.h"
#include "common/time.h"
#include "drivers/dshot.h"
@ -59,7 +60,7 @@ typedef struct rpmNotchFilter_s {
float maxHz;
float fadeRangeHz;
float q;
uint32_t looptime;
timeUs_t looptimeUs;
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)
{
config->gyro_rpm_notch_harmonics = 3;
config->gyro_rpm_notch_min = 100;
config->gyro_rpm_notch_fade_range_hz = 50;
config->gyro_rpm_notch_q = 500;
config->rpm_filter_harmonics = 3;
config->rpm_filter_min_hz = 100;
config->rpm_filter_fade_range_hz = 50;
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->minHz = config->gyro_rpm_notch_min;
filter->maxHz = 0.48f * 1e6f / looptime; // don't go quite to nyquist to avoid oscillations
filter->fadeRangeHz = config->gyro_rpm_notch_fade_range_hz;
filter->q = config->gyro_rpm_notch_q / 100.0f;
filter->looptime = looptime;
filter->harmonics = config->rpm_filter_harmonics;
filter->minHz = config->rpm_filter_min_hz;
filter->maxHz = 0.48f * 1e6f / looptimeUs; // don't go quite to nyquist to avoid oscillations
filter->fadeRangeHz = config->rpm_filter_fade_range_hz;
filter->q = config->rpm_filter_q / 100.0f;
filter->looptimeUs = looptimeUs;
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
for (int motor = 0; motor < getMotorCount(); motor++) {
for (int i = 0; i < filter->harmonics; i++) {
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;
if (config->gyro_rpm_notch_harmonics) {
if (config->rpm_filter_harmonics) {
gyroFilter = &filters[numberRpmNotchFilters++];
rpmNotchFilterInit(gyroFilter, config, pidLooptime);
} else {
@ -134,7 +135,7 @@ void rpmFilterInit(const rpmFilterConfig_t *config)
}
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);
@ -163,7 +164,7 @@ float rpmFilterGyro(const int axis, float value)
return applyFilter(gyroFilter, axis, value);
}
FAST_CODE_NOINLINE void rpmFilterUpdate()
FAST_CODE_NOINLINE void rpmFilterUpdate(void)
{
if (gyroFilter == NULL) {
return;
@ -195,7 +196,7 @@ FAST_CODE_NOINLINE void rpmFilterUpdate()
}
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++) {
biquadFilter_t *clone = &currentFilter->notch[axis][currentMotor][currentHarmonic];
@ -223,10 +224,10 @@ FAST_CODE_NOINLINE void rpmFilterUpdate()
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) {
minMotorFrequency = 10000.0f;

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

@ -25,18 +25,19 @@
typedef struct rpmFilterConfig_s
{
uint8_t gyro_rpm_notch_harmonics; // how many harmonics should be covered with notches? 0 means filter off
uint8_t gyro_rpm_notch_min; // 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 gyro_rpm_notch_q; // q of the notches
uint8_t rpm_filter_harmonics; // how many harmonics should be covered with notches? 0 means filter off
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
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;
PG_DECLARE(rpmFilterConfig_t, rpmFilterConfig);
void rpmFilterInit(const rpmFilterConfig_t *config);
float rpmFilterGyro(const int axis, float value);
void rpmFilterUpdate();
void rpmFilterUpdate(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);
#endif
#if defined(USE_RPM_FILTER)
sbufWriteU8(dst, rpmFilterConfig()->gyro_rpm_notch_harmonics);
sbufWriteU8(dst, rpmFilterConfig()->gyro_rpm_notch_min);
sbufWriteU8(dst, rpmFilterConfig()->rpm_filter_harmonics);
sbufWriteU8(dst, rpmFilterConfig()->rpm_filter_min_hz);
#else
sbufWriteU8(dst, 0);
sbufWriteU8(dst, 0);
@ -2675,8 +2675,8 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
sbufReadU16(src);
#endif
#if defined(USE_RPM_FILTER)
rpmFilterConfigMutable()->gyro_rpm_notch_harmonics = sbufReadU8(src);
rpmFilterConfigMutable()->gyro_rpm_notch_min = sbufReadU8(src);
rpmFilterConfigMutable()->rpm_filter_harmonics = sbufReadU8(src);
rpmFilterConfigMutable()->rpm_filter_min_hz = sbufReadU8(src);
#else
sbufReadU8(src);
sbufReadU8(src);