From a9ccf038d06d60aa7f2aba79980ea0e7be124173 Mon Sep 17 00:00:00 2001 From: Jaakko Laurikainen Date: Sun, 6 Dec 2015 15:53:39 +0200 Subject: [PATCH] Luxfloat integrator attenuation Integrator is disabled when angle error > 30 dps. Yaw integrator is attenuated proportional to stick position. --- src/main/flight/pid.c | 10 ++++++++-- src/main/mw.c | 45 +++++++++++++++++++++++++++++++++++-------- src/main/mw.h | 2 ++ 3 files changed, 47 insertions(+), 10 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index ef92375ceaa..a1408075aa4 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -47,6 +47,9 @@ #include "config/runtime_config.h" +//! Integrator is disabled when rate error exceeds this limit +#define LUXFLOAT_INTEGRATOR_DISABLE_LIMIT_DPS (30.0f) + extern uint16_t cycleTime; extern uint8_t motorCount; extern float dT; @@ -58,7 +61,7 @@ int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3]; #endif // PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction -uint8_t dynP8[3], dynI8[3], dynD8[3], PIDweight[3]; +uint8_t dynP8[3], dynI8[3], dynD8[3], PIDweight[3], Iweigth[3] = {0}; static int32_t errorGyroI[3] = { 0, 0, 0 }; static float errorGyroIf[3] = { 0.0f, 0.0f, 0.0f }; @@ -173,7 +176,10 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa PTerm = RateError * pidProfile->P_f[axis] * PIDweight[axis] / 100; // -----calculate I component. - errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * dT * pidProfile->I_f[axis] * 10, -250.0f, 250.0f); + if (fabsf(RateError) < LUXFLOAT_INTEGRATOR_DISABLE_LIMIT_DPS) + { + errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * dT * pidProfile->I_f[axis] * 10 * Iweigth[axis] / 100, -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 diff --git a/src/main/mw.c b/src/main/mw.c index db8ae50b0f5..3d91b4b3f48 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -112,7 +112,7 @@ uint8_t motorControlEnable = false; int16_t telemTemperature1; // gyro sensor temperature static uint32_t disarmAt; // Time of automatic disarm when "Don't spin the motors when armed" is enabled and auto_disarm_delay is nonzero -extern uint8_t dynP8[3], dynI8[3], dynD8[3], PIDweight[3]; +extern uint8_t dynP8[3], dynI8[3], dynD8[3], PIDweight[3], Iweigth[3]; static bool isRXDataNew; @@ -204,7 +204,7 @@ void filterRc(void){ void annexCode(void) { int32_t tmp, tmp2; - int32_t axis, prop1 = 0, prop2, yawWeigth; + int32_t axis, prop1 = 0, prop2, yawPDWeigth, yawIWeigth; static uint32_t vbatLastServiced = 0; static uint32_t ibatLastServiced = 0; @@ -221,29 +221,34 @@ void annexCode(void) // YAW dynamic PID adjustment if (rcData[THROTTLE] < currentControlRateProfile->tpa_yaw_breakpoint) { - yawWeigth = 100; + yawPDWeigth = 100; } else { if (rcData[THROTTLE] < 2000) { if (currentControlRateProfile->tpa_yaw_rate >= 100) { - yawWeigth = 100 + (uint16_t)(currentControlRateProfile->tpa_yaw_rate - 100) * (rcData[THROTTLE] - currentControlRateProfile->tpa_yaw_breakpoint) / (2000 - currentControlRateProfile->tpa_yaw_breakpoint); + yawPDWeigth = 100 + (uint16_t)(currentControlRateProfile->tpa_yaw_rate - 100) * (rcData[THROTTLE] - currentControlRateProfile->tpa_yaw_breakpoint) / (2000 - currentControlRateProfile->tpa_yaw_breakpoint); } else { - yawWeigth = 100 - (uint16_t)currentControlRateProfile->tpa_yaw_rate * (rcData[THROTTLE] - currentControlRateProfile->tpa_yaw_breakpoint) / (2000 - currentControlRateProfile->tpa_yaw_breakpoint); + yawPDWeigth = 100 - (uint16_t)currentControlRateProfile->tpa_yaw_rate * (rcData[THROTTLE] - currentControlRateProfile->tpa_yaw_breakpoint) / (2000 - currentControlRateProfile->tpa_yaw_breakpoint); } } else { if (currentControlRateProfile->tpa_yaw_rate >= 100) { - yawWeigth = currentControlRateProfile->tpa_yaw_rate; + yawPDWeigth = currentControlRateProfile->tpa_yaw_rate; } else { - yawWeigth = 100 - currentControlRateProfile->tpa_yaw_rate; + yawPDWeigth = 100 - currentControlRateProfile->tpa_yaw_rate; } } } + // YAW dynamic integral adjustment + // Attenuate integral term proportional to the stick position, 100% attenuation starting at full stick position + int32_t yawCommandFromCenter = ABS(rcData[YAW] - 1500); + yawIWeigth = MAX(0, (100 - (yawCommandFromCenter * 100 / 500))); + for (axis = 0; axis < 3; axis++) { tmp = MIN(ABS(rcData[axis] - masterConfig.rxConfig.midrc), 500); if (axis == ROLL || axis == PITCH) { @@ -278,10 +283,12 @@ void annexCode(void) // non coupled PID reduction scaler used in PID controller 1 and PID controller 2. YAW TPA disabled. 100 means 100% of the pids if (axis == YAW) { - PIDweight[axis] = yawWeigth; + PIDweight[axis] = yawPDWeigth; + Iweigth[axis] = yawIWeigth; } else { PIDweight[axis] = prop2; + Iweigth[axis] = 100; } if (rcData[axis] < masterConfig.rxConfig.midrc) @@ -915,3 +922,25 @@ void loop(void) } #endif } + +bool isRcAxisWithinDeadband(int32_t axis) +{ + int32_t tmp = MIN(ABS(rcData[axis] - masterConfig.rxConfig.midrc), 500); + bool ret = false; + if (axis == ROLL || axis == PITCH) + { + if (tmp <= currentProfile->rcControlsConfig.deadband) + { + ret = true; + } + } + else + { + if (tmp <= currentProfile->rcControlsConfig.yaw_deadband) + { + ret = true; + } + } + + return ret; +} diff --git a/src/main/mw.h b/src/main/mw.h index d1d49bdf724..0915b4f3856 100644 --- a/src/main/mw.h +++ b/src/main/mw.h @@ -24,3 +24,5 @@ void handleInflightCalibrationStickPosition(); void mwDisarm(void); void mwArm(void); + +bool isRcAxisWithinDeadband(int32_t axis);