第三十四章 QMI8658A实验
本章，我们将介绍一款高性价比六轴（三轴角速度（陀螺仪）+三轴加速度）传感器：QMI8658A，该传感器可广泛用于四轴、平衡车和空中鼠标等设计，性价比极高，具有非常广泛的应用范围。我们将继续使用ESP32-P4的硬件IIC接口去驱动板载QMI8658A传感器，读取其原始数据，并把原始数据转换为pitch俯仰角、roll翻滚角和yaw偏航角，最终显示在LCD上。
本章分为如下几个小节：
34.1 QMI8658A介绍
34.2 硬件设计
34.3 程序设计
34.4 下载验证
34.1 QMI8658A介绍
QMI8658A内部集成有3轴陀螺仪和3轴加速度计，可以通过I3C、I2C和3线或4线SPI的接口和单片机进行数据交互，传输速率可达400kHZ/s。陀螺仪的角速度测量范围最高达±2048°/s，具有良好的动态响应特性。加速度计的测量范围最大为±16g（g为重力加速度），静态测量精度高。
QMI8658A的特点包括：1、陀螺仪13 mdps/Hz的低噪声，低延迟，宽带宽；2、150μg/Hz的加速度计噪声；3、自带一个温度数字传感器；4、可编程数字滤波器；5、集成计步器，轻拍，任意运动，移动，显著运动检测；5、自带1536字节FIFO可用于缓冲传感器数据，降低系统功耗；6、超小封装尺寸：2.5×3.0×0.86mm(LGA)。
QMI8658A传感器的检测轴如图34.1.1所示：
图34.1.1QMI8658A检测轴及其方向
QMI8658A的内部框图如图34.1.2所示：
图34.1.2 QMI8658A框图
QMI8658A的引脚说明如下表所示。
表34.1.1 QMI8658A引脚说明
QMI8658A和主控芯片只需要连接SCL、SDA和INT，就可以实现驱动。其SCL和SDA同24C02、XL9555、AP3216C共用，连接在IO32和IO33上，INT脚连接在XL9555的IO1_4上。
34.1.1 QMI8658A寻址
要进行IIC通信，首先得知道器件地址，QMI8658A器件地址是7位的，具体格式如下表。
表34.1.1.1 QMI8658A地址格式
QMI8658A的器件地址会由SA0决定的，简单来说，由SA0引脚实际的硬件连接决定。ESP32-P4开发板上的QMI8658A电路中，SA0是连接到3V3即高电平，所以QMI8658A的器件地址为“1101011”即0x6B。读操作地址就为0xD7（0x6B << 1 | 0x1），即1101 0111；写操作地址就为0xD6（0x6B << 1 | 0x0），即1101 0110。
34.1.2 QMI8658A寄存器介绍
在这里简单介绍一下本实验用到的QMI8658A比较重要的寄存器，其他寄存器的描述和说明，请大家参考QMI8658A的数据手册。
芯片信息寄存器（Chip Information）
该寄存器地址为0x00和0x01，各位描述如表34.1.2.1所示。
表34.1.2.1 芯片信息寄存器及其说明
这两个寄存器主要用来存放芯片的信息，设备标识符可从0x00地址处读到0x05，而设备的版本ID可从0x01地址处读到0x7C。这是芯片内部固定的信息，一般一开始验证与传感器是否通信成功，都是通过读取芯片固定信息去确认。
陀螺仪配置寄存器（Gyroscope Settings）
该寄存器地址为0x04，各位描述如下表所示。
表34.1.2.2 陀螺仪配置寄存器及其说明
该寄存器主要关心gFS[2:0]和gODR[3:0]。gFS[2:0]用于设置陀螺仪的满量程范围，一般设置为011，即±128dps。由于陀螺仪的ADC为16位，灵敏度为：65536/256=256LSB(dps)。gODR[3:0]用于设置陀螺仪输出数据速率，设置为0100，即约500Hz的速率。
加速度传感器配置寄存器（Accelerometer Settings）
该寄存器地址为0x03，各位描述如下表所示：
表34.1.2.3 加速度传感器配置寄存器及其说明
该寄存器主要关心aFS[2:0]和aODR[3:0]这些位。aFS[2:0]用于设置加速度计的满量程范围，一般设置为011，即±16g。因为加速度传感器的ADC也是16位分辨率，所以得到灵敏度为：65536/32=2048LSB/g。aODR[3:0]用于设置加速度计输出数据速率，设置为0100，即约500Hz的速率。
传感器数据处理寄存器（Accelerometer Settings）
该寄存器地址为0x03，各位描述如下表所示：
表34.1.2.4 传感器数据处理寄存器及其说明
该寄存器主要关心低通滤波器(LPF_MODE)设置位，即：gLPF_MODE和aLPF_MODE，陀螺仪和加速度传感器分别根据这两个位的配置进行过滤。例程中，将加速度传感器和陀螺仪的低通过滤器模式选择位：gLPF_MODE[1:0]和aLPF_MODE[1:0]都设置为11，即ODR的13.37%。
温度传感器数据输出寄存器（Temp Sensor Output）
该寄存器地址为0x33~0x34，各位描述如下表所示：
表34.1.2.5 温度传感器数据输出寄存器及其说明
这个寄存器可以读取温度传感器的值，可以通过读取0X33（高8位）和0X34（低8位）寄存器得到，温度换算公式为：
T= TEMP_H + (TEMP_L/256)
其中，T为计算得到的温度值，单位为℃。
加速度传感器数据输出寄存器（Temp Sensor Output）
该寄存器地址为0x35~0x3A，各位描述如下表所示：
表34.1.2.6 加速度传感器数据输出寄存器及其说明
通过读取这6个寄存器，便可以读到加速度传感器x/y/z轴的值，比如读x轴的数据，可以通过读取0X35（低8位）和0X36（高8位）寄存器得到，其他轴以此类推。
加速度传感器数据输出寄存器（Temp Sensor Output）
该寄存器地址为0x3B~0x40，各位描述如下表所示：
表34.1.2.7 陀螺仪传感器数据输出寄存器及其说明
通过读取这6个寄存器，便可以读到陀螺仪传感器x/y/z轴的值，比如读x轴的数据，可以通过读取0X3B（低8位）和0X3C（高8位）寄存器得到，其他轴以此类推。
关于QMI8658A的基础介绍，我们就介绍到这。QMI8658A的详细资料和相关寄存器介绍，请参考光盘：7，硬件资料→2，芯片资料→C3021082_姿态传感器-陀螺仪_QMI8658A_规格书_WJ411228.pdf这个文档，供大家参考学习。
34.1.3 QMI8658A时序介绍
ESP32-P4是通过IIC总线跟QMI8658A进行通信的，对QMI8658A相关寄存器进行写入配置，后面就是对相关数据寄存器进行读取。这里的时序主要就是写寄存器时序和读寄存器时序，我们一一介绍。
写寄存器时序
QMI8658A的写寄存器时序如下图所示。
图34.1.3.1 QMI8658A写寄存器时序
图中，先发送QMI8658A的写操作地址0xD6（器件地址0X6B << 1 | 读写位0x0），随后发送8位寄存器地址，最后发送8位寄存器值。其中：S，表示IIC起始信号；W，表示读/写标志位（W=0表示写，W=1表示读）；A，表示应答信号；P，表示IIC停止信号。
读寄存器时序图
QMI8658A的读寄存器时序如下图所示。
图34.1.3.2 QMI8658A读寄存器时序
图中，同样是先发送QMI8658A的写操作地址0xD6，然后再发送寄存器地址，随后，重新发送起始信号（Sr），发送QMI8658A的读操作地址0xD7（器件地址0X6B << 1 | 读写位0x1），然后读取寄存器值。其中：Sr，表示重新发送IIC起始信号；N，表示不对QMI8658A进行应答；其他简写同上。
QMI8658A的简介，我们就介绍到这里，关于该芯片的详细说明，请大家参考其数据手册。
34.2 硬件设计
34.2.1 例程功能
开机的时候先检测QMI8658A是否存在，如检测不到QMI8658A，则在LCD屏上面显示报错信息。如果检测到QMI8658A，则显示正常，并在主循环里面循环读取加速度计、陀螺仪和温度传感器原始数据，并利用加速度计数据和陀螺仪数据姿态计算后得到欧拉角。其中程序还支持通过串口上报给上位机（温度不上报），利用上位机软件（ANO_TC匿名科创地面站v4.exe），可以实时显示QMI8658A的传感器状态曲线，并显示3D姿态，可以通过BOOT按键开启/关闭数据上传功能。LED0用来指示程序正在运行。
34.2.2 硬件资源
1）LED灯
LED 0 - IO51
2）RGBLCD / MIPILCD（引脚太多，不罗列出来）
3）QMI8658A
IIC_INT - EXIO_12（XL9555）
IIC_SDA - IO33
IIC_SCL - IO32
34.2.3 原理图
QMI8658A相关原理图，如下图所示。
图33.2.3.1 AP3216C原理图
这里说明一下，QMI8658A的QMI_INT脚是连接在XL9555器件的IO1_4脚上，如果大家要使用QMI8658A的中断输出功能，必须先初始化XL9555器件并配置IO1_4为输入功能，监测XL9555中断引脚是否有中断产生。若发现有中断产生，则判断是否是IO1_4导致的，从而检测到QMI8658A的中断。在本章中，并没有用到QMI8658A中断功能，所以没有对XL9555器件的IO1_4做设置。
34.3 程序设计
IIC外设驱动已经在第十九章19.3.1小节做了说明，这里就不再赘述了。
34.3.1 程序流程图
图34.3.1.1 QMI8658A实验程序流程图
34.3.2 程序解析
在25_qmi8658a例程中，作者在25_qmi8658a\components\BSP路径下新增了1个文件夹QMI8658A，并且需要更改CMakeLists.txt内容，以便在其他文件上调用。
1. QMI8658A驱动代码
这里我们只讲解核心代码，详细的源码请大家参考光盘本实验对应源码。QMI8658A驱动源码包括四个文件：qmi8658a.c、qmi8658a.h、imu.c和imu.h。
其中qim8658a.c和qmi8658a.h主要是QMI8658A器件的驱动文件，而imu.c和imu.h主要是关于姿态解算的算法实现。
在qmi8658a.h中，定义了许多宏，包括对QMI8658A的配置，QMI8658A寄存器、寄存器相关位等，大家自行查看源码即可。
下面我们再解析qmi8658a.c的程序，首先先来看一下初始化函数qmi8658a_init，代码如下：
/**
* @brief 初始化QMI8658
* @param 无
* @retval 初始化结果:0成功;1:失败
*/
esp_err_t qmi8658_init(void)
{
/* 未调用myiic_init初始化IIC */
if (bus_handle == NULL)
{
ESP_ERROR_CHECK(myiic_init());
}
i2c_device_config_t qmi8658_i2c_dev_conf = {
.dev_addr_length = I2C_ADDR_BIT_LEN_7, /* 从机地址长度 */
.scl_speed_hz = IIC_SPEED_CLK, /* 传输速率 */
.device_address = QMI8658_ADDR, /* 从机7位的地址 */
};
/* I2C总线上添加qmi8658设备 */
ESP_ERROR_CHECK(i2c_master_bus_add_device(bus_handle, &qmi8658_i2c_dev_conf,
&qmi8658_handle));
qmi8658_reset(); /* 复位传感器 */
if(qmi8658_check_whoami()) /* 检查设备ID是否正确 */
{
return 1;
}
if(qmi8658_calibration()) /* 陀螺仪校准 */
{
return 1;
}
ESP_LOGI(qmi8658_tag, "qmi8658 calibration succeed");
qmi8658a_register_write_byte(Register_Ctrl1, 0x60); /* I2C驱动 */
qmi8658a_register_write_byte(Register_Ctrl7, 0x00); /* 关闭陀螺仪、加速度计 */
qmi8658_config_reg(0); /* 配置陀螺仪和加速度计的量程和数据输出速率等参数 */
qmi8658_enablesensors(g_imu.cfg.ensensors); /* 使能陀螺仪、加速度计 */
return 0;
}
在QMI8658A初始化函数中，首先对qmi8658_i2c_dev_conf变量的成员进行赋值，设置QMI8658A的地址长度、设备地址以及传输速率，然后调用i2c_master_bus_add_device函数对QMI8658A设备进行初始化。初始化QMI8658A后，通过调用qmi8658_reset复位传感器，调用qmi8658_check_whoami检查设备ID是否正常，调用qmi8658_calibration进行陀螺仪校准。最后就通过调用qmi8658a_register_write函数对陀螺仪和加速度计进行配置。
接下来，看一下前面提及到的向QMI8658A寄存器写入数据的函数qmi8658a_register_write，代码如下。
/**
* @brief 向qmi8658a寄存器写数据
* @param reg:要写入的寄存器地址
* @param data:要写入的数据
* @retval ESP_OK:成功; 其他值:错误
*/
static esp_err_t qmi8658a_register_write_byte(uint8_t reg, uint8_t data)
{
esp_err_t ret;
uint8_t *buf = malloc(2);
if (buf == NULL)
{
ESP_LOGE(qmi8658_tag, "%s memory failed", __func__);
return ESP_ERR_NO_MEM;
}
buf[0] = reg;
buf[1] = data;
ret = i2c_master_transmit(qmi8658_handle, buf, 2, -1);
free(buf);
return ret;
}
该函数的实现，主要调用i2c_master_transmit函数。在这里需要进行数据整合，把写入数据的目的地址和要写入的数据重新存放到一个buf。在这里需要注意存放顺序，写入数据的目的地址要在要写入数据的前面，这样通过i2c_master_transmit函数发送出去的数据才符合QMI8658A的写数据操作。
继续看一下读取QMI8658A寄存器数据的函数qmi8658a_register_read，代码如下。
/**
* @brief 读取qmi8658a寄存器的数据
* @param reg:要读取的寄存器地址
* @param data:读取的数据
* @param len:数据大小
* @retval ESP_OK:成功;其他值:错误
*/
esp_err_t qmi8658a_register_read(const uint8_t reg, uint8_t *data,
const size_t len)
{
return i2c_master_transmit_receive(qmi8658_handle, ®, 1, data, len, -1);
}
该函数的实现主要调用i2c_master_transmit_receive函数。从QMI8658A的reg内存地址处读出len长度的数据，存放到data的buf中。
程序中很多函数都是通过调用qmi8658a_register_write和qmi8658a_register_read接口完成功能的。比如qmi8658_config_acc和qmi8658_config_gyro，这两个函数是要对加速度计和陀螺仪进行配置的，具体涉及的寄存器为Ctrl2、Ctrl3和Ctrl5。
初始化好QMI8658A传感器之后，就可以对其采集到的数据进行获取，实现这个读取过程的函数为qmi8658_get_temperature和qmi8658_read_xyz，代码如下。
/**
* @brief 获取传感器温度
* @param 无
* @retval 温度值,单位为℃
*/
float qmi8658_get_temperature(void)
{
short temp = 0;
float temp_f = 0;
uint8_t buf[2];
qmi8658a_register_read(Register_Tempearture_L, buf, 2); /* 读取温度数据 */
temp = ((short)buf[1] << 8) | buf[0];
temp_f = (float)temp / 256.0f;
return temp_f;
}
/**
* @brief 判断数据更新后，在读取补偿后QMI8658陀螺仪和加速度的数据(推荐使用)
* @param acc:加速度计 X,Y,Z缓存区;
* @param gyro:陀螺仪 X,Y,Z缓存区;
* @retval 无
*/
void qmi8658_read_xyz(float *acc, float *gyro)
{
unsigned char status = 0;
unsigned char data_ready = 0;
int retry = 0;
while (retry++ < 3)
{
#if defined(QMI8658_SYNC_SAMPLE_MODE)
qmi8658a_register_read(Register_StatusInt, &status, 1);
if (status & 0x01)
{
/* delay 12us <=500Hz， 12us 1000Hz, 4us 2000Hz 2us > 2000Hz */
delay_us(12);
}
if ((status & 0x01) || (status & 0x03))
{
data_ready = 1;
break;
}
#else
/* 检查加速度计和陀螺仪数据是否可用 */
qmi8658a_register_read(Register_Status0, &status, 1);
if (status & 0x03)
{
data_ready = 1;
break;
}
#endif
}
if (data_ready)
{
qmi8658_read_sensor_data(acc, gyro);
g_imu.imu[0] = acc[0];
g_imu.imu[1] = acc[1];
g_imu.imu[2] = acc[2];
g_imu.imu[3] = gyro[0];
g_imu.imu[4] = gyro[1];
g_imu.imu[5] = gyro[2];
}
else
{
acc[0] = g_imu.imu[0];
acc[1] = g_imu.imu[1];
acc[2] = g_imu.imu[2];
gyro[0] = g_imu.imu[3];
gyro[1] = g_imu.imu[4];
gyro[2] = g_imu.imu[5];
}
}
qmi8658_get_temperature函数：读取温度数据。而qmi8658_read_sensor_data函数：读取补偿后加速度计和陀螺仪的x，y，z轴原始数据。
接下来，介绍一下imu.c文件，即姿态解算相关算法。如何进行姿态解算获得欧拉角，主要就是需要调用imu_get_eulerian_angles函数去实现。
imu_get_eulerian_angles函数，如下代码所示。
/**
* @brief 获取欧拉角数据
* @note 姿态解算融合, 核心算法，互补滤波算法，卡尔曼滤波算法
* 尽量保证该函数的调用频率为: IMU_DELTA_T , 否则YAW会相应的偏大/偏小
* @param gyro:3轴陀螺仪数据
* @param acc:3轴加速度数据
* @param rpy:欧拉角存放buf
* @param dt:调用频率
* @retval 无
*/
void imu_get_eulerian_angles(float acc[3], float gyro[3], float *rpy, float dt)
{
float normalise;
float ex, ey, ez;
float halfT = 0.5f * dt;
float accBuf[3] = {0.0f};
/* 加速度计输出有效时，利用加速度计补偿陀螺仪 */
if (acc[0] != 0.0f || acc[1] != 0.0f || acc[2] != 0.0f)
{
/* 单位化加速计测量值 */
normalise = inv_sqrt(acc[0]*acc[0] + acc[1]*acc[1] + acc[2]*acc[2]);
acc[0] *= normalise;
acc[1] *= normalise;
acc[2] *= normalise;
/* 加速计读取的方向与重力加速计方向的差值，用向量叉乘计算 */
ex = acc[1] * rMat[2][2] - acc[2] * rMat[2][1];
ey = acc[2] * rMat[2][0] - acc[0] * rMat[2][2];
ez = acc[0] * rMat[2][1] - acc[1] * rMat[2][0];
/* 误差累计，与积分常数相乘 */
exInt += Ki * ex * dt;
eyInt += Ki * ey * dt;
ezInt += Ki * ez * dt;
/* 用叉积误差来做PI修正陀螺零偏，即抵消陀螺读数中的偏移量 */
gyro[0] += Kp * ex + exInt;
gyro[1] += Kp * ey + eyInt;
gyro[2] += Kp * ez + ezInt;
}
/* 一阶近似算法，四元数运动学方程的离散化形式和积分 */
float q0Last = q0, q1Last = q1, q2Last = q2, q3Last = q3;
q0 += (-q1Last * gyro[0] - q2Last * gyro[1] - q3Last * gyro[2]) * halfT;
q1 += ( q0Last * gyro[0] + q2Last * gyro[2] - q3Last * gyro[1]) * halfT;
q2 += ( q0Last * gyro[1] - q1Last * gyro[2] + q3Last * gyro[0]) * halfT;
q3 += ( q0Last * gyro[2] + q1Last * gyro[1] - q2Last * gyro[0]) * halfT;
/* 单位化四元数 */
normalise = inv_sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= normalise;
q1 *= normalise;
q2 *= normalise;
q3 *= normalise;
compute_rotation_matrix();
/* 计算欧拉角 */
rpy[0] = asinf(rMat[2][0]) * RAD2DEG;
rpy[1] = atan2f(rMat[2][1], rMat[2][2]) * RAD2DEG;
rpy[2] = atan2f(rMat[1][0], rMat[0][0]) * RAD2DEG;
/* 使用卡尔曼滤波器来更新 YAW */
kalman_filter(rpy[2], gyro[2], dt);
/* 返回经过滤波的角度 */
rpy[2] = angle;
if (!isGravityCalibrated)
{
accBuf[2] = acc[0]*rMat[2][0] + acc[1]*rMat[2][1] + acc[2]*rMat[2][2];
calibrate_gravity(accBuf);
}
}
该函数得到欧拉角的过程涉及多个步骤，包括加速度计的校准、陀螺仪的误差修正、四元数的更新以及欧拉角的计算。这里牵扯的知识面有点广，大家有兴趣可以自行去学习。
其他QMI8658A驱动代码，都有比较详细的注释，自行查看学习即可。
2. CMakeLists.txt文件
本例程的功能实现主要依靠QMI8658A驱动。要在main函数中，成功调用QMI8658A文件中的内容，就得需要修改BSP文件夹下的CMakeLists.txt文件，修改如下：
set(src_dirs
LED
KEY
LCD
MYIIC
QMI8658A
UART)
set(include_dirs
LED
KEY
LCD
MYIIC
QMI8658A
UART)
set(requires
driver
esp_lcd
esp_common)
idf_component_register( SRC_DIRS ${src_dirs} INCLUDE_DIRS ${include_dirs} REQUIRES ${requires})
component_compile_options(-ffast-math -O3 -Wno-error=format=-Wno-format)
3. main.c驱动代码
在main.c里面编写如下代码。
/**
* @brief 显示数据
* @param x: 标题起始X坐标
* @param y: 标题起始Y坐标
* @param data: 温度或俯仰角或横滚角或航向角数据
* @retval 无
*/
static void show_data(uint16_t x, uint16_t y, float data)
{
int16_t temp;
temp = data *10;
if (temp < 0)
{
lcd_show_char(x + 6 * 8, y, '-', 16, 0, BLUE);
temp = -temp;
}
else
{
lcd_show_char(x + 6 * 8, y, ' ', 16, 0, BLUE);
}
lcd_show_num(x + 7 * 8, y, temp / 10, 3, 16, BLUE);
lcd_show_num(x + 11 * 8, y, temp % 10, 1, 16, BLUE);
}
/**
* @brief 传送数据给 ANO_TC匿名科创地面站v4.exe
* @param fun : 功能字. 0XA0~0XAF
* @param data : 数据缓存区,最多28字节!!
* @param len : data区有效数据个数
* @retval 无
*/
void usart0_niming_report(uint8_t fun, uint8_t *data, uint8_t len)
{
unsigned char send_buf[32] = {0};
uint8_t i;
if (len > 28)
{
return; /* 最多28字节数据 */
}
send_buf[len + 4] = 0; /* 校验数置零 */
send_buf[0] = 0XAA; /* 帧头 */
send_buf[1] = 0XAA; /* 帧头 */
send_buf[2] = fun; /* 功能字 */
send_buf[3] = len; /* 数据长度 */
for (i = 0; i < len; i++)
{
send_buf[4 + i] = data[i]; /* 复制数据 */
}
for (i = 0; i < len + 4; i++)
{
send_buf[len + 4] += send_buf[i]; /* 计算校验和 */
}
uart_write_bytes(USART_UX, (const char*)send_buf, len + 5);
}
/**
* @brief 发送加速度传感器数据和陀螺仪数据
* @param aacx,aacy,aacz : x,y,z三个方向上面的加速度值
* @param gyrox,gyroy,gyroz : x,y,z三个方向上面的陀螺仪值
* @retval 无
*/
void qmi8658a_send_data(short aacx, short aacy, short aacz, short gyrox, short gyroy, short gyroz)
{
uint8_t tbuf[18];
tbuf[0] = (aacx >> 8) & 0XFF;
tbuf[1] = aacx & 0XFF;
tbuf[2] = (aacy >> 8) & 0XFF;
tbuf[3] = aacy & 0XFF;
tbuf[4] = (aacz >> 8) & 0XFF;
tbuf[5] = aacz & 0XFF;
tbuf[6] = (gyrox >> 8) & 0XFF;
tbuf[7] = gyrox & 0XFF;
tbuf[8] = (gyroy >> 8) & 0XFF;
tbuf[9] = gyroy & 0XFF;
tbuf[10] = (gyroz >> 8) & 0XFF;
tbuf[11] = gyroz & 0XFF;
/* 无磁力计数据,所以这里直接屏蔽掉.用0替代. */
tbuf[12]=0;
tbuf[13]=0;
tbuf[14]=0;
tbuf[15]=0;
tbuf[16]=0;
tbuf[17]=0;
usart0_niming_report(0X02, tbuf, 18); /* 自定义帧,0X02 */
}
/**
* @brief 通过串口1上报结算后的姿态数据给电脑
* @param roll: 横滚角.单位0.1度。 -9000 -> 9000 对应 -90.00 -> 90.00度
* @param pitch: 俯仰角.单位 0.1度。-18000 -> 18000 对应 -180.00 -> 180.00 度
* @param yaw: 航向角.单位为0.1度 -18000 -> 18000 对应 -180.00 -> 180.00 度
* @param prs: 气压计高度,单位:cm
* @param fly_mode : 飞行模式
* @param armed: 锁定状态
* @retval 无
*/
void usart0_report_imu(short roll, short pitch, short yaw, int prs, uint8_t fly_mode, uint8_t armed)
{
uint8_t tbuf[12];
tbuf[0] = (roll >> 8) & 0XFF;
tbuf[1] = roll & 0XFF;
tbuf[2] = (pitch >> 8) & 0XFF;
tbuf[3] = pitch & 0XFF;
tbuf[4] = (yaw >> 8) & 0XFF;
tbuf[5] = yaw & 0XFF;
tbuf[6] = (prs >> 24) & 0XFF;
tbuf[7] = (prs >> 16) & 0XFF;
tbuf[8] = (prs >> 8) & 0XFF;
tbuf[9] = prs & 0XFF;
tbuf[10] = fly_mode;
tbuf[11] = armed;
usart0_niming_report(0X01, tbuf, 12); /* 状态帧,0X01 */
}
void app_main(void)
{
esp_err_t ret;
uint8_t t = 0;
uint8_t key;
uint8_t report = 0;
float gyro[3];
float accel[3];
float euler_angle[3] = {0, 0, 0};
ret = nvs_flash_init(); /* 初始化NVS */
if(ret == ESP_ERR_NVS_NO_FREE_PAGES || ret == ESP_ERR_NVS_NEW_VERSION_FOUND)
{
ESP_ERROR_CHECK(nvs_flash_erase());
ESP_ERROR_CHECK(nvs_flash_init());
}
led_init(); /* LED初始化 */
key_init(); /* KEY初始化 */
lcd_init(); /* LCD屏初始化 */
uart0_init(115200); /* 初始化UART */
lcd_show_string(30, 50, 200, 16, 16, "ESP32-P4", RED);
lcd_show_string(30, 70, 200, 16, 16, "QMI8658A TEST", RED);
lcd_show_string(30, 90, 200, 16, 16, "ATOM@ALIENTEK", RED);
lcd_show_string(30, 110, 200, 16, 16, "BOOT:UPLOAD ON/OFF", RED);
while (qmi8658_init() != 0) /* 初始化QMI8658A */
{
lcd_show_string(30, 130, 200, 16, 16, "QMI8658A Error!", RED);
vTaskDelay(pdMS_TO_TICKS(200));
lcd_fill(30, 130, 239, 156, WHITE);
vTaskDelay(pdMS_TO_TICKS(200));
}
lcd_show_string(30, 130, 200, 16, 16, "QMI8658A Ready!", RED);
lcd_show_string(30, 150, 200, 16, 16, "UPLOAD OFF", BLUE);
lcd_show_string(30, 170, 200, 16, 16, " Temp: . C", BLUE);
lcd_show_string(30, 190, 200, 16, 16, "Pitch: . C", BLUE);
lcd_show_string(30, 210, 200, 16, 16, " Roll: . C", BLUE);
lcd_show_string(30, 230, 200, 16, 16, " Yaw: . C", BLUE);
while (1)
{
qmi8658_read_xyz(accel, gyro);
key = key_scan(0);
if (key == BOOT_PRES) /* 0.2秒左右更新一次三轴原始值 */
{
/* 切换匿名数据上报开关 */
report = !report;
if (report == 0)
{
lcd_show_string(30, 150, 200, 16, 16, "UPLOAD OFF", BLUE);
}
else
{
lcd_show_string(30, 150, 200, 16, 16, "UPLOAD ON ", BLUE);
}
}
/* 获取并显示温度 */
show_data(30, 170, qmi8658_get_temperature());
/* 获取并显示欧拉角 */
if (g_imu_init)
{
imu_get_eulerian_angles(accel, gyro, euler_angle, IMU_DELTA_T);
show_data(30, 190, euler_angle[0]);
show_data(30, 210, euler_angle[1]);
show_data(30, 230, euler_angle[2]);
if (report != 0)
{
/* 上报匿名状态帧 */
qmi8658a_send_data(accel[0], accel[1], accel[2], gyro[0],
gyro[1], gyro[2]); /* 发送加速度+陀螺仪原始数据 */
/* Pitch和Roll角位置调换 */
usart0_report_imu((int)(euler_angle[1] * 100),
(int)(euler_angle[0]*100), (int)(euler_angle[2]*100), 0, 0, 0);
}
}
t++;
if (t == 100)
{
t = 0;
LED0_TOGGLE();
}
vTaskDelay(pdMS_TO_TICKS(10));
}
}
这部分代码除了app_main函数，还有几个函数，用于上报数据给上位机软件，利用上位机软件显示传感器波形以及3D姿态显示，有助于更好的调试QMI8658A。上位机软件使用ANO_TC匿名科创地面站v4.exe，该软件在：开发板光盘→6，软件资料→软件→5，其他软件→匿名地面站文件夹里可以找到，该软件的使用方法，见该文件夹下的：飞控通信协议v1.3-0720.pdf，这里我们不做介绍。其中，usart0_niming_report函数用于将数据打包、计算校验和，然后上报给匿名地面站上位机软件。qmi8658a_send_data函数用于上报加速度和陀螺仪的原始数据，可用于波形显示传感器数据，通过传感器帧（0X02）发送。而uasrt0_report_imu函数，则用于发送匿名状态帧，可以实时3D显示QMI8658A的姿态，传感器数据等，通过状态帧（0X01）发送。
在app_main函数中，设置一个while循环去初始化QMI8658A，若QMI8658A成功被初始化，程序便可往下执行。在while循环中会调用qmi8658_read_xyz和qmi8658_get_temperature函数读取加速度计、陀螺仪和温度，然后imu_get_eulerian_angles函数会根据加速度计和陀螺仪的值进行运算得出欧拉角。若按下BOOT按键便可以开启数据上传功能，通过上位机（ANO_TC匿名科创地面站）可以实时显示QMI8658A的传感器状态曲线，并显示3D姿态。
34.4 下载验证
将程序下载到开发板后，可以看到LED0不停的闪烁，提示程序已经在运行了。LCD显示的内容如下图所示：
图34.4.1 QMI8658A实验程序运行效果图
屏幕显示了QMI8658A的温度、俯仰角（Pitch）、横滚角（Roll）和航向角（Yaw）的数值。然后，我们可以晃动开发板，看看各角度的变化。
另外，模块默认是关闭状态，通过按下BOOT可以开启或关闭数据上报，在开启状态下，我们可以打开：ANO_TC匿名科创地面站v4.exe这个软件，接收ESP32-P4上传的数据，从而图形化显示飞行姿态，如图34.4.2所示：
图34.4.2 飞控状态显示