概述

LSM6DSV16X包含三軸陀螺儀與三軸加速度計。
姿態有多種數學表示方式，常見的是四元數，歐拉角，矩陣和軸角。他們各自有其自身的優點，在不同的領域使用不同的表示方式。在四軸飛行器中使用到了四元數和歐拉角。
姿態解算選用的旋轉順序為ZYX，即IMU坐標系初始時刻與大地坐標系重合，然后依次繞自己的Z、Y、X軸進行旋轉：
繞IMU的Z軸旋轉：航向角yaw
繞IMU的Y軸旋轉：俯仰角pitch
繞IMU的X軸旋轉：橫滾角row

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

在這里插入圖片描述

橫滾roll，俯仰pitch，偏航yaw的實際含義如下圖：

在這里插入圖片描述
由于需要解析姿態角，故將陀螺儀速度修改快一點。

在這里插入圖片描述

視頻教學

https://www.bilibili.com/video/BV18M411d7Wg/

樣品申請

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

完整代碼下載

https://download.csdn.net/download/qq_24312945/88598263

歐拉角

橫滾角φ：機體繞OBXB轉動，軸Y'B與平面OBXBYB構成的夾角。
俯仰角θ：機體繞OBYB轉動，軸Z'B與平面OBYBZB構成的夾角。
偏航角ψ：機體繞OBZB轉動，軸X'B與平面OBXBZB構成的夾角。

在這里插入圖片描述
將姿態角從機體坐標系轉換到慣性坐標系中是為了便于分析無人機狀態，反映無人機在慣性坐標系下的姿態運動狀態，利用齊次線性變換可實現坐標系的轉換，旋轉矩陣就是在線性變化中產生的，用REB表示慣性坐標系{E}到機體坐標系{B}的變換。
例如，繞OBXB旋轉必角，此時兩個坐標系存在必的角度差，不再重合。點(x, y, z)的轉換方程為:
在這里插入圖片描述
可提取轉換矩陣：

同理，繞口OBYB旋轉θ角得:
在這里插入圖片描述

而繞OBZB旋轉ψ角得:
在這里插入圖片描述

不同旋轉順序有不同的旋轉矩陣，按照偏航，俯仰，橫滾的順序，即分別繞X-Y-Z旋轉，就可計算出旋轉矩陣REB，REB等于依次旋轉所得的矩陣連乘，且順序為從右向左排列。

在這里插入圖片描述

萬向節死鎖

當俯仰角θ=±Π/2時，橫滾運動與偏航運動的旋轉軸重合，出現萬向節死鎖現象，在空間失去了一個自由度。如式所示，φ或ψ的變化具有相同的效果，因此不再具有唯一性啊。

在這里插入圖片描述

四元數法

本文選擇的是四元數法進行姿態解算。無人機姿態解算方法主要有四種，它們各自的優缺點如下圖所示。歐拉角法不能用于計算飛行器的全姿態角，且難以實時計算而不易于工程應用。方向余弦法不會出現“奇點”現象，但計算量大，效率低。四元數法避免了復雜的三角函數運算，變為求解線性微分方程，算法簡單易操作，且不存在角度奇異性問題，可以更好的線性化系統，是一種更實用的工程方法。

在這里插入圖片描述

四元數的概念誕生在1843年的愛爾蘭，是數學家哈密頓研究空間幾何時提出。在如今的導航技術領域，四元數的優勢逐漸被發現，得到了研究者們的廣泛關注，并逐漸應用在姿態解算領域。

四元數是由四個元構成的數Q（q0,q1,q2,q3） = q0 + q1i + q2j + q3k;其中，q0,q1,q2,q3是實數，i,j,k既是互相正交的單位向量，又是虛單位根號-1。四元數即可看作四維空間中的一個向量，又可以看做一個超復數。對于后續有一個重要的變化需要記住：
Q=q0 + q1i + q2j + q3k
可視為一個超復數，Q 的共軛復數記為
Q'=q0 - q1i - q2j - q3k
Q°稱為Q的共軛四元數。
同時，有
ij=k，jk=i，ki=j，ji=-k，kj=-i，ik=-j
i2 = j2 = k2 =ijk=-1
在這里插入圖片描述
其中，i、j、k是相互正交的單位向量，其幾何意義可理解為分別繞三個坐標軸的旋轉，q0、q1、q2、q3為常數，有

