使用LSM6DSO加速度计检测倾角

1、前言

LSM6DSO实际上是六轴传感器，本文只使用到了其中的加速度计，关于LSM6DSO的基础应用可参考ST六轴传感器LSM6DSO使用说明。

2、理论依据

参考链接
（1）如果芯片水平静置，X、Y方向的重力分量为0g，而Z轴方向的重力分量为g。如下图所示，X=0,Y=0, Z=g。

（2）如果各边与水平方向有一些夹角，如下图所示：
X轴方向的加速度大小为Ax，其与水平线的夹角为α1，与重力加速度的夹角α；
Y轴方向的加速度为Ay，与水平线的加速度为 β1，与重力加速度g的夹角为β；
Z轴方向的加速度为Az，与水平线的加速度为 γ1，与重力加速度g的夹角为γ。

它们的关系为：
α = 90度- α1,
β = 90度- β1,
γ = 90度- γ1;
那么g在各轴方向上的分量为：
Ax = gcosα,
Ay = gcosβ,
Az = gcosγ;
通过计算得到：
Ax = gcosα = gcos(90度- α1) = gsinα1,
Ay = gsinβ1,
Az = gsinγ1;

其中各垂直虚线的大小为：
g*g = Ax*Ax + gcosα1*gcosα1， 则g在各轴方向上的分量为：
gcosα1 = sqrt(g*g - Ax*Ax),
gcosβ1 = sqrt(g*g - Ay*Ay),
gcosγ1 = sqrt(g*g - Az*Az)。

根据立体几何中，g相当于立方体的对角线，Ax、Ay、Az相当于三条边，如图上图所示，虚线大小等于 Ay*Ay+Az*Az，所以根据勾股定理Ax*Ax + Ay*Ay + Az*Az = g*g。
根据以上可以得出，(以X轴为例) ：
sinα1 = Ax/g， cosα1 = sqrt(g*g - Ax*Ax) / g ，
那么，tanα1 =(Ax/g) / [sqrt(g*g - Ax*Ax) / g] ，
= Ax / sqrt(g*g - Ax*Ax)，
= Ax / sqrt(Ay*Ay + Az*Az) ，
同理推出 tanβ1、tanγ1：
tanβ1 = Ay /sqrt(Ax*Ax+Az*Az) ,
tanγ1 = Az /sqrt(Ax*Ax+Ay*Ay)。

即加速度传感器值与角速度值(弧度)的关系为：
tanα1 = Ax / sqrt(Ay*Ay + Az*Az) ，
tanβ1 = Ay / sqrt(Ax*Ax + Az*Az) ，
tanγ1 = Az / sqrt(Ax*Ax + Ay*Ay)。

其中 α1 、β1 、γ1分别是X、Y、Z轴和水平线的角速度值即弧度值(并不是我们所说的角度值)，Ax 、Ay、Az是三个轴上的加速度值。

那么弧度值分别为：
α1=atan(Ax/sqrt(Ay*Ay + Az*Az))，
β1=atan(Ay/sqrt(Ax*Ax+Az*Az))，
γ1=atan(Az/sqrt(Ax*Ax +Ay*Ay))，
接下来就得使用数据公式：弧度= θπR/180 。 这样算得θ = 弧度*180/πR,其中R取1。最后得到的各轴的角度值分别为：
θx=α1*180/π=[atan(Ax/sqrt(Ay*Ay+Az*Az))]*180/π，
θy=β1*180/π=[atan(Ay/sqrt(Ax*Ax+Az*Az))]*180/π，
θz=γ1*180/π=[atan(Az/sqrt(Ax*Ax+Ay*Ay))]*180/π 。

注：上述公式中的atan、sqrt均是c语言数学库函数math.h中的函数。

3、应用

uint8_t Anglecheck(void)
{
	static uint32_t curtick = 0;	
	float acc_x,acc_y,acc_z;
	float angle_x,angle_y,angle_z;
	uint8_t  temp;
	
	if(SystemGetTick()-curtick>=100)
	{
		LSM6DSOGetAcc(&acc_x,&acc_y,&acc_z);		
		angle_x=(atan(acc_x/(sqrt(acc_y*acc_y+acc_z*acc_z))))*180/3.14;
		angle_y=(atan(acc_y/(sqrt(acc_x*acc_x+acc_z*acc_z))))*180/3.14;
		angle_z=(atan(acc_z/(sqrt(acc_x*acc_x+acc_y*acc_y))))*180/3.14;		
	}
    temp=90-(uint8_t )angle_z;
	return temp;
}

LSM6DSOGetAcc函数用于获取原始数据值，100ms读一次传感器，在本文的应用中，通过计算z轴的角度，判断物体的倾斜度，如果要更精确的姿态计算，可以加入x，y的角度。