From 07b9aceb7c3c6afa93f73e0a3c89bcd0aa201efd Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Thu, 9 Jul 2015 23:56:11 +0200 Subject: [PATCH] Luxfloat Optimisation and new defaults Remove constraining of delta Yaw D default to 10 Default change and rebase with master --- src/main/config/config.c | 18 +++++++++--------- src/main/flight/pid.c | 15 ++++++++------- 2 files changed, 17 insertions(+), 16 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 2c04ef33b..230433d7a 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -175,15 +175,15 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->pterm_cut_hz = 0; pidProfile->dterm_cut_hz = 0; - pidProfile->P_f[ROLL] = 2.5f; // new PID with preliminary defaults test carefully - pidProfile->I_f[ROLL] = 0.6f; - pidProfile->D_f[ROLL] = 0.06f; - pidProfile->P_f[PITCH] = 2.5f; - pidProfile->I_f[PITCH] = 0.6f; - pidProfile->D_f[PITCH] = 0.06f; - pidProfile->P_f[YAW] = 8.0f; - pidProfile->I_f[YAW] = 0.5f; - pidProfile->D_f[YAW] = 0.05f; + pidProfile->P_f[ROLL] = 1.5f; // new PID with preliminary defaults test carefully + pidProfile->I_f[ROLL] = 0.4f; + pidProfile->D_f[ROLL] = 0.03f; + pidProfile->P_f[PITCH] = 1.5f; + pidProfile->I_f[PITCH] = 0.4f; + pidProfile->D_f[PITCH] = 0.03f; + pidProfile->P_f[YAW] = 2.5f; + pidProfile->I_f[YAW] = 1.0f; + pidProfile->D_f[YAW] = 0.00f; pidProfile->A_level = 5.0f; pidProfile->H_level = 3.0f; pidProfile->H_sensitivity = 75; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 67155a112..a882680c0 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -110,7 +110,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa float RateError, errorAngle, AngleRate, gyroRate; float ITerm,PTerm,DTerm; int32_t stickPosAil, stickPosEle, mostDeflectedPos; - static float lastGyroRate[3]; + static float lastError[3]; static float delta1[3], delta2[3]; float delta, deltaSum; int axis; @@ -190,16 +190,17 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa if (pidProfile->pterm_cut_hz) { PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz, dT); } - // -----calculate I component. Note that PIDweight is divided by 10, because it is simplified formule from the previous multiply by 10 - errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * dT * pidProfile->I_f[axis] * PIDweight[axis] / 10, -250.0f, 250.0f); + + // -----calculate I component. + errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * dT * pidProfile->I_f[axis] * 10, -250.0f, 250.0f); // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated. // I coefficient (I8) moved before integration to make limiting independent from PID settings ITerm = errorGyroIf[axis]; //-----calculate D-term - delta = gyroRate - lastGyroRate[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800 - lastGyroRate[axis] = gyroRate; + delta = RateError - lastError[axis]; + lastError[axis] = RateError; // 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. @@ -217,12 +218,12 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa DTerm = constrainf((deltaSum / 3.0f) * pidProfile->D_f[axis] * PIDweight[axis] / 100, -300.0f, 300.0f); // -----calculate total PID output - axisPID[axis] = constrain(lrintf(PTerm + ITerm - DTerm), -1000, 1000); + axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -1000, 1000); #ifdef BLACKBOX axisPID_P[axis] = PTerm; axisPID_I[axis] = ITerm; - axisPID_D[axis] = -DTerm; + axisPID_D[axis] = DTerm; #endif } }