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

Merge pull request #6042 from mikeller/optimise_pid_loop 

Optimised the use of static variables in the PID loop.
This commit is contained in:
Michael Keller 2018-06-04 20:48:10 +12:00 committed by GitHub
parent a2c75d2009 e29fa595f5
commit 6fa1a26e71
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 6 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -64,6 +64,7 @@ static FAST_RAM_ZERO_INIT bool pidStabilisationEnabled;
static FAST_RAM_ZERO_INIT bool inCrashRecoveryMode = false; static FAST_RAM_ZERO_INIT bool inCrashRecoveryMode = false;
static FAST_RAM_ZERO_INIT float dT; static FAST_RAM_ZERO_INIT float dT;
static FAST_RAM_ZERO_INIT float pidFrequency;
PG_REGISTER_WITH_RESET_TEMPLATE(pidConfig_t, pidConfig, PG_PID_CONFIG, 2); PG_REGISTER_WITH_RESET_TEMPLATE(pidConfig_t, pidConfig, PG_PID_CONFIG, 2);
@ -168,7 +169,8 @@ void pgResetFn_pidProfiles(pidProfile_t *pidProfiles)
static void pidSetTargetLooptime(uint32_t pidLooptime) static void pidSetTargetLooptime(uint32_t pidLooptime)
{ {
targetPidLooptime = pidLooptime; targetPidLooptime = pidLooptime;
dT = (float)targetPidLooptime * 0.000001f; dT = targetPidLooptime * 1e-6f;
pidFrequency = 1.0f / dT;
} }
static FAST_RAM float itermAccelerator = 1.0f; static FAST_RAM float itermAccelerator = 1.0f;
@ -230,7 +232,7 @@ void pidInitFilters(const pidProfile_t *pidProfile)
return; return;
} }
const uint32_t pidFrequencyNyquist = (1.0f / dT) / 2; // No rounding needed const uint32_t pidFrequencyNyquist = pidFrequency / 2; // No rounding needed
uint16_t dTermNotchHz; uint16_t dTermNotchHz;
if (pidProfile->dterm_notch_hz <= pidFrequencyNyquist) { if (pidProfile->dterm_notch_hz <= pidFrequencyNyquist) {
@ -882,7 +884,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
// calculated deltaT whenever another task causes the PID // calculated deltaT whenever another task causes the PID
// loop execution to be delayed. // loop execution to be delayed.
const float delta = const float delta =
- (gyroRateDterm[axis] - previousGyroRateDterm[axis]) / dT; - (gyroRateDterm[axis] - previousGyroRateDterm[axis]) * pidFrequency;
detectAndSetCrashRecovery(pidProfile->crash_recovery, axis, currentTimeUs, delta, errorRate); detectAndSetCrashRecovery(pidProfile->crash_recovery, axis, currentTimeUs, delta, errorRate);
@ -910,7 +912,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
#endif // USE_RC_SMOOTHING_FILTER #endif // USE_RC_SMOOTHING_FILTER
const float pidFeedForward = const float pidFeedForward =
pidCoefficient[axis].Kd * dynCd * transition * pidSetpointDelta * tpaFactor / dT; pidCoefficient[axis].Kd * dynCd * transition * pidSetpointDelta * tpaFactor * pidFrequency;
#if defined(USE_SMART_FEEDFORWARD) #if defined(USE_SMART_FEEDFORWARD)
bool addFeedforward = true; bool addFeedforward = true;
if (smartFeedforward) { if (smartFeedforward) {