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

Put each PID in its own .c file

Browse source
This commit is contained in:
Martin Budden 2016-08-04 09:02:31 +01:00 • committed by borisbstyle
parent ade8e9fe3a
commit cead17456f
1 changed files with 10 additions and 8 deletions
Download patch file Download diff file Expand all files Collapse all files

18
src/main/flight/pid_betaflight.c
View file

@ -62,6 +62,9 @@ extern biquadFilter_t dtermFilterNotch[3];
extern bool dtermNotchInitialised; extern bool dtermNotchInitialised;
extern bool dtermBiquadLpfInitialised; extern bool dtermBiquadLpfInitialised;
// PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction
uint8_t PIDweight[3];
void initFilters(const pidProfile_t *pidProfile); void initFilters(const pidProfile_t *pidProfile);
float getdT(void); float getdT(void);
@ -72,7 +75,7 @@ void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
float errorRate = 0, rP = 0, rD = 0, PVRate = 0; float errorRate = 0, rP = 0, rD = 0, PVRate = 0;
float ITerm,PTerm,DTerm; float ITerm,PTerm,DTerm;
static float lastRateError[2]; static float lastRateError[2];
static float Kp[3], Ki[3], Kd[3], b[3], c[3], rollPitchMaxVelocity, yawMaxVelocity, previousSetpoint[3]; static float Kp[3], Ki[3], Kd[3], b[3], c[3], yawMaxVelocity, yawPreviousRate;
float delta; float delta;
int axis; int axis;
float horizonLevelStrength = 1; float horizonLevelStrength = 1;
@ -134,14 +137,13 @@ void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
configD[axis] = pidProfile->D8[axis]; configD[axis] = pidProfile->D8[axis];
} }
// Limit abrupt yaw inputs / stops // Limit abrupt yaw inputs
float maxVelocity = (axis == YAW) ? yawMaxVelocity : rollPitchMaxVelocity; if (axis == YAW && pidProfile->pidMaxVelocityYaw) {
if (maxVelocity) { float yawCurrentVelocity = setpointRate[axis] - yawPreviousRate;
float currentVelocity = setpointRate[axis] - previousSetpoint[axis]; if (ABS(yawCurrentVelocity) > yawMaxVelocity) {
if (ABS(currentVelocity) > maxVelocity) { setpointRate[axis] = (yawCurrentVelocity > 0) ? yawPreviousRate + yawMaxVelocity : yawPreviousRate - yawMaxVelocity;
setpointRate[axis] = (currentVelocity > 0) ? previousSetpoint[axis] + maxVelocity : previousSetpoint[axis] - maxVelocity;
} }
previousSetpoint[axis] = setpointRate[axis]; yawPreviousRate = setpointRate[axis];
} }
// Yaw control is GYRO based, direct sticks control is applied to rate PID // Yaw control is GYRO based, direct sticks control is applied to rate PID