| 153,7 → 153,7 |
|---|
| int16_t yawTerm, throttleTerm, term[2]; |
| |
| // PID controller variables |
| int16_t PPArt[2], DPart[2], PPartYaw, DPartYaw; |
| int16_t PDPart[2],/* DPart[2],*/ PDPartYaw /*, DPartYaw */; |
| static int32_t IPart[2] = { 0, 0 }; |
| static uint16_t emergencyFlightTime; |
| static int8_t debugDataTimer = 1; |
| 285,7 → 285,7 |
|---|
| /************************************************************************/ |
| // 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 |
| PDPart[axis] = angle[axis] * gyroIFactor / (44000 / CONTROL_SCALING); // P-Part - Proportional to Integral |
| |
| /* |
| * Now blend in the D-part - proportional to the Differential of the integral = the rate. |
| 292,12 → 292,12 |
|---|
| * Read this as: PDPart = PPart + rate_PID * pfactor * CONTROL_SCALING |
| * where pfactor is in [0..1]. |
| */ |
| DPart[axis] = (int32_t) ((int32_t) rate_PID[axis] * gyroPFactor / (256L / CONTROL_SCALING)) + (differential[axis] * (int16_t) dynamicParams.gyroD) / 16; |
| PDPart[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); |
| } |
| |
| PPartYaw = (int32_t) (yawAngleDiff * yawIFactor) / (2 * (44000 / CONTROL_SCALING)); |
| DPartYaw = (int32_t) (yawRate * 2 * (int32_t) yawPFactor) / (256L / CONTROL_SCALING); |
| PDPartYaw = (int32_t) (yawAngleDiff * yawIFactor) / (2 * (44000 / CONTROL_SCALING)); |
| PDPartYaw += (int32_t) (yawRate * 2 * (int32_t) yawPFactor) / (256L / CONTROL_SCALING); |
| |
| // limit control feedback |
| // CHECK_MIN_MAX(PDPartYaw, -SENSOR_LIMIT, SENSOR_LIMIT); |
| 367,7 → 367,7 |
|---|
| // Integration mode: Integrate (angle - stick) = the difference between angle and stick pos. |
| // That means: Holding the stick a little forward will, at constant flight attitude, cause this to grow (decline??) over time. |
| // TODO: Find out why this seems to be proportional to stick position - not integrating it at all. |
| IPart[axis] += PPart[axis] - controls[axis]; // Integrate difference between P part (the angle) and the stick pos. |
| IPart[axis] += PDPart[axis] - controls[axis]; // Integrate difference between P part (the angle) and the stick pos. |
| } else { |
| // "HH" mode: Integrate (rate - stick) = the difference between rotation rate and stick pos. |
| // To keep up with a full stick PDPart should be about 156... |