通過四元數進行歐拉角求解，可以減少芯片運算負擔，提高運算速度。
在這里插入圖片描述

一個矢量V相對于坐標系OXYZ固定：V = xi + yj + zk；坐標系OXYZ轉動了Q得到一個新坐標系OX’Y’Z’:V = x’i’ + y’j‘ + z’k’；設四元數Ve、Ve‘
Ve = xi + yj + zk;
Ve’ = x’i + y’j + z’k;
則Ve’ = Q* Ve * Q'；
設Q = q0 + q1i + q2j + q3k;則Q' = q0 - q1i - q2j - q3k;
則Ve’ = Q* Ve * Q'=(q0 + q1i + q2j + q3k) * (0+xi + yj + zk) + (q0 - q1i - q2j - q3k)
可以算出
x’=(q0 ^2+q1 ^2-q2 ^2-q3 ^2)x+2(q1q2+ q1q3)y+2(q1q3-q0q2)z
y’ = 2(q1q2-q0q3)x+(q0 ^2-q1 ^2+q2 ^2-q3 ^2)y+2(q2q3+q0q1)z
z’ = 2(q1q3+q0q2)x+2(q2q3-q0q1)y+(q0 ^2-q1 ^2-q2 ^2+q3 ^2)z

在這里插入圖片描述

結合
在這里插入圖片描述
可以反推

Pitch  = asin(2 * q2 * q3 + 2 * q0* q1)* 57.3; // pitch ,轉換為度數
        Roll = atan2(-2 * q1 * q3 + 2 * q0 * q2, q0*q0-q1*q1-q2*q2+q3*q3)* 57.3; // rollv
        Yaw = atan2(2*(q1*q2 - q0*q3),q0*q0-q1*q1+q2*q2-q3*q3) * 57.3;   //偏移太大，

將加速度的三維向量轉為單位向量

// 測量正常化
        norm = sqrt(ax*ax + ay*ay + az*az);      
        ax = ax / norm;                   //單位化
        ay = ay / norm;
        az = az / norm;

世界坐標系重力分向量是通過方向旋轉矩陣的最后一列的三個元素乘上加速度就可以算出機體坐標系中的重力向量。

// 估計方向的重力
        vx = 2*(q1*q3 - q0*q2);//由下向上方向的加速度在加速度計X分量 
        vy = 2*(q0*q1 + q2*q3);//由下向上方向的加速度在加速度計X分量 
        vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;//由下向上方向的加速度在加速度計Z分量

姿態解算

在這里插入圖片描述

雙環PI控制器

陀螺儀能夠迅速響應設備的旋轉，在短時間內誤差較小且可靠。然而，因為溫度漂移、零漂移和積分誤差會隨時間累積，陀螺儀的長時間精度受到影響。在靜止狀態下，加速度計的漂移很小，其傾角求解過程中不存在積分誤差，但在飛行過程中，加速度計受到發動機和機架振動以及轉動和運動加速度的干擾。磁羅盤測量的地磁向量在特定地理范圍內可視為不變，但磁羅盤易受硬磁場和軟磁場干擾。
因此，若系統外環采用九軸姿態傳感器（包括三軸加速度計、三軸磁羅盤和三軸陀螺儀）進行數據融合，磁羅盤易受干擾可能導致融合后的數據仍有較大誤差。為此，在內環使用六軸姿態傳感器（包括三軸加速度計和三軸陀螺儀）進行數據融合，對融合后的傳感器姿態偏差進行二次修正，以提高整體精度。
外環九軸姿態傳感器數據融合，記在飛行器機體坐標系下an=[ax ay az]T和mn=[mx my mz]T分別為加速度計和磁羅盤實際測量得到的重力向量和地磁向量。
記vn=[vx vy vz]T和wn=[mx my mz]T是將地理坐標系下重力向量kb=[0 0 1g]T和地磁向量nb=[nx 0 nz]T(不考慮地理磁偏角因素，將機頭固定向北)通過四元數坐標換算成機體坐標系下的重力向量和地磁向量。向量之間的誤差為坐標軸的旋轉誤差，可以用向量的叉積en=[ex ey ez]T表示，如下所示。
在這里插入圖片描述

