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. |