| 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) { |