陀螺仪LSM6DSV80X开发(4)----FIFO 读取陀螺仪数据

嵌入式单片机MCU开发 2026-06-02 43

描述

@[TOC](陀螺仪LSM6DSV80X开发.4--FIFO 读取陀螺仪数据)

概述

本文介绍如何在 LSM6DSV80X 上配置 FIFO（Stream 模式 + Watermark），并在 FIFO 达到阈值后批量读出数据，通过 TAG 解析出陀螺仪样本并完成单位换算与输出。该方法适合连续采样、降低 MCU 读取开销的应用场景。

最近在弄ST的课程，需要样片的可以加群申请：615061293 。
fifo

视频教学

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

样品申请

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

源码下载

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

硬件准备

首先需要准备一个开发板，这里我准备的是自己绘制的开发板，需要的可以进行申请。

主控为STM32H503CB，陀螺仪为LSM6DSV80X，磁力计为LIS2MDL。

fifo

参考程序

https://github.com/CoreMaker-lab/LSM6DSV80X

https://gitee.com/CoreMaker/LSM6DSV80X

FIFO存储

FIFO 可以存：
● Gyroscope（陀螺）
● Low-g accelerometer（低量程加速度）
● High-g accelerometer（高量程加速度）
● External sensors（最多 4 路外部传感器，走 sensor hub）
● Step counter（计步）
● Timestamp（时间戳）
● Temperature（温度）
● FSM events（FSM 事件）
● High-g peak value（高 G 峰值）
● MLC features/filters/results（MLC 特征/滤波/结果）

fifo

生成STM32CUBEMX

用STM32CUBEMX生成例程，这里使用MCU为STM32H503CB。
配置时钟树，配置时钟为250M。

fifo

串口配置

查看原理图，PA9和PA10设置为开发板的串口。

fifo

配置串口，速率为2000000。

fifo

IIC配置

fifo

LSM6DSV80X最大IIC通讯速率为1M，配置IIC速度为400k

fifo

CS和SA0设置

fifo

由于还有一个磁力计，需要把该CS也使能。

fifo

INT配置

INT1管脚为PB1。

fifo

配置如下所示。

fifo

开启中断。

fifo

ICASHE

fifo

修改堆栈

fifo

串口重定向

打开魔术棒，勾选MicroLIB

fifo

在main.c中，添加头文件，若不添加会出现 identifier "FILE" is undefined报错。

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

函数声明和串口重定向：

/* 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/lsm6dsv80x-pid

初始换管脚

由于需要向LSM6DSV80X_I2C_ADD_L写入以及为IIC模式。

fifo

所以使能CS为高电平，配置为IIC模式。
配置SA0为低电平。

printf("HELLO!n");
  HAL_GPIO_WritePin(CS1_GPIO_Port, CS1_Pin, GPIO_PIN_SET);
  HAL_GPIO_WritePin(SA0_GPIO_Port, SA0_Pin, GPIO_PIN_RESET);
  HAL_GPIO_WritePin(CS2_GPIO_Port, CS2_Pin, GPIO_PIN_SET);
	HAL_Delay(100);
	
	
	lsm6dsv80x_pin_int1_route_t pin_int = { 0 };

  /* 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();
  /* Wait sensor boot time */
  platform_delay(BOOT_TIME);

获取ID

可以向WHO_AM_I (0Fh)获取固定值，判断是否为0x73。

fifo

lsm6dsv80x_device_id_get为获取函数。

fifo

对应的获取ID驱动程序,如下所示。

/* Check device ID */
  lsm6dsv80x_device_id_get(&dev_ctx, &whoamI);
	printf("LSM6DSV80X_ID=0x%x,whoamI=0x%x",LSM6DSV80X_ID,whoamI);
  if (whoamI != LSM6DSV80X_ID)
    while (1);

软件上电复位

写 0x01（FUNC_CFG_ACCESS）这个寄存器，把 SW_POR 置 1，然后等待芯片完成复位。
fifo

lsm6dsv80x_sw_por为软件上电复位 / 全局复位函数。

fifo

对应的驱动程序,如下所示。

/* Perform device power-on-reset */
  lsm6dsv80x_sw_por(&dev_ctx);

BDU设置

