陀螺仪LSM6DSV80X开发(3)----基于SFLP输出欧拉角的中断驱动

嵌入式单片机MCU开发 2026-06-07 1289
加入交流群

嵌入式单片机MCU开发

370 文章 122.3w阅读 255粉丝

描述

概述

在现代的运动跟踪和姿态检测应用中，低功耗、高精度的传感器数据融合处理变得越来越重要。LSM6DSV80X传感器集成了SFLP（Sensor Fusion Low Power）算法模块，可以在低功耗模式下实现六轴传感器数据的高效融合。SFLP模块通过处理加速度计和陀螺仪的数据，生成一个表示设备姿态的四元数，这为游戏、增强现实（AR）、虚拟现实（VR）等应用中的精准运动追踪提供了技术支持。在本文中，我们将深入探讨如何利用SFLP模块获取四元数数据，并分析其在实际应用中的优势和实现方法。

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

开发板

视频教学

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

样品申请

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

源码下载

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

硬件准备

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

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

开发板

参考程序

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

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

SFLP

LSM6DSV80X 特性涉及到的是一种低功耗的传感器融合算法（Sensor Fusion Low Power, SFLP）.
低功耗传感器融合（SFLP）算法：
该算法旨在以节能的方式结合加速度计和陀螺仪的数据。传感器融合算法通过结合不同传感器的优势，提供更准确、可靠的数据。
6轴游戏旋转向量：
SFLP算法能够生成游戏旋转向量。这种向量是一种表示设备在空间中方向的数据，特别适用于游戏和增强现实应用，这些应用中理解设备的方向和运动非常关键。
四元数表示法：
旋转向量以四元数的形式表示。四元数是一种编码3D旋转的方法，它避免了欧拉角等其他表示法的一些限制（如万向节锁）。一个四元数有四个分量（X, Y, Z 和 W），其中 X, Y, Z 代表向量部分，W 代表标量部分。
FIFO存储：
四元数的 X, Y, Z 分量存储在 LSM6DSV80X 的 FIFO（先进先出）缓冲区中。FIFO 缓冲区是一种数据存储方式，允许临时存储传感器数据。这对于有效管理数据流非常有用，特别是在数据处理可能不如数据收集那么快的系统中。

开发板

图片包含了关于 LSM6DSV80X 传感器的低功耗传感器融合（Sensor Fusion Low Power, SFLP）功能的说明。这里是对图片内容的解释： SFLP 功能：

  1. SFLP 单元用于生成基于加速度计和陀螺仪数据处理的以下数据：
  2. 游戏旋转向量：以四元数形式表示设备的姿态。
  3. 重力向量：提供一个三维向量，表示重力方向。
  4. 陀螺仪偏差：提供一个三维向量，表示陀螺仪的偏差。 激活与重置：
  5. 通过在 EMB_FUNC_EN_A（04h）嵌入式功能寄存器中设置 SFLP_GAME_EN 位为 1 来激活 SFLP 单元。
  6. 通过在 EMB_FUNC_INIT_A（66h）嵌入式功能寄存器中设置 SFLP_GAME_INIT 位为 1 来重置 SFLP 单元。 性能参数表： 表格展示了 SFLP 功能在不同情况下的性能，包括静态精度、低动态精度和高动态精度，以及校准时间和方向稳定时间。这些参数反映了传感器在不同运动状态下的精确度和响应速度。

开发板

生成STM32CUBEMX

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

开发板

串口配置

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

开发板

配置串口，速率为2000000。

开发板

IIC配置

开发板

开发板

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

开发板

CS和SA0设置

开发板

开发板

开发板

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

开发板

开发板

开发板

INT配置

INT1管脚为PB1。

开发板

开发板

配置如下所示。

开发板

开启中断。

开发板

ICASHE

开发板

修改堆栈

开发板

串口重定向

打开魔术棒，勾选MicroLIB

开发板

在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]

变量定义

/* USER CODE BEGIN 0 */
#define SENSOR_BUS hi2c1

/* Private macro -------------------------------------------------------------*/
#define    BOOT_TIME            10 //ms
#define    CNT_FOR_OUTPUT       100

/* Private variables ---------------------------------------------------------*/
static int16_t data_raw_motion[3];

static uint8_t whoamI;
static uint8_t tx_buffer[1000];

static lsm6dsv80x_filt_settling_mask_t filt_settling_mask;

/* Extern variables ----------------------------------------------------------*/

/* Private functions ---------------------------------------------------------*/

