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

Fix bug with unset PID's

This commit is contained in:
borisbstyle 2017-04-02 22:38:58 +02:00
parent 02b8204fb1
commit ca1ae28b8e
3 changed files with 11 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/main/flight/mixer.c
View File

@ -487,7 +487,7 @@ void mixTable(pidProfile_t *pidProfile)
constrainf((axisPID_P[FD_PITCH] + axisPID_I[FD_PITCH] + axisPID_D[FD_PITCH]) / PID_MIXER_SCALING, constrainf((axisPID_P[FD_PITCH] + axisPID_I[FD_PITCH] + axisPID_D[FD_PITCH]) / PID_MIXER_SCALING,
-pidProfile->pidSumLimit, pidProfile->pidSumLimit); -pidProfile->pidSumLimit, pidProfile->pidSumLimit);
scaledAxisPIDf[FD_YAW] = scaledAxisPIDf[FD_YAW] =
constrainf((axisPID_P[FD_YAW] + axisPID_I[FD_YAW] + axisPID_D[FD_YAW]) / PID_MIXER_SCALING, constrainf((axisPID_P[FD_YAW] + axisPID_I[FD_YAW]) / PID_MIXER_SCALING,
-pidProfile->pidSumLimit, pidProfile->pidSumLimitYaw); -pidProfile->pidSumLimit, pidProfile->pidSumLimitYaw);
// Calculate voltage compensation // Calculate voltage compensation

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

@ -47,8 +47,6 @@ static bool pidStabilisationEnabled;
float axisPID_P[3], axisPID_I[3], axisPID_D[3]; float axisPID_P[3], axisPID_I[3], axisPID_D[3];
static float previousGyroIf[3];
static float dT; static float dT;
void pidSetTargetLooptime(uint32_t pidLooptime) void pidSetTargetLooptime(uint32_t pidLooptime)
@ -60,7 +58,7 @@ void pidSetTargetLooptime(uint32_t pidLooptime)
void pidResetErrorGyroState(void) void pidResetErrorGyroState(void)
{ {
for (int axis = 0; axis < 3; axis++) { for (int axis = 0; axis < 3; axis++) {
previousGyroIf[axis] = 0.0f; axisPID_I[axis] = 0.0f;
} }
} }
@ -224,7 +222,6 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
// ----------PID controller---------- // ----------PID controller----------
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
float currentPidSetpoint = getSetpointRate(axis); float currentPidSetpoint = getSetpointRate(axis);
float PTerm = 0, ITerm = 0, DTerm = 0;
if(maxVelocity[axis]) if(maxVelocity[axis])
currentPidSetpoint = accelerationLimit(axis, currentPidSetpoint); currentPidSetpoint = accelerationLimit(axis, currentPidSetpoint);
@ -244,26 +241,19 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
const float errorRate = currentPidSetpoint - gyroRate; // r - y const float errorRate = currentPidSetpoint - gyroRate; // r - y
// -----calculate P component and add Dynamic Part based on stick input // -----calculate P component and add Dynamic Part based on stick input
PTerm = Kp[axis] * errorRate * tpaFactor; axisPID_P[axis] = Kp[axis] * errorRate * tpaFactor;
if (axis == FD_YAW) { if (axis == FD_YAW) {
PTerm = ptermYawFilterApplyFn(ptermYawFilter, PTerm); axisPID_P[axis] = ptermYawFilterApplyFn(ptermYawFilter, axisPID_P[axis]);
} }
// -----calculate I component // -----calculate I component
ITerm = previousGyroIf[axis];
if (motorMixRange < 1.0f) { if (motorMixRange < 1.0f) {
// Only increase ITerm if motor output is not saturated // Only increase ITerm if motor output is not saturated
ITerm += Ki[axis] * errorRate * dT * dynKi * itermAccelerator; axisPID_I[axis] += Ki[axis] * errorRate * dT * dynKi * itermAccelerator;
previousGyroIf[axis] = ITerm;
} }
// -----calculate D component // -----calculate D component
if (axis == FD_YAW) { if (axis != FD_YAW) {
// no DTerm for yaw axis
axisPID_P[FD_YAW] = PTerm;
axisPID_I[FD_YAW] = ITerm;
axisPID_D[FD_YAW] = 0;
} else {
float dynC = dtermSetpointWeight; float dynC = dtermSetpointWeight;
if (pidProfile->setpointRelaxRatio < 100) { if (pidProfile->setpointRelaxRatio < 100) {
dynC *= MIN(getRcDeflectionAbs(axis) * relaxFactor, 1.0f); dynC *= MIN(getRcDeflectionAbs(axis) * relaxFactor, 1.0f);
@ -273,12 +263,12 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
const float delta = (rD - previousRateError[axis]) / dT; const float delta = (rD - previousRateError[axis]) / dT;
previousRateError[axis] = rD; previousRateError[axis] = rD;
DTerm = Kd[axis] * delta * tpaFactor; axisPID_D[axis] = Kd[axis] * delta * tpaFactor;
DEBUG_SET(DEBUG_DTERM_FILTER, axis, DTerm); DEBUG_SET(DEBUG_DTERM_FILTER, axis, axisPID_D[axis]);
// apply filters // apply filters
DTerm = dtermNotchFilterApplyFn(dtermFilterNotch[axis], DTerm); axisPID_D[axis] = dtermNotchFilterApplyFn(dtermFilterNotch[axis], axisPID_D[axis]);
DTerm = dtermLpfApplyFn(dtermFilterLpf[axis], DTerm); axisPID_D[axis] = dtermLpfApplyFn(dtermFilterLpf[axis], axisPID_D[axis]);
} }
// Disable PID control at zero throttle // Disable PID control at zero throttle

2
src/main/flight/servos.c
View File

@ -345,7 +345,7 @@ STATIC_UNIT_TESTED void servoMixer(void)
// Assisted modes (gyro only or gyro+acc according to AUX configuration in Gui // Assisted modes (gyro only or gyro+acc according to AUX configuration in Gui
input[INPUT_STABILIZED_ROLL] = (axisPID_P[FD_ROLL] + axisPID_I[FD_ROLL] + axisPID_D[FD_ROLL]) * PID_SERVO_MIXER_SCALING; input[INPUT_STABILIZED_ROLL] = (axisPID_P[FD_ROLL] + axisPID_I[FD_ROLL] + axisPID_D[FD_ROLL]) * PID_SERVO_MIXER_SCALING;
input[INPUT_STABILIZED_PITCH] = (axisPID_P[FD_PITCH] + axisPID_I[FD_PITCH] + axisPID_D[FD_PITCH]) * PID_SERVO_MIXER_SCALING; input[INPUT_STABILIZED_PITCH] = (axisPID_P[FD_PITCH] + axisPID_I[FD_PITCH] + axisPID_D[FD_PITCH]) * PID_SERVO_MIXER_SCALING;
input[INPUT_STABILIZED_YAW] = (axisPID_P[FD_YAW] + axisPID_I[FD_YAW] + axisPID_D[FD_PITCH]) * PID_SERVO_MIXER_SCALING; input[INPUT_STABILIZED_YAW] = (axisPID_P[FD_YAW] + axisPID_I[FD_YAW]) * PID_SERVO_MIXER_SCALING;
// Reverse yaw servo when inverted in 3D mode // Reverse yaw servo when inverted in 3D mode
if (feature(FEATURE_3D) && (rcData[THROTTLE] < rxConfig->midrc)) { if (feature(FEATURE_3D) && (rcData[THROTTLE] < rxConfig->midrc)) {