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, 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]) / PID_MIXER_SCALING, constrainf((axisPID_P[FD_YAW] + axisPID_I[FD_YAW] + axisPID_D[FD_YAW]) / PID_MIXER_SCALING,
-pidProfile->pidSumLimit, pidProfile->pidSumLimitYaw); -pidProfile->pidSumLimit, pidProfile->pidSumLimitYaw);
// Calculate voltage compensation // 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 // -----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);
@ -276,11 +281,11 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
DTerm = dtermLpfApplyFn(dtermFilterLpf[axis], DTerm); DTerm = dtermLpfApplyFn(dtermFilterLpf[axis], DTerm);
} }
// Enable PID control only when stabilisation on // Disable PID control at zero throttle
if (pidStabilisationEnabled) { if (!pidStabilisationEnabled) {
axisPID_P[axis] = PTerm; axisPID_P[axis] = 0;
axisPID_I[axis] = ITerm; axisPID_I[axis] = 0;
axisPID_D[axis] = DTerm; 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 // 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]) * 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 // Reverse yaw servo when inverted in 3D mode
if (feature(FEATURE_3D) && (rcData[THROTTLE] < rxConfig->midrc)) { if (feature(FEATURE_3D) && (rcData[THROTTLE] < rxConfig->midrc)) {