概述

在現(xiàn)代的運(yùn)動(dòng)跟蹤和姿態(tài)檢測(cè)應(yīng)用中，低功耗、高精度的傳感器數(shù)據(jù)融合處理變得越來(lái)越重要。LSM6DSV16X傳感器集成了SFLP（Sensor Fusion Low Power）算法模塊，可以在低功耗模式下實(shí)現(xiàn)六軸傳感器數(shù)據(jù)的高效融合。SFLP模塊通過(guò)處理加速度計(jì)和陀螺儀的數(shù)據(jù)，生成一個(gè)表示設(shè)備姿態(tài)的四元數(shù)，這為游戲、增強(qiáng)現(xiàn)實(shí)（AR）、虛擬現(xiàn)實(shí)（VR）等應(yīng)用中的精準(zhǔn)運(yùn)動(dòng)追蹤提供了技術(shù)支持。在本文中，我們將深入探討如何利用SFLP模塊獲取四元數(shù)數(shù)據(jù)，并分析其在實(shí)際應(yīng)用中的優(yōu)勢(shì)和實(shí)現(xiàn)方法。

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

視頻教學(xué)

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

樣品申請(qǐng)

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

源碼下載

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

硬件準(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

SFLP

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

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

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

生成STM32CUBEMX

用STM32CUBEMX生成例程，這里使用MCU為STM32H503CB。
配置時(shí)鐘樹(shù)，配置時(shí)鐘為250M。

串口配置

查看原理圖，PA9和PA10設(shè)置為開(kāi)發(fā)板的串口。

配置串口，速率為2000000。

IIC配置

配置IIC為快速模式，速度為400k。

CS和SA0設(shè)置

ICASHE

修改堆棧

串口重定向

打開(kāi)魔術(shù)棒，勾選MicroLIB

在main.c中，添加頭文件，若不添加會(huì)出現(xiàn) identifier "FILE" is undefined報(bào)錯(cuò)。

/* USER CODE BEGIN Includes */
#include "stdio.h"
/* USER CODE END Includes */

函數(shù)聲明和串口重定向：

/* USER CODE BEGIN PFP */
int fputc(int ch, FILE *f){
    HAL_UART_Transmit(&huart1 , (uint8_t *)&ch, 1, 0xFFFF);
    return ch;
}
/* USER CODE END PFP */

參考程序

https://github.com/STMicroelectronics/lsm6dsv16x-pid

初始換管腳

由于需要向LSM6DSV16X_I2C_ADD_L寫(xiě)入以及為IIC模式。

所以使能CS為高電平，配置為IIC模式。 配置SA0為高電平。

printf("HELLO!n");
  HAL_GPIO_WritePin(CS_GPIO_Port, CS_Pin, GPIO_PIN_SET);
  HAL_GPIO_WritePin(SA0_GPIO_Port, SA0_Pin, GPIO_PIN_RESET);
    HAL_Delay(100);


  lsm6dsv16x_fifo_status_t fifo_status;
  stmdev_ctx_t dev_ctx;
  lsm6dsv16x_reset_t rst;

  /* Initialize mems driver interface */
  dev_ctx.write_reg = platform_write;
  dev_ctx.read_reg = platform_read;
  dev_ctx.mdelay = platform_delay;
  dev_ctx.handle = &SENSOR_BUS;

  /* Init test platform */
//  platform_init(dev_ctx.handle);
  /* Wait sensor boot time */
  platform_delay(BOOT_TIME);

獲取ID

可以向WHO_AM_I (0Fh)獲取固定值，判斷是否為0x70。

lsm6dsv16x_device_id_get為獲取函數(shù)。

對(duì)應(yīng)的獲取ID驅(qū)動(dòng)程序,如下所示。

/* 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);

復(fù)位操作

可以向CTRL3 (12h)的SW_RESET寄存器寫(xiě)入1進(jìn)行復(fù)位。

lsm6dsv16x_reset_set為重置函數(shù)。

對(duì)應(yīng)的驅(qū)動(dòng)程序,如下所示。

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

BDU設(shè)置

在很多傳感器中，數(shù)據(jù)通常被存儲(chǔ)在輸出寄存器中，這些寄存器分為兩部分：MSB和LSB。這兩部分共同表示一個(gè)完整的數(shù)據(jù)值。例如，在一個(gè)加速度計(jì)中，MSB和LSB可能共同表示一個(gè)加速度的測(cè)量值。
連續(xù)更新模式（BDU = ‘0’）：在默認(rèn)模式下，輸出寄存器的值會(huì)持續(xù)不斷地被更新。這意味著在你讀取MSB和LSB的時(shí)候，寄存器中的數(shù)據(jù)可能會(huì)因?yàn)樾碌臏y(cè)量數(shù)據(jù)而更新。這可能導(dǎo)致一個(gè)問(wèn)題：當(dāng)你讀取MSB時(shí)，如果寄存器更新了，接下來(lái)讀取的LSB可能就是新的測(cè)量值的一部分，而不是與MSB相對(duì)應(yīng)的值。這樣，你得到的就是一個(gè)“拼湊”的數(shù)據(jù)，它可能無(wú)法準(zhǔn)確代表任何實(shí)際的測(cè)量時(shí)刻。
塊數(shù)據(jù)更新（BDU）模式（BDU = ‘1’）：當(dāng)激活BDU功能時(shí)，輸出寄存器中的內(nèi)容不會(huì)在讀取MSB和LSB之間更新。這就意味著一旦開(kāi)始讀取數(shù)據(jù)（無(wú)論是先讀MSB還是LSB），寄存器中的那一組數(shù)據(jù)就被“鎖定”，直到兩部分都被讀取完畢。這樣可以確保你讀取的MSB和LSB是同一測(cè)量時(shí)刻的數(shù)據(jù)，避免了讀取到代表不同采樣時(shí)刻的數(shù)據(jù)。
簡(jiǎn)而言之，BDU位的作用是確保在讀取數(shù)據(jù)時(shí)，輸出寄存器的內(nèi)容保持穩(wěn)定，從而避免讀取到拼湊或錯(cuò)誤的數(shù)據(jù)。這對(duì)于需要高精度和穩(wěn)定性的應(yīng)用尤為重要。
可以向CTRL3 (12h)的BDU寄存器寫(xiě)入1進(jìn)行開(kāi)啟。

對(duì)應(yīng)的驅(qū)動(dòng)程序,如下所示。

/* Enable Block Data Update */
  lsm6dsv16x_block_data_update_set(&dev_ctx, PROPERTY_ENABLE);

設(shè)置量程

速率可以通過(guò)CTRL1 (10h)設(shè)置加速度速率和CTRL2 (11h)進(jìn)行設(shè)置角速度速率。

設(shè)置加速度量程可以通過(guò)CTRL8 (17h)進(jìn)行設(shè)置。 設(shè)置角速度量程可以通過(guò)CTRL6 (15h)進(jìn)行設(shè)置。

設(shè)置加速度和角速度的量程和速率可以使用如下函數(shù)。

/* Set full scale */
  lsm6dsv16x_xl_full_scale_set(&dev_ctx, LSM6DSV16X_4g);
  lsm6dsv16x_gy_full_scale_set(&dev_ctx, LSM6DSV16X_2000dps);

初始化SFLP步驟

啟用 LSM6DSV16X 傳感器中的旋轉(zhuǎn)向量低功耗傳感器融合（Rotation Vector SFLP）功能的步驟。旋轉(zhuǎn)向量是一個(gè)四元數(shù)，它提供了一個(gè)精確的設(shè)備姿態(tài)估計(jì)。這通常用于游戲控制、增強(qiáng)現(xiàn)實(shí)和虛擬現(xiàn)實(shí)等應(yīng)用。下面是函數(shù)各部分的作用：

1. 函數(shù)定義：LSM6DSV16XSensor_Enable_Rotation_Vector 旨在啟用旋轉(zhuǎn)向量功能，并返回操作的結(jié)果。如果成功，返回 0；如果出現(xiàn)錯(cuò)誤，則返回錯(cuò)誤代碼。
2. 設(shè)置滿(mǎn)量程：函數(shù)首先設(shè)置加速度計(jì)和陀螺儀的滿(mǎn)量程，這是傳感器能夠測(cè)量的最大范圍。這里分別設(shè)置為 4g 和 2000 度每秒（dps）。
3. 獲取 FIFO SFLP 設(shè)置：然后，它讀取當(dāng)前的 FIFO SFLP（傳感器融合低功耗）配置。
4. 啟用旋轉(zhuǎn)向量 SFLP 特性：通過(guò)將 fifo_sflp.game_rotation 設(shè)為 1 來(lái)啟用游戲旋轉(zhuǎn)向量功能。
5. 設(shè)置 FIFO 模式：將 FIFO 設(shè)置為流模式（也稱(chēng)為連續(xù)模式），在此模式下，數(shù)據(jù)持續(xù)地流入 FIFO，如果 FIFO 滿(mǎn)了，新數(shù)據(jù)會(huì)覆蓋舊數(shù)據(jù)。
6. 設(shè)置數(shù)據(jù)輸出率：為加速度計(jì)和陀螺儀以及 SFLP 設(shè)置數(shù)據(jù)輸出率（ODR），在這里都設(shè)置為每秒 120 次采樣（120Hz）。
7. 啟用 SFLP 低功耗模式：最后，啟用 SFLP 游戲旋轉(zhuǎn)向量特性，確保以低功耗模式運(yùn)行。

FIFO 閾值

設(shè)置 FIFO 的“水位閾值”，即 當(dāng) FIFO 中存儲(chǔ)的數(shù)據(jù)量 ≥ 該閾值時(shí)，傳感器會(huì)觸發(fā) Watermark 中斷（如果配置了中斷）。
表示當(dāng) FIFO 中累積了FIFO_WATERMARK(32)時(shí)，就會(huì)滿(mǎn)足“水位條件”。

LSM6DSV16X 的 FIFO_CTRL1（0x07）寄存器，是設(shè)置 FIFO 水位閾值（watermark threshold） 的關(guān)鍵寄存器。

1 LSB 表示 1個(gè)樣本，每個(gè)樣本由：

● 1 byte TAG（標(biāo)識(shí)是哪類(lèi)數(shù)據(jù)，如 GYRO/ACC）

● 6 byte 數(shù)據(jù)（例如三軸陀螺儀 X/Y/Z，每軸2字節(jié)）

設(shè)置了 FIFO 的“容量警戒線(xiàn)”為 32 個(gè)樣本，等于 224 字節(jié)，一旦數(shù)據(jù)滿(mǎn)到這個(gè)程度，F(xiàn)IFO_WTM_IA 標(biāo)志就會(huì)被置位，從而引發(fā)中斷或被 MCU 輪詢(xún)識(shí)別到，隨后就可以讀取 FIFO 里的數(shù)據(jù)了。

/*
   * Set FIFO watermark (number of unread sensor data TAG + 6 bytes
   * stored in FIFO) to FIFO_WATERMARK samples
   */
  lsm6dsv16x_fifo_watermark_set(&dev_ctx, FIFO_WATERMARK);

配置 FIFO 模式

設(shè)置 SFLP 模塊中要寫(xiě)入 FIFO 的三類(lèi)輸出：

那么 FIFO 中的每個(gè) SFLP 樣本，可能是：

● 四元數(shù)：QX, QY, QZ, QW

● 重力向量：3 軸重力向量

● 陀螺儀偏置：3 軸陀螺儀偏置

/* Set FIFO batch of sflp data */
  fifo_sflp.game_rotation = 1;
  fifo_sflp.gravity = 1;
  fifo_sflp.gbias = 1;
  lsm6dsv16x_fifo_sflp_batch_set(&dev_ctx, fifo_sflp);

設(shè)置FIFO模式為Stream（連續(xù)）模式

Stream Mode 是最常用于實(shí)時(shí)流數(shù)據(jù)采集的方式 —— 即使 FIFO 滿(mǎn)了，也繼續(xù)采集，不丟失最新數(shù)據(jù)（會(huì)覆蓋舊數(shù)據(jù)）。

/* Set FIFO mode to Stream mode (aka Continuous Mode) */
  lsm6dsv16x_fifo_mode_set(&dev_ctx, LSM6DSV16X_STREAM_MODE);

設(shè)置 加速度計(jì) ODR

設(shè)置 加速度計(jì) ODR（輸出數(shù)據(jù)率）為 30Hz。

/* Set Output Data Rate */
  lsm6dsv16x_xl_data_rate_set(&dev_ctx, LSM6DSV16X_ODR_AT_30Hz);

設(shè)置 陀螺儀 ODR

設(shè)置 陀螺儀 ODR 為 30Hz。

lsm6dsv16x_gy_data_rate_set(&dev_ctx, LSM6DSV16X_ODR_AT_30Hz);

設(shè)定 SFLP 輸出速率

設(shè)定 SFLP 模塊自身的輸出速率，控制四元數(shù)/重力/陀螺儀偏置等數(shù)據(jù)輸出頻率。

lsm6dsv16x_sflp_data_rate_set(&dev_ctx, LSM6DSV16X_SFLP_30Hz);

啟用 SFLP 四元數(shù)輸出

啟用 SFLP 的四元數(shù)輸出。

lsm6dsv16x_sflp_game_rotation_set(&dev_ctx, PROPERTY_ENABLE);

設(shè)置陀螺儀偏置（Gyro Bias）值

初始化時(shí)設(shè)置陀螺儀偏置（Gyro Bias）值，目的是為了提升姿態(tài)融合（四元數(shù)）算法的準(zhǔn)確性，特別適用于你使用的 SFLP（Sensor Fusion Low Power）模塊。

/*
   * here application may initialize offset with latest values
   * calculated from previous run and saved to non volatile memory.
   */
  gbias.gbias_x = 0.0f;
  gbias.gbias_y = 0.0f;
  gbias.gbias_z = 0.0f;
  lsm6dsv16x_sflp_game_gbias_set(&dev_ctx, &gbias);

讀取四元數(shù)數(shù)據(jù)

FIFO_STATUS1（1Bh）和 FIFO_STATUS2（1Ch）寄存器中的 DIFF_FIFO [8:0] 字段包含在 FIFO 中收集的字（1 字節(jié)標(biāo)簽 + 6 字節(jié)數(shù)據(jù)）的數(shù)量。

/* Read watermark flag */
    status=lsm6dsv16x_fifo_status_get(&dev_ctx, &fifo_status);

之后需要通過(guò)FIFO_DATA_OUT_TAG (78h)判斷是什么數(shù)據(jù)準(zhǔn)備好，當(dāng)為SFLP game rotation vector(0X13)時(shí)候，為四元數(shù)準(zhǔn)備完畢。

之后讀取FIFO_DATA_OUT_X_L (79h)到FIFO_DATA_OUT_Z_H (7Eh)共6個(gè)字節(jié)數(shù)據(jù)，進(jìn)行四元數(shù)讀取。

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

    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;
        uint8_t *axis;
        float_t quat[4];
        float_t gravity_mg[3];
        float_t 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 = &f_data.data[0];
          gbias_mdps[0] = lsm6dsv16x_from_fs125_to_mdps(axis[0] | (axis[1] < < 8));
          gbias_mdps[1] = lsm6dsv16x_from_fs125_to_mdps(axis[2] | (axis[3] < < 8));
          gbias_mdps[2] = lsm6dsv16x_from_fs125_to_mdps(axis[4] | (axis[5] < < 8));
          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 = &f_data.data[0];
          gravity_mg[0] = lsm6dsv16x_from_sflp_to_mg(axis[0] | (axis[1] < < 8));
          gravity_mg[1] = lsm6dsv16x_from_sflp_to_mg(axis[2] | (axis[3] < < 8));
          gravity_mg[2] = lsm6dsv16x_from_sflp_to_mg(axis[4] | (axis[5] < < 8));
          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("[%02x %02x %02x %02x %02x %02x] Game Rotation tX: %2.3ftY: %2.3ftZ: %2.3ftW: %2.3frn",
                   f_data.data[0], f_data.data[1],f_data.data[2],f_data.data[3],f_data.data[4],f_data.data[5],
                   (double_t)quat[0], (double_t)quat[1], (double_t)quat[2], (double_t)quat[3]);


          break;
        default:
         break;
        }
      }

      printf("------ rnrn");

    }        


    /* USER CODE END WHILE */

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

最后轉(zhuǎn)換為姿態(tài)角。

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

    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;

      sprintf((char *)tx_buffer, "-- 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 */

演示

審核編輯 黃宇