四轴飞行器角度换算

四轴飞行器角度换算 mpu6050角度换算 (原创:这个东西我在网上找了很久，都没有直接说明其原理的 如果觉得我是小白，勿喷，直接忽视即可，这其中也有我个人一些鄙薄的理解。如有错误欢迎指正，有交流才有进步。 其实原理很简单，但是对刚开始接触6050的人来说，了解其原理应该会比直接移植代码理解更深刻吧。 大家可以直接把6050的加速度计部分的检测元件想象成一个方盒子，方盒子里面放着个小球，小球因重力作用会在盒子的各个面上产生垂直于作用面的力， 各个力矢量合成就是和重力相平衡的力了(不考虑除重力加速度以外的加速度) 上图是一个手绘的，水平不咋样别见怪。上图是6050绕Y轴旋转一定角度产生的力的合成图，绕一个轴旋转哈理解一些。 gx = g重*cosΦ2; gz = g重*cosΦ1; 同理gy = g重*cosΦ3;(该图上Φ3 = 90?) gx就是X轴输出的数值除以最小分辨率。如?2g时，最小分辨率是2^16/4 =16384 LSB/g;(资料上是16384LSB/mg当时我怎么看都不能明白，这里就提醒下，如果都懂就当我没说) 很多人以为都以为角度是加速度读出来的值除以180?-0?的线性关系，其实用在飞思卡尔平衡车上的线性关系只是相对一个很小角度变换范围里的线性，在0-360的范围里肯定是不适用的， 只有根据反余切变换才能得到他的真是角度。 说明一下，如果是用在飞控上，如果某个方向上出现了除重力加速度分量以外的加速度时，单单加速度读出的值是比该角度对应的加速度值大的。 附上我根据飞思卡尔官方出的陀螺仪和加速度计融合而写的程序，自己写的，如有错误欢迎指正 void Offset_alignment(void) { static s32 A_Temp_X,A_Temp_Y,A_Temp_Z,G_Temp_X,G_Temp_Y,G_Temp_Z; static u8 Cnt=0; if(Calibration_Flag != 1) { A_Temp_X += (s32)X_Angle; A_Temp_Y += (s32)Y_Angle; A_Temp_Z += (s32)Z_Angle; G_Temp_X += (s32)X_Angle_acceleration; G_Temp_Y += (s32)Y_Angle_acceleration; G_Temp_Z += (s32)Z_Angle_acceleration; Cnt++; } if(Cnt == 200) { A_Offset_X = A_Temp_X/Cnt; A_Offset_Y = A_Temp_Y/Cnt; A_Offset_Z = A_Temp_Z/Cnt; G_Offset_X = G_Temp_X/Cnt; G_Offset_Y = G_Temp_Y/Cnt; G_Offset_Z = G_Temp_Z/Cnt; Cnt = 0; Calibration_Flag = 1; } } void Data_Fusion(float P_Component) { float X_Angle_Feedback,Y_Angle_Feedback,Z_Angle_Feedback; X_Angle_Feedback = (X_Angle - A_Offset_X - X_Current_A)*P_Component; X_Current_A += (Y_Angle_acceleration - G_Offset_Y+X_Angle_Feedback)*INTEGRAL_TIME; Y_Angle_Feedback = (Y_Angle - A_Offset_Y - Y_Current_A)*P_Component; Y_Current_A += (Y_Angle_acceleration - G_Offset_Y+Y_Angle_Feedback)*INTEGRAL_TIME; Z_Angle_Feedback = (Z_Angle - A_Offset_Z - Z_Current_A)*P_Component; Z_Current_A += (Z_Angle_acceleration - G_Offset_Z+Z_Angle_Feedback)*INTEGRAL_TIME; }