| 268,9 → 268,9 |
|---|
| int16_t sinroll = int_sin(rollAngle); |
| int16_t tanpitch = int_tan(pitchAngle); |
| #define ANTIOVF 1024 |
| ACPitchRate = ((int32_t)pitchRate * cosroll + (int32_t)yawRate * sinroll) / (int32_t)MATH_UNIT_FACTOR; |
| 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)); |
| ACYawRate = (-(int32_t)pitchRate * sinroll) / cospitch + ((int32_t)yawRate * cosroll) / cospitch; |
| ACPitchRate = ((int32_t)pitchRate * cosroll - (int32_t)yawRate * sinroll) / (int32_t)MATH_UNIT_FACTOR; |
| 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)); |
| ACYawRate = ((int32_t)pitchRate * sinroll) / cospitch + ((int32_t)yawRate * cosroll) / cospitch; |
| } |
| |
| void integrate(void) { |
| 305,10 → 305,17 |
|---|
| * Calculate yaw gyro integral (~ to rotation angle) |
| * Limit yawGyroHeading proportional to 0 deg to 360 deg |
| */ |
| |
| yawGyroHeading += ACYawRate; |
| |
| // Why is yawAngle not wrapped 'round? |
| yawAngle += ACYawRate; |
| if(yawGyroHeading >= YAWOVER360) yawGyroHeading -= YAWOVER360; // 360 deg. wrap |
| else if(yawGyroHeading < 0) yawGyroHeading += YAWOVER360; |
| |
| if(yawGyroHeading >= YAWOVER360) { |
| yawGyroHeading -= YAWOVER360; // 360 deg. wrap |
| } else if(yawGyroHeading < 0) { |
| yawGyroHeading += YAWOVER360; |
| } |
| |
| /* |
| * Pitch axis integration and range boundary wrap. |