在很多传感器中，数据通常被存储在输出寄存器中，这些寄存器分为两部分：MSB和LSB。这两部分共同表示一个完整的数据值。例如，在一个加速度计中，MSB和LSB可能共同表示一个加速度的测量值。
连续更新模式（BDU = '0'）：在默认模式下，输出寄存器的值会持续不断地被更新。这意味着在你读取MSB和LSB的时候，寄存器中的数据可能会因为新的测量数据而更新。这可能导致一个问题：当你读取MSB时，如果寄存器更新了，接下来读取的LSB可能就是新的测量值的一部分，而不是与MSB相对应的值。这样，你得到的就是一个“拼凑”的数据，它可能无法准确代表任何实际的测量时刻。
块数据更新（BDU）模式（BDU = '1'）：当激活BDU功能时，输出寄存器中的内容不会在读取MSB和LSB之间更新。这就意味着一旦开始读取数据（无论是先读MSB还是LSB），寄存器中的那一组数据就被“锁定”，直到两部分都被读取完毕。这样可以确保你读取的MSB和LSB是同一测量时刻的数据，避免了读取到代表不同采样时刻的数据。
简而言之，BDU位的作用是确保在读取数据时，输出寄存器的内容保持稳定，从而避免读取到拼凑或错误的数据。这对于需要高精度和稳定性的应用尤为重要。
可以向CTRL3 (12h)的BDU寄存器写入1进行开启。

fifo

对应的驱动程序,如下所示。

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

设置FIFO水印

FIFO控制寄存器1 (FIFO_CTRL1)：这个寄存器用于设置FIFO的水印阈值。
● WTM_[7:0]：FIFO水印阈值。当FIFO中写入的字节数大于或等于这个阈值时，水印标志位会被置高。
具体描述如下：
● 1 LSb = TAG (1 Byte) + 1 sensor (6 Bytes) written in FIFO：
○ 每个LSb表示一个TAG（1字节）和一个传感器的数据（6字节）被写入FIFO。
○ 因此，每个水印阈值单位对应的大小是7字节（1字节的TAG加上6字节的传感器数据）。
fifo

下面代码设置了FIFO的水印阈值。当FIFO中存储的数据达到该阈值时，传感器会产生一个中断信号，以通知主处理器读取数据。水印值是未读传感器数据TAG和6个字节的数据样本总数。

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

在配置LSM6DSV80X传感器的FIFO功能时，每个传感器数据样本的大小为6字节。这是因为加速度计和陀螺仪的每个数据样本都包含三个轴向的数据，每个轴向的数据用2字节表示。具体来说：
● 加速度计数据：包含X、Y、Z三个轴向的数据，每个轴向的数据大小为2字节。因此，加速度计的一个完整数据样本大小为3轴 * 2字节 = 6字节。
● 陀螺仪数据：同样包含X、Y、Z三个轴向的数据，每个轴向的数据大小也是2字节。因此，陀螺仪的一个完整数据样本大小也是3轴 * 2字节 = 6字节。
#define FIFO_WATERMARK 128的定义是为了在FIFO中存储128个样本后触发中断。因为每个样本大小为7字节（1字节的TAG和6字节的传感器数据），所以当FIFO中存储的数据达到896字节（128 * 7字节）时，会触发中断通知主处理器读取数据。
这可以通过以下公式计算：
水印阈值字节数=128×(1字节的TAG+6字节的传感器数据)=896字节
这个设置可以确保在适当的时间间隔内读取数据，既避免了频繁中断带来的开销，又不会因为FIFO溢出而丢失数据。

低 g 加速度写 FIFO 速率

● 对应寄存器：FIFO_CTRL3 (0x09)
● 对应位域：bdr_xl（Batch Data Rate for XL）
● 作用：决定低 g 加速度（XL_NC_TAG）以多少 Hz 写入 FIFO（这里是 60Hz）。
fifo
对应的驱动程序,如下所示。

/* Set FIFO batch XL/Gyro ODR */
  lsm6dsv80x_fifo_xl_batch_set(&dev_ctx, LSM6DSV80X_XL_BATCHED_AT_60Hz);

启动高 g 加速度写 FIFO

● 对应寄存器：COUNTER_BDR_REG1 (0x0B)
● 对应位域：xl_hg_batch_en
● 作用：开关：是否把高 g 加速度（XL_HG_TAG）也写入 FIFO
● 1：允许高 g 数据入 FIFO
● 0：高 g 不进 FIFO（即使高 g 通道在工作）