/*
 *   WARNING:
 *   Functions declare in this section are defined at the end of this file
 *   and are strictly related to the hardware platform used.
 *
 */
static int32_t platform_write(void *handle, uint8_t reg, const uint8_t *bufp,
                              uint16_t len);
static int32_t platform_read(void *handle, uint8_t reg, uint8_t *bufp,
                             uint16_t len);
static void tx_com( uint8_t *tx_buffer, uint16_t len );
static void platform_delay(uint32_t ms);
static void platform_init(void);

static   stmdev_ctx_t dev_ctx;
volatile  uint8_t thread_wake = 0;


typedef struct
{
  float yaw;
  float pitch;
  float roll;
} euler_angle_t;

typedef struct
{
  float quat_w;
  float quat_x;
  float quat_y;
  float quat_z;
} quaternion_t;

static float_t npy_half_to_float(uint16_t h)
{
    union { float_t ret; uint32_t retbits; } conv;
    conv.retbits = lsm6dsv80x_from_f16_to_f32(h);
    return conv.ret;
}

static void sflp2q(float_t quat[4], uint16_t sflp[3])
{
  float_t sumsq = 0;

  quat[0] = npy_half_to_float(sflp[0]);
  quat[1] = npy_half_to_float(sflp[1]);
  quat[2] = npy_half_to_float(sflp[2]);

  for (uint8_t i = 0; i < 3; i++)
    sumsq += quat[i] * quat[i];

  if (sumsq > 1.0f) {
    float_t n = sqrtf(sumsq);
    quat[0] /= n;
    quat[1] /= n;
    quat[2] /= n;
    sumsq = 1.0f;
  }

  quat[3] = sqrtf(1.0f - sumsq);
}


void HAL_GPIO_EXTI_Rising_Callback(uint16_t GPIO_Pin)
{
  if (GPIO_Pin == INT1_Pin) {
    thread_wake = 1;
  }
}

void quaternion_to_euler_angle(quaternion_t *q,euler_angle_t *euler)
{
        if (q- >quat_w < 0.0f) {
            q- >quat_x *= -1.0f;
            q- >quat_y *= -1.0f;
            q- >quat_z *= -1.0f;
            q- >quat_w *= -1.0f;
        }

        float sqx = q- >quat_x * q- >quat_x;
        float sqy = q- >quat_y * q- >quat_y;
        float sqz = q- >quat_z * q- >quat_z;

        euler- >yaw = -atan2f(2.0f * (q- >quat_y * q- >quat_w + q- >quat_x * q- >quat_z), 1.0f - 2.0f * (sqy + sqx));
        euler- >pitch = -atan2f(2.0f * (q- >quat_x * q- >quat_y + q- >quat_z * q- >quat_w), 1.0f - 2.0f * (sqx + sqz));
        euler- >roll = -asinf(2.0f * (q- >quat_x * q- >quat_w - q- >quat_y * q- >quat_z));

        if (euler- >yaw < 0.0f)
        {
            euler- >yaw += 2.0f * 3.1415926;
        }    

        euler- >yaw = 57.29578 * euler- >yaw;
        euler- >pitch = 57.29578 * euler- >pitch;
        euler- >roll = 57.29578 * euler- >roll;
}
/* USER CODE END 0 */

初始化

/* USER CODE BEGIN 2 */
    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);

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

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


  /* Enable Block Data Update */
  lsm6dsv80x_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.
   */
  lsm6dsv80x_xl_setup(&dev_ctx, LSM6DSV80X_ODR_AT_120Hz, LSM6DSV80X_XL_HIGH_PERFORMANCE_MD);
  lsm6dsv80x_gy_setup(&dev_ctx, LSM6DSV80X_ODR_AT_120Hz, LSM6DSV80X_GY_HIGH_PERFORMANCE_MD);
    lsm6dsv80x_hg_xl_data_rate_set(&dev_ctx, LSM6DSV80X_HG_XL_ODR_AT_480Hz, 1);
    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);


  /* --------- SFLP: 120Hz + Game rotation --------- */
  lsm6dsv80x_sflp_data_rate_set(&dev_ctx, LSM6DSV80X_SFLP_120Hz);
  lsm6dsv80x_sflp_game_rotation_set(&dev_ctx, PROPERTY_ENABLE);

  /* 推荐：写入 gbias 初值（与你能跑通版本一致） */
  lsm6dsv80x_sflp_gbias_t gbias = {0};
  gbias.gbias_x = 0.0f;
  gbias.gbias_y = 0.0f;
  gbias.gbias_z = 0.0f;
  lsm6dsv80x_sflp_game_gbias_set(&dev_ctx, &gbias);

  /* --------- INT1 route: 用 DRDY_XL 做唤醒（你现在中断链路已通） --------- */
  lsm6dsv80x_pin_int1_route_t pin_int1 = {0};
  pin_int1.drdy_xl = PROPERTY_ENABLE;
  lsm6dsv80x_pin_int1_route_set(&dev_ctx, &pin_int1);

  /* USER CODE END 2 */

