| 204,10 → 204,8 |
|---|
| uint8_t axis; |
| |
| for (axis = PITCH; axis <= ROLL; axis++) { |
| rate_PID[axis] = gyro_PID[axis] / HIRES_GYRO_INTEGRATION_FACTOR |
| + driftComp[axis]; |
| rate_ATT[axis] = gyro_ATT[axis] / HIRES_GYRO_INTEGRATION_FACTOR |
| + driftComp[axis]; |
| rate_PID[axis] = gyro_PID[axis] / HIRES_GYRO_INTEGRATION_FACTOR + driftComp[axis]; |
| rate_ATT[axis] = gyro_ATT[axis] / HIRES_GYRO_INTEGRATION_FACTOR + driftComp[axis]; |
| differential[axis] = gyroD[axis]; |
| averageAcc[axis] += acc[axis]; |
| } |
| 218,6 → 216,7 |
|---|
| // We are done reading variables from the analog module. |
| // Interrupt-driven sensor reading may restart. |
| analogDataReady = 0; |
| J4HIGH; |
| analog_start(); |
| } |
| |
| 228,20 → 227,18 |
|---|
| * changed accordingly. |
| */ |
| void trigAxisCoupling(void) { |
| J5HIGH; |
| int16_t cospitch = int_cos(angle[PITCH]); |
| int16_t cosroll = int_cos(angle[ROLL]); |
| int16_t sinroll = int_sin(angle[ROLL]); |
| |
| ACRate[PITCH] = (((int32_t) rate_ATT[PITCH] * cosroll - (int32_t) yawRate |
| ACRate[PITCH] = (((int32_t)rate_ATT[PITCH] * cosroll - (int32_t)yawRate |
| * sinroll) >> MATH_UNIT_FACTOR_LOG); |
| |
| ACRate[ROLL] = rate_ATT[ROLL] + (((((int32_t) rate_ATT[PITCH] * sinroll |
| + (int32_t) yawRate * cosroll) >> MATH_UNIT_FACTOR_LOG) * int_tan( |
| ACRate[ROLL] = rate_ATT[ROLL] + (((((int32_t)rate_ATT[PITCH] * sinroll |
| + (int32_t)yawRate * cosroll) >> MATH_UNIT_FACTOR_LOG) * int_tan( |
| angle[PITCH])) >> MATH_UNIT_FACTOR_LOG); |
| |
| ACYawRate = ((int32_t) rate_ATT[PITCH] * sinroll) / cospitch |
| + ((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. |
| 249,7 → 246,6 |
|---|
| // First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate. |
| uint8_t axis; |
| if (!looping && (staticParams.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE)) { |
| // The rotary rate limiter bit is abused for selecting axis coupling algorithm instead. |
| trigAxisCoupling(); |
| } else { |
| ACRate[PITCH] = rate_ATT[PITCH]; |
| 282,7 → 278,6 |
|---|
| angle[axis] += PITCHROLLOVER360; |
| } |
| } |
| J5LOW; |
| } |
| |
| /************************************************************************ |