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; |
} |
|
/************************************************************************ |