初始换管脚

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

开发板

所以使能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。

开发板

lsm6dsv80x_device_id_get为获取函数。

开发板

对应的获取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，然后等待芯片完成复位。
开发板

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

开发板

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

/* 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进行开启。

开发板

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

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

设置低量程加速度速率

低量程加速度速率可以通过CTRL1 (10h)设置加速度速率。

开发板

设置加速度速率可以使用如下函数。

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

设置高量程加速度速率

高量程加速度速率可以通过CTRL1_XL_HG (4Eh)设置高量程加速度速率。

开发板

设置高量程加速度速率可以使用如下函数。

lsm6dsv80x_hg_xl_data_rate_set(&dev_ctx, LSM6DSV80X_HG_XL_ODR_AT_480Hz, 1);

设置角速度量程速率

角速度量程可以通过CTRL2 (11h)设置高量程加速度速率。

开发板

设置角速度速率可以使用如下函数。

lsm6dsv80x_gy_setup(&dev_ctx,LSM6DSV80X_ODR_AT_120Hz,LSM6DSV80X_GY_HIGH_PERFORMANCE_MD);

设置低量程加速度量程

低量程加速度量程可以通过CTRL8 (17h)设置。

开发板

设置加速度量程可以使用如下函数。

/* Set full scale */
  lsm6dsv80x_xl_full_scale_set(&dev_ctx, LSM6DSV80X_2g);

设置高量程加速度量程

高量程加速度量程可以通过CTRL1_XL_HG (4Eh)设置。

开发板

设置高量程加速度量程可以使用如下函数。

lsm6dsv80x_hg_xl_full_scale_set(&dev_ctx, LSM6DSV80X_80g);

需要注意的是高量程加速度的精度问题。

开发板

设置角速度量程

角速度量程可以通过CTRL6 (15h)设置。

开发板

设置角速度量程量程可以使用如下函数。

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

SFLP速率设置

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

开发板

/* 配置 SFLP (Sensor Fusion Low Power) 融合引擎的数据输出速率 */
  lsm6dsv80x_sflp_data_rate_set(&dev_ctx, LSM6DSV80X_SFLP_120Hz);

开启SFLP

EMB_FUNC_EN_A (0x04) 的 SFLP_GAME_EN位。使能SFLP输出，即开启基于低功耗融合的四元数计算。

开发板

/* 开启 Game Rotation Vector 融合算法输出（即四元数输出） */
  lsm6dsv80x_sflp_game_rotation_set(&dev_ctx, PROPERTY_ENABLE);

变量定义

/* 推荐：写入 gbias 初值（与你能跑通版本一致） */
  lsm6dsv80x_sflp_gbias_t gbias = {0};
  gbias.gbias_x = 0.0f;
  gbias.gbias_y = 0.0f;
  gbias.gbias_z = 0.0f;
  lsm6dsv80x_sflp_game_gbias_set(&dev_ctx, &gbias);

中断配置

把“低 g 加速度计（XL）的 Data-Ready 事件（DRDY_XL）路由到芯片的 INT1 引脚上，从而让 INT1 产生脉冲/中断 去唤醒 MCU

/* --------- INT1 route: 用 DRDY_XL 做唤醒（你现在中断链路已通） --------- */
  lsm6dsv80x_pin_int1_route_t pin_int1 = {0};
  pin_int1.drdy_xl = PROPERTY_ENABLE;
  lsm6dsv80x_pin_int1_route_set(&dev_ctx, &pin_int1);

主函数

/* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {
          if (thread_wake) {
      thread_wake = 0;
      /* 关键：必须先确认 XL new data（你验证过必须这样才有数据） */
      lsm6dsv80x_data_ready_t status;
      lsm6dsv80x_flag_data_ready_get(&dev_ctx, &status);
      if (status.drdy_xl) {

        /* 建议：读一次 XL 输出，等价“消费/清源”，保证下一次中断继续来 */
        lsm6dsv80x_acceleration_raw_get(&dev_ctx, data_raw_motion);

        /* 如果你希望再稳一点，可以加 SFLP ready 门控 */
        lsm6dsv80x_all_sources_t all_sources;
        lsm6dsv80x_all_sources_get(&dev_ctx, &all_sources);
        if (!all_sources.emb_func_stand_by) {
          continue;
        }

        /* 读 SFLP 四元数 */
        lsm6dsv80x_quaternion_t q_st;
        lsm6dsv80x_sflp_quaternion_get(&dev_ctx, &q_st);

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

        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);
      }
    }


//    if (thread_wake) {
//      lsm6dsv80x_data_ready_t status;

//      thread_wake = 0;

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

//      if (status.drdy_xl == 1) {
//        status.drdy_xl = 0;

//        lsm6dsv80x_acceleration_raw_get(&dev_ctx, data_raw_motion);

//        lsm6dsv80x_quaternion_t q_st;
//        lsm6dsv80x_sflp_quaternion_get(&dev_ctx, &q_st);

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

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

//        int16_t roll_i  = (int16_t)(e.roll  * 100.0f);
//        int16_t pitch_i = (int16_t)(e.pitch * 100.0f);
//        int16_t yaw_i   = (int16_t)(e.yaw   * 100.0f - 18000.0f);

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

    /* USER CODE END WHILE */

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

上报匿名上位机

添加上报定义。

uint8_t sumcheck = 0;
    uint8_t addcheck = 0;        

    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：融合状态

上报主函数

/* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {
          if (thread_wake) {
      thread_wake = 0;
      /* 关键：必须先确认 XL new data（你验证过必须这样才有数据） */
      lsm6dsv80x_data_ready_t status;
      lsm6dsv80x_flag_data_ready_get(&dev_ctx, &status);
      if (status.drdy_xl) {

        /* 建议：读一次 XL 输出，等价“消费/清源”，保证下一次中断继续来 */
        lsm6dsv80x_acceleration_raw_get(&dev_ctx, data_raw_motion);

        /* 如果你希望再稳一点，可以加 SFLP ready 门控 */
        lsm6dsv80x_all_sources_t all_sources;
        lsm6dsv80x_all_sources_get(&dev_ctx, &all_sources);
        if (!all_sources.emb_func_stand_by) {
          continue;
        }

        /* 读 SFLP 四元数 */
        lsm6dsv80x_quaternion_t q_st;
        lsm6dsv80x_sflp_quaternion_get(&dev_ctx, &q_st);

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

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


            // 定义用于存放欧拉角的整数变量（放大100倍，用于发送）
            int16_t    Roll_int16;
            int16_t    Pitch_int16;                    
            int16_t    Yaw_int16;    
            // 将浮点欧拉角转成定点整数，单位：度 × 100
            Roll_int16 = (int16_t)((e.roll) * 100);
            Pitch_int16 = (int16_t)((e.pitch) * 100);
            Yaw_int16 = (int16_t)((e.yaw) * 100 - 18000);

//            printf("2:Roll=%d,Pitch=%d,Yaw=%d n",
//                                            Roll_int16,Pitch_int16,Yaw_int16);    


            data_angular_rate_raw[7] = (uint8_t)(Roll_int16 > > 8);//roll
            data_angular_rate_raw[8] = (uint8_t)(Roll_int16 );
            data_angular_rate_raw[9] = (uint8_t)(Pitch_int16 > > 8);//pitch
            data_angular_rate_raw[10] = (uint8_t)(Pitch_int16 );
            data_angular_rate_raw[11] = (uint8_t)(Yaw_int16 > > 8);//yaw
            data_angular_rate_raw[12] = (uint8_t)(Yaw_int16 );

            data_angular_rate_raw[13] = 0;
            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);                

      }
    }
    /* USER CODE END WHILE */

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

演示

开发板

开发板

开发板

打开APP阅读更多精彩内容
声明：本文内容及配图由入驻作者撰写或者入驻合作网站授权转载。文章观点仅代表作者本人，不代表电子发烧友网立场。文章及其配图仅供工程师学习之用，如有内容侵权或者其他违规问题，请联系本站处理。 举报投诉

全部0条评论

快来发表一下你的评论吧 !

下载APP
|投诉反馈|电子发烧友网
© 2021 elecfans.com
湘ICP备2023036445号-105
×
20
完善资料，
赚取积分