fifo

对应的驱动程序,如下所示。

lsm6dsv80x_fifo_hg_xl_batch_set(&dev_ctx, 1);

陀螺仪写 FIFO 速率

● 对应寄存器：FIFO_CTRL3 (0x09)
● 对应位域：bdr_gy（Batch Data Rate for GY）
● 作用：决定陀螺仪（GY_NC_TAG）以多少 Hz 写入 FIFO（这里是 120Hz）。

fifo
对应的驱动程序,如下所示。

lsm6dsv80x_fifo_gy_batch_set(&dev_ctx, LSM6DSV80X_GY_BATCHED_AT_120Hz);

SFLP速率设置

SFLP_ODR (5Eh)设置 SFLP (Sensor Fusion Low Power) 引擎的输出速率。配置 SFLP 的融合引擎120Hz。

fifo

对应的驱动程序,如下所示。

/* Set FIFO batch SFLP */
  lsm6dsv80x_sflp_data_rate_set(&dev_ctx, LSM6DSV80X_SFLP_120Hz);

初始化SFLP

● game_rotation=1: 把 game rotation quaternion 放进 FIFO（对应 LSM6DSV80X_SFLP_GAME_ROTATION_VECTOR_TAG）
● gravity=1: 把 gravity vector 放进 FIFO（对应 LSM6DSV80X_SFLP_GRAVITY_VECTOR_TAG）
● gbias=1: 把 gyro bias 放进 FIFO（对应 LSM6DSV80X_SFLP_GYROSCOPE_BIAS_TAG）

fifo

对应的驱动程序,如下所示。

lsm6dsv80x_fifo_sflp_raw_t sflp =
        {
          .game_rotation = 1,
          .gravity = 1,
          .gbias = 1,
        };
  lsm6dsv80x_fifo_sflp_batch_set(&dev_ctx, sflp);

启动SFLP

让 SFLP Game Rotation 引擎开始工作并产出四元数。

fifo

对应的驱动程序,如下所示。

lsm6dsv80x_sflp_game_rotation_set(&dev_ctx, PROPERTY_ENABLE);

配置 FIFO 工作模式

让 FIFO 进入连续写入模式；FIFO 满时覆盖最旧数据，确保系统持续输出最新的 XL/GY/SFLP 数据，配合 watermark 中断/轮询实现稳定的 FIFO 读数。
LSM6DSV80X_STREAM_MODE 这个枚举值在 lsm6dsv80x_reg.h 里定义为：
● LSM6DSV80X_STREAM_MODE = 0x6（二进制就是 110b）
也就是说：把 FIFO_CTRL4.fifo_mode 写成 0x6 → FIFO 进入 Stream 模式。

fifo

对应的驱动程序,如下所示。

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

设置FIFO时间戳批处理速率

LSM6DSV80X传感器的时间戳批处理速率、温度数据批处理速率、增强的EIS陀螺仪输出批处理，以及FIFO的工作模式。这些配置确保传感器数据能够以适当的速率和模式进行批处理和存储，以满足不同的应用需求。
fifo
对应的驱动程序,如下所示。

lsm6dsv80x_fifo_timestamp_batch_set(&dev_ctx, LSM6DSV80X_TMSTMP_DEC_32);

使能时间戳

FUNCTIONS_ENABLE寄存器（地址50h）的TIMESTAMP_EN可以使能时间戳计数器。计数器的值可以从TIMESTAMP0（40h），TIMESTAMP1（41h），TIMESTAMP2（42h）和TIMESTAMP3（43h）寄存器读取。
fifo

lsm6dsv80x_timestamp_set(&dev_ctx, PROPERTY_ENABLE);

速率设置

/* Set Output Data Rate.
   * Selected data rate have to be equal or greater with respect
   * with MLC data rate.
   */
  lsm6dsv80x_xl_setup(&dev_ctx, LSM6DSV80X_ODR_AT_60Hz, LSM6DSV80X_XL_HIGH_PERFORMANCE_MD);
  lsm6dsv80x_hg_xl_data_rate_set(&dev_ctx, LSM6DSV80X_HG_XL_ODR_AT_480Hz, 0);
  lsm6dsv80x_gy_setup(&dev_ctx, LSM6DSV80X_ODR_AT_120Hz, LSM6DSV80X_GY_HIGH_PERFORMANCE_MD);

量程设置

