Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-16 21:05:35 +03:00
Code Issues Wiki Activity

Configurable Dterm average count

Browse source
This commit is contained in:
borisbstyle 2016-02-17 21:44:07 +01:00
parent 33eef46db3
commit 4c2acde6e2
4 changed files with 18 additions and 15 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -134,7 +134,7 @@ static uint32_t activeFeaturesLatch = 0;
static uint8_t currentControlRateProfileIndex = 0; static uint8_t currentControlRateProfileIndex = 0;
controlRateConfig_t *currentControlRateProfile; controlRateConfig_t *currentControlRateProfile;
static const uint8_t EEPROM_CONF_VERSION = 122; static const uint8_t EEPROM_CONF_VERSION = 123;
static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
{ {
@ -177,6 +177,7 @@ static void resetPidProfile(pidProfile_t *pidProfile)
pidProfile->D8[PIDVEL] = 1; pidProfile->D8[PIDVEL] = 1;
pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX; pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX;
pidProfile->dterm_average_count = 3;
pidProfile->dterm_lpf_hz = 0; // filtering ON by default pidProfile->dterm_lpf_hz = 0; // filtering ON by default
pidProfile->deltaMethod = DELTA_FROM_MEASUREMENT; pidProfile->deltaMethod = DELTA_FROM_MEASUREMENT;
pidProfile->airModeInsaneAcrobilityFactor = 0; pidProfile->airModeInsaneAcrobilityFactor = 0;

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

@ -58,7 +58,7 @@ int16_t axisPID[3];
int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3]; int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
#endif #endif
#define DELTA_TOTAL_SAMPLES 3 #define DELTA_MAX_SAMPLES 10
// PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction // PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction
uint8_t dynP8[3], dynI8[3], dynD8[3], PIDweight[3]; uint8_t dynP8[3], dynI8[3], dynD8[3], PIDweight[3];
@ -141,7 +141,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
float RateError, AngleRate, gyroRate; float RateError, AngleRate, gyroRate;
float ITerm,PTerm,DTerm; float ITerm,PTerm,DTerm;
static float lastErrorForDelta[3]; static float lastErrorForDelta[3];
static float previousDelta[3][DELTA_TOTAL_SAMPLES]; static float previousDelta[3][DELTA_MAX_SAMPLES];
float delta, deltaSum; float delta, deltaSum;
int axis, deltaCount; int axis, deltaCount;
float horizonLevelStrength = 1; float horizonLevelStrength = 1;
@ -244,10 +244,10 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
} else { } else {
// Apply moving average // Apply moving average
deltaSum = 0; deltaSum = 0;
for (deltaCount = DELTA_TOTAL_SAMPLES-1; deltaCount > 0; deltaCount--) previousDelta[axis][deltaCount] = previousDelta[axis][deltaCount-1]; for (deltaCount = pidProfile->dterm_average_count-1; deltaCount > 0; deltaCount--) previousDelta[axis][deltaCount] = previousDelta[axis][deltaCount-1];
previousDelta[axis][0] = delta; previousDelta[axis][0] = delta;
for (deltaCount = 0; deltaCount < DELTA_TOTAL_SAMPLES; deltaCount++) deltaSum += previousDelta[axis][deltaCount]; for (deltaCount = 0; deltaCount < pidProfile->dterm_average_count; deltaCount++) deltaSum += previousDelta[axis][deltaCount];
delta = (deltaSum / DELTA_TOTAL_SAMPLES); delta = (deltaSum / pidProfile->dterm_average_count);
} }
DTerm = constrainf(delta * pidProfile->D_f[axis] * PIDweight[axis] / 100, -300.0f, 300.0f); DTerm = constrainf(delta * pidProfile->D_f[axis] * PIDweight[axis] / 100, -300.0f, 300.0f);
@ -283,10 +283,10 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control
int32_t rc, error, errorAngle, delta, gyroError; int32_t rc, error, errorAngle, delta, gyroError;
int32_t PTerm, ITerm, PTermACC, ITermACC, DTerm; int32_t PTerm, ITerm, PTermACC, ITermACC, DTerm;
static int16_t lastErrorForDelta[2]; static int16_t lastErrorForDelta[2];
static int32_t previousDelta[3][DELTA_TOTAL_SAMPLES]; static int32_t previousDelta[2][DELTA_MAX_SAMPLES];
if (!deltaStateIsSet && pidProfile->dterm_lpf_hz) { if (!deltaStateIsSet && pidProfile->dterm_lpf_hz) {
for (axis = 0; axis < 3; axis++) BiQuadNewLpf(pidProfile->dterm_lpf_hz, &deltaBiQuadState[axis], targetLooptime); for (axis = 0; axis < 2; axis++) BiQuadNewLpf(pidProfile->dterm_lpf_hz, &deltaBiQuadState[axis], targetLooptime);
deltaStateIsSet = true; deltaStateIsSet = true;
} }
@ -361,10 +361,10 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control
} else { } else {
// Apply moving average // Apply moving average
DTerm = 0; DTerm = 0;
for (deltaCount = DELTA_TOTAL_SAMPLES -1; deltaCount > 0; deltaCount--) previousDelta[axis][deltaCount] = previousDelta[axis][deltaCount-1]; for (deltaCount = pidProfile->dterm_average_count-1; deltaCount > 0; deltaCount--) previousDelta[axis][deltaCount] = previousDelta[axis][deltaCount-1];
previousDelta[axis][0] = delta; previousDelta[axis][0] = delta;
for (deltaCount = 0; deltaCount < DELTA_TOTAL_SAMPLES; deltaCount++) DTerm += previousDelta[axis][deltaCount]; for (deltaCount = 0; deltaCount < pidProfile->dterm_average_count; deltaCount++) DTerm += previousDelta[axis][deltaCount];
delta = DTerm; delta = (DTerm / pidProfile->dterm_average_count) * 3; // Keep same original scaling
} }
DTerm = ((int32_t)DTerm * dynD8[axis]) >> 5; // 32 bits is needed for calculation DTerm = ((int32_t)DTerm * dynD8[axis]) >> 5; // 32 bits is needed for calculation
@ -432,7 +432,7 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co
int axis, deltaCount; int axis, deltaCount;
int32_t PTerm, ITerm, DTerm, delta, deltaSum; int32_t PTerm, ITerm, DTerm, delta, deltaSum;
static int32_t lastErrorForDelta[3] = { 0, 0, 0 }; static int32_t lastErrorForDelta[3] = { 0, 0, 0 };
static int32_t previousDelta[3][DELTA_TOTAL_SAMPLES]; static int32_t previousDelta[3][DELTA_MAX_SAMPLES];
int32_t AngleRateTmp, RateError, gyroRate; int32_t AngleRateTmp, RateError, gyroRate;
int8_t horizonLevelStrength = 100; int8_t horizonLevelStrength = 100;
@ -538,10 +538,10 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co
} else { } else {
// Apply moving average // Apply moving average
deltaSum = 0; deltaSum = 0;
for (deltaCount = DELTA_TOTAL_SAMPLES -1; deltaCount > 0; deltaCount--) previousDelta[axis][deltaCount] = previousDelta[axis][deltaCount-1]; for (deltaCount = pidProfile->dterm_average_count -1; deltaCount > 0; deltaCount--) previousDelta[axis][deltaCount] = previousDelta[axis][deltaCount-1];
previousDelta[axis][0] = delta; previousDelta[axis][0] = delta;
for (deltaCount = 0; deltaCount < DELTA_TOTAL_SAMPLES; deltaCount++) deltaSum += previousDelta[axis][deltaCount]; for (deltaCount = 0; deltaCount < pidProfile->dterm_average_count; deltaCount++) deltaSum += previousDelta[axis][deltaCount];
delta = deltaSum; delta = (deltaSum / pidProfile->dterm_average_count) * 3; // Keep same original scaling
} }
DTerm = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8; DTerm = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8;

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

@ -68,6 +68,7 @@ typedef struct pidProfile_s {
float dterm_lpf_hz; // Delta Filter in hz float dterm_lpf_hz; // Delta Filter in hz
uint8_t deltaMethod; // Alternative delta Calculation uint8_t deltaMethod; // Alternative delta Calculation
uint16_t yaw_p_limit; uint16_t yaw_p_limit;
uint8_t dterm_average_count; // Configurable delta count for dterm
#ifdef GTUNE #ifdef GTUNE
uint8_t gtune_lolimP[3]; // [0..200] Lower limit of P during G tune uint8_t gtune_lolimP[3]; // [0..200] Lower limit of P during G tune

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

@ -676,6 +676,7 @@ const clivalue_t valueTable[] = {
{ "mag_declination", VAR_INT16 | MASTER_VALUE, &masterConfig.mag_declination, .config.minmax = { -18000, 18000 } }, { "mag_declination", VAR_INT16 | MASTER_VALUE, &masterConfig.mag_declination, .config.minmax = { -18000, 18000 } },
{ "pid_delta_method", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.deltaMethod, .config.lookup = { TABLE_DELTA_METHOD } }, { "pid_delta_method", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.deltaMethod, .config.lookup = { TABLE_DELTA_METHOD } },
{ "dterm_lpf_hz", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } }, { "dterm_lpf_hz", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } },
{ "dterm_average_count", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_average_count, .config.minmax = {2, 10 } },
{ "pid_controller", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.pidController, .config.lookup = { TABLE_PID_CONTROLLER } }, { "pid_controller", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.pidController, .config.lookup = { TABLE_PID_CONTROLLER } },