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_LEVEL] = { 50, 50, 75, 0 },
[PID_MAG] = { 40, 0, 0, 0 }, [PID_MAG] = { 40, 0, 0, 0 },
}, },
.pidSumLimit = PIDSUM_LIMIT, .pidSumLimit = PIDSUM_LIMIT,
.pidSumLimitYaw = PIDSUM_LIMIT_YAW, .pidSumLimitYaw = PIDSUM_LIMIT_YAW,
.yaw_lowpass_hz = 0, .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_hz = 0,
.dterm_notch_cutoff = 0, .dterm_notch_cutoff = 0,
.dterm_filter_type = FILTER_PT1,
.dterm_filter2_type = FILTER_PT1,
.itermWindupPointPercent = 40, .itermWindupPointPercent = 40,
.vbatPidCompensation = 0, .vbatPidCompensation = 0,
.pidAtMinThrottle = PID_STABILISATION_ON, .pidAtMinThrottle = PID_STABILISATION_ON,
@ -169,9 +164,13 @@ void resetPidProfile(pidProfile_t *pidProfile)
.antiGravityMode = ANTI_GRAVITY_SMOOTH, .antiGravityMode = ANTI_GRAVITY_SMOOTH,
.dyn_lpf_dterm_max_hz = 200, .dyn_lpf_dterm_max_hz = 200,
.dyn_lpf_dterm_idle = 20, .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 #ifdef USE_DYN_LPF
pidProfile->dterm_lowpass_hz = 150; pidProfile->dterm_lowpass_hz = 120;
pidProfile->dterm_lowpass2_hz = 180; pidProfile->dterm_lowpass2_hz = 180;
pidProfile->dterm_filter_type = FILTER_BIQUAD; pidProfile->dterm_filter_type = FILTER_BIQUAD;
pidProfile->dterm_filter2_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 int8_t dynLpfFilter = DYN_LPF_NONE;
static FAST_RAM_ZERO_INIT float dynLpfIdle; static FAST_RAM_ZERO_INIT float dynLpfIdle;
static FAST_RAM_ZERO_INIT float dynLpfIdlePoint; static FAST_RAM_ZERO_INIT float dynLpfIdlePoint;
static FAST_RAM_ZERO_INIT float dynLpfInvIdlePoint; static FAST_RAM_ZERO_INIT float dynLpfInvIdlePointScaled;
static FAST_RAM_ZERO_INIT int16_t dynLpfDiff;
static FAST_RAM_ZERO_INIT uint16_t dynLpfMin; static FAST_RAM_ZERO_INIT uint16_t dynLpfMin;
#endif #endif
@ -537,15 +535,15 @@ void pidInitConfig(const pidProfile_t *pidProfile)
if (pidProfile->dterm_lowpass_hz > 0 ) { if (pidProfile->dterm_lowpass_hz > 0 ) {
dynLpfMin = pidProfile->dterm_lowpass_hz; dynLpfMin = pidProfile->dterm_lowpass_hz;
switch (pidProfile->dterm_filter_type) { switch (pidProfile->dterm_filter_type) {
case FILTER_PT1: case FILTER_PT1:
dynLpfFilter = DYN_LPF_PT1; dynLpfFilter = DYN_LPF_PT1;
break; break;
case FILTER_BIQUAD: case FILTER_BIQUAD:
dynLpfFilter = DYN_LPF_BIQUAD; dynLpfFilter = DYN_LPF_BIQUAD;
break; break;
default: default:
dynLpfFilter = DYN_LPF_NONE; dynLpfFilter = DYN_LPF_NONE;
break; break;
} }
} }
} else { } else {
@ -553,8 +551,7 @@ void pidInitConfig(const pidProfile_t *pidProfile)
} }
dynLpfIdle = pidProfile->dyn_lpf_dterm_idle / 100.0f; dynLpfIdle = pidProfile->dyn_lpf_dterm_idle / 100.0f;
dynLpfIdlePoint = (dynLpfIdle - (dynLpfIdle * dynLpfIdle * dynLpfIdle) / 3.0f) * 1.5f; dynLpfIdlePoint = (dynLpfIdle - (dynLpfIdle * dynLpfIdle * dynLpfIdle) / 3.0f) * 1.5f;
dynLpfInvIdlePoint = 1 / (1 - dynLpfIdlePoint); dynLpfInvIdlePointScaled = 1 / (1 - dynLpfIdlePoint) * (pidProfile->dyn_lpf_dterm_max_hz - dynLpfMin);
dynLpfDiff = pidProfile->dyn_lpf_dterm_max_hz - dynLpfMin;
#endif #endif
} }
@ -1180,21 +1177,20 @@ bool pidAntiGravityEnabled(void)
#ifdef USE_DYN_LPF #ifdef USE_DYN_LPF
void dynLpfDTermUpdate(float throttle) void dynLpfDTermUpdate(float throttle)
{ {
if (dynLpfFilter == DYN_LPF_PT1 || dynLpfFilter == DYN_LPF_BIQUAD) { if (dynLpfFilter != DYN_LPF_NONE) {
uint16_t cutoffFreq = dynLpfMin; uint16_t cutoffFreq = dynLpfMin;
if (throttle > dynLpfIdle) { if (throttle > dynLpfIdle) {
const float dynThrottle = (throttle - (throttle * throttle * throttle) * 0.333f) * 1.5f; const float dynThrottle = (throttle - (throttle * throttle * throttle) / 3.0f) * 1.5f;
cutoffFreq += (dynThrottle - dynLpfIdlePoint) * dynLpfInvIdlePoint * dynLpfDiff; cutoffFreq += (dynThrottle - dynLpfIdlePoint) * dynLpfInvIdlePointScaled;
} }
if (dynLpfFilter == DYN_LPF_PT1) { if (dynLpfFilter == DYN_LPF_PT1) {
const float gyroDt = gyro.targetLooptime * 1e-6f;
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
pt1FilterUpdateCutoff(&dtermLowpass[axis].pt1Filter, pt1FilterGain(cutoffFreq, gyroDt)); pt1FilterUpdateCutoff(&dtermLowpass[axis].pt1Filter, pt1FilterGain(cutoffFreq, dT));
} }
} else { } else {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { 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) void pgResetFn_gyroConfig(gyroConfig_t *gyroConfig)
{ {
gyroConfig->gyro_align = ALIGN_DEFAULT; gyroConfig->gyro_align = ALIGN_DEFAULT;
gyroConfig->gyroCalibrationDuration = 125; // 1gyroConfig->25 seconds gyroConfig->gyroCalibrationDuration = 125; // 1.25 seconds
gyroConfig->gyroMovementCalibrationThreshold = 48; gyroConfig->gyroMovementCalibrationThreshold = 48;
gyroConfig->gyro_sync_denom = GYRO_SYNC_DENOM_DEFAULT; gyroConfig->gyro_sync_denom = GYRO_SYNC_DENOM_DEFAULT;
gyroConfig->gyro_hardware_lpf = GYRO_HARDWARE_LPF_NORMAL; 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_max_hz = 400;
gyroConfig->dyn_lpf_gyro_idle = 20; gyroConfig->dyn_lpf_gyro_idle = 20;
#ifdef USE_DYN_LPF #ifdef USE_DYN_LPF
gyroConfig->gyro_lowpass_hz = 150; gyroConfig->gyro_lowpass_hz = 120;
#endif #endif
} }
@ -543,8 +543,7 @@ bool gyroInit(void)
static FAST_RAM int8_t dynLpfFilter = DYN_LPF_NONE; static FAST_RAM int8_t dynLpfFilter = DYN_LPF_NONE;
static FAST_RAM_ZERO_INIT float dynLpfIdle; static FAST_RAM_ZERO_INIT float dynLpfIdle;
static FAST_RAM_ZERO_INIT float dynLpfIdlePoint; static FAST_RAM_ZERO_INIT float dynLpfIdlePoint;
static FAST_RAM_ZERO_INIT float dynLpfInvIdlePoint; static FAST_RAM_ZERO_INIT float dynLpfInvIdlePointScaled;
static FAST_RAM_ZERO_INIT int16_t dynLpfDiff;
static FAST_RAM_ZERO_INIT uint16_t dynLpfMin; static FAST_RAM_ZERO_INIT uint16_t dynLpfMin;
static void dynLpfFilterInit() static void dynLpfFilterInit()
@ -553,15 +552,15 @@ static void dynLpfFilterInit()
if (gyroConfig()->gyro_lowpass_hz > 0 ) { if (gyroConfig()->gyro_lowpass_hz > 0 ) {
dynLpfMin = gyroConfig()->gyro_lowpass_hz; dynLpfMin = gyroConfig()->gyro_lowpass_hz;
switch (gyroConfig()->gyro_lowpass_type) { switch (gyroConfig()->gyro_lowpass_type) {
case FILTER_PT1: case FILTER_PT1:
dynLpfFilter = DYN_LPF_PT1; dynLpfFilter = DYN_LPF_PT1;
break; break;
case FILTER_BIQUAD: case FILTER_BIQUAD:
dynLpfFilter = DYN_LPF_BIQUAD; dynLpfFilter = DYN_LPF_BIQUAD;
break; break;
default: default:
dynLpfFilter = DYN_LPF_NONE; dynLpfFilter = DYN_LPF_NONE;
break; break;
} }
} }
} else { } else {
@ -569,8 +568,7 @@ static void dynLpfFilterInit()
} }
dynLpfIdle = gyroConfig()->dyn_lpf_gyro_idle / 100.0f; dynLpfIdle = gyroConfig()->dyn_lpf_gyro_idle / 100.0f;
dynLpfIdlePoint = (dynLpfIdle - (dynLpfIdle * dynLpfIdle * dynLpfIdle) / 3.0f) * 1.5f; dynLpfIdlePoint = (dynLpfIdle - (dynLpfIdle * dynLpfIdle * dynLpfIdle) / 3.0f) * 1.5f;
dynLpfInvIdlePoint = 1 / (1 - dynLpfIdlePoint); dynLpfInvIdlePointScaled = 1 / (1 - dynLpfIdlePoint) * (gyroConfig()->dyn_lpf_gyro_max_hz - dynLpfMin);
dynLpfDiff = gyroConfig()->dyn_lpf_gyro_max_hz - dynLpfMin;
} }
#endif #endif
@ -1202,13 +1200,13 @@ uint8_t gyroReadRegister(uint8_t whichSensor, uint8_t reg)
#ifdef USE_DYN_LPF #ifdef USE_DYN_LPF
void dynLpfGyroUpdate(float throttle) void dynLpfGyroUpdate(float throttle)
{ {
if (dynLpfFilter == DYN_LPF_PT1 || dynLpfFilter == DYN_LPF_BIQUAD) { if (dynLpfFilter != DYN_LPF_NONE) {
uint16_t cutoffFreq = dynLpfMin; uint16_t cutoffFreq = dynLpfMin;
if (throttle > dynLpfIdle) { if (throttle > dynLpfIdle) {
const float dynThrottle = (throttle - (throttle * throttle * throttle) * 0.333f) * 1.5f; const float dynThrottle = (throttle - (throttle * throttle * throttle) / 3.0f) * 1.5f;
cutoffFreq += (dynThrottle - dynLpfIdlePoint) * dynLpfInvIdlePoint * dynLpfDiff; cutoffFreq += (dynThrottle - dynLpfIdlePoint) * dynLpfInvIdlePointScaled;
} }
if (dynLpfFilter == DYN_LPF_PT1) { if (dynLpfFilter == DYN_LPF_PT1) {
const float gyroDt = gyro.targetLooptime * 1e-6f; const float gyroDt = gyro.targetLooptime * 1e-6f;
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
#ifdef USE_MULTI_GYRO #ifdef USE_MULTI_GYRO

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

@ -57,7 +57,7 @@ enum {
DYN_LPF_NONE = 0, DYN_LPF_NONE = 0,
DYN_LPF_PT1, DYN_LPF_PT1,
DYN_LPF_BIQUAD DYN_LPF_BIQUAD
} ; };
#define GYRO_CONFIG_USE_GYRO_1 0 #define GYRO_CONFIG_USE_GYRO_1 0
#define GYRO_CONFIG_USE_GYRO_2 1 #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_THROTTLE_BOOST
#define USE_RC_SMOOTHING_FILTER #define USE_RC_SMOOTHING_FILTER
#define USE_ITERM_RELAX #define USE_ITERM_RELAX
//#define USE_DYN_LPF #define USE_DYN_LPF
#ifdef USE_SERIALRX_SPEKTRUM #ifdef USE_SERIALRX_SPEKTRUM
#define USE_SPEKTRUM_BIND #define USE_SPEKTRUM_BIND