230,6 → 230,10 |
int16_t cosroll = int_cos(angle[ROLL]); |
int16_t sinroll = int_sin(angle[ROLL]); |
int16_t tanpitch = int_tan(angle[PITCH]); |
|
int16_t test; |
|
|
#define ANTIOVF 512 |
ACRate[PITCH] = ((int32_t) rate_ATT[PITCH] * cosroll - (int32_t) yawRate |
* sinroll) / (int32_t) MATH_UNIT_FACTOR; |
236,6 → 240,13 |
ACRate[ROLL] = rate_ATT[ROLL] + (((int32_t) rate_ATT[PITCH] * sinroll |
/ ANTIOVF * tanpitch + (int32_t) yawRate * int_cos(angle[ROLL]) / ANTIOVF |
* tanpitch) / ((int32_t) MATH_UNIT_FACTOR / ANTIOVF * MATH_UNIT_FACTOR)); |
|
test = rate_ATT[ROLL] + |
(((rate_ATT[PITCH] * sinroll + yawRate * cosroll) >> 14) * tanpitch) >> 14; |
|
DebugOut.Analog[20] = ACRate[ROLL]; |
DebugOut.Analog[21] = test; |
|
ACYawRate = ((int32_t) rate_ATT[PITCH] * sinroll) / cospitch |
+ ((int32_t) yawRate * cosroll) / cospitch; |
} |