来自 技术 2019-04-17 00:00 的文章

LIS3DH三轴加速度计-实现欧拉角(俯仰角,横滚角)

1. LIS3DH管脚定义

PS:LIS3DH和mpu6050的X和Y方向是相反的, mpu6050如下图所示:

2.LIS3DH加速度计介绍

由于LIS3DH只可以得到XYZ加速度,无法获取角速度,所以LIS3DH是无法测出偏航角(yaw).

3. LIS3DH之转换欧拉角介绍

3.1偏航角(yaw)

如下图所示,偏航角是指机头在水平面上的投影与地轴之间的夹角,以机头右偏为正.范围为[-180,180]

PS:由于LIS3DH无法测出,跳过.

3.2俯仰角(pitch)

如下图所示, 俯仰角是指机头与水平面的夹角,当飞机平行时则为0,抬头时则为正,范围为[-180,180]

以45°为例,来解析LIS3DH

如下图所示:

转换为角度图为:

所以转换为角度为:

pitch = (short)(atan2((float)(0-y),z) * 180 / 3.14159); //转换为度数

3.3 横滚角(roll)

如下图所示, 横滚角指飞机两翼所在的平面与平行线之间的夹角,机体向右滚为正,范围为[-180,180].

横滚角和俯仰角类似,不过变成了X与Z轴之间比例了.

所以转换为角度为:

pitch = (short)(atan2((float)(0-y),z) * 180 / 3.14159); //转换为度数

4.初始化代码

unsigned char Lis3dh_init(void){ u8 val; unsigned char data; i2cInit(); data = 0x47; if(i2cwrite(0x18, 0x20, 1, &data))return 1; //50HZ reg1 data = 0; if(i2cwrite(0x18, 0x21, 1, &data))return 2; // reg2 data = 0x00; if(i2cwrite(0x18, 0x22, 1, &data))return 3; // reg3 data = 0X00; if(i2cwrite(0x18, 0x23, 1, &data))return 4; // +-2g reg4 data = 0x00; if(i2cwrite(0x18, 0x24, 1, &data))return 5; // reg5 return 0;}

设置为+2g量程,并且传感器刷新率为50hz,也就是说我们延时20ms左右去读一次传感器即可.

5.运行代码

void CalcXYZ(short x,short y,short z){ short pitch ,roll; u16 maxG=0; pitch = (short)(atan2((float)(0-y),z) * 180 / 3.14159); //转换为度数 roll = (short)(atan2((float)(x),z) * 180 / 3.14159); //转换为度数 printf("x%03dmg y%03dmg z%03dmg pitch:%3d roll:%3d \r\n",x,y,z,pitch,roll);}int main(){ u8 i; u8 buf[6]; short X,Y,Z; printf("Gsensor_init%d\r\n",Lis3dh_init()); while(1) { i2cread(0x18,0x27, 1,&i); //读取0x27,判断是否有数据 if((i&0x08)) { for(i=0;i<6;i++) i2cread(0x18,0X28+i, 1,&buf[i]); X = buf[1]*256+ buf[0]; Y = buf[3]*256 + buf[2]; Z = buf[5]*256 + buf[4]; X=(short)((float)(X)*4/65536*1000); Y=(short)((float)(Y)*4/65536*1000); Z=(short)((float)(Z)*4/65536*1000); CalcXYZ(X,Y,Z); } }}

6.打印截图

6.1当俯仰角pitch接近90°时

如上图可以看到,y是负的1000mg,这是因为它的y方向向下,所以计算俯仰角时,我们用的(0-y).

6.1当俯仰角pitch为0,翻滚角为45°时

如上图可以看到X值为707左右,由于物体自重力为1000,所以707/1000,刚好对应sin45°(0.707)