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)