内容介绍

1.本项目使用的主控制器：

STM32U575ZI-Q

STM32U575ZI-Q属于超低功耗微控制器（STM32U5系列），基于高性能Arm^® Cortex^®-M33 32位RISC内核。工作频率高达160 MHz。Cortex^®-M33内核带有单精度FPU（浮点运算单元），支持所有ARM^®单精度数据处理指令和所有数据类型。Cortex^®-M33内核还具备一套完整的DSP（数字信号处理）指令集和增强应用安全的MPU（存储器保护单元）。这些器件内嵌高速存储器（最高2 MB Flash存储器和786 KB SRAM）、一个面向静态存储器（用于采用90或更多引脚封装的器件）的FSMC（灵活外部存储控制器）、两个Octo-SPI Flash存储器接口（至少一个适用于所有封装的四路SPI接口）、以及大量连至3条APB总线、3条AHB总线和1个32位多AHB总线矩阵的增强型I/O与外设。

NUCLEO-U575ZI-Q - STM32 Nucleo-144 development board

2.本项目主要使用的传感器：

LSM6DSO16IS

MEMS 3D加速度计 (±2/±4/±8/±16 g) + 3D陀螺仪 (±125/±250/±500/±1000/±2000 dps) 与ISPU（智能处理单元），是一颗广泛用于消费级产品的6轴陀螺仪传感器，被大量用于手机和智能手表等设备的制造。本项目中用以计算并输出欧拉角。

详细信息：LSM6DSO16IS - iNEMO inertial module: always-on 3-axis accelerometer and 3-axis gyroscope with ISPU - intelligent sensor processing unit - STMicroelectronics

参考的应用手册LSM6DSO：始终开启的3D加速度计和3D陀螺仪 - 应用笔记 (st.com.cn)

LIS2MDL

MEMS 3D磁力计 (±50 gauss)，本项目用以修正加速度计的参数，以弥补偏航角的误差。

详细信息：LIS2MDL - 磁传感器、数字输出、50高斯磁场动态范围、超低功耗、高性能、3轴磁力计 - 意法半导体STMicroelectronics

LPS22DF

低功率和高精度MEMS压力传感器，260-1260 hPa绝对数字输出气压计，用以计算海拔高度。

详细信息：LPS22DF - Low-power and high-precision MEMS nano pressure sensor: 260-1260 hPa absolute digital output barometer - STMicroelectronics

STTS22H

低电压，超低功耗，0.5°C精度的温度传感器（–40 °C到+125 °C）,用以校准温漂和共同计算海拔。

详细信息：STTS22H - Low-voltage, ultra-low-power, 0.5 °C accuracy I2C/SMBus 3.0 temperature sensor - STMicroelectronics

3.本项目主要软件设计：

加速度/角速度/磁力计换算

主控得到的陀螺仪的角速度和加速度计的加速度值需要换算

角速度：

加速度：

由于传感器加速度的单位是mg 需要换算成标准单位G，故计算公式为：

部分代码截取：

void imu(MOTION_SENSOR_Axes_t AccValue,
		 MOTION_SENSOR_Axes_t GyrValue,
		 MOTION_SENSOR_Axes_t MagValue){
	FAccValue.x=(AccValue.x)/163831750;
	FAccValue.y=(AccValue.y)/163831750;
	FAccValue.z=(AccValue.z)/163831750;
	FGyrValue.x=((GyrValue.x)*3.14/16.384)/180/1000;
	FGyrValue.y=((GyrValue.y)*3.14/16.384)/180/1000;
	FGyrValue.z=((GyrValue.z)*3.14/16.384)/180/1000;
	FMagValue.x=MagValue.x*1.5;
	FMagValue.y=MagValue.y*1.5;
	FMagValue.z=MagValue.z*1.5;
	//return FAccValue,FGyrValue,FMagValue;
}

磁力计航向角换算：

由于磁力计是3D的数据，需要转换成水平和垂直方向的角度：

float Magnetometer(float Bx,float By,float Bz)
{
	float Mag_B = sqrt(Bx*Bx + By*By + Bz*Bz);
	float Mag_theta = atan2(By, Bx) * 180.0 / M_PI; 
	float Mag_phi = acos(Bz / Mag_B) * 180.0 / M_PI; 
	printf("YAW: %d\n",(int)Mag_theta);
  return Mag_theta;
}

海拔高度计算

用板卡上的海拔和温度可以简单计算海拔高度，其中，Hypsometric公式是一种用于根据大气压和温度来估算海拔高度的方法。为了解出 h，假设大气是干洁的，并且遵循国际标准大气（ISA）模型（在海拔11000米以下，海拔高度每上升1000米，温度下降6.5°C（0.0065°C/m）。以此来得到公式：

$h (a lt) = ([(P_{0} / P)^{(} 1/5.257) - 1] * (T + 273.15)) /0.0065$

实际测量的大气压强P（单位：kPa）和温度T（单位：℃）与标准大气压强P0（101.325kPa）相比较。

示例代码为：

#define P0 1013.25                   // 单位：hPa
#define STANDARD_TEMPERATURE 288.15  // 单位：开尔文
#define LapseRate 0.0065             // 单位：K/m
#define GasConstant 8.31432          // 单位：J/(mol·K)
#define MolarMassAir 0.029           // 单位：kg/mol

int count_1=0;
// 计算海拔高度的函数
float Altitude(float P, float T)
{
    float alt;
    alt = ((pow((P0 / P), (1 / 5.257)) - 1) * (T + 273.15)) / 0.0065;
    return alt;
}

