STATIC_UNIT_TESTED int16_t pidLuxFloatCore(int axis, const pidProfile_t *pidProfile, float gyroRate, float angleRate) { static float lastRateForDelta[3]; SET_PID_LUX_FLOAT_CORE_LOCALS(axis); const float rateError = angleRate - gyroRate; // -----calculate P component float PTerm = luxPTermScale * rateError * pidProfile->P8[axis] * PIDweight[axis] / 100; // Constrain YAW by yaw_p_limit value if not servo driven, in that case servolimits apply if (axis == YAW) { if (pidProfile->yaw_lpf_hz) { PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT()); } if (pidProfile->yaw_p_limit && motorCount >= 4) { PTerm = constrainf(PTerm, -pidProfile->yaw_p_limit, pidProfile->yaw_p_limit); } } // -----calculate I component float ITerm = lastITermf[axis] + luxITermScale * rateError * getdT() * pidProfile->I8[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 = constrainf(ITerm, -PID_MAX_I, PID_MAX_I); // Anti windup protection if (rcModeIsActive(BOXAIRMODE)) { if (STATE(ANTI_WINDUP) || motorLimitReached) { ITerm = constrainf(ITerm, -ITermLimitf[axis], ITermLimitf[axis]); } else { ITermLimitf[axis] = ABS(ITerm); } } lastITermf[axis] = ITerm; // -----calculate D component float DTerm; if (pidProfile->D8[axis] == 0) { // optimisation for when D8 is zero, often used by YAW axis DTerm = 0; } else { float delta; if (pidProfile->deltaMethod == PID_DELTA_FROM_MEASUREMENT) { delta = -(gyroRate - lastRateForDelta[axis]); lastRateForDelta[axis] = gyroRate; } else { delta = rateError - lastRateForDelta[axis]; lastRateForDelta[axis] = rateError; } // Divide delta by targetLooptime to get differential (ie dr/dt) delta /= getdT(); // Filter delta if (pidProfile->dterm_notch_hz) { delta = biquadFilterApply(&dtermFilterNotch[axis], delta); } if (pidProfile->dterm_lpf_hz) { if (pidProfile->dterm_filter_type == FILTER_BIQUAD) { delta = biquadFilterApply(&dtermFilterLpf[axis], delta); } else { // DTerm delta low pass filter delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT()); } } DTerm = luxDTermScale * delta * pidProfile->D8[axis] * PIDweight[axis] / 100; DTerm = constrainf(DTerm, -PID_MAX_D, PID_MAX_D); } #ifdef BLACKBOX axisPID_P[axis] = PTerm; axisPID_I[axis] = ITerm; axisPID_D[axis] = DTerm; #endif GET_PID_LUX_FLOAT_CORE_LOCALS(axis); // -----calculate total PID output return lrintf(PTerm + ITerm + DTerm); }
STATIC_UNIT_TESTED int16_t pidLuxFloatCore(int axis, const pidProfile_t *pidProfile, float gyroRate, float angleRate) { static float lastRateForDelta[3]; static float deltaState[3][DTERM_AVERAGE_COUNT]; SET_PID_LUX_FLOAT_CORE_LOCALS(axis); const float rateError = angleRate - gyroRate; // -----calculate P component float PTerm = luxPTermScale * rateError * pidProfile->P8[axis] * PIDweight[axis] / 100; // Constrain YAW by yaw_p_limit value if not servo driven, in that case servolimits apply if (axis == YAW && pidProfile->yaw_p_limit && motorCount >= 4) { PTerm = constrainf(PTerm, -pidProfile->yaw_p_limit, pidProfile->yaw_p_limit); } // -----calculate I component float ITerm = lastITermf[axis] + luxITermScale * rateError * dT * pidProfile->I8[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 = constrainf(ITerm, -PID_MAX_I, PID_MAX_I); // Anti windup protection if (rcModeIsActive(BOXAIRMODE)) { if (STATE(ANTI_WINDUP) || motorLimitReached) { ITerm = constrainf(ITerm, -ITermLimitf[axis], ITermLimitf[axis]); } else { ITermLimitf[axis] = ABS(ITerm); } } lastITermf[axis] = ITerm; // -----calculate D component float DTerm; if (pidProfile->D8[axis] == 0) { // optimisation for when D8 is zero, often used by YAW axis DTerm = 0; } else { // delta calculated from measurement float delta = -(gyroRate - lastRateForDelta[axis]); lastRateForDelta[axis] = gyroRate; // Divide delta by dT to get differential (ie dr/dt) delta *= (1.0f / dT); if (pidProfile->dterm_cut_hz) { // DTerm delta low pass filter delta = applyBiQuadFilter(delta, &deltaFilterState[axis]); } else { // When DTerm low pass filter disabled apply moving average to reduce noise delta = filterApplyAveragef(delta, DTERM_AVERAGE_COUNT, deltaState[axis]); } DTerm = luxDTermScale * delta * pidProfile->D8[axis] * PIDweight[axis] / 100; DTerm = constrainf(DTerm, -PID_MAX_D, PID_MAX_D); } #ifdef BLACKBOX axisPID_P[axis] = PTerm; axisPID_I[axis] = ITerm; axisPID_D[axis] = DTerm; #endif GET_PID_LUX_FLOAT_CORE_LOCALS(axis); // -----calculate total PID output return lrintf(PTerm + ITerm + DTerm); }