Rev 1864 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1864 | Rev 1866 | ||
---|---|---|---|
Line 228... | Line 228... | ||
228 | void trigAxisCoupling(void) { |
228 | void trigAxisCoupling(void) { |
229 | int16_t cospitch = int_cos(angle[PITCH]); |
229 | int16_t cospitch = int_cos(angle[PITCH]); |
230 | int16_t cosroll = int_cos(angle[ROLL]); |
230 | int16_t cosroll = int_cos(angle[ROLL]); |
231 | int16_t sinroll = int_sin(angle[ROLL]); |
231 | int16_t sinroll = int_sin(angle[ROLL]); |
232 | int16_t tanpitch = int_tan(angle[PITCH]); |
232 | int16_t tanpitch = int_tan(angle[PITCH]); |
- | 233 | ||
- | 234 | int16_t test; |
|
- | 235 | ||
- | 236 | ||
233 | #define ANTIOVF 512 |
237 | #define ANTIOVF 512 |
234 | ACRate[PITCH] = ((int32_t) rate_ATT[PITCH] * cosroll - (int32_t) yawRate |
238 | ACRate[PITCH] = ((int32_t) rate_ATT[PITCH] * cosroll - (int32_t) yawRate |
235 | * sinroll) / (int32_t) MATH_UNIT_FACTOR; |
239 | * sinroll) / (int32_t) MATH_UNIT_FACTOR; |
236 | ACRate[ROLL] = rate_ATT[ROLL] + (((int32_t) rate_ATT[PITCH] * sinroll |
240 | ACRate[ROLL] = rate_ATT[ROLL] + (((int32_t) rate_ATT[PITCH] * sinroll |
237 | / ANTIOVF * tanpitch + (int32_t) yawRate * int_cos(angle[ROLL]) / ANTIOVF |
241 | / ANTIOVF * tanpitch + (int32_t) yawRate * int_cos(angle[ROLL]) / ANTIOVF |
238 | * tanpitch) / ((int32_t) MATH_UNIT_FACTOR / ANTIOVF * MATH_UNIT_FACTOR)); |
242 | * tanpitch) / ((int32_t) MATH_UNIT_FACTOR / ANTIOVF * MATH_UNIT_FACTOR)); |
- | 243 | ||
- | 244 | test = rate_ATT[ROLL] + |
|
- | 245 | (((rate_ATT[PITCH] * sinroll + yawRate * cosroll) >> 14) * tanpitch) >> 14; |
|
- | 246 | ||
- | 247 | DebugOut.Analog[20] = ACRate[ROLL]; |
|
- | 248 | DebugOut.Analog[21] = test; |
|
- | 249 | ||
239 | ACYawRate = ((int32_t) rate_ATT[PITCH] * sinroll) / cospitch |
250 | ACYawRate = ((int32_t) rate_ATT[PITCH] * sinroll) / cospitch |
240 | + ((int32_t) yawRate * cosroll) / cospitch; |
251 | + ((int32_t) yawRate * cosroll) / cospitch; |
241 | } |
252 | } |
Line 242... | Line 253... | ||
242 | 253 |