static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) { UNUSED(rxConfig); int axis; int32_t PTerm, ITerm, DTerm, delta; static int32_t lastError[3] = { 0, 0, 0 }; static int32_t previousErrorGyroI[3] = { 0, 0, 0 }; int32_t AngleRateTmp, RateError, gyroRate; int8_t horizonLevelStrength = 100; if (!deltaStateIsSet && pidProfile->dterm_lpf_hz) { for (axis = 0; axis < 3; axis++) BiQuadNewLpf(pidProfile->dterm_lpf_hz, &deltaBiQuadState[axis], 0); deltaStateIsSet = true; } if (FLIGHT_MODE(HORIZON_MODE)) { // Figure out the raw stick positions const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc)); const int32_t stickPosEle = ABS(getRcStickDeflection(FD_PITCH, rxConfig->midrc)); const int32_t mostDeflectedPos = MAX(stickPosAil, stickPosEle); // Progressively turn off the horizon self level strength as the stick is banged over horizonLevelStrength = (500 - mostDeflectedPos) / 5; // 100 at centre stick, 0 = max stick deflection // Using Level D as a Sensitivity for Horizon. 0 more level to 255 more rate. Default value of 100 seems to work fine. // For more rate mode increase D and slower flips and rolls will be possible horizonLevelStrength = constrain((10 * (horizonLevelStrength - 100) * (10 * pidProfile->D8[PIDLEVEL] / 80) / 100) + 100, 0, 100); } // ----------PID controller---------- for (axis = 0; axis < 3; axis++) { uint8_t rate = 10; // -----Get the desired angle rate depending on flight mode if (axis == YAW || !pidProfile->airModeInsaneAcrobilityFactor || !IS_RC_MODE_ACTIVE(BOXAIRMODE)) { rate = controlRateConfig->rates[axis]; } // -----Get the desired angle rate depending on flight mode if (axis == FD_YAW) { // YAW is always gyro-controlled (MAG correction is applied to rcCommand) AngleRateTmp = ((int32_t)(rate + 27) * rcCommand[YAW]) >> 5; } else {
static float calcHorizonRateMagnitude(const pidProfile_t *pidProfile, const rxConfig_t *rxConfig) { // Figure out the raw stick positions const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc)); const int32_t stickPosEle = ABS(getRcStickDeflection(FD_PITCH, rxConfig->midrc)); const int32_t mostDeflectedPos = MAX(stickPosAil, stickPosEle); const float modeTransitionStickPos = constrain(pidProfile->D8[PIDLEVEL], 0, 100) / 100.0f; float horizonRateMagnitude = mostDeflectedPos / 500.0f; if (horizonRateMagnitude <= modeTransitionStickPos) { horizonRateMagnitude *= STICK_DEFLECTION_AT_MODE_TRANSITION / modeTransitionStickPos; } else { horizonRateMagnitude = (horizonRateMagnitude - modeTransitionStickPos) * (1.0f - STICK_DEFLECTION_AT_MODE_TRANSITION) / (1.0f - modeTransitionStickPos) + STICK_DEFLECTION_AT_MODE_TRANSITION; } return horizonRateMagnitude; }
// Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage. // Based on 2DOF reference design (matlab) void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig) { 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], rollPitchMaxVelocity, yawMaxVelocity, previousSetpoint[3]; float delta; int axis; float horizonLevelStrength = 1; float tpaFactor = PIDweight[0] / 100.0f; // tpa is now float initFilters(pidProfile); if (FLIGHT_MODE(HORIZON_MODE)) { // Figure out the raw stick positions const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc)); const int32_t stickPosEle = ABS(getRcStickDeflection(FD_PITCH, rxConfig->midrc)); const int32_t mostDeflectedPos = MAX(stickPosAil, stickPosEle); // Progressively turn off the horizon self level strength as the stick is banged over horizonLevelStrength = (float)(500 - mostDeflectedPos) / 500; // 1 at centre stick, 0 = max stick deflection if(pidProfile->D8[PIDLEVEL] == 0){ horizonLevelStrength = 0; } else { horizonLevelStrength = constrainf(((horizonLevelStrength - 1) * (100 / pidProfile->D8[PIDLEVEL])) + 1, 0, 1); } } // Yet Highly experimental and under test and development // Throttle coupled to Igain like inverted TPA // 50hz calculation (should cover all rx protocols) static float kiThrottleGain = 1.0f; if (pidProfile->itermThrottleGain) { const uint16_t maxLoopCount = 20000 / targetPidLooptime; const float throttleItermGain = (float)pidProfile->itermThrottleGain * 0.001f; static int16_t previousThrottle; static uint16_t loopIncrement; if (loopIncrement >= maxLoopCount) { kiThrottleGain = 1.0f + constrainf((float)(ABS(rcCommand[THROTTLE] - previousThrottle)) * throttleItermGain, 0.0f, 5.0f); // Limit to factor 5 previousThrottle = rcCommand[THROTTLE]; loopIncrement = 0; } else { loopIncrement++; } } // ----------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 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->yawRateAccelLimit * 1000 * getdT(); rollPitchMaxVelocity = pidProfile->rateAccelLimit * 1000 * getdT(); configP[axis] = pidProfile->P8[axis]; configI[axis] = pidProfile->I8[axis]; configD[axis] = pidProfile->D8[axis]; } // Limit abrupt yaw inputs / stops float maxVelocity = (axis == YAW) ? yawMaxVelocity : rollPitchMaxVelocity; if (maxVelocity) { float currentVelocity = setpointRate[axis] - previousSetpoint[axis]; if (ABS(currentVelocity) > maxVelocity) { setpointRate[axis] = (currentVelocity > 0) ? previousSetpoint[axis] + maxVelocity : previousSetpoint[axis] - maxVelocity; } previousSetpoint[axis] = 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) { // calculate error angle and limit the angle to the max inclination #ifdef GPS const float errorAngle = (constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here #else const float errorAngle = (constrain(2 * rcCommand[axis], -((int) max_angle_inclination), +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here #endif if (FLIGHT_MODE(ANGLE_MODE)) { // ANGLE mode - control is angle based, so control loop is needed setpointRate[axis] = errorAngle * pidProfile->P8[PIDLEVEL] / 10.0f; } else { // HORIZON mode - direct sticks control is applied to rate PID // mix up angle error to desired AngleRate to add a little auto-level feel setpointRate[axis] = setpointRate[axis] + (errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 10.0f); } } PVRate = gyroADCf[axis] / 16.4f; // Process variable from gyro output in deg/sec // --------low-level gyro-based PID based on 2DOF PID controller. ---------- // ---------- 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[axis] * setpointRate[axis] - PVRate; // br - y // Slowly restore original setpoint with more stick input float diffRate = errorRate - rP; rP += diffRate * rcInput[axis]; // Reduce Hunting effect and jittering near setpoint. Limit multiple zero crossing within deadband and lower PID affect during low error amount float dynReduction = tpaFactor; if (pidProfile->toleranceBand) { const float minReduction = (float)pidProfile->toleranceBandReduction / 100.0f; static uint8_t zeroCrossCount[3]; static uint8_t currentErrorPolarity[3]; if (ABS(errorRate) < pidProfile->toleranceBand) { if (zeroCrossCount[axis]) { if (currentErrorPolarity[axis] == POSITIVE_ERROR) { if (errorRate < 0 ) { zeroCrossCount[axis]--; currentErrorPolarity[axis] = NEGATIVE_ERROR; } } else { if (errorRate > 0 ) { zeroCrossCount[axis]--; currentErrorPolarity[axis] = POSITIVE_ERROR; } } } else { dynReduction *= constrainf(ABS(errorRate) / pidProfile->toleranceBand, minReduction, 1.0f); } } else { zeroCrossCount[axis] = pidProfile->zeroCrossAllowanceCount; currentErrorPolarity[axis] = (errorRate > 0) ? POSITIVE_ERROR : NEGATIVE_ERROR; } } // -----calculate P component PTerm = Kp[axis] * rP * dynReduction; // -----calculate I component. // Reduce strong Iterm accumulation during higher stick inputs float accumulationThreshold = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate; float setpointRateScaler = constrainf(1.0f - (1.5f * (ABS(setpointRate[axis]) / accumulationThreshold)), 0.0f, 1.0f); // Handle All windup Scenarios // limit maximum integrator value to prevent WindUp float itermScaler = setpointRateScaler * kiThrottleGain; 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]; //-----calculate D-term (Yaw D not yet supported) if (axis == YAW) { if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT()); axisPID[axis] = lrintf(PTerm + ITerm); 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[axis] * delta * dynReduction; // Filter delta if (dtermNotchInitialised) delta = biquadFilterApply(&dtermFilterNotch[axis], delta); if (pidProfile->dterm_lpf_hz) { if (dtermBiquadLpfInitialised) { delta = biquadFilterApply(&dtermFilterLpf[axis], delta); } else { delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT()); } } DTerm = Kd[axis] * delta * dynReduction; // -----calculate total PID output axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -900, 900); } // Disable PID control at zero throttle if (!pidStabilisationEnabled) axisPID[axis] = 0; #ifdef GTUNE if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { calculate_Gtune(axis); } #endif #ifdef BLACKBOX axisPID_P[axis] = PTerm; axisPID_I[axis] = ITerm; axisPID_D[axis] = DTerm; #endif } }
void pidLuxFloat(const pidProfile_t *pidProfile, const controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig) { pidFilterIsSetCheck(pidProfile); float horizonLevelStrength; if (FLIGHT_MODE(HORIZON_MODE)) { // Figure out the most deflected stick position const int32_t stickPosAil = ABS(getRcStickDeflection(ROLL, rxConfig->midrc)); const int32_t stickPosEle = ABS(getRcStickDeflection(PITCH, rxConfig->midrc)); const int32_t mostDeflectedPos = MAX(stickPosAil, stickPosEle); // Progressively turn off the horizon self level strength as the stick is banged over horizonLevelStrength = (float)(500 - mostDeflectedPos) / 500; // 1 at centre stick, 0 = max stick deflection if(pidProfile->D8[PIDLEVEL] == 0){ horizonLevelStrength = 0; } else { horizonLevelStrength = constrainf(((horizonLevelStrength - 1) * (100 / pidProfile->D8[PIDLEVEL])) + 1, 0, 1); } } // ----------PID controller---------- for (int axis = 0; axis < 3; axis++) { const uint8_t rate = controlRateConfig->rates[axis]; // -----Get the desired angle rate depending on flight mode float angleRate; if (axis == FD_YAW) { // YAW is always gyro-controlled (MAG correction is applied to rcCommand) 100dps to 1100dps max yaw rate angleRate = (float)((rate + 27) * rcCommand[YAW]) / 32.0f; } else { // control is GYRO based for ACRO and HORIZON - direct sticks control is applied to rate PID angleRate = (float)((rate + 27) * rcCommand[axis]) / 16.0f; // 200dps to 1200dps max roll/pitch rate if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { // calculate error angle and limit the angle to the max inclination // multiplication of rcCommand corresponds to changing the sticks scaling here #ifdef GPS const float errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int)max_angle_inclination), max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]; #else const float errorAngle = constrain(2 * rcCommand[axis], -((int)max_angle_inclination), max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]; #endif if (FLIGHT_MODE(ANGLE_MODE)) { // ANGLE mode angleRate = errorAngle * pidProfile->P8[PIDLEVEL] / 16.0f; } else { // HORIZON mode // mix in errorAngle to desired angleRate to add a little auto-level feel. // horizonLevelStrength has been scaled to the stick input angleRate += errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 16.0f; } } } // --------low-level gyro-based PID. ---------- const float gyroRate = luxGyroScale * gyroADC[axis] * gyro.scale; axisPID[axis] = pidLuxFloatCore(axis, pidProfile, gyroRate, angleRate); //axisPID[axis] = constrain(axisPID[axis], -PID_LUX_FLOAT_MAX_PID, PID_LUX_FLOAT_MAX_PID); #ifdef GTUNE if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { calculate_Gtune(axis); } #endif } }
static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) { float RateError, errorAngle, AngleRate, gyroRate; float ITerm,PTerm,DTerm; int32_t stickPosAil, stickPosEle, mostDeflectedPos; static float lastError[3]; static float delta1[3], delta2[3]; float delta, deltaSum; float dT; int axis; float horizonLevelStrength = 1; dT = (float)cycleTime * 0.000001f; if (FLIGHT_MODE(HORIZON_MODE)) { // Figure out the raw stick positions stickPosAil = getRcStickDeflection(FD_ROLL, rxConfig->midrc); stickPosEle = getRcStickDeflection(FD_PITCH, rxConfig->midrc); if(ABS(stickPosAil) > ABS(stickPosEle)){ mostDeflectedPos = ABS(stickPosAil); } else { mostDeflectedPos = ABS(stickPosEle); } // Progressively turn off the horizon self level strength as the stick is banged over horizonLevelStrength = (float)(500 - mostDeflectedPos) / 500; // 1 at centre stick, 0 = max stick deflection if(pidProfile->H_sensitivity == 0){ horizonLevelStrength = 0; } else { horizonLevelStrength = constrainf(((horizonLevelStrength - 1) * (100 / pidProfile->H_sensitivity)) + 1, 0, 1); } } // ----------PID controller---------- for (axis = 0; axis < 3; axis++) { // -----Get the desired angle rate depending on flight mode uint8_t rate = controlRateConfig->rates[axis]; if (axis == FD_YAW) { // YAW is always gyro-controlled (MAG correction is applied to rcCommand) 100dps to 1100dps max yaw rate AngleRate = (float)((rate + 10) * rcCommand[YAW]) / 50.0f; } else { // calculate error and limit the angle to the max inclination #ifdef GPS errorAngle = (constrain(rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here #else errorAngle = (constrain(rcCommand[axis], -((int) max_angle_inclination), +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here #endif #ifdef AUTOTUNE if (shouldAutotune()) { errorAngle = autotune(rcAliasToAngleIndexMap[axis], &inclination, errorAngle); } #endif if (FLIGHT_MODE(ANGLE_MODE)) { // it's the ANGLE mode - control is angle based, so control loop is needed AngleRate = errorAngle * pidProfile->A_level; } else { //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID AngleRate = (float)((rate + 20) * rcCommand[axis]) / 50.0f; // 200dps to 1200dps max roll/pitch rate if (FLIGHT_MODE(HORIZON_MODE)) { // mix up angle error to desired AngleRate to add a little auto-level feel AngleRate += errorAngle * pidProfile->H_level * horizonLevelStrength; } } } gyroRate = gyroADC[axis] * gyro.scale; // gyro output scaled to dps // --------low-level gyro-based PID. ---------- // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes // -----calculate scaled error.AngleRates // multiplication of rcCommand corresponds to changing the sticks scaling here RateError = AngleRate - gyroRate; // -----calculate P component PTerm = RateError * pidProfile->P_f[axis] * PIDweight[axis] / 100; // Pterm low pass if (pidProfile->pterm_cut_hz) { PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz); } // -----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 = 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. delta *= (1.0f / dT); // add moving average here to reduce noise deltaSum = delta1[axis] + delta2[axis] + delta; delta2[axis] = delta1[axis]; delta1[axis] = delta; // Dterm low pass if (pidProfile->dterm_cut_hz) { deltaSum = filterApplyPt1(deltaSum, &DTermState[axis], pidProfile->dterm_cut_hz); } 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); #ifdef BLACKBOX axisPID_P[axis] = PTerm; axisPID_I[axis] = ITerm; axisPID_D[axis] = DTerm; #endif } }
static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) { float RateError, AngleRate, gyroRate; float ITerm,PTerm,DTerm; static float lastError[3]; float delta; int axis; float horizonLevelStrength = 1; static float previousErrorGyroIf[3] = { 0.0f, 0.0f, 0.0f }; if (!deltaStateIsSet && pidProfile->dterm_lpf_hz) { for (axis = 0; axis < 3; axis++) BiQuadNewLpf(pidProfile->dterm_lpf_hz, &deltaBiQuadState[axis], 0); deltaStateIsSet = true; } if (FLIGHT_MODE(HORIZON_MODE)) { // Figure out the raw stick positions const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc)); const int32_t stickPosEle = ABS(getRcStickDeflection(FD_PITCH, rxConfig->midrc)); const int32_t mostDeflectedPos = MAX(stickPosAil, stickPosEle); // Progressively turn off the horizon self level strength as the stick is banged over horizonLevelStrength = (float)(500 - mostDeflectedPos) / 500; // 1 at centre stick, 0 = max stick deflection if(pidProfile->H_sensitivity == 0){ horizonLevelStrength = 0; } else { horizonLevelStrength = constrainf(((horizonLevelStrength - 1) * (100 / pidProfile->H_sensitivity)) + 1, 0, 1); } } // ----------PID controller---------- for (axis = 0; axis < 3; axis++) { uint8_t rate = 10; // -----Get the desired angle rate depending on flight mode if (axis == YAW || !pidProfile->airModeInsaneAcrobilityFactor || !IS_RC_MODE_ACTIVE(BOXAIRMODE)) { rate = controlRateConfig->rates[axis]; } if (axis == FD_YAW) { // YAW is always gyro-controlled (MAG correction is applied to rcCommand) 100dps to 1100dps max yaw rate AngleRate = (float)((rate + 10) * rcCommand[YAW]) / 50.0f; } else { // ACRO mode, control is GYRO based, direct sticks control is applied to rate PID AngleRate = (float)((rate + 20) * rcCommand[axis]) / 50.0f; // 200dps to 1200dps max roll/pitch rate if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { // calculate error angle and limit the angle to the max inclination #ifdef GPS const float errorAngle = (constrain(rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here #else const float errorAngle = (constrain(rcCommand[axis], -((int) max_angle_inclination), +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here #endif if (FLIGHT_MODE(ANGLE_MODE)) { // ANGLE mode - control is angle based, so control loop is needed AngleRate = errorAngle * pidProfile->A_level; } else { // HORIZON mode - direct sticks control is applied to rate PID // mix up angle error to desired AngleRate to add a little auto-level feel AngleRate += errorAngle * pidProfile->H_level * horizonLevelStrength; } } } gyroRate = gyroADC[axis] * gyro.scale; // gyro output scaled to dps // --------low-level gyro-based PID. ---------- // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes // -----calculate scaled error.AngleRates // multiplication of rcCommand corresponds to changing the sticks scaling here RateError = AngleRate - gyroRate; // -----calculate P component 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 (IS_RC_MODE_ACTIVE(BOXAIRMODE)) { airModePlus(&airModePlusAxisState[axis], axis, pidProfile); errorGyroIf[axis] *= airModePlusAxisState[axis].iTermScaler; } if (allowITermShrinkOnly || motorLimitReached) { if (ABS(errorGyroIf[axis]) < ABS(previousErrorGyroIf[axis])) { previousErrorGyroIf[axis] = errorGyroIf[axis]; } else { errorGyroIf[axis] = constrain(errorGyroIf[axis], -ABS(previousErrorGyroIf[axis]), ABS(previousErrorGyroIf[axis])); } } else { previousErrorGyroIf[axis] = errorGyroIf[axis]; } // 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 = RateError - lastError[axis]; lastError[axis] = RateError; if (deltaStateIsSet) { delta = applyBiQuadFilter(delta, &deltaBiQuadState[axis]); } // 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. delta *= (1.0f / dT); DTerm = constrainf(delta * pidProfile->D_f[axis] * PIDweight[axis] / 100, -300.0f, 300.0f); // -----calculate total PID output axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -1000, 1000); if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) { axisPID[axis] = lrintf(airModePlusAxisState[axis].factor + airModePlusAxisState[axis].wowFactor * axisPID[axis]); } #ifdef GTUNE if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { calculate_Gtune(axis); } #endif #ifdef BLACKBOX axisPID_P[axis] = PTerm; axisPID_I[axis] = ITerm; axisPID_D[axis] = DTerm; #endif } }