From 112543efb2865cb7fc335f420d52d0a985e76f60 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Mon, 25 Jan 2016 22:34:07 +0100 Subject: [PATCH] Add alternative delta approach (set delta_from_gyro = ON) --- src/main/config/config.c | 3 ++- src/main/flight/pid.c | 22 ++++++++++++++++------ src/main/flight/pid.h | 1 + src/main/io/serial_cli.c | 1 + 4 files changed, 20 insertions(+), 7 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 5a8367f787..7f84197a17 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -134,7 +134,7 @@ static uint32_t activeFeaturesLatch = 0; static uint8_t currentControlRateProfileIndex = 0; 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) { @@ -178,6 +178,7 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->gyro_lpf_hz = 60; // filtering ON by default pidProfile->dterm_lpf_hz = 0; // filtering ON by default + pidProfile->deltaFromGyro = 0; pidProfile->airModeInsaneAcrobilityFactor = 0; pidProfile->P_f[ROLL] = 1.1f; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index f77f2765de..61fd3b9295 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -121,7 +121,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa { float RateError, AngleRate, gyroRate; float ITerm,PTerm,DTerm; - static float lastError[3]; + static float lastError[3], lastGyroRate[3]; static float previousDelta[3][DELTA_TOTAL_SAMPLES]; float delta, deltaSum; int axis, deltaCount; @@ -213,8 +213,13 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa ITerm = errorGyroIf[axis]; //-----calculate D-term - delta = RateError - lastError[axis]; - lastError[axis] = RateError; + if (!pidProfile->deltaFromGyro) { + 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; + } if (deltaStateIsSet) { 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; static int32_t lastError[3] = { 0, 0, 0 }; 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; int8_t horizonLevelStrength = 100; @@ -356,8 +361,13 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat } //-----calculate D-term - delta = RateError - lastError[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800 - lastError[axis] = RateError; + 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 + 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) { delta = lrintf(applyBiQuadFilter((float) delta, &deltaBiQuadState[axis])) * 3; // Keep same scaling as unfiltered delta diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index afc28becc1..781d4a6b5c 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -61,6 +61,7 @@ typedef struct pidProfile_s { uint16_t airModeInsaneAcrobilityFactor; // Air mode acrobility factor uint8_t gyro_lpf_hz; // Gyro Soft filter in hz uint8_t dterm_lpf_hz; // Delta Filter in hz + uint8_t deltaFromGyro; // Alternative delta Calculation #ifdef GTUNE uint8_t gtune_lolimP[3]; // [0..200] Lower limit of P during G tune diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index a91222c8fa..6bb2bac080 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -618,6 +618,7 @@ const clivalue_t valueTable[] = { { "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 } }, + { "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 } },