STM32平台博世BSXLite IMU解算算法移植方案

背景

笔者试提出使用博世公司的库文件进行解算，以达到减轻陌生领域开发精力的前提下拿到较准确数据的目的

BSXlite 算法介绍

综述

BsXlite算法最新一次更新于2021年7月，为一款为 Cortex-M 单片机开发的IMU解算算法，并支持硬浮点与软浮点计算方法，也支持 armcc/armclang 与 arm-gnu-toolchain两套编译链，提供库文件，使用时只需要引入库文件，简单便捷。

文件大小（摘自博世官方文档）

编译后的 BSXlite 库总大小为 17688 bytes (compiled for cortex M3 platform)

• 编译器: GCC - GNU Tools for ARM Embedded Processors - arm-none-eabi-gcc.exe
• 版本: 5.4.1
• 编译器配置: -std=c99 -mcpu=cortex-m3 -mthumb -Os -fno-common

性能指标

共进行3次测试

1. 测试时间7分钟，无晃动，平台为G473，自研板BMI088，Yaw轴零漂33度7分钟，
2. 测试时间7分钟，有晃动，平台为H723，C板BMI088，Yaw轴零,15度7分钟，
3. 测试时间5分钟，有晃动，平台为H723，C板BMI088，Yaw轴零,约0.1rad7分钟，
   大幅度反复晃动后IMU波形 使用OZONE观察：
   

文件夹构成

[Generic]BSXlite_v1.0.2
├─documentation（文档介绍）
├─example(样例)
├─lib（库文件）
│  ├─ARMCC_OUT（适宜armclang与armcc编译，即Keil以及使用上面两种编译器的CMake用户）
│  │  ├─libalgobsxm0
│  │  ├─libalgobsxm0plus
│  │  ├─libalgobsxm1
│  │  ├─libalgobsxm3
│  │  ├─libalgobsxm4
│  │  ├─libalgobsxm4fpu
│  │  └─libalgobsxm7
│  └─GCC_OUT（适宜arm-gnu-tool-chain用户）
│      ├─libalgobsxa7
│      ├─libalgobsxa7hardFP
│      ├─libalgobsxa7softFP
│      ├─libalgobsxa9
│      ├─libalgobsxa9hardFP
│      ├─libalgobsxa9softFP
│      ├─libalgobsxm0
│      ├─libalgobsxm0plus
│      ├─libalgobsxm1
│      ├─libalgobsxm3
│      ├─libalgobsxm33
│      ├─libalgobsxm33hardFP
│      ├─libalgobsxm33softFP
│      ├─libalgobsxm4
│      ├─libalgobsxm4hardFP（G474为Cortex M4结构单片机，我们用硬浮点，选这个）
│      ├─libalgobsxm4softFP
│      ├─libalgobsxm7
│      ├─libalgobsxm7hardFP（H723为Cortex M7结构单片机，我们用硬浮点，选这个）
│      └─libalgobsxm7softFP
├─bsxlite_interface.h
└─releasenotes

bsxlite下载点这里

结构体介绍

bsxlite_instance_t

模块实例

typedef size_t bsxlite_instance_t;

vector_3d_t输入数据结构体

typedef struct 
{ 
    float x; 
    float y; 
    float z; 
}vector_3d_t;

与角速度计对应关系：

x -> pitch
y -> roll
z -> yaw

quaternion_t输出数据1：四元数

typedef struct 
{ 
    float x; 
    float y; 
    float z; 
    float w;
}quaternion_t;

euler_angles_t输出数据2：欧拉角

typedef struct 
{ 
    float heading; 
    float pitch; 
    float roll; 
    float yaw; 
}euler_angles_t;

bsxlite_out_t算法输出结构体

typedef struct 
{ 
    quaternion_t rotation_vector; 
    euler_angles_t orientation; 
    uint8_t accel_calibration_status; 
    uint8_t gyro_calibration_status; 
}bsxlite_out_t;