由于我的LSM6DS3TR-C為六軸，不帶三軸陀螺儀，故代碼如下。

//這個叉積向量仍舊是位于機體坐標系上的，而陀螺積分誤差也是在機體坐標系，而且叉積的大小與陀螺積分誤差成正比，正好拿來糾正陀螺。
//（你可以自己拿東西想象一下）由于陀螺是對機體直接積分，所以對陀螺的糾正量會直接體現在對機體坐標系的糾正。

		ex = (ay*vz - az*vy);
        ey = (az*vx - ax*vz);
        ez = (ax*vy - ay*vx);

由于陀螺儀是對機體直接積分，所以，陀螺儀的誤差可以體現為機體坐標的誤差。因此修正坐標軸的誤差可以達到修正陀螺儀誤差的目的，從而將加速度計和磁羅盤進行修正陀螺儀，實現了九軸的數據融合。即如果陀螺儀按照叉積誤差的軸，轉動叉積誤差的角度，就可以消除機體坐標上實際測量的重力向量和地磁向量和坐標換算后的重力向量和地磁向量之間的誤差。
PI調節器的比例部分用于迅速糾正陀螺儀誤差，積分部分用于消除穩態偏差。PI調節器的比例系數和積分系數自己去修正。陀螺儀經過外環PI控制器修正姿態誤差后輸出值為了gn =[gx gy gz]T
在這里插入圖片描述

// 積分誤差比例積分增益,計算陀螺儀測量的重力向量與估計方向的重力向量之間的誤差。
        exInt = exInt + ex*Ki;
        eyInt = eyInt + ey*Ki;
        ezInt = ezInt + ez*Ki;
 
        // 調整后的陀螺儀測量,使用叉積誤差來進行比例-積分（PI）修正陀螺儀的零偏。將修正量乘以比例增益Kp，并加上之前計算的積分誤差exInt、eyInt和ezInt。
        gx = gx + Kp*ex + exInt;
        gy = gy + Kp*ey + eyInt;
        gz = gz + Kp*ez + ezInt;

內環的六軸姿態傳感器數據融合是將地理坐標系下的重力場向量與加速度計在機體坐標系下采集到的重力向量進行叉乘，求出兩者向量誤差。并通過PI控制器修正向量誤差，從而達到修正外環九軸數據融合后的陀螺儀的偏差的目的。在每個姿態解算周期讀取出機體坐標系下雙環PI控制后的陀螺儀的角速率

在這里插入圖片描述

整合四元數率和正常化,根據陀螺儀的測量值和比例-積分修正值，對四元數進行更新。
在這里插入圖片描述

// 整合四元數率和正常化,根據陀螺儀的測量值和比例-積分修正值，對四元數進行更新。根據微分方程的離散化形式，將四元數的每個分量加上相應的微分項乘以采樣周期的一半（halfT）。
        q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
        q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
        q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
        q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;  
 
        // 正常化四元數
        norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
        q0 = q0 / norm;
        q1 = q1 / norm;
        q2 = q2 / norm;
        q3 = q3 / norm;

偏航角

六軸傳感器（包括三軸加速度計和三軸陀螺儀）可以用于估算設備在空間中的姿態，包括俯仰角（Pitch）、橫滾角（Roll）和偏航角（Yaw）。然而，六軸傳感器僅依賴陀螺儀和加速度計數據，可能無法準確測量偏航角（Yaw），原因如下：
無磁場參考：六軸傳感器缺少磁羅盤，沒有固定的參考方向。因此，在長時間內，陀螺儀的積分誤差可能導致偏航角估計漂移。
陀螺儀誤差累積：陀螺儀測量的是角速度，要得到偏航角，需要將角速度積分。由于陀螺儀存在零漂、噪聲和溫度漂移等誤差，這些誤差在積分過程中會累積，使得偏航角估計產生較大的漂移。
雖然六軸傳感器可能無法準確測量偏航角，但可以通過將其與磁羅盤（三軸磁場傳感器）結合，形成九軸傳感器（包括三軸加速度計、三軸磁羅盤和三軸陀螺儀），以提高偏航角估計的準確性。九軸傳感器融合了磁場信息，為偏航角提供了一個穩定的參考方向，有助于減小陀螺儀誤差對偏航角估計的影響。