/* Set full scale */
  lsm6dsv80x_xl_full_scale_set(&dev_ctx, LSM6DSV80X_2g);
  lsm6dsv80x_hg_xl_full_scale_set(&dev_ctx, LSM6DSV80X_80g);
  lsm6dsv80x_gy_full_scale_set(&dev_ctx, LSM6DSV80X_2000dps);

数字滤波链

/* Configure filtering chain */
  filt_settling_mask.drdy = PROPERTY_ENABLE;
  filt_settling_mask.irq_xl = PROPERTY_ENABLE;
  filt_settling_mask.irq_g = PROPERTY_ENABLE;
  lsm6dsv80x_filt_settling_mask_set(&dev_ctx, filt_settling_mask);
  lsm6dsv80x_filt_gy_lp1_set(&dev_ctx, PROPERTY_ENABLE);
  lsm6dsv80x_filt_gy_lp1_bandwidth_set(&dev_ctx, LSM6DSV80X_GY_ULTRA_LIGHT);
  lsm6dsv80x_filt_xl_lp2_set(&dev_ctx, PROPERTY_ENABLE);
  lsm6dsv80x_filt_xl_lp2_bandwidth_set(&dev_ctx, LSM6DSV80X_XL_STRONG);

FIFO 水位中断

● 寄存器：INT1_CTRL
● 位域：int1_fifo_th
● 作用：当 FIFO 达到 watermark（fifo_watermark_set() 配的阈值）时，在 INT1 引脚输出中断。

fifo

pin_int.fifo_th = PROPERTY_ENABLE;
  lsm6dsv80x_pin_int1_route_set(&dev_ctx, &pin_int);
  //lsm6dsv80x_pin_int2_route_set(&dev_ctx, &pin_int);

读取四元数数据

FIFO_STATUS1（1Bh）和 FIFO_STATUS2（1Ch）寄存器中的 DIFF_FIFO [8:0] 字段包含在 FIFO 中收集的字（1 字节标签 + 6 字节数据）的数量。

fifo
fifo

/* Read watermark flag */
    lsm6dsv80x_fifo_status_get(&dev_ctx, &fifo_status);

之后需要通过FIFO_DATA_OUT_TAG (78h)判断是什么数据准备好，当为SFLP game rotation vector(0X13)时候，为四元数准备完毕。

fifo
之后读取FIFO_DATA_OUT_X_L (79h)到FIFO_DATA_OUT_Z_H (7Eh)共6个字节数据，进行四元数读取。
fifo

其中时间戳速度单位为21.7us。
fifo

最后转换为姿态角。

