diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 971f2447f1..b0c891d319 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -984,9 +984,9 @@ static void loadMainState(timeUs_t currentTimeUs) blackboxCurrent->time = currentTimeUs; for (int i = 0; i < XYZ_AXIS_COUNT; i++) { - blackboxCurrent->axisPID_P[i] = axisPID_P[i]; - blackboxCurrent->axisPID_I[i] = axisPID_I[i]; - blackboxCurrent->axisPID_D[i] = axisPID_D[i]; + blackboxCurrent->axisPID_P[i] = pidData[i].P; + blackboxCurrent->axisPID_I[i] = pidData[i].I; + blackboxCurrent->axisPID_D[i] = pidData[i].D; blackboxCurrent->gyroADC[i] = lrintf(gyro.gyroADCf[i]); blackboxCurrent->accADC[i] = lrintf(acc.accADC[i]); #ifdef USE_MAG diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index c9ebd1640c..c138ce67ed 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -578,10 +578,10 @@ bool processRx(timeUs_t currentTimeUs) const uint8_t lowThrottleLimit = pidConfig()->runaway_takeoff_deactivate_throttle; const uint8_t midThrottleLimit = constrain(lowThrottleLimit * 2, lowThrottleLimit * 2, RUNAWAY_TAKEOFF_HIGH_THROTTLE_PERCENT); if ((((throttlePercent >= lowThrottleLimit) && areSticksActive(RUNAWAY_TAKEOFF_DEACTIVATE_STICK_PERCENT)) || (throttlePercent >= midThrottleLimit)) - && (fabsf(axisPIDSum[FD_PITCH]) < RUNAWAY_TAKEOFF_DEACTIVATE_PIDSUM_LIMIT) - && (fabsf(axisPIDSum[FD_ROLL]) < RUNAWAY_TAKEOFF_DEACTIVATE_PIDSUM_LIMIT) - && (fabsf(axisPIDSum[FD_YAW]) < RUNAWAY_TAKEOFF_DEACTIVATE_PIDSUM_LIMIT)) { - + && (fabsf(pidData[FD_PITCH].Sum) < RUNAWAY_TAKEOFF_DEACTIVATE_PIDSUM_LIMIT) + && (fabsf(pidData[FD_ROLL].Sum) < RUNAWAY_TAKEOFF_DEACTIVATE_PIDSUM_LIMIT) + && (fabsf(pidData[FD_YAW].Sum) < RUNAWAY_TAKEOFF_DEACTIVATE_PIDSUM_LIMIT)) { + inStableFlight = true; if (runawayTakeoffDeactivateUs == 0) { runawayTakeoffDeactivateUs = currentTimeUs; @@ -813,9 +813,9 @@ static void subTaskPidController(timeUs_t currentTimeUs) && !runawayTakeoffTemporarilyDisabled && (!feature(FEATURE_MOTOR_STOP) || isAirmodeActive() || (calculateThrottleStatus() != THROTTLE_LOW))) { - if (((fabsf(axisPIDSum[FD_PITCH]) >= RUNAWAY_TAKEOFF_PIDSUM_THRESHOLD) - || (fabsf(axisPIDSum[FD_ROLL]) >= RUNAWAY_TAKEOFF_PIDSUM_THRESHOLD) - || (fabsf(axisPIDSum[FD_YAW]) >= RUNAWAY_TAKEOFF_PIDSUM_THRESHOLD)) + if (((fabsf(pidData[FD_PITCH].Sum) >= RUNAWAY_TAKEOFF_PIDSUM_THRESHOLD) + || (fabsf(pidData[FD_ROLL].Sum) >= RUNAWAY_TAKEOFF_PIDSUM_THRESHOLD) + || (fabsf(pidData[FD_YAW].Sum) >= RUNAWAY_TAKEOFF_PIDSUM_THRESHOLD)) && ((ABS(gyroAbsRateDps(FD_PITCH)) > RUNAWAY_TAKEOFF_GYRO_LIMIT_RP) || (ABS(gyroAbsRateDps(FD_ROLL)) > RUNAWAY_TAKEOFF_GYRO_LIMIT_RP) || (ABS(gyroAbsRateDps(FD_YAW)) > RUNAWAY_TAKEOFF_GYRO_LIMIT_YAW))) { diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 042b079d5e..6eb6d63aec 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -358,10 +358,9 @@ bool mixerIsOutputSaturated(int axis, float errorRate) { if (axis == FD_YAW && mixerIsTricopter()) { return mixerTricopterIsServoSaturated(errorRate); - } else { - return motorMixRange >= 1.0f; } - return false; + + return motorMixRange >= 1.0f; } // All PWM motor scaling is done to standard PWM range of 1000-2000 for easier tick conversion with legacy code / configurator @@ -743,13 +742,13 @@ void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensation) // Find min and max throttle based on conditions. Throttle has to be known before mixing calculateThrottleAndCurrentMotorEndpoints(currentTimeUs); - // Calculate and Limit the PIDsum + // Calculate and Limit the PID sum const float scaledAxisPidRoll = - constrainf(axisPIDSum[FD_ROLL], -currentPidProfile->pidSumLimit, currentPidProfile->pidSumLimit) / PID_MIXER_SCALING; + constrainf(pidData[FD_ROLL].Sum, -currentPidProfile->pidSumLimit, currentPidProfile->pidSumLimit) / PID_MIXER_SCALING; const float scaledAxisPidPitch = - constrainf(axisPIDSum[FD_PITCH], -currentPidProfile->pidSumLimit, currentPidProfile->pidSumLimit) / PID_MIXER_SCALING; + constrainf(pidData[FD_PITCH].Sum, -currentPidProfile->pidSumLimit, currentPidProfile->pidSumLimit) / PID_MIXER_SCALING; float scaledAxisPidYaw = - constrainf(axisPIDSum[FD_YAW], -currentPidProfile->pidSumLimitYaw, currentPidProfile->pidSumLimitYaw) / PID_MIXER_SCALING; + constrainf(pidData[FD_YAW].Sum, -currentPidProfile->pidSumLimitYaw, currentPidProfile->pidSumLimitYaw) / PID_MIXER_SCALING; if (!mixerConfig()->yaw_motors_reversed) { scaledAxisPidYaw = -scaledAxisPidYaw; } diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 47f70966b2..82e26abb3c 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -53,12 +53,12 @@ FAST_RAM uint32_t targetPidLooptime; +FAST_RAM pidAxisData_t pidData[XYZ_AXIS_COUNT]; + static FAST_RAM bool pidStabilisationEnabled; static FAST_RAM bool inCrashRecoveryMode = false; -FAST_RAM float axisPID_P[3], axisPID_I[3], axisPID_D[3], axisPIDSum[3]; - static FAST_RAM float dT; PG_REGISTER_WITH_RESET_TEMPLATE(pidConfig_t, pidConfig, PG_PID_CONFIG, 2); @@ -154,7 +154,7 @@ static void pidSetTargetLooptime(uint32_t pidLooptime) void pidResetITerm(void) { for (int axis = 0; axis < 3; axis++) { - axisPID_I[axis] = 0.0f; + pidData[axis].I = 0.0f; } } @@ -274,7 +274,13 @@ void pidInitFilters(const pidProfile_t *pidProfile) pt1FilterInit(&throttleLpf, pt1FilterGain(pidProfile->throttle_boost_cutoff, dT)); } -static FAST_RAM float Kp[3], Ki[3], Kd[3]; +typedef struct pidCoefficient_s { + float Kp; + float Ki; + float Kd; +} pidCoefficient_t; + +static FAST_RAM pidCoefficient_t pidCoefficient[3]; static FAST_RAM float maxVelocity[3]; static FAST_RAM float relaxFactor; static FAST_RAM float dtermSetpointWeight; @@ -297,9 +303,9 @@ static FAST_RAM bool itermRotation; void pidInitConfig(const pidProfile_t *pidProfile) { for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { - Kp[axis] = PTERM_SCALE * pidProfile->pid[axis].P; - Ki[axis] = ITERM_SCALE * pidProfile->pid[axis].I; - Kd[axis] = DTERM_SCALE * pidProfile->pid[axis].D; + pidCoefficient[axis].Kp = PTERM_SCALE * pidProfile->pid[axis].P; + pidCoefficient[axis].Ki = ITERM_SCALE * pidProfile->pid[axis].I; + pidCoefficient[axis].Kd = DTERM_SCALE * pidProfile->pid[axis].D; } dtermSetpointWeight = pidProfile->dtermSetpointWeight / 127.0f; @@ -440,10 +446,9 @@ static float accelerationLimit(int axis, float currentPidSetpoint) static timeUs_t crashDetectedAtUs; -void handleCrashRecovery( +static void handleCrashRecovery( const pidCrashRecovery_e crash_recovery, const rollAndPitchTrims_t *angleTrim, - const int axis, const timeUs_t currentTimeUs, const float gyroRate, - float *axisPID_I, float *currentPidSetpoint, float *errorRate) + const int axis, const timeUs_t currentTimeUs, const float gyroRate, float *currentPidSetpoint, float *errorRate) { if (inCrashRecoveryMode && cmpTimeUs(currentTimeUs, crashDetectedAtUs) > crashTimeDelayUs) { if (crash_recovery == PID_CRASH_RECOVERY_BEEP) { @@ -462,7 +467,7 @@ void handleCrashRecovery( } // reset ITerm, since accumulated error before crash is now meaningless // and ITerm windup during crash recovery can be extreme, especially on yaw axis - axisPID_I[axis] = 0.0f; + pidData[axis].I = 0.0f; if (cmpTimeUs(currentTimeUs, crashDetectedAtUs) > crashTimeLimitUs || (getMotorMixRange() < 1.0f && ABS(gyro.gyroADCf[FD_ROLL]) < crashRecoveryRate @@ -483,7 +488,7 @@ void handleCrashRecovery( } } -void detectAndSetCrashRecovery( +static void detectAndSetCrashRecovery( const pidCrashRecovery_e crash_recovery, const uint8_t axis, const timeUs_t currentTimeUs, const float delta, const float errorRate) { @@ -510,7 +515,7 @@ void detectAndSetCrashRecovery( } } -void handleItermRotation(const float deltaT, float *axisPID_I) +static void handleItermRotation(const float deltaT) { // rotate old I to the new coordinate system const float gyroToAngle = deltaT * RAD; @@ -518,9 +523,9 @@ void handleItermRotation(const float deltaT, float *axisPID_I) int i_1 = (i + 1) % 3; int i_2 = (i + 2) % 3; float angle = gyro.gyroADCf[i] * gyroToAngle; - float newPID_I_i_1 = axisPID_I[i_1] + axisPID_I[i_2] * angle; - axisPID_I[i_2] -= axisPID_I[i_1] * angle; - axisPID_I[i_1] = newPID_I_i_1; + float newPID_I_i_1 = pidData[i_1].I + pidData[i_2].I * angle; + pidData[i_2].I -= pidData[i_1].I * angle; + pidData[i_1].I = newPID_I_i_1; } } @@ -528,11 +533,25 @@ void handleItermRotation(const float deltaT, float *axisPID_I) // Based on 2DOF reference design (matlab) void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, timeUs_t currentTimeUs) { - static float previousGyroRateFiltered[2]; + static float previousGyroRateDterm[2]; static float previousPidSetpoint[2]; + static timeUs_t previousTimeUs; + + // Disable PID control if at zero throttle or if gyro overflow detected + if (!pidStabilisationEnabled || gyroOverflowDetected()) { + for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) { + pidData[axis].P = 0; + pidData[axis].I = 0; + pidData[axis].D = 0; + + pidData[axis].Sum = 0; + } + + return; + } + const float tpaFactor = getThrottlePIDAttenuation(); const float motorMixRange = getMotorMixRange(); - static timeUs_t previousTimeUs; // calculate actual deltaT in seconds const float deltaT = (currentTimeUs - previousTimeUs) * 0.000001f; @@ -546,12 +565,20 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an // Dynamic d component, enable 2-DOF PID controller only for rate mode const float dynCd = flightModeFlags ? 0.0f : dtermSetpointWeight; + // Precalculate gyro deta for D-term here, this allows loop unrolling + float gyroRateDterm[2]; + for (int axis = FD_ROLL; axis < FD_YAW; ++axis) { + gyroRateDterm[axis] = dtermNotchApplyFn((filter_t *) &dtermNotch[axis], gyro.gyroADCf[axis]); + gyroRateDterm[axis] = dtermLowpassApplyFn((filter_t *) &dtermLowpass[axis], gyroRateDterm[axis]); + gyroRateDterm[axis] = dtermLowpass2ApplyFn((filter_t *) &dtermLowpass2[axis], gyroRateDterm[axis]); + } + if (itermRotation) { - handleItermRotation(deltaT, axisPID_I); + handleItermRotation(deltaT); } // ----------PID controller---------- - for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { + for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) { float currentPidSetpoint = getSetpointRate(axis); if (maxVelocity[axis]) { currentPidSetpoint = accelerationLimit(axis, currentPidSetpoint); @@ -567,63 +594,52 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an handleCrashRecovery( pidProfile->crash_recovery, angleTrim, axis, currentTimeUs, gyroRate, - axisPID_I, ¤tPidSetpoint, &errorRate); + ¤tPidSetpoint, &errorRate); // --------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 (dtermSetpointWeight) can be tuned (amount derivative on measurement or error). // -----calculate P component and add Dynamic Part based on stick input - axisPID_P[axis] = Kp[axis] * errorRate * tpaFactor; + pidData[axis].P = pidCoefficient[axis].Kp * errorRate * tpaFactor; if (axis == FD_YAW) { - axisPID_P[axis] = ptermYawLowpassApplyFn((filter_t *) &ptermYawLowpass, axisPID_P[axis]); + pidData[axis].P = ptermYawLowpassApplyFn((filter_t *) &ptermYawLowpass, pidData[axis].P); } // -----calculate I component - const float ITerm = axisPID_I[axis]; - const float ITermNew = constrainf(ITerm + Ki[axis] * errorRate * dynCi, -itermLimit, itermLimit); + const float ITerm = pidData[axis].I; + const float ITermNew = constrainf(ITerm + pidCoefficient[axis].Ki * errorRate * dynCi, -itermLimit, itermLimit); const bool outputSaturated = mixerIsOutputSaturated(axis, errorRate); if (outputSaturated == false || ABS(ITermNew) < ABS(ITerm)) { // Only increase ITerm if output is not saturated - axisPID_I[axis] = ITermNew; + pidData[axis].I = ITermNew; } // -----calculate D component if (axis != FD_YAW) { - // apply filters - float gyroRateFiltered = dtermNotchApplyFn((filter_t *) &dtermNotch[axis], gyroRate); - gyroRateFiltered = dtermLowpassApplyFn((filter_t *) &dtermLowpass[axis], gyroRateFiltered); - gyroRateFiltered = dtermLowpass2ApplyFn((filter_t *) &dtermLowpass2[axis], gyroRateFiltered); - // no transition if relaxFactor == 0 - float transition = 1; - if (relaxFactor > 0) { - transition = getRcDeflectionAbs(axis) * relaxFactor; - } + float transition = relaxFactor > 0 ? getRcDeflectionAbs(axis) * relaxFactor : 1; + // Divide rate change by deltaT to get differential (ie dr/dt) const float delta = ( dynCd * transition * (currentPidSetpoint - previousPidSetpoint[axis]) - - (gyroRateFiltered - previousGyroRateFiltered[axis])) / deltaT; - + (gyroRateDterm[axis] - previousGyroRateDterm[axis])) / deltaT; + previousPidSetpoint[axis] = currentPidSetpoint; - previousGyroRateFiltered[axis] = gyroRateFiltered; + previousGyroRateDterm[axis] = gyroRateDterm[axis]; detectAndSetCrashRecovery(pidProfile->crash_recovery, axis, currentTimeUs, delta, errorRate); - axisPID_D[axis] = Kd[axis] * delta * tpaFactor; - axisPIDSum[axis] = axisPID_P[axis] + axisPID_I[axis] + axisPID_D[axis]; - } else { - axisPIDSum[axis] = axisPID_P[axis] + axisPID_I[axis]; - } - - // Disable PID control if at zero throttle or if gyro overflow detected - if (!pidStabilisationEnabled || gyroOverflowDetected()) { - axisPID_P[axis] = 0; - axisPID_I[axis] = 0; - axisPID_D[axis] = 0; - axisPIDSum[axis] = 0; + pidData[axis].D = pidCoefficient[axis].Kd * delta * tpaFactor; } } + + // calculating the PID sum + pidData[FD_ROLL].Sum = pidData[FD_ROLL].P + pidData[FD_ROLL].I + pidData[FD_ROLL].D; + pidData[FD_PITCH].Sum = pidData[FD_PITCH].P + pidData[FD_PITCH].I + pidData[FD_PITCH].D; + + // YAW has no D + pidData[FD_YAW].Sum = pidData[FD_YAW].P + pidData[FD_YAW].I; } bool crashRecoveryModeActive(void) diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 8648846b4c..e8795331d6 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -128,13 +128,20 @@ PG_DECLARE(pidConfig_t, pidConfig); union rollAndPitchTrims_u; void pidController(const pidProfile_t *pidProfile, const union rollAndPitchTrims_u *angleTrim, timeUs_t currentTimeUs); -extern float axisPID_P[3], axisPID_I[3], axisPID_D[3]; -extern float axisPIDSum[3]; -bool airmodeWasActivated; +typedef struct pidAxisData_s { + float P; + float I; + float D; + + float Sum; +} pidAxisData_t; + +extern pidAxisData_t pidData[3]; + extern uint32_t targetPidLooptime; -// PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction -extern uint8_t PIDweight[3]; +extern float throttleBoost; +extern pt1Filter_t throttleLpf; void pidResetITerm(void); void pidStabilisationState(pidStabilisationState_e pidControllerState); @@ -145,5 +152,3 @@ void pidInit(const pidProfile_t *pidProfile); void pidCopyProfile(uint8_t dstPidProfileIndex, uint8_t srcPidProfileIndex); bool crashRecoveryModeActive(void); -extern float throttleBoost; -extern pt1Filter_t throttleLpf; diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index 1e2cc2a8fd..b439a02908 100644 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -401,9 +401,9 @@ void servoMixer(void) input[INPUT_STABILIZED_YAW] = rcCommand[YAW]; } else { // Assisted modes (gyro only or gyro+acc according to AUX configuration in Gui - input[INPUT_STABILIZED_ROLL] = axisPIDSum[FD_ROLL] * PID_SERVO_MIXER_SCALING; - input[INPUT_STABILIZED_PITCH] = axisPIDSum[FD_PITCH] * PID_SERVO_MIXER_SCALING; - input[INPUT_STABILIZED_YAW] = axisPIDSum[FD_YAW] * PID_SERVO_MIXER_SCALING; + input[INPUT_STABILIZED_ROLL] = pidData[FD_ROLL].Sum * PID_SERVO_MIXER_SCALING; + input[INPUT_STABILIZED_PITCH] = pidData[FD_PITCH].Sum * PID_SERVO_MIXER_SCALING; + input[INPUT_STABILIZED_YAW] = pidData[FD_YAW].Sum * PID_SERVO_MIXER_SCALING; // Reverse yaw servo when inverted in 3D mode if (feature(FEATURE_3D) && (rcData[THROTTLE] < rxConfig()->midrc)) { diff --git a/src/main/io/pidaudio.c b/src/main/io/pidaudio.c index 9f3dfb0b65..31ba3811cb 100644 --- a/src/main/io/pidaudio.c +++ b/src/main/io/pidaudio.c @@ -83,19 +83,19 @@ void pidAudioUpdate(void) switch (pidAudioMode) { case PID_AUDIO_PIDSUM_X: { - const uint32_t pidSumX = MIN(ABS(axisPIDSum[FD_ROLL]), PIDSUM_LIMIT); + const uint32_t pidSumX = MIN(ABS(pidData[FD_ROLL].Sum), PIDSUM_LIMIT); tone = scaleRange(pidSumX, 0, PIDSUM_LIMIT, TONE_MAX, TONE_MIN); break; } case PID_AUDIO_PIDSUM_Y: { - const uint32_t pidSumY = MIN(ABS(axisPIDSum[FD_PITCH]), PIDSUM_LIMIT); + const uint32_t pidSumY = MIN(ABS(pidData[FD_PITCH].Sum), PIDSUM_LIMIT); tone = scaleRange(pidSumY, 0, PIDSUM_LIMIT, TONE_MAX, TONE_MIN); break; } case PID_AUDIO_PIDSUM_XY: { - const uint32_t pidSumXY = MIN((ABS(axisPIDSum[FD_ROLL]) + ABS(axisPIDSum[FD_PITCH])) / 2, PIDSUM_LIMIT); + const uint32_t pidSumXY = MIN((ABS(pidData[FD_ROLL].Sum) + ABS(pidData[FD_PITCH].Sum)) / 2, PIDSUM_LIMIT); tone = scaleRange(pidSumXY, 0, PIDSUM_LIMIT, TONE_MAX, TONE_MIN); break; }