| 55,10 → 55,6 |
|---|
| #include "flight.h" |
| #include "output.h" |
| |
| // Only for debug. Remove. |
| //#include "analog.h" |
| //#include "rc.h" |
| |
| // Necessary for external control and motor test |
| #include "uart0.h" |
| #include "twimaster.h" |
| 182,7 → 178,7 |
|---|
| /************************************************************************/ |
| |
| if (controlMixer_getSignalQuality() <= SIGNAL_BAD) { // the rc-frame signal is not reveived or noisy |
| beepRCAlarm(); |
| if (controlMixer_didReceiveSignal) beepRCAlarm(); |
| if (emergencyFlightTime) { |
| // continue emergency flight |
| emergencyFlightTime--; |
| 220,9 → 216,7 |
|---|
| */ |
| if (isFlying < 256) { |
| IPart[PITCH] = IPart[ROLL] = 0; |
| // TODO: Don't stomp on other modules' variables!!! |
| // controlYaw = 0; |
| PDPartYaw = 0; // instead. |
| PDPartYaw = 0; |
| if (isFlying == 250) { |
| // HC_setGround(); |
| updateCompassCourse = 1; |
| 285,13 → 279,8 |
|---|
| // The P-part is the P of the PID controller. That's the angle integrals (not rates). |
| for (axis = PITCH; axis <= ROLL; axis++) { |
| 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. |
| * Read this as: PDPart = PPart + rate_PID * pfactor * CONTROL_SCALING |
| * where pfactor is in [0..1]. |
| */ |
| PDPart[axis] += (int32_t) ((int32_t) rate_PID[axis] * gyroPFactor / (256L / CONTROL_SCALING)) + (differential[axis] * (int16_t) dynamicParams.gyroD) / 16; |
| PDPart[axis] += ((int32_t) rate_PID[axis] * gyroPFactor / (256L / CONTROL_SCALING)); |
| PDPart[axis] += (differential[axis] * (int16_t) dynamicParams.gyroD) / 16; |
| CHECK_MIN_MAX(PDPart[axis], -SENSOR_LIMIT, SENSOR_LIMIT); |
| } |
| |
| 334,7 → 323,6 |
|---|
| debugOut.digital[0] |= DEBUG_CLIP; |
| yawTerm = throttleTerm / 2; |
| } |
| //CHECK_MIN_MAX(yawTerm, - (throttleTerm / 2), (throttleTerm / 2)); |
| } else { |
| if (yawTerm < -MIN_YAWGAS / 2) { |
| debugOut.digital[0] |= DEBUG_CLIP; |
| 343,10 → 331,8 |
|---|
| debugOut.digital[0] |= DEBUG_CLIP; |
| yawTerm = MIN_YAWGAS / 2; |
| } |
| //CHECK_MIN_MAX(yawTerm, - (MIN_YAWGAS / 2), (MIN_YAWGAS / 2)); |
| } |
| |
| // FIXME: Throttle may exceed maxThrottle (there is no check no more). |
| tmp_int = staticParams.maxThrottle * CONTROL_SCALING; |
| if (yawTerm < -(tmp_int - throttleTerm)) { |
| yawTerm = -(tmp_int - throttleTerm); |
| 392,7 → 378,6 |
|---|
| } else if (term[axis] > tmp_int) { |
| debugOut.digital[1] |= DEBUG_CLIP; |
| } |
| CHECK_MIN_MAX(term[axis], -tmp_int, tmp_int); |
| } |
| |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |