概述

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

最近在弄ST的課程，需要樣片的可以加群申請：615061293 。

視頻教學(xué)

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

樣品申請

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

源碼下載

[https://download.csdn.net/download/qq_24312945/8985177

硬件準(zhǔn)備

首先需要準(zhǔn)備一個開發(fā)板，這里我準(zhǔn)備的是自己繪制的開發(fā)板，需要的可以進行申請。

主控為STM32H503CB，陀螺儀為LSM6DSV16X，磁力計為LIS2MDL。

上位機通訊

這里使用的是匿名助手的上位機

[https://gitee.com/anotc/AnoAssistant]

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

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

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

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

陀螺儀工作方式

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

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

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

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

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

兩個相同的質(zhì)量塊以方向相反的做水平震蕩。當(dāng)外部施加一個角速率，就會出現(xiàn)一個科氏力，力的方向垂直于質(zhì)量運動方向，如垂直方向箭頭所示。產(chǎn)生的科氏力使感測質(zhì)量發(fā)生位移，位移大小與所施加的角速率大小成正比，科氏力引起的電容變化即可計算出角速率大小。 科里奧利效應(yīng)指出，當(dāng)質(zhì)量 (m) 以速度 (v) 沿特定方向移動并施加外部角速率 (Ω)（紅色箭頭）時，科里奧利效應(yīng)會產(chǎn)生一個力（黃色箭頭），導(dǎo)致質(zhì)量垂直移動。該位移的值與應(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 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ù)長度LEN
    data_angular_rate_raw[5]=0x00;//數(shù)據(jù)長度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);        

                // 將歐拉角數(shù)據(jù)轉(zhuǎn)換為 int16_t 格式并填充到數(shù)據(jù)幀中
                Roll_int16=Roll_int16*100;// 放大100倍以保留小數(shù)位
                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)位置。

data_angular_rate_raw[7] = Roll_int16 > > 8;   // Roll 高字節(jié)
                data_angular_rate_raw[8] = Roll_int16 & 0xFF; // Roll 低字節(jié)
                data_angular_rate_raw[9] = Pitch_int16 > > 8;  // Pitch 高字節(jié)
                data_angular_rate_raw[10] = Pitch_int16 & 0xFF;// Pitch 低字節(jié)
                data_angular_rate_raw[11] = Yaw_int16 > > 8;   // Yaw 高字節(jié)
                data_angular_rate_raw[12] = Yaw_int16 & 0xFF; // Yaw 低字節(jié)

校驗和計算

使用了雙層循環(huá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]; // 按字節(jié)累加計算 sumcheck
                        addcheck += sumcheck;                 // 累加 sumcheck 生成 addcheck
                }
                data_angular_rate_raw[14] = sumcheck;     // 將校驗和寫入幀
                data_angular_rate_raw[15] = addcheck;     // 寫入最終的累加值

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

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

HAL_UART_Transmit(&huart1 , (uint8_t *)&data_angular_rate_raw, 16, 0xFFFF);

演示

LSM6DSV16X 特性涉及到的是一種低功耗的傳感器融合算法（Sensor Fusion Low Power, SFLP）.
低功耗傳感器融合（SFLP）算法：
該算法旨在以節(jié)能的方式結(jié)合加速度計和陀螺儀的數(shù)據(jù)。傳感器融合算法通過結(jié)合不同傳感器的優(yōu)勢，提供更準(zhǔn)確、可靠的數(shù)據(jù)。
6軸游戲旋轉(zhuǎn)向量：
SFLP算法能夠生成游戲旋轉(zhuǎn)向量。這種向量是一種表示設(shè)備在空間中方向的數(shù)據(jù)，特別適用于游戲和增強現(xiàn)實應(yīng)用，這些應(yīng)用中理解設(shè)備的方向和運動非常關(guān)鍵。
四元數(shù)表示法：
旋轉(zhuǎn)向量以四元數(shù)的形式表示。四元數(shù)是一種編碼3D旋轉(zhuǎn)的方法，它避免了歐拉角等其他表示法的一些限制（如萬向節(jié)鎖）。一個四元數(shù)有四個分量（X, Y, Z 和 W），其中 X, Y, Z 代表向量部分，W 代表標(biāo)量部分。
FIFO存儲：
四元數(shù)的 X, Y, Z 分量存儲在 LSM6DSV16X 的 FIFO（先進先出）緩沖區(qū)中。FIFO 緩沖區(qū)是一種數(shù)據(jù)存儲方式，允許臨時存儲傳感器數(shù)據(jù)。這對于有效管理數(shù)據(jù)流非常有用，特別是在數(shù)據(jù)處理可能不如數(shù)據(jù)收集那么快的系統(tǒng)中。