陀螺儀解析代碼

//加速度單位g，陀螺儀rad/s
void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)
{

        float norm;
        float vx, vy, vz;
        float ex, ey, ez;  
 
        // 測量正常化,把加計的三維向量轉成單位向量。
        norm = sqrt(ax*ax + ay*ay + az*az);      
        ax = ax / norm;                   //單位化
        ay = ay / norm;
        az = az / norm;      
 
        // 估計方向的重力,世界坐標系重力分向量是通過方向旋轉矩陣的最后一列的三個元素乘上加速度就可以算出機體坐標系中的重力向量。
        vx = 2*(q1*q3 - q0*q2);//由下向上方向的加速度在加速度計X分量 
        vy = 2*(q0*q1 + q2*q3);//由下向上方向的加速度在加速度計X分量 
        vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;//由下向上方向的加速度在加速度計Z分量
 

//這個叉積向量仍舊是位于機體坐標系上的，而陀螺積分誤差也是在機體坐標系，而且叉積的大小與陀螺積分誤差成正比，正好拿來糾正陀螺。
//（你可以自己拿東西想象一下）由于陀螺是對機體直接積分，所以對陀螺的糾正量會直接體現在對機體坐標系的糾正。

		ex = (ay*vz - az*vy);
        ey = (az*vx - ax*vz);
        ez = (ax*vy - ay*vx);
 
        // 積分誤差比例積分增益,計算陀螺儀測量的重力向量與估計方向的重力向量之間的誤差。
		
        exInt = exInt + ex*Ki;
        eyInt = eyInt + ey*Ki;
        ezInt = ezInt + ez*Ki;				
 
        // 調整后的陀螺儀測量,使用叉積誤差來進行比例-積分（PI）修正陀螺儀的零偏。將修正量乘以比例增益Kp，并加上之前計算的積分誤差exInt、eyInt和ezInt。
        gx = gx + Kp*ex + exInt;
        gy = gy + Kp*ey + eyInt;
        gz = gz + Kp*ez + ezInt; 
 
        // 整合四元數率和正常化,根據陀螺儀的測量值和比例-積分修正值，對四元數進行更新。根據微分方程的離散化形式，將四元數的每個分量加上相應的微分項乘以采樣周期的一半（halfT）。
        q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
        q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
        q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
        q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;  
 
        // 正常化四元數
        norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
        q0 = q0 / norm;
        q1 = q1 / norm;
        q2 = q2 / norm;
        q3 = q3 / norm;
 
        Pitch  = asin(2 * q2 * q3 + 2 * q0* q1)* 57.3; // pitch ,轉換為度數
        Roll = atan2(-2 * q1 * q3 + 2 * q0 * q2, q0*q0-q1*q1-q2*q2+q3*q3)* 57.3; // rollv
        Yaw = atan2(2*(q1*q2 - q0*q3),q0*q0-q1*q1+q2*q2-q3*q3) * 57.3;   //偏移太大，等我找一個好用的

}

上位機通訊

