2019年3月5日-MPU9250使用（二）

感觉MPU6050的DMP算的不准，想YAW只与Z轴的角速度“gyroz”有关，只有一个参数的话，自己写个公式计算一下应该更准确；

尝试自己写，首先

MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);    //得到陀螺仪数据

然后，记录两次计算的时间间隔

TIM3_Init(5000-1,9000-1);       //定时器3初始化，(定时器3挂在APB1上，时钟为HCLK/2)，HCLK = 180MHz，定时器时钟为90M，分频系数为9000-1，所以定时器3的频率为90M/9000=10K，也就是1s计数为10K。
自动重装载为5000-1，那么定时器周期就是500ms（这个不重要）

最后就是计算，角速度的积分就是角度了

ys1_yaw = = ys1_yaw + gyroz/63.5*(TIM3->CNT)；
//63.5因为我设置的满量程范围为正负500°
if(ys1_yaw > 180)
{
	ys1_yaw = ys1_yaw-360;
}
else if(ys1_yaw < -180)
{
	ys1_yaw = ys1_yaw+360;
}
//将角度限制在正负180°

然后，失败了。

角度一直在转圈圈，分析数据得知，gyroz有初始值；添加初始误差修正；

if(initial_gyroz_count < 1000 && initial_gyroz_count != 0)
{
	initial_gyroz_sum = initial_gyroz_sum + gyroz;
	initial_gyroz = initial_gyroz_sum / 999;
	initial_gyroz_count++;
}//采样999次，求平均值

//单位时间yaw变化值---gyroz_temp
gyroz_temp = (gyroz-initial_gyroz)/63.5*(TIM3->CNT)；

的确解决了初始角度一直跑的情况，测试了一下，发现还不如DMP算的准。。。

分析可能时有测量数据有噪音，在网上学习了一段窗口平滑滤波；

#define N 6	//6窗口平均滤波
short filter_value_buff[N];                     //N相当于选定一个窗口大小，对窗口数据做平均！
char filter_i=0;

short filter(short data)
{
	char count;
    int sum=0;
    filter_value_buff[filter_i++]=data;
    if(filter_i==N)
        filter_i=0;              //当数据大于数组长度，替换数据组的一个数据，相当于环形队列更新，先进先出！
	for(count=0;countCNT;

结果发现还是不准。

分析原因有两方面，（一）单靠陀螺仪可能就是测量的不是特别准确；（二）MPU放置不是绝对的水平，在旋转的过程中，Z轴与重力方向存在一定程度上的偏移，而且由于不是放在桌面上，偏移不是固定的；需要时时把这个偏移计算在内。