int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value)
{
/*
* Use 0 as the throttle angle correction if we are inverted, vertical or with a
* small angle < 0.86 deg
* TODO: Define this small angle in config.
*/
if (rMat[2][2] <= 0.015f) {
return 0;
}
int angle = lrintf(acos_approx(rMat[2][2]) * throttleAngleScale);
if (angle > 900)
angle = 900;
return lrintf(throttle_correction_value * sin_approx(angle / (900.0f * M_PIf / 2.0f)));
}
STATIC_UNIT_TESTED void imuUpdateEulerAngles(void)
{
/* Compute pitch/roll angles */
attitude.values.roll = RADIANS_TO_DECIDEGREES(atan2_approx(rMat[2][1], rMat[2][2]));
attitude.values.pitch = RADIANS_TO_DECIDEGREES((0.5f * M_PIf) - acos_approx(-rMat[2][0]));
attitude.values.yaw = RADIANS_TO_DECIDEGREES(-atan2_approx(rMat[1][0], rMat[0][0])) + magneticDeclination;
if (attitude.values.yaw < 0)
attitude.values.yaw += 3600;
/* Update small angle state */
if (rMat[2][2] > smallAngleCosZ) {
ENABLE_STATE(SMALL_ANGLE);
} else {
DISABLE_STATE(SMALL_ANGLE);
}
}
STATIC_UNIT_TESTED void imuUpdateEulerAngles(void)
{
#ifndef GPS
// this local variable should be optimized out when GPS is not used.
float magneticDeclination = 0.0f;
#endif
/* Compute pitch/roll angles */
attitude.values.roll = lrintf(atan2_approx(rMat[2][1], rMat[2][2]) * (1800.0f / M_PIf));
attitude.values.pitch = lrintf(((0.5f * M_PIf) - acos_approx(-rMat[2][0])) * (1800.0f / M_PIf));
attitude.values.yaw = lrintf((-atan2_approx(rMat[1][0], rMat[0][0]) * (1800.0f / M_PIf) + magneticDeclination));
if (attitude.values.yaw < 0)
attitude.values.yaw += 3600;
/* Update small angle state */
if (rMat[2][2] > smallAngleCosZ) {
ENABLE_STATE(SMALL_ANGLE);
} else {
DISABLE_STATE(SMALL_ANGLE);
}
}
