概述

本文介紹了如何將 LSM6DSV16X 傳感器的姿態(tài)數(shù)據(jù)通過(guò)匿名通信協(xié)議上報(bào)到上位機(jī)。通過(guò)獲取傳感器的四元數(shù)數(shù)據(jù)，并將其轉(zhuǎn)換為歐拉角（Roll、Pitch、Yaw），然后按照協(xié)議格式化數(shù)據(jù)幀并通過(guò)串口傳輸?shù)缴衔粰C(jī)。上位機(jī)接收后可進(jìn)行實(shí)時(shí)顯示和分析。這種方式廣泛應(yīng)用于姿態(tài)檢測(cè)和控制系統(tǒng)，特別適合無(wú)人機(jī)、機(jī)器人等需要姿態(tài)控制的場(chǎng)景。

最近在弄ST和瑞薩RA的課程，需要樣片的可以加群申請(qǐng)：615061293 。

視頻教學(xué)

[https://www.bilibili.com/video/BV1sS5bzkEU9/]

樣品申請(qǐng)

[https://www.wjx.top/vm/OhcKxJk.aspx#]

源碼下載

[https://download.csdn.net/download/qq_24312945/91359810]

硬件準(zhǔn)備

首先需要準(zhǔn)備一個(gè)開(kāi)發(fā)板，這里我準(zhǔn)備的是自己繪制的開(kāi)發(fā)板，需要的可以進(jìn)行申請(qǐng)。

主控為STM32H503CB，陀螺儀為L(zhǎng)SM6DSV16X，磁力計(jì)為L(zhǎng)IS2MDL。

參考程序

[https://github.com/CoreMaker-lab/STM32H503_LSM6DSV16X_LIS2MDL]

[https://gitee.com/CoreMaker/STM32H503_LSM6DSV16X_LIS2MDL]

上位機(jī)通訊

這里使用的是匿名助手的上位機(jī)

https://gitee.com/anotc/AnoAssistant

有專門的通訊協(xié)議

串口通訊協(xié)議格式如下所示，需要注意傳輸為小端模式傳輸。

對(duì)應(yīng)的源地址和目標(biāo)地址分別為0xFD和0xFE。

我們只需要上報(bào)加速度和陀螺儀數(shù)據(jù)，所以功能碼為0x01，數(shù)據(jù)長(zhǎng)度為0x0D，需要主要為小端模式傳輸。

陀螺儀工作方式

加速度計(jì)測(cè)量線性加速度，而陀螺儀測(cè)量角旋轉(zhuǎn)。為此，他們測(cè)量了科里奧利效應(yīng)產(chǎn)生的力。

陀螺儀是一種運(yùn)動(dòng)傳感器，能夠感測(cè)物體在一軸或多軸上的旋轉(zhuǎn)角速度。它能夠精確地感測(cè)自由空間中復(fù)雜的移動(dòng)動(dòng)作，因此成為追蹤物體移動(dòng)方位和旋轉(zhuǎn)動(dòng)作的必要設(shè)備。與加速計(jì)和電子羅盤不同，陀螺儀不需要依賴外部力量（如重力或磁場(chǎng)），可以自主地發(fā)揮其功能。因此，從理論上講，只使用陀螺儀就可以完成姿態(tài)導(dǎo)航的任務(wù)。

陀螺儀的每個(gè)通道檢測(cè)一個(gè)軸的旋轉(zhuǎn)。也就是說(shuō)陀螺儀通過(guò)測(cè)量自身的旋轉(zhuǎn)狀態(tài)，判斷出設(shè)備當(dāng)前運(yùn)動(dòng)狀態(tài)，是向前、向后、向上、向下、向左還是向右呢，是加速（角速度）還是減速（角速度）呢，都可以實(shí)現(xiàn)，但是要判斷出設(shè)備的方位（東西南北），陀螺儀就沒(méi)有辦法。

MEMS陀螺儀主要利用科里奧利力（旋轉(zhuǎn)物體在有徑向運(yùn)動(dòng)時(shí)所受到的切向力）原理，公開(kāi)的微機(jī)械陀螺儀均采用振動(dòng)物體傳感角速度的概念，利用振動(dòng)來(lái)誘導(dǎo)和探測(cè)科里奧利力。

MEMS陀螺儀的核心是一個(gè)微加工機(jī)械單元，在設(shè)計(jì)上按照一個(gè)音叉機(jī)制共振運(yùn)動(dòng)，通過(guò)科里奧利力原理把角速率轉(zhuǎn)換成一個(gè)特定感測(cè)結(jié)構(gòu)的位移。

兩個(gè)相同的質(zhì)量塊以方向相反的做水平震蕩。當(dāng)外部施加一個(gè)角速率，就會(huì)出現(xiàn)一個(gè)科氏力，力的方向垂直于質(zhì)量運(yùn)動(dòng)方向，如垂直方向箭頭所示。產(chǎn)生的科氏力使感測(cè)質(zhì)量發(fā)生位移，位移大小與所施加的角速率大小成正比，科氏力引起的電容變化即可計(jì)算出角速率大小。

科里奧利效應(yīng)指出，當(dāng)質(zhì)量 (m) 以速度 (v) 沿特定方向移動(dòng)并施加外部角速率 (Ω)（紅色箭頭）時(shí)，科里奧利效應(yīng)會(huì)產(chǎn)生一個(gè)力（黃色箭頭），導(dǎo)致質(zhì)量垂直移動(dòng)。該位移的值與應(yīng)用的角速率直接相關(guān)。

變量定義。

/* USER CODE BEGIN 2 */
    float Yaw,Pitch,Roll;  //偏航角，俯仰角，翻滾角
    int16_t    acc_int16[3]    ={0,0,0};
    int16_t    gyr_int16[3]        ={0,0,0};    
    float acc[3] = {0};
    float gyr[3] = {0};    

    uint8_t data[21]={0};
    data[0]=0xAB;//幀頭
    data[1]=0xFD;//源地址
    data[2]=0xFE;//目標(biāo)地址        
    data[3]=0x01;//功能碼ID    
    data[4]=0x0D;//數(shù)據(jù)長(zhǎng)度LEN
    data[5]=0x00;//數(shù)據(jù)長(zhǎng)度LEN 13

    uint8_t sumcheck = 0;
    uint8_t addcheck = 0;        


    int16_t angular_rate_raw[3]={0,0,0};    //pitch,roll,yaw
    uint8_t data_angular_rate_raw[16]={0};
    data_angular_rate_raw[0]=0xAB;//幀頭
    data_angular_rate_raw[1]=0xFD;//源地址
    data_angular_rate_raw[2]=0xFE;//目標(biāo)地址        
    data_angular_rate_raw[3]=0x03;//功能碼ID    
    data_angular_rate_raw[4]=0x08;//數(shù)據(jù)長(zhǎng)度LEN
    data_angular_rate_raw[5]=0x00;//數(shù)據(jù)長(zhǎng)度LEN 8
    data_angular_rate_raw[6]=0x01;//mode = 1    

    data_angular_rate_raw[13]=0x00;//FUSION _STA：融合狀態(tài)        
  /* USER CODE END 2 */

歐拉角數(shù)據(jù)的轉(zhuǎn)換

將歐拉角 Roll、Pitch、Yaw 乘以 100，以保留兩位小數(shù)的精度。并且為 Yaw 數(shù)據(jù)減去了 18000，這通常是為了將歐拉角的范圍轉(zhuǎn)換為 [-18000, 18000] 這樣方便傳輸?shù)姆秶?/p>

Roll=euler[2];
                Pitch=euler[1];
                Yaw=euler[0];                    

                int16_t    Roll_int16;
                int16_t    Pitch_int16;                    
                int16_t    Yaw_int16;    

                Roll_int16 = (int16_t)(Roll);
                Pitch_int16 = (int16_t)(Pitch);
                Yaw_int16 = (int16_t)(Yaw);        


                Roll_int16=Roll_int16*100;
                Pitch_int16=Pitch_int16*100;
                Yaw_int16=Yaw_int16*100-18000;

數(shù)據(jù)幀填充

將轉(zhuǎn)換后的 Roll_int16、Pitch_int16 和 Yaw_int16 數(shù)據(jù)依次填充到數(shù)據(jù)幀的相應(yīng)位置。

//                Roll=Roll*100;
//                Pitch=Pitch*100;
//                Yaw=Yaw*100;
                data_angular_rate_raw[7]=Roll_int16 >>8;//roll
                data_angular_rate_raw[8]=Roll_int16;
                data_angular_rate_raw[9]=Pitch_int16 >>8;//pitch
                data_angular_rate_raw[10]=Pitch_int16;
                data_angular_rate_raw[11]=Yaw_int16 >>8;//yaw
                data_angular_rate_raw[12]=Yaw_int16;

校驗(yàn)和計(jì)算

使用了雙層循環(huán)求和來(lái)計(jì)算校驗(yàn)和，這是一種累加和的方法，確保幀數(shù)據(jù)的完整性。

data_angular_rate_raw[13]=0;
                sumcheck = 0;
                addcheck = 0;
                for(uint16_t i=0; i < 14; i++)
                {
                sumcheck += data_angular_rate_raw[i]; //從幀頭開(kāi)始，對(duì)每一字節(jié)進(jìn)行求和，直到 DATA 區(qū)結(jié)束
                addcheck += sumcheck; //每一字節(jié)的求和操作，進(jìn)行一次 sumcheck 的累加
                }
                data_angular_rate_raw[14]=sumcheck;
                data_angular_rate_raw[15]=addcheck;

數(shù)據(jù)發(fā)送

通過(guò) UART 發(fā)送封裝好的 16 字節(jié)數(shù)據(jù)幀。

HAL_UART_Transmit(&huart1 , (uint8_t *)&data_angular_rate_raw, 16, 0xFFFF);    
//                printf("Roll=%.2f,Pitch=%.2f,Yaw=%.2fn",Roll,Pitch,Yaw)

演示