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. |