[蓝点无限] UWB 定位数据融合 之 MPU9250 驱动
UWB定位，不论是TWR 还是TDOA，实质都是基于电磁信号空中飞行时间(TOF),然后换算成距离，通过软件算法获取被定位坐标。
电磁信号，收到干扰和遮挡等会出现定位误差再说难免。
而结合运动传感器进行数据融合则是对UWB定位误差的一个动态补偿。 目前运动传感器封装小，易于集成，已经很多实际项目采用UWB+运动传感器方式进行定位。
但是这些资料很少，往往只能见于某篇论文或者某个商品的介绍。
这里，我们做抛砖引玉，做一些基础功能，供各位开发者参考。
我们这里使用的是MPU9250 9轴传感器。传感器介绍各位可以参考官方介绍:
https://invensense.tdk.com/products/motion-tracking/9-axis/mpu-9250/
MPU9250 支持I2C 和 SPI接口，我们板子使用的I2C接口，我们这里使用STM32F4 软件方式I2C 驱动，并获取9轴数据。
UWB数据融合，我们打算一共做分三部分
1 MPU9250 驱动，主要是使用STM32 成功读取MPU9250 内部数据， 数据是融合的前提。
2 UWB固件，将MPU9250 集成到 UWB 程序内，我们打算把这部分功能集成到之前开源的多标签多基站固件，做到彻底开源。
3 上位机解析，修改我们之前开源上位机，上位机可以解析获取MPU9250 原始数据。
剩下的，上位机中融合自行实现，大家可以自行对某一次定位更新，自行决定是使用UWB数据还是传感器数据。
例如，MPU9250 数据显示当前各个加速度 角速度和上次比没有太大变化，我们可以推理模块可能静止，但是UWB 这次算出模块位置和上次差异大于1米。
这个时候更偏向使用MPU9250，而UWB可能出现误差。忽略本次UWB位置更新，保存使用上次的位置坐标。
类似如上，完成融合算法或者更为复杂的算法。
测试硬件:
内部板子情况:UWB+PA功放，MPU9250, 600ma内置锂电池，振动小电机。 后期补一张内部靓照。
板子MPU9250 部分原理图：
MPU9250 I2C 固件说明
i2c_sw.c 配置软件I2C
mpu9250.c MPU9250初始化以及寄存器操作
代码配置I2C 对应接口
#define GPIO_SW_I2C1_SCL GPIOA
#define GPIO_SW_I2C1_SDA GPIOB
#define GPIO_SW_I2C1_SCL_PIN GPIO_PIN_8
#define GPIO_SW_I2C1_SDA_PIN GPIO_PIN_4
测试接口代码：
#include "i2c_sw.h"
void Sw_I2C_Init()
{
SW_I2C_initial();
i2c_port_initial(SW_I2C1);
}
#include "mpu9250.h"
/* Sensor Handler */
MPU9250_t mpu9250;
HAL_StatusTypeDef whoAmI_Check(MPU9250_t *mpu9250);
int Mpu9250_Test(void)
{
Sw_I2C_Init();
MPU9250_Init(&mpu9250, MPU9250_Device_0, ACCEL_SCALE_16G, GYRO_SCALE_2000dps, MAG_SCALE_16bit);
while (1)
{
MPU9250_ReadAcc(&mpu9250);
printf("acc.x = %0.2f acc.y = %0.2f acc.z= %0.2f\n",mpu9250.acc[0],mpu9250.acc[1],mpu9250.acc[2]);
MPU9250_ReadGyro(&mpu9250);
HAL_Delay(300);
printf("gyro.x = %0.2f gyro.y = %0.2f gyro.z= %0.2f\n",mpu9250.gyro[0],mpu9250.gyro[1],mpu9250.gyro[2]);
MPU9250_ReadMag(&mpu9250);
printf("mag.x = %0.2f mag.y = %0.2f mag.z = %0.2f\n",mpu9250.mag[0],mpu9250.mag[1],mpu9250.mag[2]);
HAL_Delay(300);
MPU9250_ReadTemperature(&mpu9250);
printf("Temp = %0.2f\n",mpu9250.temp);
HAL_Delay(300);
}
}
测试结果：
驱动代码下载：
完整代码请到蓝点论坛下载
