内容介绍

项目介绍

本项目使用意法半导体的传感器板和MCU，运用其单片机开发库和运动库进行传感器融合，对三维运动姿态进行解算，并使用加速度计进行累加，实现简易里程计对VR体感追踪器相对位置进行追踪。将结算数据传输到上位机，进行显示。

主要技术路线

嵌入式侧

使用CUBE MX生成STM32G0的配置，对平台进行快速初始化；
使用CMake和VS Code对工程进行管理；
使用HAL库对STM32进行开发；
使用MotionFX库对传感器数据进行融合；
通过串口将最终数据上传到上位机。

上位机

使用python开发上位机，使用vofa+的JustFloat协议作为数据帧传输协议；
调用serial和struct包，接收并解析帧数据；
调用numpy库进行矩阵运算，对接收的四元数数据进行解算；
根据结算出的顶点位置使用matplotlib和mpl_toolkits.mplot3d.art3d库绘制，
使用GPT完成代码编写。

程序流程图

单片机主程序和终端程序流程图

上位机程序流程图

代码介绍

单片机代码

主函数

初始化流程

  IKS4A1_I2C_Init();                                                              // 初始化I2C
  IKS4A1_ENV_SENSOR_Init(IKS4A1_LPS22DF_0, ENV_PRESSURE);                         // 初始化ENV_SENSOR IKS4A1_LPS22DF_0采集Pressure
  IKS4A1_MOTION_SENSOR_Init(IKS4A1_LSM6DSO16IS_0, MOTION_GYRO | MOTION_ACCELERO); // 初始化MOTION_SENSOR LSM6DSO16IS_0采集Gyro
  IKS4A1_MOTION_SENSOR_Init(IKS4A1_LSM6DSV16X_0, MOTION_GYRO | MOTION_ACCELERO);  // 初始化MOTION_SENSOR LSM6DSV16X_0采集Accelerometer
  IKS4A1_MOTION_SENSOR_Init(IKS4A1_LIS2DUXS12_0, MOTION_ACCELERO);                //
  IKS4A1_MOTION_SENSOR_Init(IKS4A1_LIS2MDL_0, MOTION_MAGNETO);                    // 初始化MOTION_SENSOR LIS2MDL_0采集Magnetometer

  MotionFX_CM0P_initialize(MFX_CM0P_MCU_STM32);																		// 初始化MotionFX库

  MotionFX_CM0P_setOrientation("nwu", "nwu", "swu");															// 配置MotionFX库
  MotionFX_CM0P_enable_9X(MFX_CM0P_ENGINE_DISABLE);
  MotionFX_CM0P_enable_6X(MFX_CM0P_ENGINE_ENABLE);
  MotionFX_CM0P_enable_euler(MFX_CM0P_ENGINE_ENABLE);
  MotionFX_CM0P_enable_gbias(MFX_CM0P_ENGINE_ENABLE);

进入初始化后，对IKS4A1所需要使用的通信接口进行初始化，然后对项目中需要使用的部分传感器进行初始化，最后将Motion库进行初始化，启用姿态解算。

校准流程

  HAL_Delay(2000);
  HAL_GPIO_WritePin(LED_GREEN_GPIO_Port, LED_GREEN_Pin, GPIO_PIN_SET); //
  // HAL_Delay(2000);

  IKS4A1_MOTION_SENSOR_Axes_t temp_axes;
  int32_t off_gyro[3] = {0, 0, 0};
  float off_gyro_f[3] = {0, 0, 0};
  float get_gyro_f[3] = {0, 0, 0};
  int ENV_SENSOR_ERR = 0;