圖片包含了關(guān)于 LSM6DSV16X 傳感器的低功耗傳感器融合（Sensor Fusion Low Power, SFLP）功能的說明。這里是對圖片內(nèi)容的解釋： SFLP 功能：

1. SFLP 單元用于生成基于加速度計和陀螺儀數(shù)據(jù)處理的以下數(shù)據(jù)：
2. 游戲旋轉(zhuǎn)向量：以四元數(shù)形式表示設(shè)備的姿態(tài)。
3. 重力向量：提供一個三維向量，表示重力方向。
4. 陀螺儀偏差：提供一個三維向量，表示陀螺儀的偏差。 激活與重置：
5. 通過在 EMB_FUNC_EN_A（04h）嵌入式功能寄存器中設(shè)置 SFLP_GAME_EN 位為 1 來激活 SFLP 單元。
6. 通過在 EMB_FUNC_INIT_A（66h）嵌入式功能寄存器中設(shè)置 SFLP_GAME_INIT 位為 1 來重置 SFLP 單元。 性能參數(shù)表： 表格展示了 SFLP 功能在不同情況下的性能，包括靜態(tài)精度、低動態(tài)精度和高動態(tài)精度，以及校準(zhǔn)時間和方向穩(wěn)定時間。這些參數(shù)反映了傳感器在不同運動狀態(tài)下的精確度和響應(yīng)速度。

開啟INT中斷

陀螺儀LSM6DSV16X的中斷管腳接到了PB0，需要將PB0設(shè)置為中端口。

開啟中斷。

中斷讀取傳感器數(shù)據(jù)

INT1_CTRL (0Dh) 是 LSM6DSV16X 傳感器的中斷控制寄存器，用于配置和啟用 INT1 引腳的各種中斷信號。該寄存器的每一位對應(yīng)于不同的中斷源，通過設(shè)置這些位可以啟用或禁用相應(yīng)的中斷信號。
INT1_FIFO_TH (bit 3)：
● 啟用 FIFO 閾值中斷，將其路由到 INT1 引腳。當(dāng) FIFO 達到設(shè)定的閾值時觸發(fā)該中斷。默認(rèn)值為 0（禁用）。

mian.c中定義變量。

/* USER CODE BEGIN 0 */
uint8_t fifo_flag = 0;
/* USER CODE END 0 */

mian.c中開啟中斷。

lsm6dsv16x_pin_int_route_t pin_int;    
  pin_int.fifo_th = PROPERTY_ENABLE;
  lsm6dsv16x_pin_int1_route_set(&dev_ctx, &pin_int);

在stm32h5xx_it.c中添加回調(diào)函數(shù)引用。

/* USER CODE BEGIN 0 */
extern void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin);


/* USER CODE END 0 */

處理PB0外部中斷線0（EXTI Line0）的中斷。

/**
  * @brief This function handles EXTI Line0 interrupt.
  */
void EXTI0_IRQHandler(void)
{
  /* USER CODE BEGIN EXTI0_IRQn 0 */
    HAL_GPIO_EXTI_Callback(INT1_Pin);
  /* USER CODE END EXTI0_IRQn 0 */
  HAL_GPIO_EXTI_IRQHandler(INT1_Pin);
  /* USER CODE BEGIN EXTI0_IRQn 1 */

  /* USER CODE END EXTI0_IRQn 1 */
}

在main.c中添加回調(diào)函數(shù)的定義，檢查中斷是否由 GPIO_PIN_0引腳觸發(fā)。

