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