#define CALIBRATION_TIMES 200
  for (int i = 0; i < CALIBRATION_TIMES; i++)
  {
    ENV_SENSOR_ERR = IKS4A1_MOTION_SENSOR_GetAxes(IKS4A1_LSM6DSV16X_0, MOTION_GYRO, &temp_axes);
    if (ENV_SENSOR_ERR == 0)
    {
      off_gyro[0] += temp_axes.x;
      off_gyro[1] += temp_axes.y;
      off_gyro[2] += temp_axes.z;
    }
    HAL_Delay(40);
  }
  off_gyro_f[0] = (float)off_gyro[0] / (CALIBRATION_TIMES * 1000);
  off_gyro_f[1] = (float)off_gyro[1] / (CALIBRATION_TIMES * 1000);
  off_gyro_f[2] = (float)off_gyro[2] / (CALIBRATION_TIMES * 1000);
  MotionFX_CM0P_setGbias(off_gyro_f);
  MotionFX_CM0P_getGbias(get_gyro_f);
  int getStatus_gbias = MotionFX_CM0P_getStatus_gbias();

  HAL_GPIO_WritePin(LED_GREEN_GPIO_Port, LED_GREEN_Pin, GPIO_PIN_RESET); // 关灯表示校准完成
  HAL_TIM_Base_Start_IT(&htim6);

延迟一段时间后，点亮绿色LED(LD4)表示开始校准，保持静止状态采集多次GYRO值，求平均值得到初始偏移量，使用MotionFX库中的接口设置初始偏移量，实现对陀螺仪的校准，关闭LED表示校准完成，启动定时器中断。

中断服务函数

数据采集

  if (cnt % 4 == 0)
  {
    ENV_SENSOR_ERR = IKS4A1_ENV_SENSOR_GetValue(IKS4A1_LPS22DF_0, ENV_PRESSURE, &pressure_value);
    if (ENV_SENSOR_ERR == 0)
    {
      myFrame.fdata[0] = pressure_value;
    }
    else
    {
      myFrame.fdata[0] = ENV_SENSOR_ERR;
    }
  }

  ENV_SENSOR_ERR = IKS4A1_MOTION_SENSOR_GetAxes(IKS4A1_LSM6DSV16X_0, MOTION_GYRO, &axes_gyro);
  if (ENV_SENSOR_ERR == 0)
  {
    data_in.gyro[0] = (float)axes_gyro.x / 1000.0;
    data_in.gyro[1] = (float)axes_gyro.y / 1000.0;
    data_in.gyro[2] = (float)axes_gyro.z / 1000.0;
  }
  else
  {
  }

  ENV_SENSOR_ERR = IKS4A1_MOTION_SENSOR_GetAxes(IKS4A1_LSM6DSV16X_0, MOTION_ACCELERO, &axes_acc);
  if (ENV_SENSOR_ERR == 0)
  {
    data_in.acc[0] = (float)axes_acc.x / 1000.0;
    data_in.acc[1] = (float)axes_acc.y / 1000.0;
    data_in.acc[2] = (float)axes_acc.z / 1000.0;
  }
  else
  {
  }

根据手册信息，通过复用中断的方式，使采集频率与六轴传感器和压力传感器的输出频率匹配，并将采集的信号传入融合库，压力数据采集后存入数据帧缓存，准备传输到上位机。

数据融合处理

	MotionFX_CM0P_update(&data_out, &data_in, 0.01f);
  myFrame.fdata[11] = data_out.quaternion_6X[0];
  myFrame.fdata[12] = data_out.quaternion_6X[1];
  myFrame.fdata[13] = data_out.quaternion_6X[2];
  myFrame.fdata[14] = data_out.quaternion_6X[3];

调用MotionFX库，融合六轴数据，计算出四元数，存入数据帧缓存，准备传输。

