Rev 2045 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 2045 | Rev 2047 | ||
---|---|---|---|
Line 230... | Line 230... | ||
230 | int16_t cospitch = cos_360(pitchAngleInDegrees); |
230 | int16_t cospitch = cos_360(pitchAngleInDegrees); |
231 | int16_t cosroll = cos_360(rollAngleInDegrees); |
231 | int16_t cosroll = cos_360(rollAngleInDegrees); |
232 | int16_t sinroll = sin_360(rollAngleInDegrees); |
232 | int16_t sinroll = sin_360(rollAngleInDegrees); |
Line 233... | Line 233... | ||
233 | 233 | ||
234 | ACRate[PITCH] = (((int32_t)rate_ATT[PITCH] * cosroll - (int32_t)yawRate |
234 | ACRate[PITCH] = (((int32_t)rate_ATT[PITCH] * cosroll - (int32_t)yawRate |
Line 235... | Line 235... | ||
235 | * sinroll) >> MATH_UNIT_FACTOR_LOG); |
235 | * sinroll) >> LOG_MATH_UNIT_FACTOR); |
236 | 236 | ||
Line 237... | Line 237... | ||
237 | ACRate[ROLL] = rate_ATT[ROLL] + (((((int32_t)rate_ATT[PITCH] * sinroll |
237 | ACRate[ROLL] = rate_ATT[ROLL] + (((((int32_t)rate_ATT[PITCH] * sinroll |
Line 238... | Line 238... | ||
238 | + (int32_t)yawRate * cosroll) >> MATH_UNIT_FACTOR_LOG) * tan_360(pitchAngleInDegrees)) >> MATH_UNIT_FACTOR_LOG); |
238 | + (int32_t)yawRate * cosroll) >> LOG_MATH_UNIT_FACTOR) * tan_360(pitchAngleInDegrees)) >> LOG_MATH_UNIT_FACTOR); |
239 | 239 |