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

Posted 2021-02-16 lifexy

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 
",x,y,z,pitch,roll);
}

int main()
{
    u8 i;
    u8 buf[6];
    short X,Y,Z;

    printf("Gsensor_init%d
",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)