简易里程计计算

  int16_t x_stop_time = 0;
  int16_t y_stop_time = 0;
  int16_t z_stop_time = 0;

  if (fabs(data_out.linear_acceleration_6X[0]) > 0.01)
  {
    x_stop_time = 30;
    point_xyz[0] = point_xyz[0] + speed_xyz[0] * 0.1 + 0.5 * data_out.linear_acceleration_6X[0] * 0.1 * 0.1;
    speed_xyz[0] += data_out.linear_acceleration_6X[0] * 0.1;
  }
  else if (x_stop_time != 0)
  {
    x_stop_time--;
    point_xyz[0] = point_xyz[0] + speed_xyz[0] * 0.1 + 0.5 * data_out.linear_acceleration_6X[0] * 0.1 * 0.1;
    speed_xyz[0] += data_out.linear_acceleration_6X[0] * 0.1;
  }
  else
  {
    speed_xyz[0] = 0;
  }

  if (fabs(data_out.linear_acceleration_6X[1]) > 0.01)
  {
    y_stop_time = 30;
    point_xyz[1] = point_xyz[1] + speed_xyz[1] * 0.1 + 0.5 * data_out.linear_acceleration_6X[1] * 0.1 * 0.1;
    speed_xyz[1] += data_out.linear_acceleration_6X[1] * 0.1;
  }
  else if (y_stop_time != 0)
  {
    y_stop_time--;
    point_xyz[1] = point_xyz[1] + speed_xyz[1] * 0.1 + 0.5 * data_out.linear_acceleration_6X[1] * 0.1 * 0.1;
    speed_xyz[1] += data_out.linear_acceleration_6X[1] * 0.1;
  }
  else
  {
    speed_xyz[1] = 0;
  }

  if (fabs(data_out.linear_acceleration_6X[2]) > 0.01)
  {
    z_stop_time = 30;
    point_xyz[2] = point_xyz[2] + speed_xyz[2] * 0.1 + 0.5 * data_out.linear_acceleration_6X[2] * 0.1 * 0.1;
    speed_xyz[2] += data_out.linear_acceleration_6X[2] * 0.1;
  }
  else if (z_stop_time != 0)
  {
    z_stop_time--;
    point_xyz[2] = point_xyz[2] + speed_xyz[2] * 0.1 + 0.5 * data_out.linear_acceleration_6X[2] * 0.1 * 0.1;
    speed_xyz[2] += data_out.linear_acceleration_6X[2] * 0.1;
  }
  else
  {
    speed_xyz[2] = 0;
  }

将中断时间内的运动视为匀加速运动，使用相应公式 $x = v_{0} t + \frac{1}{2} a t^{2}$ 实现简易里程计，为了消除偏移量的影响，设计启动阈值和停止延迟。

数据传输

  myFrame.fdata[1] = point_xyz[0];
  myFrame.fdata[2] = point_xyz[1];
  myFrame.fdata[3] = point_xyz[2];
  myFrame.fdata[4] = speed_xyz[0];
  myFrame.fdata[5] = speed_xyz[1];
  myFrame.fdata[6] = speed_xyz[2];
  myFrame.fdata[7] = data_out.linear_acceleration_6X[0];
  myFrame.fdata[8] = data_out.linear_acceleration_6X[1];
  myFrame.fdata[9] = data_out.linear_acceleration_6X[2];

  HAL_UART_Transmit(&huart2, (uint8_t *)&myFrame, sizeof(myFrame), 5);

将其他需要传输的数据打包到缓存中，通过串口传输数据。

上位机代码

上位机代码通过GPT完成，仅作简单讲解。

使用库

import serial
import struct
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d.art3d import Poly3DCollection
import matplotlib.font_manager as fm

代码讲解

串口数据解析

def read_frame(ser):
    buffer = bytearray()

    while True:
        # 读取一些数据并添加到缓冲区
        data = ser.read(ser.in_waiting or 1)
        buffer.extend(data)

        # 检查缓冲区是否包含一个完整的帧
        while len(buffer) >= frame_size:
            # 找到帧尾
            tail_index = buffer.find(frame_tail)
            if (tail_index == -1) or (tail_index + 4 != frame_size):
                # 如果没有找到帧尾，或者找到帧尾但位置不正确，删除缓冲区中的数据段，继续读取
                buffer = buffer[-(frame_size - 1):]
                break
            else:
                # 如果找到帧尾且位置正确，解析帧数据
                frame_data = buffer[:frame_size]
                buffer = buffer[frame_size:]  # 移除已处理的帧数据
                fdata = struct.unpack('<' + 'f' * ch_count, frame_data[:-4])
                return fdata

四元数解析

def quaternion_to_rotation_matrix(q):
    w, x, y, z = q
    return np.array([
        [1 - 2 * y ** 2 - 2 * z ** 2, 2 * x * y - 2 * z * w, 2 * x * z + 2 * y * w],
        [2 * x * y + 2 * z * w, 1 - 2 * x ** 2 - 2 * z ** 2, 2 * y * z - 2 * x * w],
        [2 * x * z - 2 * y * w, 2 * y * z + 2 * x * w, 1 - 2 * x ** 2 - 2 * y ** 2]
    ])

绘制物体