static void lsm6dsv80x_fifo_thread(void)
{
  float_t acceleration_mg[3];
  float_t angular_rate_mdps[3];
  float_t gravity_mg[3], gbias_mdps[3], quat[4];
  uint8_t *axis;

  if (fifo_thread_run) {
    lsm6dsv80x_fifo_status_t fifo_status;
    uint16_t num;

    fifo_thread_run = 0;

    /* Read watermark flag */
    lsm6dsv80x_fifo_status_get(&dev_ctx, &fifo_status);

    num = fifo_status.fifo_level;

    while (num--) {
      lsm6dsv80x_fifo_out_raw_t f_data;

      /* Read FIFO sensor value */
      lsm6dsv80x_fifo_out_raw_get(&dev_ctx, &f_data);
      datax = (int16_t *)&f_data.data[0];
      datay = (int16_t *)&f_data.data[2];
      dataz = (int16_t *)&f_data.data[4];
      ts = (int32_t *)&f_data.data[0];

      switch (f_data.tag) {
      case LSM6DSV80X_XL_NC_TAG:
        /* Read acceleration field data */
        acceleration_mg[0] = lsm6dsv80x_from_fs2_to_mg(*datax);
        acceleration_mg[1] = lsm6dsv80x_from_fs2_to_mg(*datay);
        acceleration_mg[2] = lsm6dsv80x_from_fs2_to_mg(*dataz);

        lowg_xl_sum[0] += acceleration_mg[0];
        lowg_xl_sum[1] += acceleration_mg[1];
        lowg_xl_sum[2] += acceleration_mg[2];
        lowg_xl_cnt++;
        break;

      case LSM6DSV80X_XL_HG_TAG:
        acceleration_mg[0] = lsm6dsv80x_from_fs80_to_mg(*datax);
        acceleration_mg[1] = lsm6dsv80x_from_fs80_to_mg(*datay);
        acceleration_mg[2] = lsm6dsv80x_from_fs80_to_mg(*dataz);

        hg_xl_sum[0] += acceleration_mg[0];
        hg_xl_sum[1] += acceleration_mg[1];
        hg_xl_sum[2] += acceleration_mg[2];
        hg_xl_cnt++;
        break;

      case LSM6DSV80X_GY_NC_TAG:
        angular_rate_mdps[0] = lsm6dsv80x_from_fs2000_to_mdps(*datax);
        angular_rate_mdps[1] = lsm6dsv80x_from_fs2000_to_mdps(*datay);
        angular_rate_mdps[2] = lsm6dsv80x_from_fs2000_to_mdps(*dataz);

        gyro_sum[0] += angular_rate_mdps[0];
        gyro_sum[1] += angular_rate_mdps[1];
        gyro_sum[2] += angular_rate_mdps[2];
        gyro_cnt++;
        break;

      case LSM6DSV80X_SFLP_GYROSCOPE_BIAS_TAG:
        axis = &f_data.data[0];
        gbias_mdps[0] = lsm6dsv80x_from_fs125_to_mdps(axis[0] | (axis[1] < < 8));
        gbias_mdps[1] = lsm6dsv80x_from_fs125_to_mdps(axis[2] | (axis[3] < < 8));
        gbias_mdps[2] = lsm6dsv80x_from_fs125_to_mdps(axis[4] | (axis[5] < < 8));

        gbias_sum[0] += gbias_mdps[0];
        gbias_sum[1] += gbias_mdps[1];
        gbias_sum[2] += gbias_mdps[2];
        gbias_cnt++;
        break;

      case LSM6DSV80X_SFLP_GRAVITY_VECTOR_TAG:
        axis = &f_data.data[0];
        gravity_mg[0] = lsm6dsv80x_from_sflp_to_mg(axis[0] | (axis[1] < < 8));
        gravity_mg[1] = lsm6dsv80x_from_sflp_to_mg(axis[2] | (axis[3] < < 8));
        gravity_mg[2] = lsm6dsv80x_from_sflp_to_mg(axis[4] | (axis[5] < < 8));

        gravity_sum[0] += gravity_mg[0];
        gravity_sum[1] += gravity_mg[1];
        gravity_sum[2] += gravity_mg[2];
        gravity_cnt++;
        break;

      case LSM6DSV80X_SFLP_GAME_ROTATION_VECTOR_TAG:
      {
        uint16_t *sflp = (uint16_t *)&f_data.data[2];

        if (f_data.data[0] == 0x00) {
          /* Game Rotation first word */
          quat[0] = lsm6dsv80x_from_quaternion_lsb_to_float(sflp[0]);
          quat[1] = lsm6dsv80x_from_quaternion_lsb_to_float(sflp[1]);
        } else if (f_data.data[0] == 0x01) {
          /* Game Rotation second word */
          quat[2] = lsm6dsv80x_from_quaternion_lsb_to_float(sflp[0]);
          quat[3] = lsm6dsv80x_from_quaternion_lsb_to_float(sflp[1]);

          rot_sum[0] += quat[0];
          rot_sum[1] += quat[1];
          rot_sum[2] += quat[2];
          rot_sum[3] += quat[3];

          rot_cnt++;
        } else {
          /* error */
          printf("[%02x - %02x] wrong word rn", f_data.data[0], f_data.data[1]);
       }

        break;
      }

      case LSM6DSV80X_TIMESTAMP_TAG:
        printf("TIMESTAMP [ms] %drn", *ts);

        /* print avg low-g xl data */
        if (lowg_xl_cnt > 0) {
          acceleration_mg[0] = lowg_xl_sum[0] / lowg_xl_cnt;
          acceleration_mg[1] = lowg_xl_sum[1] / lowg_xl_cnt;
          acceleration_mg[2] = lowg_xl_sum[2] / lowg_xl_cnt;

          printf("lg xl (avg of %d samples) [mg]:%4.2ft%4.2ft%4.2frn",
                  lowg_xl_cnt, acceleration_mg[0], acceleration_mg[1], acceleration_mg[2]);
          lowg_xl_sum[0] = lowg_xl_sum[1] = lowg_xl_sum[2] = 0.0;
          lowg_xl_cnt = 0;
        }

        /* print avg high-g xl data */
        if (hg_xl_cnt > 0) {
          acceleration_mg[0] = hg_xl_sum[0] / hg_xl_cnt;
          acceleration_mg[1] = hg_xl_sum[1] / hg_xl_cnt;
          acceleration_mg[2] = hg_xl_sum[2] / hg_xl_cnt;

          printf("hg xl (avg of %d samples) [mg]:%4.2ft%4.2ft%4.2frn",
                  hg_xl_cnt, acceleration_mg[0], acceleration_mg[1], acceleration_mg[2]);
          hg_xl_sum[0] = hg_xl_sum[1] = hg_xl_sum[2] = 0.0;
          hg_xl_cnt = 0;
        }

        /* print avg gyro data */
        if (gyro_cnt > 0) {
          angular_rate_mdps[0] = gyro_sum[0] / gyro_cnt;
          angular_rate_mdps[1] = gyro_sum[1] / gyro_cnt;
          angular_rate_mdps[2] = gyro_sum[2] / gyro_cnt;

          printf("gyro (avg of %d samples) [mdps]:%4.2ft%4.2ft%4.2frn",
                  gyro_cnt, angular_rate_mdps[0], angular_rate_mdps[1], angular_rate_mdps[2]);
          gyro_sum[0] = gyro_sum[1] = gyro_sum[2] = 0.0;
          gyro_cnt = 0;
        }

         /* print SFLP gbias data */
        if (gbias_cnt > 0) {
          gbias_mdps[0] = gbias_sum[0] / gbias_cnt;
          gbias_mdps[1] = gbias_sum[1] / gbias_cnt;
          gbias_mdps[2] = gbias_sum[2] / gbias_cnt;

          printf("SFLP gbias (avg of %d samples) [mdps]:%4.2ft%4.2ft%4.2frn",
                  gbias_cnt, gbias_mdps[0], gbias_mdps[1], gbias_mdps[2]);
          gbias_sum[0] = gbias_sum[1] = gbias_sum[2] = 0.0;
          gbias_cnt = 0;
        }

         /* print SFLP gravity data */
        if (gravity_cnt > 0) {
          gravity_mg[0] = gravity_sum[0] / gravity_cnt;
          gravity_mg[1] = gravity_sum[1] / gravity_cnt;
          gravity_mg[2] = gravity_sum[2] / gravity_cnt;

          printf("SFLP gravity (avg of %d samples) [mg]:%4.2ft%4.2ft%4.2frn",
                  gravity_cnt, gravity_mg[0], gravity_mg[1], gravity_mg[2]);
          gravity_sum[0] = gravity_sum[1] = gravity_sum[2] = 0.0;
          gravity_cnt = 0;
        }

         /* print SFLP game rotation data */
        if (rot_cnt > 0) {
          quat[0] = rot_sum[0] / rot_cnt;
          quat[1] = rot_sum[1] / rot_cnt;
          quat[2] = rot_sum[2] / rot_cnt;
          quat[3] = rot_sum[3] / rot_cnt;

          printf("SFLP rotation (avg of %d samples) [quaternions]:X: %2.3ftY: %2.3ftZ: %2.3ftW: %2.3frnrn",
                  rot_cnt, quat[0], quat[1], quat[2], quat[3]);
          rot_sum[0] = rot_sum[1] = rot_sum[2] = rot_sum[3] = 0.0;
          rot_cnt = 0;
					
		

        /* 轴变换：保持你“能跑通欧拉角”工程的映射 */
        quaternion_t q;
        q.quat_w =  quat[3];
        q.quat_x = -quat[1];
        q.quat_y =  quat[2];
        q.quat_z = -quat[0];

        euler_angle_t e;
        quaternion_to_euler_angle(&q, &e);

        /* 可选：Yaw 显示到 [-180, 180] */
        float yaw_print = e.yaw;
        if (yaw_print > 180.0f) {
          yaw_print -= 360.0f;
        }

        printf("Roll=%.2f, Pitch=%.2f, Yaw=%.2frn",
               e.roll, e.pitch, yaw_print);


					
					
					
        }

        break;

      default:
        printf("[%02x] UNHANDLED TAG rn", f_data.tag);
        break;
      }
    }
  }
}

演示

fifo

审核编辑黄宇

打开APP阅读更多精彩内容