rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Optimized the PID loop a little bit (#5661) 

* * Put PID variables into the structure
* Precalculate DTerm gyro filter outside the axis loop
* Removed unused variables PIDweight[3], airmodeWasActivated
* If zero throttle or gyro overflow, we can just set values and exit, this saves checks and jumps in axis loop
* Compute PIDSUM after the axis loop, this saves branching inside the loop because of Yaw has no D term

* * Incorporated review changes from DieHertz and fujin

* * Incorporated another review requests from DieHertz
  - PidSum renamed to Sum
  - pidData[3] redone to pidData[XYZ_AXIS_COUNT]
This commit is contained in:
Miroslav Drbal [ApoC] 2018-04-10 22:22:51 +02:00 committed by Michael Keller
parent 696478d04c
commit 045557561d
7 changed files with 100 additions and 80 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/main/blackbox/blackbox.c
View File

@ -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

14
src/main/fc/fc_core.c
View File

@ -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))) {

13
src/main/flight/mixer.c
View File

@ -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;
}

116
src/main/flight/pid.c
View File

@ -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, &currentPidSetpoint, &errorRate);
&currentPidSetpoint, &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)

19
src/main/flight/pid.h
View File

@ -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;

6
src/main/flight/servos.c
View File

@ -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)) {

6
src/main/io/pidaudio.c
View File

@ -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;
}