From b4e61d49c9e3087baf3232a3ef981f2d73bd9acd Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Thu, 4 Aug 2016 00:50:11 +0200 Subject: [PATCH] More efficiency in PID code // add yaw velocity to setpoint --- src/main/config/config.c | 3 +- src/main/flight/pid.c | 68 ++++++++++++++++++++-------------------- src/main/flight/pid.h | 3 +- src/main/io/serial_cli.c | 5 ++- src/main/io/serial_msp.c | 4 +-- src/main/mw.c | 7 +++-- 6 files changed, 44 insertions(+), 46 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 991d6f5c74..c1ddd2d281 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -245,8 +245,7 @@ static void resetPidProfile(pidProfile_t *pidProfile) // Betaflight PID controller parameters pidProfile->ptermSetpointWeight = 75; pidProfile->dtermSetpointWeight = 200; - pidProfile->pidMaxVelocity = 1000; - pidProfile->pidMaxVelocityYaw = 50; + pidProfile->pidMaxVelocityYaw = 200; pidProfile->toleranceBand = 20; pidProfile->toleranceBandReduction = 40; pidProfile->zeroCrossAllowanceCount = 2; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index b6244aa5c9..ae735be925 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -133,13 +133,10 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc float errorRate = 0, rP = 0, rD = 0, PVRate = 0; float ITerm,PTerm,DTerm; static float lastRateError[2]; + static float Kp[3], Ki[3], Kd[3], b[3], c[3], yawMaxVelocity, yawPreviousRate; float delta; int axis; float horizonLevelStrength = 1; - static int16_t axisPIDState[3]; - static float velocityWindupFactor[3] = { 1.0f, 1.0f, 1.0f }; - - const float velocityFactor = getdT() * 1000.0f; float tpaFactor = PIDweight[0] / 100.0f; // tpa is now float @@ -180,13 +177,31 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc // ----------PID controller---------- for (axis = 0; axis < 3; axis++) { + static uint8_t configP[3], configI[3], configD[3]; + + // Prevent unnecessary computing and check for changed PIDs. No need for individual checks. Only pids is fine for now // Prepare all parameters for PID controller - float Kp = PTERM_SCALE * pidProfile->P8[axis]; - float Ki = ITERM_SCALE * pidProfile->I8[axis]; - float Kd = DTERM_SCALE * pidProfile->D8[axis]; - float b = pidProfile->ptermSetpointWeight / 100.0f; - float c = pidProfile->dtermSetpointWeight / 100.0f; - float velocityMax = (axis == YAW) ? (float)pidProfile->pidMaxVelocityYaw * velocityFactor : (float)pidProfile->pidMaxVelocity * velocityFactor; + if ((pidProfile->P8[axis] != configP[axis]) || (pidProfile->I8[axis] != configI[axis]) || (pidProfile->D8[axis] != configD[axis])) { + Kp[axis] = PTERM_SCALE * pidProfile->P8[axis]; + Ki[axis] = ITERM_SCALE * pidProfile->I8[axis]; + Kd[axis] = DTERM_SCALE * pidProfile->D8[axis]; + b[axis] = pidProfile->ptermSetpointWeight / 100.0f; + c[axis] = pidProfile->dtermSetpointWeight / 100.0f; + yawMaxVelocity = pidProfile->pidMaxVelocityYaw * 1000 * getdT(); + + configP[axis] = pidProfile->P8[axis]; + configI[axis] = pidProfile->I8[axis]; + configD[axis] = pidProfile->D8[axis]; + } + + // Limit abrupt yaw inputs + if (axis == YAW && pidProfile->pidMaxVelocityYaw) { + float yawCurrentVelocity = setpointRate[axis] - yawPreviousRate; + if (ABS(yawCurrentVelocity) > yawMaxVelocity) { + setpointRate[axis] = (yawCurrentVelocity > 0) ? yawPreviousRate + yawMaxVelocity : yawPreviousRate - yawMaxVelocity; + } + yawPreviousRate = setpointRate[axis]; + } // Yaw control is GYRO based, direct sticks control is applied to rate PID if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && axis != YAW) { @@ -214,9 +229,8 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc // ---------- 2-DOF PID controller with optional filter on derivative term. b = 1 and only c can be tuned (amount derivative on measurement or error). ---------- // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes // ----- calculate error / angle rates ---------- - errorRate = setpointRate[axis] - PVRate; // r - y - rP = b * setpointRate[axis] - PVRate; // br - y - rD = c * setpointRate[axis] - PVRate; // cr - y + errorRate = setpointRate[axis] - PVRate; // r - y + rP = b[axis] * setpointRate[axis] - PVRate; // br - y // Slowly restore original setpoint with more stick input float diffRate = errorRate - rP; @@ -251,7 +265,7 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc } // -----calculate P component - PTerm = Kp * rP * dynReduction; + PTerm = Kp[axis] * rP * dynReduction; // -----calculate I component. // Reduce strong Iterm accumulation during higher stick inputs @@ -260,9 +274,9 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc // Handle All windup Scenarios // limit maximum integrator value to prevent WindUp - float itermScaler = setpointRateScaler * kiThrottleGain * velocityWindupFactor[axis]; + float itermScaler = setpointRateScaler * kiThrottleGain; - errorGyroIf[axis] = constrainf(errorGyroIf[axis] + Ki * errorRate * getdT() * itermScaler, -250.0f, 250.0f); + errorGyroIf[axis] = constrainf(errorGyroIf[axis] + Ki[axis] * errorRate * getdT() * itermScaler, -250.0f, 250.0f); // I coefficient (I8) moved before integration to make limiting independent from PID settings ITerm = errorGyroIf[axis]; @@ -275,13 +289,14 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc DTerm = 0.0f; // needed for blackbox } else { + rD = c[axis] * setpointRate[axis] - PVRate; // cr - y delta = rD - lastRateError[axis]; lastRateError[axis] = rD; // Divide delta by targetLooptime to get differential (ie dr/dt) delta *= (1.0f / getdT()); - if (debugMode == DEBUG_DTERM_FILTER) debug[axis] = Kd * delta * dynReduction; + if (debugMode == DEBUG_DTERM_FILTER) debug[axis] = Kd[axis] * delta * dynReduction; // Filter delta if (dtermNotchInitialised) delta = biquadFilterApply(&dtermFilterNotch[axis], delta); @@ -294,30 +309,15 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc } } - DTerm = constrainf(Kd * delta * dynReduction, -300.0f, 300.0f); + DTerm = Kd[axis] * delta * dynReduction; // -----calculate total PID output - axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -1000, 1000); + axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -900, 900); } // Disable PID control at zero throttle if (!pidStabilisationEnabled) axisPID[axis] = 0; - // Velocity limit only active below 1000 - if (velocityMax < 1000) { - int16_t currentVelocity = axisPID[axis] - axisPIDState[axis]; - if (debugMode == DEBUG_VELOCITY) debug[axis] = currentVelocity; - if (ABS(currentVelocity) > velocityMax) { - axisPID[axis] = (currentVelocity > 0) ? axisPIDState[axis] + velocityMax : axisPIDState[axis] - velocityMax; - velocityWindupFactor[axis] = ABS(currentVelocity) / velocityMax; - } - else { - velocityWindupFactor[axis] = 1.0f; - } - - axisPIDState[axis] = axisPID[axis]; - } - #ifdef GTUNE if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { calculate_Gtune(axis); diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 9d158c3838..4f3623c319 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -100,8 +100,7 @@ typedef struct pidProfile_s { uint8_t itermThrottleGain; // Throttle coupling to iterm. Quick throttle changes will bump iterm uint8_t ptermSetpointWeight; // Setpoint weight for Pterm (lower means more PV tracking) uint8_t dtermSetpointWeight; // Setpoint weight for Dterm (0= measurement, 1= full error, 1 > agressive derivative) - uint16_t pidMaxVelocity; // velocity limiter for pid controller (per ms) - uint16_t pidMaxVelocityYaw; // velocity limiter for pid controller (per ms) yaw + uint16_t pidMaxVelocityYaw; // velocity limiter for pid controller deg/sec/ms #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 9d854f9321..860874af7c 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -832,9 +832,8 @@ const clivalue_t valueTable[] = { { "zero_cross_allowance", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.zeroCrossAllowanceCount, .config.minmax = {0, 50 } }, { "iterm_throttle_gain", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermThrottleGain, .config.minmax = {0, 200 } }, { "pterm_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.ptermSetpointWeight, .config.minmax = {30, 100 } }, - { "dterm_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dtermSetpointWeight, .config.minmax = {0, 200 } }, - { "pid_max_velocity", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pidMaxVelocity, .config.minmax = {0, 2000 } }, - { "pid_max_velocity_yaw", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pidMaxVelocityYaw, .config.minmax = {0, 2000 } }, + { "dterm_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dtermSetpointWeight, .config.minmax = {0, 300 } }, + { "max_yaw_acceleration", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pidMaxVelocityYaw, .config.minmax = {0, 1000 } }, { "iterm_ignore_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermIgnoreRate, .config.minmax = {15, 1000 } }, { "yaw_iterm_ignore_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawItermIgnoreRate, .config.minmax = {15, 1000 } }, diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 0098db8133..b5d170cfbc 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -1268,8 +1268,8 @@ static bool processOutCommand(uint8_t cmdMSP) serialize8(currentProfile->pidProfile.toleranceBand); serialize8(currentProfile->pidProfile.toleranceBandReduction); serialize8(currentProfile->pidProfile.itermThrottleGain); - serialize16(currentProfile->pidProfile.pidMaxVelocity); serialize16(currentProfile->pidProfile.pidMaxVelocityYaw); + serialize16(1000); // Reserved for future break; case MSP_SENSOR_CONFIG: headSerialReply(3); @@ -1858,8 +1858,8 @@ static bool processInCommand(void) currentProfile->pidProfile.toleranceBand = read8(); currentProfile->pidProfile.toleranceBandReduction = read8(); currentProfile->pidProfile.itermThrottleGain = read8(); - currentProfile->pidProfile.pidMaxVelocity = read16(); currentProfile->pidProfile.pidMaxVelocityYaw = read16(); + read16(); // reserved for future purposes break; case MSP_SET_SENSOR_CONFIG: masterConfig.acc_hardware = read8(); diff --git a/src/main/mw.c b/src/main/mw.c index 073e795017..c3ebdf8299 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -240,7 +240,6 @@ void processRcCommand(void) debug[3] = rxRefreshRate; } - isRXDataNew = false; for (int channel=0; channel < 4; channel++) { deltaRC[channel] = rcCommand[channel] - (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor); lastCommand[channel] = rcCommand[channel]; @@ -264,9 +263,11 @@ void processRcCommand(void) } if (readyToCalculateRate || isRXDataNew) { - isRXDataNew = false; + // Don't smooth yaw axis + int axisToCalculate = (isRXDataNew) ? 3 : 2; + for (int axis = 0; axis < axisToCalculate; axis++) setpointRate[axis] = calculateSetpointRate(axis, rcCommand[axis]); - for (int axis = 0; axis < 3; axis++) setpointRate[axis] = calculateSetpointRate(axis, rcCommand[axis]); + isRXDataNew = false; // Scaling of AngleRate to camera angle (Mixing Roll and Yaw) if (masterConfig.rxConfig.fpvCamAngleDegrees && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && !FLIGHT_MODE(HEADFREE_MODE))