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); |
} |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |