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

Configurable Dterm average count

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
Show Stats 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;
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)
{
@ -177,6 +177,7 @@ static void resetPidProfile(pidProfile_t *pidProfile)
pidProfile->D8[PIDVEL] = 1;
pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX;
pidProfile->dterm_average_count = 3;
pidProfile->dterm_lpf_hz = 0; // filtering ON by default
pidProfile->deltaMethod = DELTA_FROM_MEASUREMENT;
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];
#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
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 ITerm,PTerm,DTerm;
static float lastErrorForDelta[3];
static float previousDelta[3][DELTA_TOTAL_SAMPLES];
static float previousDelta[3][DELTA_MAX_SAMPLES];
float delta, deltaSum;
int axis, deltaCount;
float horizonLevelStrength = 1;
@ -244,10 +244,10 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
} else {
// Apply moving average
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;
for (deltaCount = 0; deltaCount < DELTA_TOTAL_SAMPLES; deltaCount++) deltaSum += previousDelta[axis][deltaCount];
delta = (deltaSum / DELTA_TOTAL_SAMPLES);
for (deltaCount = 0; deltaCount < pidProfile->dterm_average_count; deltaCount++) deltaSum += previousDelta[axis][deltaCount];
delta = (deltaSum / pidProfile->dterm_average_count);
}
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 PTerm, ITerm, PTermACC, ITermACC, DTerm;
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) {
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;
}
@ -361,10 +361,10 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control
} else {
// Apply moving average
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;
for (deltaCount = 0; deltaCount < DELTA_TOTAL_SAMPLES; deltaCount++) DTerm += previousDelta[axis][deltaCount];
delta = DTerm;
for (deltaCount = 0; deltaCount < pidProfile->dterm_average_count; deltaCount++) DTerm += previousDelta[axis][deltaCount];
delta = (DTerm / pidProfile->dterm_average_count) * 3; // Keep same original scaling
}
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;
int32_t PTerm, ITerm, DTerm, delta, deltaSum;
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;
int8_t horizonLevelStrength = 100;
@ -538,10 +538,10 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co
} else {
// Apply moving average
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;
for (deltaCount = 0; deltaCount < DELTA_TOTAL_SAMPLES; deltaCount++) deltaSum += previousDelta[axis][deltaCount];
delta = deltaSum;
for (deltaCount = 0; deltaCount < pidProfile->dterm_average_count; deltaCount++) deltaSum += previousDelta[axis][deltaCount];
delta = (deltaSum / pidProfile->dterm_average_count) * 3; // Keep same original scaling
}
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
uint8_t deltaMethod; // Alternative delta Calculation
uint16_t yaw_p_limit;
uint8_t dterm_average_count; // Configurable delta count for dterm
#ifdef GTUNE
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 } },
{ "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_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 } },