bsxlite_return_t解算返回值

typedef int16_t bsxlite_return_t;

基本函数介绍

初始化函数

bsxlite_return_t bsxlite_init(bsxlite_instance_t *instance)

解算工作

bsxlite_return_t bsxlite_do_step(const bsxlite_instance_t *instance, const int32_t w_time_stamp, const vector_3d_t * accel_in, const vector_3d_t * gyro_in, bsxlite_out * output_data);

物理量单位
- 欧拉角单位：rad(0 ~2$\pi$)
- 加速度单位：$m/s^2$
- 角速度单位：rad/s
- 时间戳单位：微秒

Reset

bsxlite_return_t bsxlite_set_to_default(const bsxlite_instance_t *instance);

移植方法

文件迁移

将.a/.lib文件导入（取决于编译器），并将bsxlite_interface.h置于有效的引用路径下
库文件原始路径，以H7为例：
[Generic]BSXlite_v1.0.2\lib\GCC_OUT\libalgobsxm7hardFP\libalgobsx.a

CMakeList引入

添加强制使用硬浮点

• M7:
  add_compile_options(-mfloat-abi=hard -mfpu=fpv5-d16)
• M4:
  add_compile_options(-mfloat-abi=hard -mfpu=fpv4-sp-d16)
  引入库(我的，不能直接抄）

# Link directories setup
target_link_directories(${CMAKE_PROJECT_NAME} PRIVATE
        ${CMAKE_SOURCE_DIR}/third/IMU/lib
        # Add user defined library search paths
)

# Add linked libraries
target_link_libraries(${CMAKE_PROJECT_NAME}
    ${CMAKE_SOURCE_DIR}/third/IMU/lib/libalgobsx.a
        # Add user defined libraries
)

Example

完整demo在这里

INS.cpp

#include <cmsis_os.h>

#include "task/ins.h"
#include "periph/bmi088.h"
#include "dwt_api.h"

extern "C"{
#include "bsxlite_interface.h"
#include "arm_math.h"
}

bsp::SPI imu_spi(hspi1);
bsp::GPIO accel_cs(*GPIOF, GPIO_PIN_3);
bsp::GPIO gyro_cs(*GPIOF, GPIO_PIN_4);
imu::Bmi088 ins(imu_spi, accel_cs, gyro_cs);

bsxlite_instance_t ins_instance;
vector_3d_t accel_data = {0, 0, 0};
vector_3d_t gyro_data = {0, 0, 0};

bsxlite_out_t bsx_out;
insData_t ins_data;

static void INS_Task(void* parameter)
{
    static bool init_flag{false};
    ins_data = {
        0,0,0,
    };
    if (!init_flag) {
        bsp::DWT_Init(170);
        ins.Init();
        while(ins.error_ != imu::Bmi088::Error::NO_ERROR){
            ins.Init();
        }
        init_flag  = true;
    }
    bsxlite_init(&ins_instance);
    while (true)
    {
        ins.Decode();

        accel_data = {
            ins.accel_data_.x, 
            ins.accel_data_.y, 
            ins.accel_data_.z
        };
        gyro_data = {
            ins.gyro_data_.pitch, 
            ins.gyro_data_.row, 
            ins.gyro_data_.yaw
        };
        bsxlite_do_step(&ins_instance, 1000*HAL_GetTick(), &accel_data, &gyro_data, &bsx_out);
       //这里用DWT达到微秒精度会更好一些，不过目前毫秒级也够用

        ins_data = {
            bsx_out.orientation.yaw,
            bsx_out.orientation.pitch,
            bsx_out.orientation.roll
        };

        osDelay(10);
    }
}

const insData_t* GetInsData() {
    return &ins_data;
}

void InsTaskStart(void)
{
    xTaskCreate(INS_Task, "INSTask", 256, NULL, 7, NULL);
}