Rev 1616 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1616 | Rev 1617 | ||
---|---|---|---|
Line 266... | Line 266... | ||
266 | int16_t cospitch = int_cos(pitchAngle); |
266 | int16_t cospitch = int_cos(pitchAngle); |
267 | int16_t cosroll = int_cos(rollAngle); |
267 | int16_t cosroll = int_cos(rollAngle); |
268 | int16_t sinroll = int_sin(rollAngle); |
268 | int16_t sinroll = int_sin(rollAngle); |
269 | int16_t tanpitch = int_tan(pitchAngle); |
269 | int16_t tanpitch = int_tan(pitchAngle); |
270 | #define ANTIOVF 1024 |
270 | #define ANTIOVF 1024 |
271 | ACPitchRate = ((int32_t)pitchRate * cosroll + (int32_t)yawRate * sinroll) / (int32_t)MATH_UNIT_FACTOR; |
271 | ACPitchRate = ((int32_t)pitchRate * cosroll - (int32_t)yawRate * sinroll) / (int32_t)MATH_UNIT_FACTOR; |
272 | ACRollRate = rollRate + (((int32_t)pitchRate * sinroll / ANTIOVF * tanpitch - (int32_t)yawRate * int_cos(rollAngle) / ANTIOVF * tanpitch) / ((int32_t)MATH_UNIT_FACTOR / ANTIOVF * MATH_UNIT_FACTOR)); |
272 | ACRollRate = rollRate + (((int32_t)pitchRate * sinroll / ANTIOVF * tanpitch + (int32_t)yawRate * int_cos(rollAngle) / ANTIOVF * tanpitch) / ((int32_t)MATH_UNIT_FACTOR / ANTIOVF * MATH_UNIT_FACTOR)); |
273 | ACYawRate = (-(int32_t)pitchRate * sinroll) / cospitch + ((int32_t)yawRate * cosroll) / cospitch; |
273 | ACYawRate = ((int32_t)pitchRate * sinroll) / cospitch + ((int32_t)yawRate * cosroll) / cospitch; |
274 | } |
274 | } |
Line 275... | Line 275... | ||
275 | 275 | ||
276 | void integrate(void) { |
276 | void integrate(void) { |
277 | // First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate. |
277 | // First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate. |
Line 303... | Line 303... | ||
303 | /* |
303 | /* |
304 | * Yaw |
304 | * Yaw |
305 | * Calculate yaw gyro integral (~ to rotation angle) |
305 | * Calculate yaw gyro integral (~ to rotation angle) |
306 | * Limit yawGyroHeading proportional to 0 deg to 360 deg |
306 | * Limit yawGyroHeading proportional to 0 deg to 360 deg |
307 | */ |
307 | */ |
- | 308 | ||
308 | yawGyroHeading += ACYawRate; |
309 | yawGyroHeading += ACYawRate; |
- | 310 | ||
- | 311 | // Why is yawAngle not wrapped 'round? |
|
309 | yawAngle += ACYawRate; |
312 | yawAngle += ACYawRate; |
- | 313 | ||
- | 314 | if(yawGyroHeading >= YAWOVER360) { |
|
310 | if(yawGyroHeading >= YAWOVER360) yawGyroHeading -= YAWOVER360; // 360 deg. wrap |
315 | yawGyroHeading -= YAWOVER360; // 360 deg. wrap |
- | 316 | } else if(yawGyroHeading < 0) { |
|
311 | else if(yawGyroHeading < 0) yawGyroHeading += YAWOVER360; |
317 | yawGyroHeading += YAWOVER360; |
- | 318 | } |
|
Line 312... | Line 319... | ||
312 | 319 | ||
313 | /* |
320 | /* |
314 | * Pitch axis integration and range boundary wrap. |
321 | * Pitch axis integration and range boundary wrap. |
315 | */ |
322 | */ |