void gyroUpdate(void) { int16_t gyroADCRaw[XYZ_AXIS_COUNT]; // range: +/- 8192; +/- 2000 deg/sec if (!gyro.read(gyroADCRaw)) { return; } for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { gyroADC[axis] = gyroADCRaw[axis]; } alignSensors(gyroADC, gyroADC, gyroAlign); if (!isGyroCalibrationComplete()) { performGyroCalibration(gyroConfig->gyroMovementCalibrationThreshold); } applyGyroZero(); if (gyroSoftLpfHz) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { if (debugMode == DEBUG_GYRO) debug[axis] = gyroADC[axis]; if (gyroSoftLpfType == FILTER_BIQUAD) gyroADCf[axis] = biquadFilterApply(&gyroFilterLPF[axis], (float) gyroADC[axis]); else if (gyroSoftLpfType == FILTER_PT1) gyroADCf[axis] = pt1FilterApply4(&gyroFilterPt1[axis], (float) gyroADC[axis], gyroSoftLpfHz, gyroDt); else gyroADCf[axis] = firFilterDenoiseUpdate(&gyroDenoiseState[axis], (float) gyroADC[axis]); if (debugMode == DEBUG_NOTCH) debug[axis] = lrintf(gyroADCf[axis]); if (gyroSoftNotchHz1) gyroADCf[axis] = biquadFilterApply(&gyroFilterNotch_1[axis], gyroADCf[axis]); if (gyroSoftNotchHz2) gyroADCf[axis] = biquadFilterApply(&gyroFilterNotch_2[axis], gyroADCf[axis]); gyroADC[axis] = lrintf(gyroADCf[axis]); } } else { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) gyroADCf[axis] = gyroADC[axis]; } }
void updateAccelerationReadings(void) { if (!acc.read(accADCRaw)) { return; } for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) accADC[axis] = accADCRaw[axis]; if (accLpfCutHz) { if (!accFilterInitialised) { if (targetLooptime) { /* Initialisation needs to happen once sample rate is known */ for (int axis = 0; axis < 3; axis++) { biquadFilterInit(&accFilterState[axis], accLpfCutHz, 0); } accFilterInitialised = true; } } if (accFilterInitialised) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { accADC[axis] = lrintf(biquadFilterApply(&accFilterState[axis], (float) accADC[axis])); } } } if (!isAccelerationCalibrationComplete()) { performAcclerationCalibration(); } applyAccelerationZero(accZero, accGain); alignSensors(accADC, accADC, accAlign); }
void filterServos(void) { static int16_t servoIdx; static bool servoFilterIsSet; static biquadFilter_t servoFilter[MAX_SUPPORTED_SERVOS]; #if defined(MIXER_DEBUG) uint32_t startTime = micros(); #endif if (servoMixerConfig->servo_lowpass_enable) { for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) { if (!servoFilterIsSet) { biquadFilterInitLPF(&servoFilter[servoIdx], servoMixerConfig->servo_lowpass_freq, targetPidLooptime); servoFilterIsSet = true; } servo[servoIdx] = lrintf(biquadFilterApply(&servoFilter[servoIdx], (float)servo[servoIdx])); // Sanity check servo[servoIdx] = constrain(servo[servoIdx], servoConf[servoIdx].min, servoConf[servoIdx].max); } } #if defined(MIXER_DEBUG) debug[0] = (int16_t)(micros() - startTime); #endif }
void voltageMeterADCRefresh(void) { for (uint8_t i = 0; i < MAX_VOLTAGE_SENSOR_ADC && i < ARRAYLEN(voltageMeterAdcChannelMap); i++) { voltageMeterADCState_t *state = &voltageMeterADCStates[i]; #ifdef USE_ADC // store the battery voltage with some other recent battery voltage readings const voltageSensorADCConfig_t *config = voltageSensorADCConfig(i); uint8_t channel = voltageMeterAdcChannelMap[i]; uint16_t rawSample = adcGetChannel(channel); uint16_t filteredSample = biquadFilterApply(&state->filter, rawSample); // always calculate the latest voltage, see getLatestVoltage() which does the calculation on demand. state->voltageFiltered = voltageAdcToVoltage(filteredSample, config); state->voltageUnfiltered = voltageAdcToVoltage(rawSample, config); #else UNUSED(voltageAdcToVoltage); state->voltageFiltered = 0; state->voltageUnfiltered = 0; #endif } }
void gyroUpdate(void) { // range: +/- 8192; +/- 2000 deg/sec if (!gyro.dev.read(&gyro.dev)) { return; } // Prepare a copy of int32_t gyroADC for mangling to prevent overflow gyro.gyroADC[X] = gyro.dev.gyroADCRaw[X]; gyro.gyroADC[Y] = gyro.dev.gyroADCRaw[Y]; gyro.gyroADC[Z] = gyro.dev.gyroADCRaw[Z]; if (gyroConfig->gyro_soft_lpf_hz) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { gyro.gyroADC[axis] = lrintf(biquadFilterApply(&gyroFilterLPF[axis], (float)gyro.gyroADC[axis])); } } if (!isGyroCalibrationComplete()) { performAcclerationCalibration(gyroConfig->gyroMovementCalibrationThreshold); } gyro.gyroADC[X] -= gyroZero[X]; gyro.gyroADC[Y] -= gyroZero[Y]; gyro.gyroADC[Z] -= gyroZero[Z]; alignSensors(gyro.gyroADC, gyro.dev.gyroAlign); }
void gyroUpdate(void) { // range: +/- 8192; +/- 2000 deg/sec if (!gyro.read(gyroADCRaw)) { return; } gyroADC[X] = gyroADCRaw[X]; gyroADC[Y] = gyroADCRaw[Y]; gyroADC[Z] = gyroADCRaw[Z]; alignSensors(gyroADC, gyroADC, gyroAlign); if (!isGyroCalibrationComplete()) { performAcclerationCalibration(gyroConfig()->gyroMovementCalibrationThreshold); } gyroADC[X] -= gyroZero[X]; gyroADC[Y] -= gyroZero[Y]; gyroADC[Z] -= gyroZero[Z]; if (gyroConfig()->gyro_soft_lpf_hz) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { if (debugMode == DEBUG_GYRO) debug[axis] = gyroADC[axis]; if (gyroConfig()->gyro_soft_type == FILTER_BIQUAD) gyroADCf[axis] = biquadFilterApply(&gyroFilterLPF[axis], (float) gyroADC[axis]); else gyroADCf[axis] = pt1FilterApply(&gyroFilterPt1[axis], (float) gyroADC[axis]); if (debugMode == DEBUG_NOTCH) debug[axis] = lrintf(gyroADCf[axis]); if (gyroConfig()->gyro_soft_notch_hz) gyroADCf[axis] = biquadFilterApply(&gyroFilterNotch[axis], gyroADCf[axis]); gyroADC[axis] = lrintf(gyroADCf[axis]); } } else { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { gyroADCf[axis] = gyroADC[axis]; } } }
void voltageMeterESCRefresh(void) { #ifdef USE_ESC_SENSOR escSensorData_t *escData = getEscSensorData(ESC_SENSOR_COMBINED); if (escData) { voltageMeterESCState.voltageUnfiltered = escData->dataAge <= ESC_BATTERY_AGE_MAX ? escData->voltage / 10 : 0; voltageMeterESCState.voltageFiltered = biquadFilterApply(&voltageMeterESCState.filter, voltageMeterESCState.voltageUnfiltered); } #endif }
void filterRc(bool isRXDataNew) { static int16_t lastCommand[4] = { 0, 0, 0, 0 }; static int16_t deltaRC[4] = { 0, 0, 0, 0 }; static int16_t factor, rcInterpolationFactor; static biquadFilter_t filteredCycleTimeState; static bool filterInitialised; uint16_t filteredCycleTime; uint16_t rxRefreshRate; // Set RC refresh rate for sampling and channels to filter initRxRefreshRate(&rxRefreshRate); // Calculate average cycle time (1Hz LPF on cycle time) if (!filterInitialised) { biquadFilterInitLPF(&filteredCycleTimeState, 1, gyro.targetLooptime); filterInitialised = true; } filteredCycleTime = biquadFilterApply(&filteredCycleTimeState, (float) cycleTime); rcInterpolationFactor = rxRefreshRate / filteredCycleTime + 1; if (isRXDataNew) { for (int channel=0; channel < 4; channel++) { deltaRC[channel] = rcCommand[channel] - (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor); lastCommand[channel] = rcCommand[channel]; } factor = rcInterpolationFactor - 1; } else { factor--; } // Interpolate steps of rcCommand if (factor > 0) { for (int channel=0; channel < 4; channel++) { rcCommand[channel] = lastCommand[channel] - deltaRC[channel] * factor/rcInterpolationFactor; } } else { factor = 0; } }
static void updateBatteryVoltage(void) { static biquadFilter_t vbatFilter; static bool vbatFilterIsInitialised; // store the battery voltage with some other recent battery voltage readings uint16_t vbatSample = vbatLatestADC = adcGetChannel(ADC_BATTERY); if (debugMode == DEBUG_BATTERY) debug[0] = vbatSample; if (!vbatFilterIsInitialised) { biquadFilterInitLPF(&vbatFilter, VBATT_LPF_FREQ, 50000); //50HZ Update vbatFilterIsInitialised = true; } vbatSample = biquadFilterApply(&vbatFilter, vbatSample); vbat = batteryAdcToVoltage(vbatSample); if (debugMode == DEBUG_BATTERY) debug[1] = vbat; }
void voltageMeterUpdate(void) { uint16_t vbatSample; for (uint8_t i = 0; i < MAX_VOLTAGE_METERS && i < ARRAYLEN(voltageMeterAdcChannelMap); i++) { // store the battery voltage with some other recent battery voltage readings voltageMeterState_t *state = &voltageMeterStates[i]; voltageMeterConfig_t *config = voltageMeterConfig(i); uint8_t channel = voltageMeterAdcChannelMap[i]; vbatSample = state->vbatLatestADC = adcGetChannel(channel); vbatSample = biquadFilterApply(&state->vbatFilterState, vbatSample); // always calculate the latest voltage, see getLatestVoltage() which does the calculation on demand. state->vbat = voltageAdcToVoltage(vbatSample, config); } }
void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims) { int16_t accADCRaw[XYZ_AXIS_COUNT]; if (!acc.read(accADCRaw)) { return; } for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { if (debugMode == DEBUG_ACCELEROMETER) debug[axis] = accADCRaw[axis]; accSmooth[axis] = accADCRaw[axis]; } if (accLpfCutHz) { if (!accFilterInitialised) { if (accTargetLooptime) { /* Initialisation needs to happen once sample rate is known */ for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { biquadFilterInitLPF(&accFilter[axis], accLpfCutHz, accTargetLooptime); } accFilterInitialised = true; } } if (accFilterInitialised) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { accSmooth[axis] = lrintf(biquadFilterApply(&accFilter[axis], (float)accSmooth[axis])); } } } alignSensors(accSmooth, accSmooth, accAlign); if (!isAccelerationCalibrationComplete()) { performAcclerationCalibration(rollAndPitchTrims); } if (feature(FEATURE_INFLIGHT_ACC_CAL)) { performInflightAccelerationCalibration(rollAndPitchTrims); } applyAccelerationTrims(accelerationTrims); }
void filterServos(void) { int servoIdx; if (mixerConfig->servo_lowpass_enable) { // Initialize servo lowpass filter (servos are calculated at looptime rate) if (!servoFilterIsSet) { for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) { biquadFilterInit(&servoFitlerState[servoIdx], mixerConfig->servo_lowpass_freq, 0); } servoFilterIsSet = true; } for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) { // Apply servo lowpass filter and do sanity cheching servo[servoIdx] = (int16_t) biquadFilterApply(&servoFitlerState[servoIdx], (float)servo[servoIdx]); } } for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) { servo[servoIdx] = constrain(servo[servoIdx], servoConf[servoIdx].min, servoConf[servoIdx].max); } }
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); }
// 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 } }