AHRS互补滤波算法（Mahony算法）

AHRS（Attitude and Heading Reference System，姿态和航向参考系统）算法是一类用于在没有GPS信号的情况下，仅使用MEMS（微机电系统）传感器（如加速度计、陀螺仪和磁力计）来估计载体的姿态和航向的算法。AHRS算法通常用于飞行器、机器人、汽车和其他需要准确姿态信息的系统中。

AHRS算法的思路是将重力加速度的偏差角度转换为等同于角速度计测量的角速度大小，再将其与角速度计测量值通过比例+积分融合计算总等效角速度，最后将等效角速度积分得到对应姿态。它是一种适用于资源受限系统的轻量级滤波器。

该算法的所有代码已开源，开源网址为Open source IMU and AHRS algorithms – x-io Technologies

代码部分截取（有修改）：

void MahonyAHRSupdateIMU_G_A(
		Flote_date AccValue,
		Flote_date GyrValue) {
	float recipNorm;
	float halfvx, halfvy, halfvz;
	float halfex, halfey, halfez;

	float qa, qb, qc;

	gx=GyrValue.x;
	gy=GyrValue.y;
	gz=GyrValue.z;

	ax=AccValue.x;
	ay=AccValue.y;
	az=AccValue.z;

	// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)

	if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
		// Normalise accelerometer measurement
		recipNorm = invSqrt(ax * ax + ay * ay + az * az);
		ax *= recipNorm;
		ay *= recipNorm;
		az *= recipNorm;
		// Estimated direction of gravity and vector perpendicular to magnetic flux
		halfvx = q1 * q3 - q0 * q2;
		halfvy = q0 * q1 + q2 * q3;
		halfvz = q0 * q0 - 0.5f + q3 * q3;
		// Error is sum of cross product between estimated and measured direction of gravity
		halfex = (ay * halfvz - az * halfvy);
		halfey = (az * halfvx - ax * halfvz);
		halfez = (ax * halfvy - ay * halfvx);
		// Compute and apply integral feedback if enabled
		if(twoKi > 0.0f) {
			integralFBx += twoKi * halfex * (1.0f / sampleFreq);	// integral error scaled by Ki
			integralFBy += twoKi * halfey * (1.0f / sampleFreq);
			integralFBz += twoKi * halfez * (1.0f / sampleFreq);
			gx += integralFBx;	// apply integral feedback
			gy += integralFBy;
			gz += integralFBz;
		}
		else {
			integralFBx = 0.0f;	// prevent integral windup
			integralFBy = 0.0f;
			integralFBz = 0.0f;
		}
		// Apply proportional feedback
		gx += twoKp * halfex;
		gy += twoKp * halfey;
		gz += twoKp * halfez;
	}
	// Integrate rate of change of quaternion
	gx *= (0.5f * (1.0f / sampleFreq));		// pre-multiply common factors
	gy *= (0.5f * (1.0f / sampleFreq));
	gz *= (0.5f * (1.0f / sampleFreq));
	qa = q0;
	qb = q1;
	qc = q2;
	q0 += (-qb * gx - qc * gy - q3 * gz);
	q1 += (qa * gx + qc * gz - q3 * gy);
	q2 += (qa * gy - qb * gz + q3 * gx);
	q3 += (qa * gz + qb * gy - qc * gx); 
	// Normalise quaternion
	recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
	q0 *= recipNorm;
	q1 *= recipNorm;
	q2 *= recipNorm;
	q3 *= recipNorm;
	
	yaw   = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2 * q2 - 2 * q3 * q3 + 1) * 57.3 ;
	pitch = asin(-2 * q1 * q3 + 2 * q0 * q2) * 57.3;
	roll  = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * 57.3;
}

频率与时间常量计算

由于算法中引入了时间常量，经建模计算得，当AHRS算法在50HZ以上，且采样频率大于50HZ时。数据刚好能达到计算稳定，故调用一组定时器TIM2以测算时间，当间隔时间为0.02秒时调用计算函数。间隔时间计算公式为：

计算后的采样频率即是算法中的事件常数：

#define sampleFreq 50.0f           // sample frequency in Hz

当计时器到达设定值后，会自动调用回调函数

void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *hitm)

上位机通信

上位机需要涉及到3D模拟，这里选用Adafruit开发的串口查看器（3DModelViewer）

网站：https://adafruit.github.io/Adafruit_WebSerial_3DModelViewer/

串口发送代码：

yaw = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2 * q2 - 2 * q3 * q3 + 1) * 57.3;
pitch  = asin(-2 * q1 * q3 + 2 * q0 * q2) * 57.3;
roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * 57.3;
count++;
        if (count == 2)//发送速度太快，删了网页会被卡住
        {

            printf("Orientation: %d.00, %d.00, %d.00\n",(int)yaw,(int)pitch,(int)roll);
            //printf("Orientation: ");
            //usart_print(yaw);
            //usart_print(pitch);
            //usart_print(roll);//两种写法选一个
            count=0;
        }
    //return yaw,pitch,roll;

未命名文件.png

4.本项目的开发环境：

STM32CUBE IDE Version: 1.15.0

5.总结：

本次项目中学会了使用多种算法对传感器进行项目综合开发，感谢得捷电子和电子森林的鼎力相助，感谢群诸位大佬的疑问解答。

代码仓库地址：电子烂人/U575ZI_MEMS_FUNPACK3-3 - 码云 - 开源中国 (gitee.com)