Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-26 09:45:37 +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

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_min_hz = 150;
gyroConfig->gyro_filter_debug_axis = FD_ROLL;
gyroConfig->dyn_lpf_curve_expo = 0;
}
#ifdef USE_GYRO_DATA_ANALYSE
@ -625,8 +626,12 @@ float dynThrottle(float throttle) {
void dynLpfGyroUpdate(float throttle)
{
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) {
DEBUG_SET(DEBUG_DYN_LPF, 2, cutoffFreq);
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;
uint16_t dynLpfMin;
uint16_t dynLpfMax;
uint8_t dynLpfCurveExpo;
#endif
#ifdef USE_GYRO_OVERFLOW_CHECK
@ -197,6 +198,7 @@ typedef struct gyroConfig_s {
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 dyn_lpf_curve_expo; // set the curve for dynamic gyro lowpass filter
} gyroConfig_t;
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.dynLpfMax = gyroConfig()->dyn_lpf_gyro_max_hz;
gyro.dynLpfCurveExpo = gyroConfig()->dyn_lpf_curve_expo;
}
#endif