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

Cleanup pidsum code

This commit is contained in:
borisbstyle 2017-03-31 16:37:10 +02:00
parent 839df10e8c
commit a5872041cc
3 changed files with 13 additions and 8 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,
-pidProfile->pidSumLimit, pidProfile->pidSumLimit);
scaledAxisPIDf[FD_YAW] =
constrainf((axisPID_P[FD_YAW] + axisPID_I[FD_YAW]) / PID_MIXER_SCALING,
constrainf((axisPID_P[FD_YAW] + axisPID_I[FD_YAW] + axisPID_D[FD_YAW]) / PID_MIXER_SCALING,
-pidProfile->pidSumLimit, pidProfile->pidSumLimitYaw);
// Calculate voltage compensation

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

@ -258,7 +258,12 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
}
// -----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;
if (pidProfile->setpointRelaxRatio < 100) {
dynC *= MIN(getRcDeflectionAbs(axis) * relaxFactor, 1.0f);
@ -276,11 +281,11 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
DTerm = dtermLpfApplyFn(dtermFilterLpf[axis], DTerm);
}
// Enable PID control only when stabilisation on
if (pidStabilisationEnabled) {
axisPID_P[axis] = PTerm;
axisPID_I[axis] = ITerm;
axisPID_D[axis] = DTerm;
// Disable PID control at zero throttle
if (!pidStabilisationEnabled) {
axisPID_P[axis] = 0;
axisPID_I[axis] = 0;
axisPID_D[axis] = 0;
}
}
}

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
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_YAW] = (axisPID_P[FD_YAW] + axisPID_I[FD_YAW]) * PID_SERVO_MIXER_SCALING;
input[INPUT_STABILIZED_YAW] = (axisPID_P[FD_YAW] + axisPID_I[FD_YAW] + axisPID_D[FD_PITCH]) * PID_SERVO_MIXER_SCALING;
// Reverse yaw servo when inverted in 3D mode
if (feature(FEATURE_3D) && (rcData[THROTTLE] < rxConfig->midrc)) {