Rev 1616 | Go to most recent revision | 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 | */ |