/* USER CODE BEGIN 4 */
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin){
    if(GPIO_Pin == GPIO_PIN_0)
    {
        mlc_flag=1;
        }    
}
/* USER CODE END 4 */

主程序

/* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {

        if(mlc_flag==1)
        {
            mlc_flag=0;
            uint16_t num = 0;

    /* Read watermark flag */
    lsm6dsv16x_fifo_status_get(&dev_ctx, &fifo_status);
    if (fifo_status.fifo_th == 1) {
      num = fifo_status.fifo_level;

      printf( "-- FIFO num %d rn", num);

      while (num--) {
        lsm6dsv16x_fifo_out_raw_t f_data;
        int16_t *axis;
        float quat[4];
        float gravity_mg[3];
        float gbias_mdps[3];

        /* Read FIFO sensor value */
        lsm6dsv16x_fifo_out_raw_get(&dev_ctx, &f_data);

        switch (f_data.tag) {
//        case LSM6DSV16X_SFLP_GYROSCOPE_BIAS_TAG:
//          axis = (int16_t *)&f_data.data[0];
//          gbias_mdps[0] = lsm6dsv16x_from_fs125_to_mdps(axis[0]);
//          gbias_mdps[1] = lsm6dsv16x_from_fs125_to_mdps(axis[1]);
//          gbias_mdps[2] = lsm6dsv16x_from_fs125_to_mdps(axis[2]);
//          printf("GBIAS [mdps]:%4.2ft%4.2ft%4.2frn",
//                         (double_t)gbias_mdps[0], (double_t)gbias_mdps[1], (double_t)gbias_mdps[2]);

//          break;
//        case LSM6DSV16X_SFLP_GRAVITY_VECTOR_TAG:
//          axis = (int16_t *)&f_data.data[0];
//          gravity_mg[0] = lsm6dsv16x_from_sflp_to_mg(axis[0]);
//          gravity_mg[1] = lsm6dsv16x_from_sflp_to_mg(axis[1]);
//          gravity_mg[2] = lsm6dsv16x_from_sflp_to_mg(axis[2]);
//          printf("Gravity [mg]:%4.2ft%4.2ft%4.2frn",
//                         (double_t)gravity_mg[0], (double_t)gravity_mg[1], (double_t)gravity_mg[2]);

//          break;
        case LSM6DSV16X_SFLP_GAME_ROTATION_VECTOR_TAG:
          sflp2q(quat, (uint16_t *)&f_data.data[0]);
//          printf("Game Rotation tX: %2.3ftY: %2.3ftZ: %2.3ftW: %2.3frn",
//                  (double_t)quat[0], (double_t)quat[1], (double_t)quat[2], (double_t)quat[3]);

                    float sx=quat[1];  
                    float sy=quat[2];  
                    float sz=quat[0];  
                    float sw=quat[3];

                    if (sw< 0.0f) 
                    {
                        sx*=-1.0f;
                        sy*=-1.0f;
                        sz*=-1.0f;
                        sw*=-1.0f;
                    }

                    float sqx = sx * sx;
                    float sqy = sy * sy;
                    float sqz = sz * sz;
                    float euler[3];
                    euler[0] = -atan2f(2.0f* (sy*sw+sx*sz), 1.0f-2.0f*(sqy+sqx));
                    euler[1] = -atan2f(2.0f * (sx*sy+sz*sw),1.0f-2.0f*(sqx+sqz));
                    euler[2] = -asinf(2.0f* (sx*sw-sy*sz));

                    if (euler[0] < 0.0f)
                        euler[0] +=2.0f*3.1415926;

                    for(uint8_t i=0; i< 3; i++){
                            euler[i] = 57.29578 * (euler[i]);
                    }

                    printf("euler[0]=%f,euler[1]=%f,euler[2]=%fn",euler[0],euler[1],euler[2]);


          break;
        default:
         break;
        }
      }

    }                



        }  
    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */
  }
  /* USER CODE END 3 */
}

需要注意優(yōu)化等級。

演示

審核編輯 黃宇