diff --git a/AstroLib/Src/AsAttitudeParam_2023.cpp b/AstroLib/Src/AsAttitudeParam_2023.cpp index 58f715ba809dfab3523443a232cec2a4f17ddcb5..a2fbff06af5945887cdee10c49baf97e6d994161 100644 --- a/AstroLib/Src/AsAttitudeParam_2023.cpp +++ b/AstroLib/Src/AsAttitudeParam_2023.cpp @@ -158,6 +158,80 @@ bool AsQuatToEuler123(const CQuaternion& q, CEuler& euler) return true; } +/*********************************************************************** +将四元数转化成欧拉角_132转序 + @Author Xiong yanci + @Date 2025.02.27 + @Input + @Param q 标准化四元数 + @Output + @Param euler 欧拉角(rad) +***********************************************************************/ +bool AsQuatToEuler132(const CQuaternion& q, CEuler& euler) +{ + ////设置默认参数 + double epsilon = 1e-6; //精确度 + euler = { 0, 0, 0 }; + + double q0, q1, q2, q3; + double q0q1, q0q2, q0q3, q1q3, q1q2, q2q3; + double q0q0, q1q1, q2q2, q3q3; + double q_norm1,q_norm2; + double norm_sq; + + q0 = q.m_Qs; + q1 = q.m_Qx; + q2 = q.m_Qy; + q3 = q.m_Qz; + + q0q1 = q0 * q1; + q0q2 = q0 * q2; + q0q3 = q0 * q3; + q1q2 = q1 * q2; + q1q3 = q1 * q3; + q2q3 = q2 * q3; + q0q0 = q0 * q0; + q1q1 = q1 * q1; + q2q2 = q2 * q2; + q3q3 = q3 * q3; + + q_norm1 = q0q0 + q1q1 - q2q2 - q3q3; + q_norm2 = q0q0 - q1q1 + q2q2 - q3q3; + norm_sq = q0q0 + q1q1 + q2q2 + q3q3; + double t = 2 * q0q3 - 2 * q1q2; + + if(fabs(norm_sq - 1) > epsilon)//判断输入的四元数是否标准化 + { + cout << "请输入标准化四元数!!!" << endl; + return false;//自动归一化,输出000值 + } + else + { + // //判断是否出现奇异,当出现奇异时,令roll = Angle1 为 0,确保程序可以继续运行 + //yaw + if (t > 1) + { + euler.m_Angle3 = AsCHalfPI; + euler.m_Angle1 = 2 * atan(q1/q0); + } + + else if (t < -1) + { + euler.m_Angle3 = -AsCHalfPI; + euler.m_Angle1 = 2 * atan(q1 / q0); + } + + else + { + //XZY四元数转欧拉角 + euler.m_Angle2 = atan2((2 * q0q2 + 2 * q1q3),q_norm1); //roll + euler.m_Angle1 = atan2((2 * q0q1 + 2 * q2q3),q_norm2); //pitch + euler.m_Angle3 = asin(t); //yaw + } + + } + return true; +} ///*********************************************************************** ///将四元数转化成欧拉角_213转序