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

Cleanup code style in Harakiri PID controller merge.

This commit is contained in:
Dominic Clifton 2015-01-29 17:02:10 +01:00
parent 2c2ef14fa9
commit 104a263533
1 changed files with 29 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

43
src/main/flight/flight.c
View File

@ -530,7 +530,10 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
ACCDeltaTimeINS = FLOATcycleTime * 0.000001f; // ACCDeltaTimeINS is in seconds now
RCfactor = ACCDeltaTimeINS / (MainDptCut + ACCDeltaTimeINS); // used for pt1 element
if(FLIGHT_MODE(HORIZON_MODE)) prop = (float)MIN(MAX(ABS(rcCommand[PITCH]), ABS(rcCommand[ROLL])), 450) / 450.0f;
if (FLIGHT_MODE(HORIZON_MODE)) {
prop = (float)MIN(MAX(ABS(rcCommand[PITCH]), ABS(rcCommand[ROLL])), 450) / 450.0f;
}
for (axis = 0; axis < 2; axis++) {
rcCommandAxis = (float)rcCommand[axis]; // Calculate common values for pid controllers
if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
@ -551,31 +554,36 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
errorAngleI[axis] = constrain(errorAngleI[axis] + error * ACCDeltaTimeINS, -30.0f, +30.0f);
ITermACC = errorAngleI[axis] * (float)pidProfile->I8[PIDLEVEL] * 0.08f;
}
if (!FLIGHT_MODE(ANGLE_MODE)) {
if (ABS((int16_t)gyroData[axis]) > 2560) errorGyroI[axis] = 0.0f;
else {
if (ABS((int16_t)gyroData[axis]) > 2560) {
errorGyroI[axis] = 0.0f;
} else {
error = (rcCommandAxis * 320.0f / (float)pidProfile->P8[axis]) - gyroData[axis];
errorGyroI[axis] = constrain(errorGyroI[axis] + error * ACCDeltaTimeINS, -192.0f, +192.0f);
}
ITermGYRO = errorGyroI[axis] * (float)pidProfile->I8[axis] * 0.01f;
if (FLIGHT_MODE(HORIZON_MODE)) {
PTerm = PTermACC + prop * (rcCommandAxis - PTermACC);
ITerm = ITermACC + prop * (ITermGYRO - ITermACC);
}
else {
} else {
PTerm = rcCommandAxis;
ITerm = ITermGYRO;
}
}
else {
} else {
PTerm = PTermACC;
ITerm = ITermACC;
}
PTerm -= gyroData[axis] * dynP8[axis] * 0.003f;
delta = (gyroData[axis] - lastGyro[axis]) / ACCDeltaTimeINS;
lastGyro[axis] = gyroData[axis];
lastDTerm[axis] += RCfactor * (delta - lastDTerm[axis]);
DTerm = lastDTerm[axis] * dynD8[axis] * 0.00007f;
axisPID[axis] = lrintf(PTerm + ITerm - DTerm); // Round up result.
#ifdef BLACKBOX
@ -587,25 +595,32 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
tmp0flt = (int32_t)FLOATcycleTime & (int32_t)~3; // Filter last 2 bit jitter
tmp0flt /= 3000.0f;
if (OLD_YAW) { // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw. hardcoded for now
PTerm = ((int32_t)pidProfile->P8[FD_YAW] * (100 - (int32_t)controlRateConfig->yawRate * (int32_t)ABS(rcCommand[FD_YAW]) / 500)) / 100;
tmp0 = lrintf(gyroData[FD_YAW] * 0.25f);
PTerm = rcCommand[FD_YAW] - tmp0 * PTerm / 80;
if ((ABS(tmp0) > 640) || (ABS(rcCommand[FD_YAW]) > 100)) errorGyroI[FD_YAW] = 0;
else {
if ((ABS(tmp0) > 640) || (ABS(rcCommand[FD_YAW]) > 100)) {
errorGyroI[FD_YAW] = 0;
} else {
error = ((int32_t)rcCommand[FD_YAW] * 80 / (int32_t)pidProfile->P8[FD_YAW]) - tmp0;
errorGyroI[FD_YAW] = constrain(errorGyroI[FD_YAW] + (int32_t)(error * tmp0flt), -16000, +16000); // WindUp
ITerm = (errorGyroI[FD_YAW] / 125 * pidProfile->I8[FD_YAW]) >> 6;
}
}
else {
} else {
tmp0 = ((int32_t)rcCommand[FD_YAW] * (((int32_t)controlRateConfig->yawRate << 1) + 40)) >> 5;
error = tmp0 - lrintf(gyroData[FD_YAW] * 0.25f); // Less Gyrojitter works actually better
if (ABS(tmp0) > 50) errorGyroI[FD_YAW] = 0;
else errorGyroI[FD_YAW] = constrain(errorGyroI[FD_YAW] + (int32_t)(error * (float)pidProfile->I8[FD_YAW] * tmp0flt), -268435454, +268435454);
if (ABS(tmp0) > 50) {
errorGyroI[FD_YAW] = 0;
} else {
errorGyroI[FD_YAW] = constrain(errorGyroI[FD_YAW] + (int32_t)(error * (float)pidProfile->I8[FD_YAW] * tmp0flt), -268435454, +268435454);
}
ITerm = constrain(errorGyroI[FD_YAW] >> 13, -GYRO_I_MAX, +GYRO_I_MAX);
PTerm = ((int32_t)error * (int32_t)pidProfile->P8[FD_YAW]) >> 6;
if(motorCount > 3) { // Constrain FD_YAW by D value if not servo driven in that case servolimits apply
if (motorCount >= 4) { // Constrain FD_YAW by D value if not servo driven in that case servolimits apply
tmp0 = 300;
if (pidProfile->D8[FD_YAW]) tmp0 -= (int32_t)pidProfile->D8[FD_YAW];
PTerm = constrain(PTerm, -tmp0, tmp0);