Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-24 00:35:39 +03:00
Code Issues Wiki Activity

Minor pid.c refactoring (#12722)

Browse source 
* Minor pid.c refactoring

* Only calculate unfiltered gyro delta for that debug

Remove unnecessary comment
Change multiplier to factor of 10

* remove some archaic comments

* clarify name of D_LPF_PRE_TPA_SCALE, use inverse in define

* Remove some blank lines

* undo conversion to division thanks -ffast-math
This commit is contained in:
ctzsnooze 2023-05-12 01:06:36 +10:00 • committed by GitHub
parent f3fab663da
commit 68dd061c51
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
2 changed files with 10 additions and 26 deletions
Download patch file Download diff file Expand all files Collapse all files

34
src/main/flight/pid.c
View file

@ -245,7 +245,7 @@ void pgResetFn_pidProfiles(pidProfile_t *pidProfiles)
// Scale factors to make best use of range with D_LPF debugging, aiming for max +/-16K as debug values are 16 bit
#define D_LPF_RAW_SCALE 25
#define D_LPF_FILT_SCALE 22
#define D_LPF_PRE_TPA_SCALE 10
void pidSetItermAccelerator(float newItermAccelerator)
@ -865,19 +865,12 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
float gyroRateDterm[XYZ_AXIS_COUNT];
for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) {
gyroRateDterm[axis] = gyro.gyroADCf[axis];
// -----calculate raw, unfiltered D component
// Divide rate change by dT to get differential (ie dr/dt).
// dT is fixed and calculated from the target PID loop time
// This is done to avoid DTerm spikes that occur with dynamically
// calculated deltaT whenever another task causes the PID
// loop execution to be delayed.
// Log the unfiltered D for ROLL and PITCH
if (axis != FD_YAW) {
if (debugMode == DEBUG_D_LPF && axis != FD_YAW) {
const float delta = (previousRawGyroRateDterm[axis] - gyroRateDterm[axis]) * pidRuntime.pidFrequency / D_LPF_RAW_SCALE;
previousRawGyroRateDterm[axis] = gyroRateDterm[axis];
DEBUG_SET(DEBUG_D_LPF, axis, lrintf(delta));
DEBUG_SET(DEBUG_D_LPF, axis, lrintf(delta)); // debug d_lpf 2 and 3 used for pre-TPA D
}
gyroRateDterm[axis] = pidRuntime.dtermNotchApplyFn((filter_t *) &pidRuntime.dtermNotch[axis], gyroRateDterm[axis]);
@ -974,8 +967,6 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
#endif
// --------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 (feedforward weight) can be tuned (amount derivative on measurement or error).
// -----calculate P component
pidData[axis].P = pidRuntime.pidCoefficient[axis].Kp * errorRate * tpaFactorKp;
@ -983,6 +974,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
pidData[axis].P = pidRuntime.ptermYawLowpassApplyFn((filter_t *) &pidRuntime.ptermYawLowpass, pidData[axis].P);
}
// -----calculate I component
float Ki = pidRuntime.pidCoefficient[axis].Ki;
#ifdef USE_LAUNCH_CONTROL
@ -1030,8 +1022,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
// This is done to avoid DTerm spikes that occur with dynamically
// calculated deltaT whenever another task causes the PID
// loop execution to be delayed.
const float delta =
- (gyroRateDterm[axis] - previousGyroRateDterm[axis]) * pidRuntime.pidFrequency;
const float delta = - (gyroRateDterm[axis] - previousGyroRateDterm[axis]) * pidRuntime.pidFrequency;
float preTpaD = pidRuntime.pidCoefficient[axis].Kd * delta;
#if defined(USE_ACC)
@ -1065,10 +1056,8 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
pidData[axis].D = preTpaD * pidRuntime.tpaFactor;
// Log the value of D pre application of TPA
preTpaD *= D_LPF_FILT_SCALE;
if (axis != FD_YAW) {
DEBUG_SET(DEBUG_D_LPF, axis - FD_ROLL + 2, lrintf(preTpaD));
DEBUG_SET(DEBUG_D_LPF, axis - FD_ROLL + 2, lrintf(preTpaD * D_LPF_PRE_TPA_SCALE));
}
} else {
pidData[axis].D = 0;
@ -1080,20 +1069,15 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
previousGyroRateDterm[axis] = gyroRateDterm[axis];
// -----calculate feedforward component
#ifdef USE_ABSOLUTE_CONTROL
// include abs control correction in feedforward
pidSetpointDelta += setpointCorrection - pidRuntime.oldSetpointCorrection[axis];
pidRuntime.oldSetpointCorrection[axis] = setpointCorrection;
#endif
// no feedforward in launch control
float feedforwardGain = launchControlActive ? 0.0f : pidRuntime.pidCoefficient[axis].Kf;
if (feedforwardGain > 0) {
float feedForward = feedforwardGain * pidSetpointDelta;
pidData[axis].F = feedForward;
} else {
pidData[axis].F = 0;
}
const float feedforwardGain = launchControlActive ? 0.0f : pidRuntime.pidCoefficient[axis].Kf;
pidData[axis].F = feedforwardGain * pidSetpointDelta;
#ifdef USE_YAW_SPIN_RECOVERY
if (yawSpinActive) {