返回列表
|
1#
电梯直达
陀螺仪和加速度计矫正是如何实现的？（APingSci出品）
一、目的
获得准确的姿态角数据
二、情况
我们通过模拟I2C从MPU6050读取寄存器所获得的原始数据有两种不同程度的偏差：
1、刚上电的时候，陀螺仪的加速度计数值普遍过大，在等待一段时间后变化趋于正常。
2、当稳定下来后，并非是准确的数值，有着个位数范围内的偏差。
从DMP获得的欧拉角：
1、刚上电的时候，俯仰角，横滚角和偏航角的数值呈直线递增，三个角稳定在2，2，6左右的角度数据。
2、约30秒后，三个角分别为2，6，2
显然，这样的姿态角数据是无法进行UAV的姿态控制的，必须进行校正陀螺仪。
三、实现方法（姿态解算和获取原始数据暂不讲）
uint8_t MPU6050_OffSet(INT16_XYZ value,INT16_XYZ *offset,uint16_t sensivity)
{
static int32_t tempgx=0,tempgy=0,tempgz=0;
static uint16_t cnt_a=0;//使用static修饰的局部变量，表明次变量具有静态存储周期，也就是说该函数执行完后不释放内存
if(cnt_a==0)
{
value.X=0;
value.Y=0;
value.Z=0;
tempgx = 0;
tempgy = 0;
tempgz = 0;
cnt_a = 1;
sensivity = 0;
offset->X = 0;
offset->Y = 0;
offset->Z = 0;
}
tempgx += value.X;
tempgy += value.Y;
tempgz += value.Z-sensivity ;//加速度计校准 sensivity 等于 MPU6050初始化时设置的灵敏度值（8196LSB/g）;陀螺仪校准 sensivity = 0；
if(cnt_a==200) //200个数值求平均
{
offset->X=tempgx/cnt_a;
offset->Y=tempgy/cnt_a;
offset->Z=tempgz/cnt_a;
cnt_a = 0;
return 1;
}
cnt_a++;
return 0;
}
大体上就是累加，然后求平均值。上面的代码，首先定义了cnt_a变量，是为了累加计数，而且是静态的变量，当首次校准的时候将原始数据、偏移数据和灵敏度都清零，然后，在cnt_a累加计数到200次的时候，再将偏移的数值取平均。。
|
发表于2018-10-30 23:08:12
回复
收藏
举报
昵 称：
温馨提示：昵称只能设置一次，设置后无法修改。
只支持中文、英文和数字。
只支持中文、英文和数字。
顶部