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

Fixes from reviews.

This commit is contained in:
Kenneth Mitchell 2018-10-25 20:17:08 -04:00
parent ca460e842b
commit ff8d1a842a
No known key found for this signature in database
GPG Key ID: E27133AAF586AB21
4 changed files with 41 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
src/main/flight/pid.c
View File

@ -121,16 +121,11 @@ void resetPidProfile(pidProfile_t *pidProfile)
[PID_LEVEL] = { 50, 50, 75, 0 },
[PID_MAG] = { 40, 0, 0, 0 },
},
.pidSumLimit = PIDSUM_LIMIT,
.pidSumLimitYaw = PIDSUM_LIMIT_YAW,
.yaw_lowpass_hz = 0,
.dterm_lowpass_hz = 100, // dual PT1 filtering ON by default
.dterm_lowpass2_hz = 200, // second Dterm LPF ON by default
.dterm_notch_hz = 0,
.dterm_notch_cutoff = 0,
.dterm_filter_type = FILTER_PT1,
.dterm_filter2_type = FILTER_PT1,
.itermWindupPointPercent = 40,
.vbatPidCompensation = 0,
.pidAtMinThrottle = PID_STABILISATION_ON,
@ -169,9 +164,13 @@ void resetPidProfile(pidProfile_t *pidProfile)
.antiGravityMode = ANTI_GRAVITY_SMOOTH,
.dyn_lpf_dterm_max_hz = 200,
.dyn_lpf_dterm_idle = 20,
.dterm_lowpass_hz = 100, // dual PT1 filtering ON by default
.dterm_lowpass2_hz = 200, // second Dterm LPF ON by default
.dterm_filter_type = FILTER_PT1,
.dterm_filter2_type = FILTER_PT1,
);
#ifdef USE_DYN_LPF
pidProfile->dterm_lowpass_hz = 150;
pidProfile->dterm_lowpass_hz = 120;
pidProfile->dterm_lowpass2_hz = 180;
pidProfile->dterm_filter_type = FILTER_BIQUAD;
pidProfile->dterm_filter2_type = FILTER_BIQUAD;
@ -452,8 +451,7 @@ void pidUpdateAntiGravityThrottleFilter(float throttle)
static FAST_RAM int8_t dynLpfFilter = DYN_LPF_NONE;
static FAST_RAM_ZERO_INIT float dynLpfIdle;
static FAST_RAM_ZERO_INIT float dynLpfIdlePoint;
static FAST_RAM_ZERO_INIT float dynLpfInvIdlePoint;
static FAST_RAM_ZERO_INIT int16_t dynLpfDiff;
static FAST_RAM_ZERO_INIT float dynLpfInvIdlePointScaled;
static FAST_RAM_ZERO_INIT uint16_t dynLpfMin;
#endif
@ -537,15 +535,15 @@ void pidInitConfig(const pidProfile_t *pidProfile)
if (pidProfile->dterm_lowpass_hz > 0 ) {
dynLpfMin = pidProfile->dterm_lowpass_hz;
switch (pidProfile->dterm_filter_type) {
case FILTER_PT1:
dynLpfFilter = DYN_LPF_PT1;
break;
case FILTER_BIQUAD:
dynLpfFilter = DYN_LPF_BIQUAD;
break;
default:
dynLpfFilter = DYN_LPF_NONE;
break;
case FILTER_PT1:
dynLpfFilter = DYN_LPF_PT1;
break;
case FILTER_BIQUAD:
dynLpfFilter = DYN_LPF_BIQUAD;
break;
default:
dynLpfFilter = DYN_LPF_NONE;
break;
}
}
} else {
@ -553,8 +551,7 @@ void pidInitConfig(const pidProfile_t *pidProfile)
}
dynLpfIdle = pidProfile->dyn_lpf_dterm_idle / 100.0f;
dynLpfIdlePoint = (dynLpfIdle - (dynLpfIdle * dynLpfIdle * dynLpfIdle) / 3.0f) * 1.5f;
dynLpfInvIdlePoint = 1 / (1 - dynLpfIdlePoint);
dynLpfDiff = pidProfile->dyn_lpf_dterm_max_hz - dynLpfMin;
dynLpfInvIdlePointScaled = 1 / (1 - dynLpfIdlePoint) * (pidProfile->dyn_lpf_dterm_max_hz - dynLpfMin);
#endif
}
@ -1180,21 +1177,20 @@ bool pidAntiGravityEnabled(void)
#ifdef USE_DYN_LPF
void dynLpfDTermUpdate(float throttle)
{
if (dynLpfFilter == DYN_LPF_PT1 || dynLpfFilter == DYN_LPF_BIQUAD) {
if (dynLpfFilter != DYN_LPF_NONE) {
uint16_t cutoffFreq = dynLpfMin;
if (throttle > dynLpfIdle) {
const float dynThrottle = (throttle - (throttle * throttle * throttle) * 0.333f) * 1.5f;
cutoffFreq += (dynThrottle - dynLpfIdlePoint) * dynLpfInvIdlePoint * dynLpfDiff;
const float dynThrottle = (throttle - (throttle * throttle * throttle) / 3.0f) * 1.5f;
cutoffFreq += (dynThrottle - dynLpfIdlePoint) * dynLpfInvIdlePointScaled;
}
if (dynLpfFilter == DYN_LPF_PT1) {
const float gyroDt = gyro.targetLooptime * 1e-6f;
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
pt1FilterUpdateCutoff(&dtermLowpass[axis].pt1Filter, pt1FilterGain(cutoffFreq, gyroDt));
pt1FilterUpdateCutoff(&dtermLowpass[axis].pt1Filter, pt1FilterGain(cutoffFreq, dT));
}
} else {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilterUpdateLPF(&dtermLowpass[axis].biquadFilter, cutoffFreq, gyro.targetLooptime);
biquadFilterUpdateLPF(&dtermLowpass[axis].biquadFilter, cutoffFreq, targetPidLooptime);
}
}
}

