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

Add more configurable Dterm approaches (for testing purposes)

Browse source
This commit is contained in:
borisbstyle 2016-01-05 11:48:24 +01:00
parent 91de6fd4c2
commit 42dfba8218
5 changed files with 46 additions and 34 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -178,7 +178,7 @@ static void resetPidProfile(pidProfile_t *pidProfile)
pidProfile->gyro_soft_lpf = 1; // filtering ON by default
pidProfile->dterm_cut_hz = 0;
pidProfile->yaw_pterm_cut_hz = 0;
pidProfile->delta_from_gyro_error = 0;
pidProfile->P_f[ROLL] = 1.5f; // new PID with preliminary defaults test carefully
pidProfile->I_f[ROLL] = 0.3f;

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

@ -84,24 +84,29 @@ void pidResetErrorGyro(void)
errorGyroIf[YAW] = 0.0f;
}
void setPidDeltaSamples(void) {
void setPidDeltaSamples(uint8_t filter) {
if (!filter) {
if (targetLooptime < 1000) {
deltaTotalSamples = 8;
} else {
deltaTotalSamples = 4;
}
} else {
deltaTotalSamples = 1;
}
}
const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
static filterStatePt1_t DTermState[3];
static filterStatePt1_t yawPTermState;
//static filterStatePt1_t yawPTermState;
static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
{
float RateError, AngleRate, gyroRate;
float ITerm,PTerm,DTerm;
static float lastGyroRate[3];
static float lastError[3];
static float previousDelta[3][8];
float delta, deltaSum;
@ -109,7 +114,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
float horizonLevelStrength = 1;
static float previousErrorGyroIf[3] = { 0.0f, 0.0f, 0.0f };
if (!deltaTotalSamples) setPidDeltaSamples();
if (!deltaTotalSamples) setPidDeltaSamples(pidProfile->dterm_cut_hz);
if (FLIGHT_MODE(HORIZON_MODE)) {
// Figure out the raw stick positions
@ -167,10 +172,6 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
// -----calculate P component
PTerm = RateError * pidProfile->P_f[axis] * PIDweight[axis] / 100;
if (axis == YAW && pidProfile->yaw_pterm_cut_hz) {
PTerm = filterApplyPt1(PTerm, &yawPTermState, pidProfile->yaw_pterm_cut_hz, dT);
}
// -----calculate I component.
errorGyroIf[axis] = constrainf(errorGyroIf[axis] + 0.5f * (lastError[axis] + RateError) * dT * pidProfile->I_f[axis] * 10, -250.0f, 250.0f);
@ -189,19 +190,26 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
ITerm = errorGyroIf[axis];
//-----calculate D-term
if (!pidProfile->delta_from_gyro_error) {
delta = RateError - lastError[axis];
lastError[axis] = RateError;
} else {
delta = -(gyroRate - lastGyroRate[axis]); // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
lastGyroRate[axis] = gyroRate;
}
// Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference
// would be scaled by different dt each time. Division by dT fixes that.
delta *= (1.0f / dT);
deltaSum = 0;
if (pidProfile->dterm_cut_hz) {
// Dterm low pass
deltaSum = filterApplyPt1(delta, &DTermState[axis], pidProfile->dterm_cut_hz, dT);
} else {
}
// Apply moving average
deltaSum = 0;
if (deltaTotalSamples > 1) {
for (deltaCount = deltaTotalSamples-1; deltaCount > 0; deltaCount--) previousDelta[axis][deltaCount] = previousDelta[axis][deltaCount-1];
previousDelta[axis][0] = delta;
for (deltaCount = 0; deltaCount < deltaTotalSamples; deltaCount++) deltaSum += previousDelta[axis][deltaCount];
@ -237,12 +245,13 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
static int32_t previousDelta[3][8];
int32_t PTerm, ITerm, DTerm;
static int32_t lastError[3] = { 0, 0, 0 };
static int32_t lastGyroRate[3] = { 0, 0, 0 };
static int32_t previousErrorGyroI[3] = { 0, 0, 0 };
int32_t AngleRateTmp, RateError;
int32_t AngleRateTmp, RateError, gyroRate;
int8_t horizonLevelStrength = 100;
if (!deltaTotalSamples) setPidDeltaSamples();
if (!deltaTotalSamples) setPidDeltaSamples(pidProfile->dterm_cut_hz);
if (FLIGHT_MODE(HORIZON_MODE)) {
// Figure out the raw stick positions
@ -290,15 +299,12 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
// Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes
// -----calculate scaled error.AngleRates
// multiplication of rcCommand corresponds to changing the sticks scaling here
RateError = AngleRateTmp - (gyroADC[axis] / 4);
gyroRate = gyroADC[axis] / 4;
RateError = AngleRateTmp - gyroRate;
// -----calculate P component
PTerm = (RateError * pidProfile->P8[axis] * PIDweight[axis] / 100) >> 7;
if (axis == YAW && pidProfile->yaw_pterm_cut_hz) {
PTerm = filterApplyPt1(PTerm, &yawPTermState, pidProfile->yaw_pterm_cut_hz, dT);
}
// -----calculate I component
// there should be no division before accumulating the error to integrator, because the precision would be reduced.
// Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used.
@ -323,24 +329,30 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
}
//-----calculate D-term
if (!pidProfile->delta_from_gyro_error) { // quick and dirty solution for testing
delta = RateError - lastError[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
lastError[axis] = RateError;
} else {
delta = -(gyroRate - lastGyroRate[axis]); // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
lastGyroRate[axis] = gyroRate;
}
// Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference
// would be scaled by different dt each time. Division by dT fixes that.
delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetLooptime >> 4))) >> 6;
if (pidProfile->dterm_cut_hz) {
// Dterm delta low pass
deltaSum = filterApplyPt1(delta, &DTermState[axis], pidProfile->dterm_cut_hz, dT);
} else {
// Apply moving average
deltaSum = 0;
if (pidProfile->dterm_cut_hz) {
// Dterm low pass
deltaSum = filterApplyPt1(delta, &DTermState[axis], pidProfile->dterm_cut_hz, dT);
}
if (deltaTotalSamples > 1) {
for (deltaCount = deltaTotalSamples-1; deltaCount > 0; deltaCount--) previousDelta[axis][deltaCount] = previousDelta[axis][deltaCount-1];
previousDelta[axis][0] = delta;
for (deltaCount = 0; deltaCount < deltaTotalSamples; deltaCount++) deltaSum += previousDelta[axis][deltaCount];
deltaSum = (deltaSum / deltaTotalSamples) * 3; // get old scaling by multiplying with 3
}
deltaSum = (deltaSum / deltaTotalSamples) * 3; // get old scaling by multiplying with 3
DTerm = (deltaSum * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8;

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

@ -59,7 +59,7 @@ typedef struct pidProfile_s {
uint16_t yaw_p_limit; // set P term limit (fixed value was 300)
uint8_t dterm_cut_hz; // (default 17Hz, Range 1-50Hz) Used for PT1 element in PID1, PID2 and PID5
uint8_t yaw_pterm_cut_hz; // Used for filering Pterm noise on noisy frames
uint8_t delta_from_gyro_error; // Used for filering Pterm noise on noisy frames
uint8_t gyro_soft_lpf; // Gyro FIR filter
#ifdef GTUNE

4
src/main/io/i2c_bst.c
View file

@ -996,7 +996,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
bstWrite16(masterConfig.gyro_lpf);
bstWrite8(0);//masterConfig.profile[0].pidProfile.gyro_cut_hz);
bstWrite8(masterConfig.profile[0].pidProfile.dterm_cut_hz);
bstWrite8(masterConfig.profile[0].pidProfile.yaw_pterm_cut_hz);
bstWrite8(masterConfig.profile[0].pidProfile.delta_from_gyro_error);
break;
default:
// we do not know how to handle the (valid) message, indicate error BST
@ -1461,7 +1461,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand)
masterConfig.gyro_lpf = bstRead16();
bstRead8();//masterConfig.profile[0].pidProfile.gyro_cut_hz = bstRead8();
masterConfig.profile[0].pidProfile.dterm_cut_hz = bstRead8();
masterConfig.profile[0].pidProfile.yaw_pterm_cut_hz = bstRead8();
masterConfig.profile[0].pidProfile.delta_from_gyro_error = bstRead8();
break;
default:

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

@ -657,7 +657,7 @@ const clivalue_t valueTable[] = {
{ "gyro_soft_lpf", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.gyro_soft_lpf, .config.lookup = { TABLE_OFF_ON } },
{ "dterm_cut_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_cut_hz, .config.minmax = {0, 200 } },
{ "yaw_pterm_cut_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_pterm_cut_hz, .config.minmax = {0, 200 } },
{ "delta_from_gyro_error", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.delta_from_gyro_error, .config.minmax = {0, 1} },
#ifdef BLACKBOX
{ "blackbox_rate_num", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_rate_num, .config.minmax = { 1, 32 } },