Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-24 00:35:34 +03:00
Code Issues Wiki Activity

Merge pull request #201 from iNavFlight/pid-loose-dterm-filtering

Browse source 
Improve PID LPF filtering - revert to IIR filter for D-term for lower phase shift, use LPF on Yaw axis.
This commit is contained in:
Konstantin Sharlaimov 2016-05-13 19:07:59 +10:00
commit 3cd1f758df
4 changed files with 15 additions and 11 deletions
Download patch file Download diff file Expand all files Collapse all files

5
src/main/config/config.c
View file

@ -134,7 +134,7 @@ static uint32_t activeFeaturesLatch = 0;
static uint8_t currentControlRateProfileIndex = 0;
controlRateConfig_t *currentControlRateProfile;
static const uint8_t EEPROM_CONF_VERSION = 118;
static const uint8_t EEPROM_CONF_VERSION = 119;
static void resetAccelerometerTrims(flightDynamicsTrims_t * accZero, flightDynamicsTrims_t * accGain)
{
@ -180,7 +180,8 @@ void resetPidProfile(pidProfile_t *pidProfile)
pidProfile->acc_soft_lpf_hz = 15;
pidProfile->gyro_soft_lpf_hz = 60;
pidProfile->dterm_lpf_hz = 30;
pidProfile->dterm_lpf_hz = 40;
pidProfile->yaw_lpf_hz = 30;
pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX;

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

@ -72,8 +72,8 @@ typedef struct {
filterStatePt1_t angleFilterState;
// Rate filtering
biquad_t deltaBiQuadState;
bool deltaFilterInit;
filterStatePt1_t ptermLpfState;
filterStatePt1_t deltaLpfState;
} pidState_t;
extern uint8_t motorCount;
@ -244,6 +244,11 @@ static void pidApplyRateController(const pidProfile_t *pidProfile, pidState_t *p
newPTerm = constrain(newPTerm, -pidProfile->yaw_p_limit, pidProfile->yaw_p_limit);
}
// Additional P-term LPF on YAW axis
if (axis == FD_YAW && pidProfile->yaw_lpf_hz) {
newPTerm = filterApplyPt1(newPTerm, &pidState->ptermLpfState, pidProfile->yaw_lpf_hz, dT);
}
// Calculate new D-term
float newDTerm;
if (pidProfile->D8[axis] == 0) {
@ -259,11 +264,7 @@ static void pidApplyRateController(const pidProfile_t *pidProfile, pidState_t *p
// Apply additional lowpass
if (pidProfile->dterm_lpf_hz) {
if (!pidState->deltaFilterInit) {
filterInitBiQuad(pidProfile->dterm_lpf_hz, &pidState->deltaBiQuadState, 0);
pidState->deltaFilterInit = true;
}
newDTerm = filterApplyBiQuad(newDTerm, &pidState->deltaBiQuadState);
newDTerm = filterApplyPt1(newDTerm, &pidState->deltaLpfState, pidProfile->dterm_lpf_hz, dT);
}
}

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

@ -53,6 +53,7 @@ typedef struct pidProfile_s {
uint8_t acc_soft_lpf_hz; // Set the Low Pass Filter factor for ACC. Reducing this value would reduce ACC noise (visible in GUI), but would increase ACC lag time. Zero = no filter
uint16_t yaw_p_limit;
uint8_t yaw_lpf_hz;
int16_t max_angle_inclination[ANGLE_INDEX_COUNT]; // Max possible inclination (roll and pitch axis separately
} pidProfile_t;

5
src/main/io/serial_cli.c
View file

@ -665,10 +665,8 @@ const clivalue_t valueTable[] = {
{ "throttle_tilt_comp_str", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].throttle_tilt_compensation_strength, .config.minmax = { 0, 100 }, 0 },
{ "yaw_control_direction", VAR_INT8 | MASTER_VALUE, &masterConfig.yaw_control_direction, .config.minmax = { -1, 1 }, 0 },
{ "yaw_motor_direction", VAR_INT8 | MASTER_VALUE, &masterConfig.mixerConfig.yaw_motor_direction, .config.minmax = { -1, 1 }, 0 },
{ "yaw_jump_prevention_limit", VAR_UINT16 | MASTER_VALUE, &masterConfig.mixerConfig.yaw_jump_prevention_limit, .config.minmax = { YAW_JUMP_PREVENTION_LIMIT_LOW, YAW_JUMP_PREVENTION_LIMIT_HIGH }, 0 },
{ "yaw_p_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_p_limit, .config.minmax = { YAW_P_LIMIT_MIN, YAW_P_LIMIT_MAX }, 0 },
#ifdef USE_SERVOS
{ "tri_unarmed_servo", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mixerConfig.tri_unarmed_servo, .config.lookup = { TABLE_OFF_ON }, 0 },
@ -730,6 +728,9 @@ const clivalue_t valueTable[] = {
{ "gyro_soft_lpf_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gyro_soft_lpf_hz, .config.minmax = {0, 200 } },
{ "acc_soft_lpf_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.acc_soft_lpf_hz, .config.minmax = {0, 200 } },
{ "dterm_lpf_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 200 } },
{ "yaw_lpf_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_lpf_hz, .config.minmax = {0, 200 } },
{ "yaw_p_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_p_limit, .config.minmax = { YAW_P_LIMIT_MIN, YAW_P_LIMIT_MAX }, 0 },
#ifdef BLACKBOX
{ "blackbox_rate_num", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_rate_num, .config.minmax = { 1, 32 }, 0 },