Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-17 13:25:30 +03:00
Code Issues Wiki Activity

Cleanup delta method code code and refactor

Browse source
This commit is contained in:
borisbstyle 2016-02-06 14:00:45 +01:00
parent 71392f846d
commit 035b34bbc4
4 changed files with 28 additions and 18 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -177,7 +177,7 @@ static void resetPidProfile(pidProfile_t *pidProfile)
pidProfile->D8[PIDVEL] = 1; pidProfile->D8[PIDVEL] = 1;
pidProfile->dterm_lpf_hz = 0; // filtering ON by default pidProfile->dterm_lpf_hz = 0; // filtering ON by default
pidProfile->deltaFromGyro = 1; pidProfile->deltaMethod = DELTA_FROM_MEASUREMENT;
pidProfile->airModeInsaneAcrobilityFactor = 0; pidProfile->airModeInsaneAcrobilityFactor = 0;
pidProfile->P_f[ROLL] = 1.1f; pidProfile->P_f[ROLL] = 1.1f;

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

@ -128,7 +128,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], lastGyroRate[3]; static float lastErrorForDelta[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;
@ -215,12 +215,12 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
ITerm = errorGyroIf[axis]; ITerm = errorGyroIf[axis];
//-----calculate D-term //-----calculate D-term
if (!pidProfile->deltaFromGyro) { if (pidProfile->deltaMethod == DELTA_FROM_ERROR) {
delta = RateError - lastError[axis]; delta = RateError - lastErrorForDelta[axis];
lastError[axis] = RateError; lastErrorForDelta[axis] = RateError;
} else { } else {
delta = -(gyroRate - lastGyroRate[axis]); // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800 delta = -(gyroRate - lastErrorForDelta[axis]); // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
lastGyroRate[axis] = gyroRate; lastErrorForDelta[axis] = gyroRate;
} }
// Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference // Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference
@ -269,9 +269,8 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
int axis, deltaCount; int axis, deltaCount;
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 lastErrorForDelta[3] = { 0, 0, 0 };
static int32_t previousDelta[3][DELTA_TOTAL_SAMPLES]; static int32_t previousDelta[3][DELTA_TOTAL_SAMPLES];
static int32_t lastGyroRate[3];
int32_t AngleRateTmp, RateError, gyroRate; int32_t AngleRateTmp, RateError, gyroRate;
int8_t horizonLevelStrength = 100; int8_t horizonLevelStrength = 100;
@ -360,12 +359,12 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
ITerm = errorGyroI[axis] >> 13; ITerm = errorGyroI[axis] >> 13;
//-----calculate D-term //-----calculate D-term
if (!pidProfile->deltaFromGyro) { // quick and dirty solution for testing if (pidProfile->deltaMethod == DELTA_FROM_ERROR) {
delta = RateError - lastError[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800 delta = RateError - lastErrorForDelta[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
lastError[axis] = RateError; lastErrorForDelta[axis] = RateError;
} else { } else {
delta = -(gyroRate - lastGyroRate[axis]); // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800 delta = -(gyroRate - lastErrorForDelta[axis]); // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
lastGyroRate[axis] = gyroRate; lastErrorForDelta[axis] = gyroRate;
} }
// Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference // Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference

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

@ -42,6 +42,11 @@ typedef enum {
PID_COUNT PID_COUNT
} pidControllerType_e; } pidControllerType_e;
typedef enum {
DELTA_FROM_ERROR = 0,
DELTA_FROM_MEASUREMENT
} pidDeltaType_e;
#define IS_PID_CONTROLLER_FP_BASED(pidController) (pidController == 2) #define IS_PID_CONTROLLER_FP_BASED(pidController) (pidController == 2)
typedef struct pidProfile_s { typedef struct pidProfile_s {
@ -60,7 +65,7 @@ typedef struct pidProfile_s {
uint16_t airModeInsaneAcrobilityFactor; // Air mode acrobility factor uint16_t airModeInsaneAcrobilityFactor; // Air mode acrobility factor
float dterm_lpf_hz; // Delta Filter in hz float dterm_lpf_hz; // Delta Filter in hz
uint8_t deltaFromGyro; // Alternative delta Calculation uint8_t deltaMethod; // 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

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

@ -400,6 +400,10 @@ static const char * const lookupTableMagHardware[] = {
"AK8963" "AK8963"
}; };
static const char * const lookupDeltaMethod[] = {
"ERROR", "MEASUREMENT"
};
typedef struct lookupTableEntry_s { typedef struct lookupTableEntry_s {
const char * const *values; const char * const *values;
const uint8_t valueCount; const uint8_t valueCount;
@ -424,6 +428,7 @@ typedef enum {
TABLE_ACC_HARDWARE, TABLE_ACC_HARDWARE,
TABLE_BARO_HARDWARE, TABLE_BARO_HARDWARE,
TABLE_MAG_HARDWARE, TABLE_MAG_HARDWARE,
TABLE_DELTA_METHOD,
} lookupTableIndex_e; } lookupTableIndex_e;
static const lookupTableEntry_t lookupTables[] = { static const lookupTableEntry_t lookupTables[] = {
@ -442,7 +447,8 @@ static const lookupTableEntry_t lookupTables[] = {
{ lookupTableGyroLpf, sizeof(lookupTableGyroLpf) / sizeof(char *) }, { lookupTableGyroLpf, sizeof(lookupTableGyroLpf) / sizeof(char *) },
{ lookupTableAccHardware, sizeof(lookupTableAccHardware) / sizeof(char *) }, { lookupTableAccHardware, sizeof(lookupTableAccHardware) / sizeof(char *) },
{ lookupTableBaroHardware, sizeof(lookupTableBaroHardware) / sizeof(char *) }, { lookupTableBaroHardware, sizeof(lookupTableBaroHardware) / sizeof(char *) },
{ lookupTableMagHardware, sizeof(lookupTableMagHardware) / sizeof(char *) } { lookupTableMagHardware, sizeof(lookupTableMagHardware) / sizeof(char *) },
{ lookupDeltaMethod, sizeof(lookupDeltaMethod) / sizeof(char *) }
}; };
#define VALUE_TYPE_OFFSET 0 #define VALUE_TYPE_OFFSET 0
@ -664,7 +670,8 @@ const clivalue_t valueTable[] = {
{ "mag_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mag_hardware, .config.lookup = { TABLE_MAG_HARDWARE } }, { "mag_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mag_hardware, .config.lookup = { TABLE_MAG_HARDWARE } },
{ "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 } },
{ "delta_from_gyro", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.deltaFromGyro, .config.lookup = { TABLE_OFF_ON } }, { "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 } },
{ "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 } },
@ -704,7 +711,6 @@ const clivalue_t valueTable[] = {
{ "i_vel", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PIDVEL], .config.minmax = { 0, 200 } }, { "i_vel", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PIDVEL], .config.minmax = { 0, 200 } },
{ "d_vel", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PIDVEL], .config.minmax = { 0, 200 } }, { "d_vel", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PIDVEL], .config.minmax = { 0, 200 } },
{ "dterm_lpf_hz", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } },
{ "acro_plus_factor", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.airModeInsaneAcrobilityFactor, .config.minmax = {0, 100 } }, { "acro_plus_factor", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.airModeInsaneAcrobilityFactor, .config.minmax = {0, 100 } },
#ifdef BLACKBOX #ifdef BLACKBOX