這里使用的是匿名助手的上位機
[https://gitee.com/anotc/AnoAssistant]
有專門的通訊協議
在這里插入圖片描述

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

在這里插入圖片描述
對應的源地址和目標地址分別為0xFD和0xFE。

在這里插入圖片描述

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

在這里插入圖片描述

加速度演示

實測移動模塊分別為X、Y、Z軸向下，可以看見數值基本上為1000mg。

在這里插入圖片描述

陀螺儀工作方式

加速度計測量線性加速度，而陀螺儀測量角旋轉。為此，他們測量了科里奧利效應產生的力。
陀螺儀是一種運動傳感器，能夠感測物體在一軸或多軸上的旋轉角速度。它能夠精確地感測自由空間中復雜的移動動作，因此成為追蹤物體移動方位和旋轉動作的必要設備。與加速計和電子羅盤不同，陀螺儀不需要依賴外部力量（如重力或磁場），可以自主地發揮其功能。因此，從理論上講，只使用陀螺儀就可以完成姿態導航的任務。

在這里插入圖片描述

陀螺儀的每個通道檢測一個軸的旋轉。也就是說陀螺儀通過測量自身的旋轉狀態，判斷出設備當前運動狀態，是向前、向后、向上、向下、向左還是向右呢，是加速（角速度）還是減速（角速度）呢，都可以實現，但是要判斷出設備的方位（東西南北），陀螺儀就沒有辦法。
在這里插入圖片描述

MEMS陀螺儀主要利用科里奧利力（旋轉物體在有徑向運動時所受到的切向力）原理，公開的微機械陀螺儀均采用振動物體傳感角速度的概念，利用振動來誘導和探測科里奧利力。
MEMS陀螺儀的核心是一個微加工機械單元，在設計上按照一個音叉機制共振運動，通過科里奧利力原理把角速率轉換成一個特定感測結構的位移。

在這里插入圖片描述

兩個相同的質量塊以方向相反的做水平震蕩。當外部施加一個角速率，就會出現一個科氏力，力的方向垂直于質量運動方向，如垂直方向箭頭所示。產生的科氏力使感測質量發生位移，位移大小與所施加的角速率大小成正比，科氏力引起的電容變化即可計算出角速率大小。
科里奧利效應指出，當質量 (m) 以速度 (v) 沿特定方向移動并施加外部角速率 (Ω)（紅色箭頭）時，科里奧利效應會產生一個力（黃色箭頭），導致質量垂直移動。該位移的值與應用的角速率直接相關。

在這里插入圖片描述

/* USER CODE BEGIN 2 */
	printf("123123123");
  lsm6dsv16x_reset_t rst;
  stmdev_ctx_t dev_ctx;
  /* Initialize mems driver interface */
  dev_ctx.write_reg = platform_write;
  dev_ctx.read_reg = platform_read;
  dev_ctx.handle = &SENSOR_BUS;


  HAL_GPIO_WritePin(CS_GPIO_Port, CS_Pin, GPIO_PIN_SET);
  HAL_GPIO_WritePin(SA0_GPIO_Port, SA0_Pin, GPIO_PIN_RESET);


  /* Wait sensor boot time */
  platform_delay(BOOT_TIME);
  /* Check device ID */
  lsm6dsv16x_device_id_get(&dev_ctx, &whoamI);
	printf("LSM6DSV16X_ID=0x%x,whoamI=0x%x",LSM6DSV16X_ID,whoamI);
  if (whoamI != LSM6DSV16X_ID)
    while (1);

  /* Restore default configuration */
  lsm6dsv16x_reset_set(&dev_ctx, LSM6DSV16X_RESTORE_CTRL_REGS);
  do {
    lsm6dsv16x_reset_get(&dev_ctx, &rst);
  } while (rst != LSM6DSV16X_READY);

  /* Enable Block Data Update */
  lsm6dsv16x_block_data_update_set(&dev_ctx, PROPERTY_ENABLE);
  /* Set Output Data Rate.
   * Selected data rate have to be equal or greater with respect
   * with MLC data rate.
   */
  lsm6dsv16x_xl_data_rate_set(&dev_ctx, LSM6DSV16X_ODR_AT_960Hz);
  lsm6dsv16x_gy_data_rate_set(&dev_ctx, LSM6DSV16X_ODR_AT_960Hz);
  /* Set full scale */
  lsm6dsv16x_xl_full_scale_set(&dev_ctx, LSM6DSV16X_2g);
  lsm6dsv16x_gy_full_scale_set(&dev_ctx, LSM6DSV16X_2000dps);
  /* Configure filtering chain */
  filt_settling_mask.drdy = PROPERTY_ENABLE;
  filt_settling_mask.irq_xl = PROPERTY_ENABLE;
  filt_settling_mask.irq_g = PROPERTY_ENABLE;
  lsm6dsv16x_filt_settling_mask_set(&dev_ctx, filt_settling_mask);
  lsm6dsv16x_filt_gy_lp1_set(&dev_ctx, PROPERTY_ENABLE);
  lsm6dsv16x_filt_gy_lp1_bandwidth_set(&dev_ctx, LSM6DSV16X_GY_ULTRA_LIGHT);
  lsm6dsv16x_filt_xl_lp2_set(&dev_ctx, PROPERTY_ENABLE);
  lsm6dsv16x_filt_xl_lp2_bandwidth_set(&dev_ctx, LSM6DSV16X_XL_STRONG);
		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;//目標地址		
		data[3]=0x01;//功能碼ID	
		data[4]=0x0D;//數據長度LEN
		data[5]=0x00;//數據長度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;//目標地址		
		data_angular_rate_raw[3]=0x03;//功能碼ID	
		data_angular_rate_raw[4]=0x08;//數據長度LEN
		data_angular_rate_raw[5]=0x00;//數據長度LEN 8
		data_angular_rate_raw[6]=0x01;//mode = 1	
		
		data_angular_rate_raw[13]=0x00;//FUSION _STA：融合狀態	


  /* USER CODE END 2 */

主程序

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

    /* Read output only if new xl value is available */
    lsm6dsv16x_flag_data_ready_get(&dev_ctx, &drdy);

    if (drdy.drdy_xl) {
      /* Read acceleration field data */
      memset(data_raw_acceleration, 0x00, 3 * sizeof(int16_t));
      lsm6dsv16x_acceleration_raw_get(&dev_ctx, data_raw_acceleration);
      acceleration_mg[0] =
        lsm6dsv16x_from_fs2_to_mg(data_raw_acceleration[0]);
      acceleration_mg[1] =
        lsm6dsv16x_from_fs2_to_mg(data_raw_acceleration[1]);
      acceleration_mg[2] =
        lsm6dsv16x_from_fs2_to_mg(data_raw_acceleration[2]);
//			printf("Acceleration [mg]:%4.2ft%4.2ft%4.2frn",acceleration_mg[0], acceleration_mg[1], acceleration_mg[2]);
    }		
		
    /* Read output only if new xl value is available */
    if (drdy.drdy_gy) {
      /* Read angular rate field data */
      memset(data_raw_angular_rate, 0x00, 3 * sizeof(int16_t));
      lsm6dsv16x_angular_rate_raw_get(&dev_ctx, data_raw_angular_rate);
      angular_rate_mdps[0] =
        lsm6dsv16x_from_fs2000_to_mdps(data_raw_angular_rate[0]);
      angular_rate_mdps[1] =
        lsm6dsv16x_from_fs2000_to_mdps(data_raw_angular_rate[1]);
      angular_rate_mdps[2] =
        lsm6dsv16x_from_fs2000_to_mdps(data_raw_angular_rate[2]);
//			printf("Angular rate [mdps]:%4.2ft%4.2ft%4.2frn",angular_rate_mdps[0], angular_rate_mdps[1], angular_rate_mdps[2]);

    }

    if (drdy.drdy_temp) {
      /* Read temperature data */
      memset(&data_raw_temperature, 0x00, sizeof(int16_t));
      lsm6dsv16x_temperature_raw_get(&dev_ctx, &data_raw_temperature);
      temperature_degC = lsm6dsv16x_from_lsb_to_celsius(
                           data_raw_temperature);
//			printf("Temperature [degC]:%6.2frn", temperature_degC);

    }		


IMUupdate(angular_rate_mdps[0]/1000,angular_rate_mdps[1]/1000,angular_rate_mdps[2]/1000,acceleration_mg[0]/1000,acceleration_mg[1]/1000,acceleration_mg[2]/1000);
Roll=Roll*100;
Pitch=Pitch*100;
Yaw=Yaw*100;
data_angular_rate_raw[8]=(int16_t)Roll > >8;//roll
data_angular_rate_raw[7]=(int16_t)Roll;
data_angular_rate_raw[10]=(int16_t)Pitch > >8;//pitch
data_angular_rate_raw[9]=(int16_t)Pitch;
data_angular_rate_raw[12]=(int16_t)Yaw > >8;//yaw
data_angular_rate_raw[11]=(int16_t)Yaw;
sumcheck = 0;
addcheck = 0;
for(uint16_t i=0; i < 14; i++)
{
sumcheck += data_angular_rate_raw[i]; //從幀頭開始，對每一字節進行求和，直到 DATA 區結束
addcheck += sumcheck; //每一字節的求和操作，進行一次 sumcheck 的累加
}
data_angular_rate_raw[14]=sumcheck;
data_angular_rate_raw[15]=addcheck;

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

    /* USER CODE END WHILE */

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