38
src/main/sensors/gyro.c
View File

@ -192,7 +192,7 @@ PG_REGISTER_WITH_RESET_FN(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 5);
void pgResetFn_gyroConfig(gyroConfig_t *gyroConfig)
{
gyroConfig->gyro_align = ALIGN_DEFAULT;
gyroConfig->gyroCalibrationDuration = 125; // 1gyroConfig->25 seconds
gyroConfig->gyroCalibrationDuration = 125; // 1.25 seconds
gyroConfig->gyroMovementCalibrationThreshold = 48;
gyroConfig->gyro_sync_denom = GYRO_SYNC_DENOM_DEFAULT;
gyroConfig->gyro_hardware_lpf = GYRO_HARDWARE_LPF_NORMAL;
@ -218,7 +218,7 @@ void pgResetFn_gyroConfig(gyroConfig_t *gyroConfig)
gyroConfig->dyn_lpf_gyro_max_hz = 400;
gyroConfig->dyn_lpf_gyro_idle = 20;
#ifdef USE_DYN_LPF
gyroConfig->gyro_lowpass_hz = 150;
gyroConfig->gyro_lowpass_hz = 120;
#endif
}
@ -543,8 +543,7 @@ bool gyroInit(void)
static FAST_RAM int8_t dynLpfFilter = DYN_LPF_NONE;
static FAST_RAM_ZERO_INIT float dynLpfIdle;
static FAST_RAM_ZERO_INIT float dynLpfIdlePoint;
static FAST_RAM_ZERO_INIT float dynLpfInvIdlePoint;
static FAST_RAM_ZERO_INIT int16_t dynLpfDiff;
static FAST_RAM_ZERO_INIT float dynLpfInvIdlePointScaled;
static FAST_RAM_ZERO_INIT uint16_t dynLpfMin;
static void dynLpfFilterInit()
@ -553,15 +552,15 @@ static void dynLpfFilterInit()
if (gyroConfig()->gyro_lowpass_hz > 0 ) {
dynLpfMin = gyroConfig()->gyro_lowpass_hz;
switch (gyroConfig()->gyro_lowpass_type) {
case FILTER_PT1:
dynLpfFilter = DYN_LPF_PT1;
break;
case FILTER_BIQUAD:
dynLpfFilter = DYN_LPF_BIQUAD;
break;
default:
dynLpfFilter = DYN_LPF_NONE;
break;
case FILTER_PT1:
dynLpfFilter = DYN_LPF_PT1;
break;
case FILTER_BIQUAD:
dynLpfFilter = DYN_LPF_BIQUAD;
break;
default:
dynLpfFilter = DYN_LPF_NONE;
break;
}
}
} else {
@ -569,8 +568,7 @@ static void dynLpfFilterInit()
}
dynLpfIdle = gyroConfig()->dyn_lpf_gyro_idle / 100.0f;
dynLpfIdlePoint = (dynLpfIdle - (dynLpfIdle * dynLpfIdle * dynLpfIdle) / 3.0f) * 1.5f;
dynLpfInvIdlePoint = 1 / (1 - dynLpfIdlePoint);
dynLpfDiff = gyroConfig()->dyn_lpf_gyro_max_hz - dynLpfMin;
dynLpfInvIdlePointScaled = 1 / (1 - dynLpfIdlePoint) * (gyroConfig()->dyn_lpf_gyro_max_hz - dynLpfMin);
}
#endif
@ -1202,13 +1200,13 @@ uint8_t gyroReadRegister(uint8_t whichSensor, uint8_t reg)
#ifdef USE_DYN_LPF
void dynLpfGyroUpdate(float throttle)
{
if (dynLpfFilter == DYN_LPF_PT1 || dynLpfFilter == DYN_LPF_BIQUAD) {
if (dynLpfFilter != DYN_LPF_NONE) {
uint16_t cutoffFreq = dynLpfMin;
if (throttle > dynLpfIdle) {
const float dynThrottle = (throttle - (throttle * throttle * throttle) * 0.333f) * 1.5f;
cutoffFreq += (dynThrottle - dynLpfIdlePoint) * dynLpfInvIdlePoint * dynLpfDiff;
}
if (dynLpfFilter == DYN_LPF_PT1) {
const float dynThrottle = (throttle - (throttle * throttle * throttle) / 3.0f) * 1.5f;
cutoffFreq += (dynThrottle - dynLpfIdlePoint) * dynLpfInvIdlePointScaled;
}
if (dynLpfFilter == DYN_LPF_PT1) {
const float gyroDt = gyro.targetLooptime * 1e-6f;
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
#ifdef USE_MULTI_GYRO

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

@ -57,7 +57,7 @@ enum {
DYN_LPF_NONE = 0,
DYN_LPF_PT1,
DYN_LPF_BIQUAD
} ;
};
#define GYRO_CONFIG_USE_GYRO_1 0
#define GYRO_CONFIG_USE_GYRO_2 1

2
src/main/target/common_pre.h
View File

@ -189,7 +189,7 @@
#define USE_THROTTLE_BOOST
#define USE_RC_SMOOTHING_FILTER
#define USE_ITERM_RELAX
//#define USE_DYN_LPF
#define USE_DYN_LPF
#ifdef USE_SERIALRX_SPEKTRUM
#define USE_SPEKTRUM_BIND