216,7 → 216,6 |
// We are done reading variables from the analog module. |
// Interrupt-driven sensor reading may restart. |
analogDataReady = 0; |
J4HIGH; |
analog_start(); |
} |
|
239,6 → 238,8 |
angle[PITCH])) >> MATH_UNIT_FACTOR_LOG); |
|
ACYawRate = ((int32_t)rate_ATT[PITCH] * sinroll + (int32_t)yawRate * cosroll) / cospitch; |
|
ACYawRate = ((int32_t)rate_ATT[PITCH] * sinroll + (int32_t)yawRate * cosroll) / cospitch; |
} |
|
// 480 usec with axis coupling - almost no time without. |
245,6 → 246,7 |
void integrate(void) { |
// First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate. |
uint8_t axis; |
|
if (!looping && (staticParams.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE)) { |
trigAxisCoupling(); |
} else { |
357,19 → 359,26 |
void driftCorrection(void) { |
static int16_t timer = DRIFTCORRECTION_TIME; |
int16_t deltaCorrection; |
int16_t round; |
uint8_t axis; |
|
DebugOut.Analog[6] = (DRIFTCORRECTION_TIME + DRIFTCORRECTION_TIME/2) / DRIFTCORRECTION_TIME; |
DebugOut.Analog[7] = (-DRIFTCORRECTION_TIME + DRIFTCORRECTION_TIME/2) / DRIFTCORRECTION_TIME; |
|
if (!--timer) { |
timer = DRIFTCORRECTION_TIME; |
for (axis = PITCH; axis <= ROLL; axis++) { |
// Take the sum of corrections applied, add it to delta |
deltaCorrection = (correctionSum[axis] + DRIFTCORRECTION_TIME / 2) |
/ DRIFTCORRECTION_TIME; |
if (correctionSum[axis] >=0) |
round = DRIFTCORRECTION_TIME / 2; |
else |
round = -DRIFTCORRECTION_TIME / 2; |
deltaCorrection = (correctionSum[axis] + round) / DRIFTCORRECTION_TIME; |
// Add the delta to the compensation. So positive delta means, gyro should have higher value. |
driftComp[axis] += deltaCorrection / staticParams.GyroAccTrim; |
CHECK_MIN_MAX(driftComp[axis], -staticParams.DriftComp, staticParams.DriftComp); |
// DebugOut.Analog[11 + axis] = correctionSum[axis]; |
DebugOut.Analog[16 + axis] = correctionSum[axis]; |
DebugOut.Analog[18 + axis] = deltaCorrection / staticParams.GyroAccTrim; |
DebugOut.Analog[28 + axis] = driftComp[axis]; |
|
correctionSum[axis] = 0; |
381,18 → 390,9 |
* Main procedure. |
************************************************************************/ |
void calculateFlightAttitude(void) { |
// part1: 550 usec. |
// part1a: 550 usec. |
// part1b: 60 usec. |
getAnalogData(); |
// end part1b |
integrate(); |
// end part1a |
|
DebugOut.Analog[6] = stronglyFilteredAcc[PITCH]; |
DebugOut.Analog[7] = stronglyFilteredAcc[ROLL]; |
DebugOut.Analog[8] = stronglyFilteredAcc[Z]; |
|
DebugOut.Analog[3] = rate_PID[PITCH]; |
DebugOut.Analog[4] = rate_PID[ROLL]; |
DebugOut.Analog[5] = yawRate; |
401,7 → 401,6 |
correctIntegralsByAcc0thOrder(); |
driftCorrection(); |
#endif |
// end part1 |
} |
|
void updateCompass(void) { |