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

Add alternative delta approach (set delta_from_gyro = ON)

Browse source
This commit is contained in:
borisbstyle 2016-01-25 22:34:07 +01:00
parent b9fb178237
commit 112543efb2
4 changed files with 20 additions and 7 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 = 118; static const uint8_t EEPROM_CONF_VERSION = 119;
static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
{ {
@ -178,6 +178,7 @@ static void resetPidProfile(pidProfile_t *pidProfile)
pidProfile->gyro_lpf_hz = 60; // filtering ON by default pidProfile->gyro_lpf_hz = 60; // filtering ON by default
pidProfile->dterm_lpf_hz = 0; // filtering ON by default pidProfile->dterm_lpf_hz = 0; // filtering ON by default
pidProfile->deltaFromGyro = 0;
pidProfile->airModeInsaneAcrobilityFactor = 0; pidProfile->airModeInsaneAcrobilityFactor = 0;
pidProfile->P_f[ROLL] = 1.1f; pidProfile->P_f[ROLL] = 1.1f;

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

@ -121,7 +121,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 lastError[3]; static float lastError[3], lastGyroRate[3];
static float previousDelta[3][DELTA_TOTAL_SAMPLES]; static float previousDelta[3][DELTA_TOTAL_SAMPLES];
float delta, deltaSum; float delta, deltaSum;
int axis, deltaCount; int axis, deltaCount;
@ -213,8 +213,13 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
ITerm = errorGyroIf[axis]; ITerm = errorGyroIf[axis];
//-----calculate D-term //-----calculate D-term
if (!pidProfile->deltaFromGyro) {
delta = RateError - lastError[axis]; delta = RateError - lastError[axis];
lastError[axis] = RateError; 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;
}
if (deltaStateIsSet) { if (deltaStateIsSet) {
delta = applyBiQuadFilter(delta, &deltaBiQuadState[axis]); delta = applyBiQuadFilter(delta, &deltaBiQuadState[axis]);
@ -263,7 +268,7 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
int32_t PTerm, ITerm, DTerm, delta, deltaSum; int32_t PTerm, ITerm, DTerm, delta, deltaSum;
static int32_t lastError[3] = { 0, 0, 0 }; static int32_t lastError[3] = { 0, 0, 0 };
static int32_t previousDelta[3][DELTA_TOTAL_SAMPLES]; static int32_t previousDelta[3][DELTA_TOTAL_SAMPLES];
static int32_t previousErrorGyroI[3] = { 0, 0, 0 }; static int32_t previousErrorGyroI[3], lastGyroRate[3];
int32_t AngleRateTmp, RateError, gyroRate; int32_t AngleRateTmp, RateError, gyroRate;
int8_t horizonLevelStrength = 100; int8_t horizonLevelStrength = 100;
@ -356,8 +361,13 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
} }
//-----calculate D-term //-----calculate D-term
if (!pidProfile->deltaFromGyro) { // 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 delta = RateError - lastError[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
lastError[axis] = RateError; 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;
}
if (deltaStateIsSet) { if (deltaStateIsSet) {
delta = lrintf(applyBiQuadFilter((float) delta, &deltaBiQuadState[axis])) * 3; // Keep same scaling as unfiltered delta delta = lrintf(applyBiQuadFilter((float) delta, &deltaBiQuadState[axis])) * 3; // Keep same scaling as unfiltered delta

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

@ -61,6 +61,7 @@ typedef struct pidProfile_s {
uint16_t airModeInsaneAcrobilityFactor; // Air mode acrobility factor uint16_t airModeInsaneAcrobilityFactor; // Air mode acrobility factor
uint8_t gyro_lpf_hz; // Gyro Soft filter in hz uint8_t gyro_lpf_hz; // Gyro Soft filter in hz
uint8_t dterm_lpf_hz; // Delta Filter in hz uint8_t dterm_lpf_hz; // Delta Filter in hz
uint8_t deltaFromGyro; // Alternative delta Calculation
#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

@ -618,6 +618,7 @@ const clivalue_t valueTable[] = {
{ "mag_hardware", VAR_UINT8 | MASTER_VALUE, &masterConfig.mag_hardware, .config.minmax = { 0, MAG_MAX } }, { "mag_hardware", VAR_UINT8 | MASTER_VALUE, &masterConfig.mag_hardware, .config.minmax = { 0, MAG_MAX } },
{ "mag_declination", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].mag_declination, .config.minmax = { -18000, 18000 } }, { "mag_declination", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].mag_declination, .config.minmax = { -18000, 18000 } },
{ "delta_from_gyro", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.deltaFromGyro, .config.lookup = { TABLE_OFF_ON } },
{ "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 } },