Rev 2041 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 2041 | Rev 2045 | ||
|---|---|---|---|
| Line 222... | Line 222... | ||
| 222 | * (from airframe-local axes to a ground-based system). For some reason |
222 | * (from airframe-local axes to a ground-based system). For some reason |
| 223 | * the MK uses a left-hand coordinate system. The tranformation has been |
223 | * the MK uses a left-hand coordinate system. The tranformation has been |
| 224 | * changed accordingly. |
224 | * changed accordingly. |
| 225 | */ |
225 | */ |
| 226 | void trigAxisCoupling(void) { |
226 | void trigAxisCoupling(void) { |
| - | 227 | int16_t rollAngleInDegrees = angle[ROLL] / GYRO_DEG_FACTOR_PITCHROLL; |
|
| - | 228 | int16_t pitchAngleInDegrees = angle[PITCH] / GYRO_DEG_FACTOR_PITCHROLL; |
|
| - | 229 | ||
| 227 | int16_t cospitch = int_cos(angle[PITCH]); |
230 | int16_t cospitch = cos_360(pitchAngleInDegrees); |
| 228 | int16_t cosroll = int_cos(angle[ROLL]); |
231 | int16_t cosroll = cos_360(rollAngleInDegrees); |
| 229 | int16_t sinroll = int_sin(angle[ROLL]); |
232 | int16_t sinroll = sin_360(rollAngleInDegrees); |
| Line 230... | Line 233... | ||
| 230 | 233 | ||
| 231 | ACRate[PITCH] = (((int32_t)rate_ATT[PITCH] * cosroll - (int32_t)yawRate |
234 | ACRate[PITCH] = (((int32_t)rate_ATT[PITCH] * cosroll - (int32_t)yawRate |
| Line 232... | Line 235... | ||
| 232 | * sinroll) >> MATH_UNIT_FACTOR_LOG); |
235 | * sinroll) >> MATH_UNIT_FACTOR_LOG); |
| 233 | 236 | ||
| 234 | ACRate[ROLL] = rate_ATT[ROLL] + (((((int32_t)rate_ATT[PITCH] * sinroll |
- | |
| Line 235... | Line 237... | ||
| 235 | + (int32_t)yawRate * cosroll) >> MATH_UNIT_FACTOR_LOG) * int_tan( |
237 | ACRate[ROLL] = rate_ATT[ROLL] + (((((int32_t)rate_ATT[PITCH] * sinroll |
| Line 236... | Line 238... | ||
| 236 | angle[PITCH])) >> MATH_UNIT_FACTOR_LOG); |
238 | + (int32_t)yawRate * cosroll) >> MATH_UNIT_FACTOR_LOG) * tan_360(pitchAngleInDegrees)) >> MATH_UNIT_FACTOR_LOG); |
| 237 | 239 | ||
| Line 393... | Line 395... | ||
| 393 | accVector = temp * temp; |
395 | accVector = temp * temp; |
| 394 | temp = filteredAcc[1] >> 3; |
396 | temp = filteredAcc[1] >> 3; |
| 395 | accVector += temp * temp; |
397 | accVector += temp * temp; |
| 396 | temp = filteredAcc[2] >> 3; |
398 | temp = filteredAcc[2] >> 3; |
| 397 | accVector += temp * temp; |
399 | accVector += temp * temp; |
| 398 | debugOut.analog[18] = accVector; |
400 | //debugOut.analog[18] = accVector; |
| 399 | } |
401 | } |
| Line 400... | Line 402... | ||
| 400 | 402 | ||
| 401 | /************************************************************************ |
403 | /************************************************************************ |
| 402 | * Main procedure. |
404 | * Main procedure. |