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

Merge tag 'v3.1.3'

Browse source
This commit is contained in:
borisbstyle 2017-02-02 14:18:48 +01:00
commit be74098162
2 changed files with 35 additions and 24 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/fc/config.c
View file

@ -184,7 +184,7 @@ static void resetPidProfile(pidProfile_t *pidProfile)
pidProfile->yawRateAccelLimit = 10.0f;
pidProfile->rateAccelLimit = 0.0f;
pidProfile->itermThrottleThreshold = 350;
pidProfile->itermAcceleratorGain = 1.5f;
pidProfile->itermAcceleratorGain = 2.0f;
pidProfile->itermAcceleratorRateLimit = 80;
}

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

@ -152,7 +152,9 @@ void pidInitFilters(const pidProfile_t *pidProfile)
}
}
static float Kp[3], Ki[3], Kd[3], c[3], maxVelocity[3], relaxFactor[3];
static float Kp[3], Ki[3], Kd[3], maxVelocity[3];
static float relaxFactor;
static float dtermSetpointWeight;
static float levelGain, horizonGain, horizonTransition, ITermWindupPoint, ITermWindupPointInv, itermAcceleratorRateLimit;
void pidInitConfig(const pidProfile_t *pidProfile) {
@ -160,9 +162,9 @@ void pidInitConfig(const pidProfile_t *pidProfile) {
Kp[axis] = PTERM_SCALE * pidProfile->P8[axis];
Ki[axis] = ITERM_SCALE * pidProfile->I8[axis];
Kd[axis] = DTERM_SCALE * pidProfile->D8[axis];
c[axis] = pidProfile->dtermSetpointWeight / 100.0f;
relaxFactor[axis] = 1.0f / (pidProfile->setpointRelaxRatio / 100.0f);
}
dtermSetpointWeight = pidProfile->dtermSetpointWeight / 100.0f;
relaxFactor = 1.0f / (pidProfile->setpointRelaxRatio / 100.0f);
levelGain = pidProfile->P8[PIDLEVEL] / 10.0f;
horizonGain = pidProfile->I8[PIDLEVEL] / 10.0f;
horizonTransition = 100.0f / pidProfile->D8[PIDLEVEL];
@ -223,7 +225,7 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
const float motorMixRange = getMotorMixRange();
// Dynamic ki component to gradually scale back integration when above windup point
float dynKi = MIN((1.0f - motorMixRange) * ITermWindupPointInv, 1.0f);
const float dynKi = MIN((1.0f - motorMixRange) * ITermWindupPointInv, 1.0f);
// ----------PID controller----------
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
@ -240,7 +242,8 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
const float gyroRate = gyro.gyroADCf[axis]; // Process variable from gyro output in deg/sec
// --------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 can be tuned (amount derivative on measurement or error). ----------
// 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 error rate
const float errorRate = currentPidSetpoint - gyroRate; // r - y
@ -254,45 +257,53 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
// -----calculate I component
float ITerm = previousGyroIf[axis];
if (motorMixRange < 1.0f) {
// Only increase ITerm if motor output is not saturated and errorRate exceeds noise threshold
// Iterm will only be accelerated below steady rate threshold
if (ABS(currentPidSetpoint) < itermAcceleratorRateLimit)
dynKi *= itermAccelerator;
// Only increase ITerm if motor output is not saturated
float ITermDelta = Ki[axis] * errorRate * dT * dynKi;
if (ABS(currentPidSetpoint) < itermAcceleratorRateLimit) {
// ITerm will only be accelerated below steady rate threshold
ITermDelta *= itermAccelerator;
}
ITerm += ITermDelta;
previousGyroIf[axis] = ITerm;
}
// -----calculate D component (Yaw D not yet supported)
float DTerm = 0.0;
if (axis != FD_YAW) {
float dynC = c[axis];
// -----calculate D component
if (axis == FD_YAW) {
// no DTerm for yaw axis
// -----calculate total PID output
axisPIDf[FD_YAW] = PTerm + ITerm;
#ifdef BLACKBOX
axisPID_P[FD_YAW] = PTerm;
axisPID_I[FD_YAW] = ITerm;
axisPID_D[FD_YAW] = 0;
#endif
} else {
float dynC = dtermSetpointWeight;
if (pidProfile->setpointRelaxRatio < 100) {
dynC *= MIN(getRcDeflectionAbs(axis) * relaxFactor[axis], 1.0f);
dynC *= MIN(getRcDeflectionAbs(axis) * relaxFactor, 1.0f);
}
const float rD = dynC * currentPidSetpoint - gyroRate; // cr - y
// Divide rate change by dT to get differential (ie dr/dt)
const float delta = (rD - previousRateError[axis]) / dT;
previousRateError[axis] = rD;
DTerm = Kd[axis] * delta * tpaFactor;
float DTerm = Kd[axis] * delta * tpaFactor;
DEBUG_SET(DEBUG_DTERM_FILTER, axis, DTerm);
// apply filters
DTerm = dtermNotchFilterApplyFn(dtermFilterNotch[axis], DTerm);
DTerm = dtermLpfApplyFn(dtermFilterLpf[axis], DTerm);
}
// -----calculate total PID output
axisPIDf[axis] = PTerm + ITerm + DTerm;
// Disable PID control at zero throttle
if (!pidStabilisationEnabled) axisPIDf[axis] = 0;
#ifdef BLACKBOX
axisPID_P[axis] = PTerm;
axisPID_I[axis] = ITerm;
axisPID_D[axis] = DTerm;
#endif
}
// Disable PID control at zero throttle
if (!pidStabilisationEnabled) axisPIDf[axis] = 0;
}
}