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

Merge pull request #1945 from martinbudden/bf_pid_tidy

Browse source 
Minor tidy of PID code
This commit is contained in:
borisbstyle 2016-12-31 00:54:18 +01:00 • committed by GitHub
commit 882735e91b
1 changed files with 17 additions and 16 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -157,20 +157,20 @@ void pidInitConfig(const pidProfile_t *pidProfile) {
maxVelocity[FD_YAW] = pidProfile->yawRateAccelLimit * 1000 * dT;
}
float currentPidSetpoint = 0, horizonLevelStrength = 1.0f;
void calcHorizonLevelStrength(const pidProfile_t *pidProfile) {
const float mostDeflectedPos = MAX(getRcDeflection(FD_ROLL), getRcDeflection(FD_PITCH));
// Progressively turn off the horizon self level strength as the stick is banged over
horizonLevelStrength = (1.0f - mostDeflectedPos); // 1 at centre stick, 0 = max stick deflection
float calcHorizonLevelStrength(const pidProfile_t *pidProfile) {
float horizonLevelStrength;
if(pidProfile->D8[PIDLEVEL] == 0){
horizonLevelStrength = 0;
} else {
const float mostDeflectedPos = MAX(getRcDeflection(FD_ROLL), getRcDeflection(FD_PITCH));
// Progressively turn off the horizon self level strength as the stick is banged over
horizonLevelStrength = (1.0f - mostDeflectedPos); // 1 at centre stick, 0 = max stick deflection
horizonLevelStrength = constrainf(((horizonLevelStrength - 1) * horizonTransition) + 1, 0, 1);
}
return horizonLevelStrength;
}
void pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim) {
float pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, float currentPidSetpoint) {
// calculate error angle and limit the angle to the max inclination
float errorAngle = pidProfile->levelSensitivity * rcCommand[axis];
#ifdef GPS
@ -184,11 +184,13 @@ void pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPitchTrims_
} else {
// HORIZON mode - direct sticks control is applied to rate PID
// mix up angle error to desired AngleRate to add a little auto-level feel
const float horizonLevelStrength = calcHorizonLevelStrength(pidProfile);
currentPidSetpoint = currentPidSetpoint + (errorAngle * horizonGain * horizonLevelStrength);
}
return currentPidSetpoint;
}
void accelerationLimit(int axis) {
float accelerationLimit(int axis, float currentPidSetpoint) {
static float previousSetpoint[3];
const float currentVelocity = currentPidSetpoint- previousSetpoint[axis];
@ -196,6 +198,7 @@ void accelerationLimit(int axis) {
currentPidSetpoint = (currentVelocity > 0) ? previousSetpoint[axis] + maxVelocity[axis] : previousSetpoint[axis] - maxVelocity[axis];
previousSetpoint[axis] = currentPidSetpoint;
return currentPidSetpoint;
}
// Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage.
@ -205,21 +208,18 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
static float previousRateError[2];
static float previousSetpoint[3];
if(FLIGHT_MODE(HORIZON_MODE))
calcHorizonLevelStrength(pidProfile);
// ----------PID controller----------
const float tpaFactor = getThrottlePIDAttenuation();
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
currentPidSetpoint = getSetpointRate(axis);
float currentPidSetpoint = getSetpointRate(axis);
if(maxVelocity[axis])
accelerationLimit(axis);
currentPidSetpoint = accelerationLimit(axis, currentPidSetpoint);
// Yaw control is GYRO based, direct sticks control is applied to rate PID
if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && axis != YAW) {
pidLevel(axis, pidProfile, angleTrim);
currentPidSetpoint = pidLevel(axis, pidProfile, angleTrim, currentPidSetpoint);
}
const float gyroRate = gyro.gyroADCf[axis]; // Process variable from gyro output in deg/sec
@ -232,6 +232,9 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
// -----calculate P component and add Dynamic Part based on stick input
float PTerm = Kp[axis] * errorRate * tpaFactor;
if (axis == FD_YAW) {
PTerm = ptermYawFilterApplyFn(ptermYawFilter, PTerm);
}
// -----calculate I component
// Reduce strong Iterm accumulation during higher stick inputs
@ -271,8 +274,6 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
DTerm = dtermNotchFilterApplyFn(dtermFilterNotch[axis], DTerm);
DTerm = dtermLpfApplyFn(dtermFilterLpf[axis], DTerm);
} else {
PTerm = ptermYawFilterApplyFn(ptermYawFilter, PTerm);
}
previousSetpoint[axis] = currentPidSetpoint;