def update_cube(ax, rotation_matrix, translation_vector, fdata, initial_z):
    # 定义立方体的顶点
    r = [-0.5, 0.5]
    vertices = np.array([[x, y, z] for x in r for y in r for z in r])

    # 旋转立方体
    rotated_vertices = vertices @ rotation_matrix.T

    # 平移立方体
    translated_vertices = rotated_vertices + translation_vector

    # 定义立方体的面
    faces = [
        [translated_vertices[j] for j in [0, 1, 3, 2]],
        [translated_vertices[j] for j in [4, 5, 7, 6]],
        [translated_vertices[j] for j in [0, 1, 5, 4]],
        [translated_vertices[j] for j in [2, 3, 7, 6]],
        [translated_vertices[j] for j in [0, 2, 6, 4]],
        [translated_vertices[j] for j in [1, 3, 7, 5]]
    ]

    ax.clear()
    ax.add_collection3d(Poly3DCollection(faces, facecolors='cyan', linewidths=1, edgecolors='r', alpha=.25))
    ax.set_xlim([-1, 1])
    ax.set_ylim([-1, 1])
    ax.set_zlim([-1, 1])
    ax.set_xlabel('X')
    ax.set_ylabel('Y')
    ax.set_zlabel('Z')

    # 显示高度、x位置、y位置和四元数信息
    height = fdata[0]
    x_position = fdata[1]
    y_position = fdata[2]
    z_position = fdata[0] - initial_z  # 使用第一个元素作为z轴坐标并减去初始值
    quaternion = fdata[11:15]

    ax.text2D(0.05, 0.95, f"高度: {height:.2f}", transform=ax.transAxes, fontproperties=prop)
    ax.text2D(0.05, 0.90, f"X位置: {x_position:.2f}", transform=ax.transAxes, fontproperties=prop)
    ax.text2D(0.05, 0.85, f"Y位置: {y_position:.2f}", transform=ax.transAxes, fontproperties=prop)
    ax.text2D(0.05, 0.80, f"Z位置: {z_position:.2f}", transform=ax.transAxes, fontproperties=prop)
    ax.text2D(0.05, 0.75, f"四元数: {quaternion}", transform=ax.transAxes, fontproperties=prop)

    plt.draw()
    plt.pause(0.001)

主函数

def main():
    # 打开串口
    with serial.Serial(port, baudrate, timeout=1) as ser:
        print("Reading data from serial port...")

        # 初始化绘图
        fig = plt.figure()
        ax = fig.add_subplot(111, projection='3d')

        # 反转绕Y轴的旋转矩阵（绕Y轴旋转180度）
        R_y_180 = np.array([
            [-1, 0, 0],
            [0, 1, 0],
            [0, 0, -1]
        ])

        initial_z = None
        while True:
            try:
                # 读取并解析帧数据
                fdata = read_frame(ser)
                print("Received data:", fdata)

                # 提取四元数数据
                quaternion = [fdata[11], fdata[12], fdata[13], fdata[14]]  # 11:scalar; 12:X; 13:Y; 14:Z
                rotation_matrix = quaternion_to_rotation_matrix(quaternion)

                # 应用反转绕Y轴的旋转矩阵
                final_rotation_matrix = R_y_180 @ rotation_matrix

                # 提取质心坐标，如果是第一次采集，保存初始的z值
                if initial_z is None:
                    initial_z = fdata[0]

                translation_vector = np.array([fdata[1], fdata[2], fdata[0] - initial_z])  # 1:x; 2:y; 0:z
                # 更新立方体
                update_cube(ax, final_rotation_matrix, translation_vector, fdata, initial_z)

            except KeyboardInterrupt:
                print("Exiting...")
                break
if __name__ == "__main__":
    main()

通过对矩阵的反转，校准实际解算方向，并绘图显示。

功能展示

单片机：

上电后首先进行初始化，随后进行传感器校准，校准结束后，指示灯由常亮变为闪烁，初始化完成。

启动终端服务程序后，对传感器数据进行采集，并通过串口将数据传输到上位机

上位机：

接收数据，对接收到的数据进行解析，绘制结果。

心得体会

通过本次项目学习，对ST的工具链和开发流程有了更多了解，以后开发玩具具备更快的性能，同时本次上位机开发，完全使用GPT生成，对GPT有了一些了解，更好使用这种工具加速开发。