| 153,14 → 153,8 |
|---|
| int16_t yawTerm, throttleTerm, term[2]; |
| |
| // PID controller variables |
| int16_t PDPart[2], PDPartYaw, PPart[2]; |
| int16_t PPArt[2], DPart[2], PPartYaw, DPartYaw; |
| static int32_t IPart[2] = { 0, 0 }; |
| // static int32_t yawControlRate = 0; |
| |
| // Removed. Too complicated, and apparently not necessary with MEMS gyros anyway. |
| // static int32_t IntegralGyroPitchError = 0, IntegralGyroRollError = 0; |
| // static int32_t CorrectionPitch, CorrectionRoll; |
| |
| static uint16_t emergencyFlightTime; |
| static int8_t debugDataTimer = 1; |
| |
| 290,7 → 284,6 |
|---|
| /* and angular velocity (gyro signal) */ |
| /************************************************************************/ |
| // The P-part is the P of the PID controller. That's the angle integrals (not rates). |
| |
| for (axis = PITCH; axis <= ROLL; axis++) { |
| PPart[axis] = angle[axis] * gyroIFactor / (44000 / CONTROL_SCALING); // P-Part - Proportional to Integral |
| |
| 299,18 → 292,15 |
|---|
| * Read this as: PDPart = PPart + rate_PID * pfactor * CONTROL_SCALING |
| * where pfactor is in [0..1]. |
| */ |
| PDPart[axis] = PPart[axis] + (int32_t) ((int32_t) rate_PID[axis] * gyroPFactor / (256L / CONTROL_SCALING)) + (differential[axis] |
| * (int16_t) dynamicParams.gyroD) / 16; |
| |
| DPart[axis] = (int32_t) ((int32_t) rate_PID[axis] * gyroPFactor / (256L / CONTROL_SCALING)) + (differential[axis] * (int16_t) dynamicParams.gyroD) / 16; |
| CHECK_MIN_MAX(PDPart[axis], -SENSOR_LIMIT, SENSOR_LIMIT); |
| } |
| |
| PDPartYaw = (int32_t) (yawRate * 2 * (int32_t) yawPFactor) / (256L |
| / CONTROL_SCALING) + (int32_t) (yawAngleDiff * yawIFactor) / (2 * (44000 |
| / CONTROL_SCALING)); |
| PPartYaw = (int32_t) (yawAngleDiff * yawIFactor) / (2 * (44000 / CONTROL_SCALING)); |
| DPartYaw = (int32_t) (yawRate * 2 * (int32_t) yawPFactor) / (256L / CONTROL_SCALING); |
| |
| // limit control feedback |
| CHECK_MIN_MAX(PDPartYaw, -SENSOR_LIMIT, SENSOR_LIMIT); |
| // CHECK_MIN_MAX(PDPartYaw, -SENSOR_LIMIT, SENSOR_LIMIT); |
| |
| /* |
| * Compose throttle term. |