一、 姿態角定義：
1. 滾轉角的定義:機體Z軸與過機體x軸的垂面的夾角。向右滾轉爲正，範圍【-180deg-180deg】。
2.俯仰角的定義:機體x軸與水平面的夾角。向上爲正，範圍【-90deg-90deg】。
二、根據重力加速度 計算姿態角的原理:
1.滾轉角計算，首先通過機體x軸與重力加速度的外積，計算出“過機體x軸的垂面”的法向量。然後通過計算內積的方法，求該法向量與機體z軸的夾角，這個夾角與滾轉角互餘。
2.俯仰角計算:通過計算內積的方法，求機體x軸與重力加速度的夾角，這個夾角與俯仰角互餘。
三、代碼實現：
本人親自測試，計算結果合格。
extern void makekam_gacc_to_att(float gravity_acc[3]/*輸入靜止狀態下的加速度*/,float *ret_roll/*計算結果*/,float *ret_pitch)；
/*通過內積法，求兩個向量的夾角*/
float makekam_vec_ang(const float vec_a[3],const float vec_b[3])/*return 0~180deg*/
{
float angle = 0.0f;
float mod_of_vec_a = 0.0f;
float mod_of_vec_b = 0.0f;
mod_of_vec_a = get_vector_mod(vec_a);
mod_of_vec_b = get_vector_mod(vec_b);
if((0.01f >= mod_of_vec_a) || (0.01f >= mod_of_vec_b)){
Serial.println("vect is zero,can not caculate angle");
}else{
float cos_value = (vec_a[0]*vec_b[0] + vec_a[1]*vec_b[1] + vec_a[2]*vec_b[2]) / (mod_of_vec_a * mod_of_vec_b);
if(cos_value > 1.0f){
cos_value=1.0f;}
if(cos_value < -1.0f){
cos_value=-1.0f;}
angle = acos(cos_value);
angle = angle * 180.0f / 3.1415f;
}
return angle;
}
/*通過外積法，求平面兩個非同向向量所在平面的法向量*/
inline void makekam_outer_product(const float vec1[3],const float vec2[3],float vecDst[3])
{
vecDst[0] = vec1[1]*vec2[2] - vec2[1]*vec1[2];/*y1z2-y2z1*/
vecDst[1] = vec2[0]*vec1[2] - vec1[0]*vec2[2];/*x2z1-x1z2*/
vecDst[2] = vec1[0]*vec2[1] - vec2[0]*vec1[1];/*x1y2-x2y1*/
}
/*根據重力加速度，計算姿態角*/
void makekam_gacc_to_att(float gravity_acc[3],float *ret_roll,float *ret_pitch)
{
float roll = 0.0f;
float pitch = 0.0f;
float flight_z_acc[3] = {0.0f,0.0f,10.0f};
float flight_x_acc[3] = {10.0f,0.0f,0.0f};
float plane_xog_normal_vec[3] ={0};
makekam_outer_product(flight_x_acc,gravity_acc,plane_xog_normal_vec);
roll = makekam_vec_ang(flight_z_acc,plane_xog_normal_vec) - 90.0f;
if(gravity_acc[2] > 0.0f){
if(roll < 0.0f){
roll = -180.0f - roll;
}else{
roll = 180 - roll;
}
}
pitch = 90.0f - makekam_vec_ang(flight_x_acc,gravity_acc);
*ret_roll = roll;
*ret_pitch = pitch;
}