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

using same curve of dyn dterm lpf

Browse source
This commit is contained in:
Nicola De Pasquale 2020-08-08 15:36:28 +02:00
parent 988024ee4e
commit d0fe84528a
6 changed files with 14 additions and 5 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -666,6 +666,7 @@ const clivalue_t valueTable[] = {
#ifdef USE_DYN_LPF #ifdef USE_DYN_LPF
{ "dyn_lpf_gyro_min_hz", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_lpf_gyro_min_hz) }, { "dyn_lpf_gyro_min_hz", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_lpf_gyro_min_hz) },
{ "dyn_lpf_gyro_max_hz", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_lpf_gyro_max_hz) }, { "dyn_lpf_gyro_max_hz", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_lpf_gyro_max_hz) },
{ "dyn_lpf_gyro_curve_expo", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 10 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_lpf_curve_expo) },
#endif #endif
{ "gyro_filter_debug_axis", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GYRO_FILTER_DEBUG }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_filter_debug_axis) }, { "gyro_filter_debug_axis", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GYRO_FILTER_DEBUG }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_filter_debug_axis) },

4
src/main/flight/pid.c
View file

@ -1189,7 +1189,7 @@ void dynLpfDTermUpdate(float throttle)
static unsigned int cutoffFreq; static unsigned int cutoffFreq;
if (pidRuntime.dynLpfFilter != DYN_LPF_NONE) { if (pidRuntime.dynLpfFilter != DYN_LPF_NONE) {
if (pidRuntime.dynLpfCurveExpo > 0) { if (pidRuntime.dynLpfCurveExpo > 0) {
cutoffFreq = dynDtermLpfCutoffFreq(throttle, pidRuntime.dynLpfMin, pidRuntime.dynLpfMax, pidRuntime.dynLpfCurveExpo); cutoffFreq = dynLpfCutoffFreq(throttle, pidRuntime.dynLpfMin, pidRuntime.dynLpfMax, pidRuntime.dynLpfCurveExpo);
} else { } else {
cutoffFreq = fmax(dynThrottle(throttle) * pidRuntime.dynLpfMax, pidRuntime.dynLpfMin); cutoffFreq = fmax(dynThrottle(throttle) * pidRuntime.dynLpfMax, pidRuntime.dynLpfMin);
} }
@ -1207,7 +1207,7 @@ void dynLpfDTermUpdate(float throttle)
} }
#endif #endif
float dynDtermLpfCutoffFreq(float throttle, uint16_t dynLpfMin, uint16_t dynLpfMax, uint8_t expo) { float dynLpfCutoffFreq(float throttle, uint16_t dynLpfMin, uint16_t dynLpfMax, uint8_t expo) {
const float expof = expo / 10.0f; const float expof = expo / 10.0f;
static float curve; static float curve;
curve = throttle * (1 - throttle) * expof + throttle; curve = throttle * (1 - throttle) * expof + throttle;

2
src/main/flight/pid.h
View file

@ -414,4 +414,4 @@ float pidGetPidFrequency();
float pidGetFfBoostFactor(); float pidGetFfBoostFactor();
float pidGetFfSmoothFactor(); float pidGetFfSmoothFactor();
float pidGetSpikeLimitInverse(); float pidGetSpikeLimitInverse();
float dynDtermLpfCutoffFreq(float throttle, uint16_t dynLpfMin, uint16_t dynLpfMax, uint8_t expo); float dynLpfCutoffFreq(float throttle, uint16_t dynLpfMin, uint16_t dynLpfMax, uint8_t expo);

9
src/main/sensors/gyro.c
View file

@ -132,6 +132,7 @@ void pgResetFn_gyroConfig(gyroConfig_t *gyroConfig)
gyroConfig->dyn_notch_q = 120; gyroConfig->dyn_notch_q = 120;
gyroConfig->dyn_notch_min_hz = 150; gyroConfig->dyn_notch_min_hz = 150;
gyroConfig->gyro_filter_debug_axis = FD_ROLL; gyroConfig->gyro_filter_debug_axis = FD_ROLL;
gyroConfig->dyn_lpf_curve_expo = 0;
} }
#ifdef USE_GYRO_DATA_ANALYSE #ifdef USE_GYRO_DATA_ANALYSE
@ -625,8 +626,12 @@ float dynThrottle(float throttle) {
void dynLpfGyroUpdate(float throttle) void dynLpfGyroUpdate(float throttle)
{ {
if (gyro.dynLpfFilter != DYN_LPF_NONE) { if (gyro.dynLpfFilter != DYN_LPF_NONE) {
const unsigned int cutoffFreq = (gyro.dynLpfMax - gyro.dynLpfMin) * dynThrottle(throttle) + gyro.dynLpfMin; static unsigned int cutoffFreq;
if (gyro.dynLpfCurveExpo > 0) {
cutoffFreq = dynLpfCutoffFreq(throttle, gyro.dynLpfMin, gyro.dynLpfMax, gyro.dynLpfCurveExpo);
} else {
cutoffFreq = fmax(dynThrottle(throttle) * gyro.dynLpfMax, gyro.dynLpfMin);
}
if (gyro.dynLpfFilter == DYN_LPF_PT1) { if (gyro.dynLpfFilter == DYN_LPF_PT1) {
DEBUG_SET(DEBUG_DYN_LPF, 2, cutoffFreq); DEBUG_SET(DEBUG_DYN_LPF, 2, cutoffFreq);
const float gyroDt = gyro.targetLooptime * 1e-6f; const float gyroDt = gyro.targetLooptime * 1e-6f;

2
src/main/sensors/gyro.h
View file

@ -122,6 +122,7 @@ typedef struct gyro_s {
uint8_t dynLpfFilter; uint8_t dynLpfFilter;
uint16_t dynLpfMin; uint16_t dynLpfMin;
uint16_t dynLpfMax; uint16_t dynLpfMax;
uint8_t dynLpfCurveExpo;
#endif #endif
#ifdef USE_GYRO_OVERFLOW_CHECK #ifdef USE_GYRO_OVERFLOW_CHECK
@ -197,6 +198,7 @@ typedef struct gyroConfig_s {
uint8_t gyro_filter_debug_axis; uint8_t gyro_filter_debug_axis;
uint8_t gyrosDetected; // What gyros should detection be attempted for on startup. Automatically set on first startup. uint8_t gyrosDetected; // What gyros should detection be attempted for on startup. Automatically set on first startup.
uint8_t dyn_lpf_curve_expo; // set the curve for dynamic gyro lowpass filter
} gyroConfig_t; } gyroConfig_t;
PG_DECLARE(gyroConfig_t, gyroConfig); PG_DECLARE(gyroConfig_t, gyroConfig);

1
src/main/sensors/gyro_init.c
View file

@ -228,6 +228,7 @@ static void dynLpfFilterInit()
} }
gyro.dynLpfMin = gyroConfig()->dyn_lpf_gyro_min_hz; gyro.dynLpfMin = gyroConfig()->dyn_lpf_gyro_min_hz;
gyro.dynLpfMax = gyroConfig()->dyn_lpf_gyro_max_hz; gyro.dynLpfMax = gyroConfig()->dyn_lpf_gyro_max_hz;
gyro.dynLpfCurveExpo = gyroConfig()->dyn_lpf_curve_expo;
} }
#endif #endif