Skip to content

Commit

Permalink
Fixes from reviews.
Browse files Browse the repository at this point in the history
  • Loading branch information
@kmitchel
kmitchel committed Oct 26, 2018
1 parent ca460e8 commit ff8d1a8
Show file tree
Hide file tree
Showing 4 changed files with 41 additions and 47 deletions.
46 changes: 21 additions & 25 deletions src/main/flight/pid.c
View file
Expand Up @@ -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,
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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

Expand Down Expand Up @@ -537,24 +535,23 @@ 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 {
dynLpfFilter = DYN_LPF_NONE;
}
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
}

Expand Down Expand Up @@ -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);
}
}
}
Expand Down
38 changes: 18 additions & 20 deletions src/main/sensors/gyro.c
View file
Expand Up @@ -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;
Expand All @@ -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
}

Expand Down Expand Up @@ -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()
Expand All @@ -553,24 +552,23 @@ 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 {
dynLpfFilter = DYN_LPF_NONE;
}
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

Expand Down Expand Up @@ -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
Expand Down
2 changes: 1 addition & 1 deletion src/main/sensors/gyro.h
View file
Expand Up @@ -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
Expand Down
2 changes: 1 addition & 1 deletion src/main/target/common_pre.h
View file
Expand Up @@ -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
Expand Down

0 comments on commit ff8d1a8

Please sign in to comment.