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:
parent
a2bbd31c9c
commit
3ee3b54ad6
|
@ -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));
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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 = ¤tFilter->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;
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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);
|
||||
|
|
Loading…
Reference in New Issue