void EulerAnglesFromQuaternion(const float quat[4], float * phi, float * theta,
float * psi)
{
*phi = atan2(2.0 * (quat[0] * quat[1] + quat[2] * quat[3]), 1.0 - 2.0
* (quat[1] * quat[1] + quat[2] * quat[2]));
*theta = asin(2.0 * (quat[0] * quat[2] - quat[1] * quat[3]));
*psi = HeadingFromQuaternion(quat);
}
void UpdateAttitude(void)
{
if (!reset_attitude_)
{
UpdateQuaternion(quat_, AngularRateVector(), DT);
UpdateGravtiyInBody(quat_, g_b_);
CorrectQuaternionWithAccelerometer(quat_);
QuaternionNormalizingFilter(quat_);
}
else
{
HandleAttitudeReset();
}
UpdateGravtiyInBody(quat_, g_b_);
heading_angle_ = HeadingFromQuaternion(quat_);
}
