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_); }