关于MPU6050计算竖直平面的物体倾角问题的记录

MPU6050的官方DMP是可以测量航向角的，但是它的航向角在没有磁力计的校正下会自增，只能加入一些滤波算法进行一些校正，比如卡尔曼滤波或者互补滤波等，但是这些都不能消除长时间的测量带来的角度自增的问题。而且测量竖直平面的物体倾角都不需要这么复杂的计算。我们只需要拿到加速度的值即可。
当MPU6050（拿的MPU9250做讲解）与水平面垂直放置时，如下图：

此时我们不需要去管Z轴，只需要留意X轴和Y轴的加速度即可。
初始状态是，加速度计读数是A_X =1g; A_Y=0; 如下图：

读出此时X、Y轴的加速度值，需要多测几组，用于去除飘移。比如测量500组，然后X、Y各拿出一个平均值，作为一个基准值。记为offset_x, offset_y。

此时，MPU6050开始基于Z轴进行旋转，与水平面形成一个夹角θ，如下图：

进行一个模拟图，如下：

此时，竖直方向X仍然是1g的加速度，所以将旋转后得到的X1和Y1进行一个矢量的叠加，反正切即可求出旋转角度θ的值，如下：

此时，即可通过反正切公式算出角度：
angle = atan2(ay1,ax1)
此时算出的角度是弧度制，若需转换为角度，则：
angle = atan2(ay1,ax1)*180 / PI
注：PI为圆周率

现在，便可开始计算竖直面物体的倾斜角度了，通过刚开始拿到的offset_x和offset_y的值，计算出静止时物体的最初角度，得到最初角度：
angle_offset = atan2(offset_y,offset_x) x 180 / PI
然后，就是当前倾斜角度的计算了，我们这时已经拿到了静止时的角度angle_offset，然后，开始读取移动后mpu6050的加速度值，为了消除一些外部影响，我们做个简单的滤波处理，定义n个数组，初始化的时候就给这n个数组分别赋与此时XY的加速度值，比如我设n=5，如下：

static int acc_x[5] = {0};
static int acc_y[5] = {0};
short ax,ay,az;
u8 i;

	for(i = 0; i < 5; i++){        //如果数组没有拿到5个值，就先拿满五个值 这儿都是初始化时才会直接拿五个
		if(acc_x[i] == 0){
			MPU_Get_Accelerometer(&ax,&ay,&az);
			acc_x[i] = ax;
			acc_y[i] = ay;
		}
	}

n的取值也会影响角度的读取，加大n值能使角度变化更细腻，减小n值会使角度变化反应更迅速，需自己去测试一个中间值。
然后，我们通过计算，拿到此时的角度值angle，注意，现在拿到的这个角度值不是物体当前的倾斜角度，物体当前的倾斜角度需要在进行计算一下。
我们通过读取此时Y轴的加速度，来判断物体往左偏还是右偏，如下：

if(ay >= 0){         //判断角度的正反（即往左还是往右偏），此代码中左偏为负，右偏为正
		angle = (angle - fabs(angle_offset));  //此时的值减去初始值，就是当前的倾斜角度
}

现在便能读取到角度，最后再加入防止360°反偏的处理，即可得到竖直平面的任意倾斜角度。

最后附上代码链接：https://download.csdn.net/download/cxieyunsky/12241408

--------------------------------------------------------------------------------------------------------------------------@曼珠沙华
---------------------------------------------------------------------------------------------------------------------------2020/3/11