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

increase possible Q to 30, default Q to 10 and set scheduler optimize rate policy by default

Browse source
This commit is contained in:
Thorsten Laux 2019-01-27 08:59:22 +01:00
parent aaaf313747
commit 81a514c9ac
4 changed files with 21 additions and 19 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -1411,11 +1411,12 @@ const clivalue_t valueTable[] = {
#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 = { 1, 1000 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, gyro_rpm_notch_q) },
{ "gyro_rpm_notch_q", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1, 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) },
{ "dterm_rpm_notch_harmonics", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 3 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, dterm_rpm_notch_harmonics) },
{ "dterm_rpm_notch_q", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1, 1000 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, dterm_rpm_notch_q) },
{ "dterm_rpm_notch_q", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1, 3000 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, dterm_rpm_notch_q) },
{ "dterm_rpm_notch_min", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 50, 200 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, dterm_rpm_notch_min) },
{ "rpm_lpf", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 100, 500 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, rpm_lpf) },
#endif
#ifdef USE_RX_FLYSKY

6
src/main/fc/config.c
View file

@ -63,6 +63,7 @@
#include "sensors/acceleration.h"
#include "sensors/battery.h"
#include "sensors/gyro.h"
#include "sensors/rpm_filter.h"
#include "scheduler/scheduler.h"
@ -337,6 +338,11 @@ static void validateAndFixConfig(void)
}
#endif
#if defined(USE_DSHOT_TELEMETRY)
systemConfigMutable()->schedulerOptimizeRate = systemConfigMutable()->schedulerOptimizeRate ||
(rpmFilterConfig()->gyro_rpm_notch_harmonics + rpmFilterConfig()->dterm_rpm_notch_harmonics);
#endif
// clear features that are not supported.
// I have kept them all here in one place, some could be moved to sections of code above.

28
src/main/sensors/rpm_filter.c
View file

@ -24,15 +24,16 @@
#include "platform.h"
#include "build/debug.h"
#include "common/filter.h"
#include "common/maths.h"
#include "drivers/pwm_output_counts.h"
#include "flight/mixer.h"
#include "flight/pid.h"
#include "pg/pg_ids.h"
#include "scheduler/scheduler.h"
#include "sensors/rpm_filter.h"
#include "sensors/gyro.h"
#define RPM_FILTER_MAXHARMONICS 3
#define RPM_MOTOR_FILTER_CUTOFF 150
#define SECONDS_PER_MINUTE 60.0f
#define ERPM_PER_LSB 100.0f
#define MIN_UPDATE_T 0.001f
@ -61,7 +62,7 @@ static rpmNotchFilter_t filters[2];
static rpmNotchFilter_t* gyroFilter;
static rpmNotchFilter_t* dtermFilter;
PG_REGISTER_WITH_RESET_FN(rpmFilterConfig_t, rpmFilterConfig, PG_RPM_FILTER_CONFIG, 2);
PG_REGISTER_WITH_RESET_FN(rpmFilterConfig_t, rpmFilterConfig, PG_RPM_FILTER_CONFIG, 3);
void pgResetFn_rpmFilterConfig(rpmFilterConfig_t *config)
{
@ -69,9 +70,11 @@ void pgResetFn_rpmFilterConfig(rpmFilterConfig_t *config)
config->gyro_rpm_notch_min = 100;
config->gyro_rpm_notch_q = 500;
config->dterm_rpm_notch_harmonics = 1;
config->dterm_rpm_notch_harmonics = 0;
config->dterm_rpm_notch_min = 100;
config->dterm_rpm_notch_q = 500;
config->rpm_lpf = 150;
}
static void rpmNotchFilterInit(rpmNotchFilter_t* filter, int harmonics, int minHz, int q, float looptime)
@ -112,7 +115,7 @@ void rpmFilterInit(const rpmFilterConfig_t *config)
}
for (int i = 0; i < getMotorCount(); i++) {
pt1FilterInit(&rpmFilters[i], pt1FilterGain(RPM_MOTOR_FILTER_CUTOFF, pidLooptime));
pt1FilterInit(&rpmFilters[i], pt1FilterGain(config->rpm_lpf, pidLooptime));
}
erpmToHz = ERPM_PER_LSB / SECONDS_PER_MINUTE / (motorConfig()->motorPoleCount / 2.0f);
@ -121,6 +124,9 @@ void rpmFilterInit(const rpmFilterConfig_t *config)
numberFilters = getMotorCount() * (filters[0].harmonics + filters[1].harmonics);
const float filtersPerLoopIteration = numberFilters / loopIterationsPerUpdate;
filterUpdatesPerIteration = rintf(filtersPerLoopIteration + 0.49f);
if (gyroFilter || dtermFilter) {
schedulerOptimizeRate(true);
}
}
static float applyFilter(rpmNotchFilter_t* filter, int axis, float value)
@ -168,19 +174,7 @@ void rpmFilterUpdate()
for (int i = 0; i < filterUpdatesPerIteration; i++) {
float frequency = (harmonic + 1) * motorFrequency[motor];
const float deactivateFreq = 1000.0f;
if (frequency < currentFilter->minHz) {
if (frequency < 0.5f * currentFilter->minHz) {
frequency = deactivateFreq;
} else {
frequency = currentFilter->minHz;
}
}
if (frequency > deactivateFreq) {
frequency = deactivateFreq;
}
float frequency = constrainf((harmonic + 1) * motorFrequency[motor], 0.0f, currentFilter->minHz);
biquadFilter_t* template = &currentFilter->notch[0][motor][harmonic];
// uncomment below to debug filter stepping. Need to also comment out motor rpm DEBUG_SET above
/* DEBUG_SET(DEBUG_RPM_FILTER, 0, harmonic); */

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

@ -33,6 +33,7 @@ typedef struct rpmFilterConfig_s
uint8_t dterm_rpm_notch_min; // minimum frequency of the notches
uint16_t dterm_rpm_notch_q; // q of the notches
uint8_t rpm_lpf; // the cutoff of the lpf on reported motor rpm
} rpmFilterConfig_t;
PG_DECLARE(rpmFilterConfig_t, rpmFilterConfig);