第十八章 六轴传感器——姿态解算实验
在上一章节中，介绍了板载六轴传感器的使用，仅需简单的使用提供的驱动，便可从六轴传感器上获取温度、加速度、陀螺仪等数据，本章将继续且更加深入地介绍板载六轴传感器的使用。通过本章地学习，读者将学习到通过板载六轴传感器获取加速度和角速度数据后进行姿态解算的使用。
本章分为如下几个小节：
18.1 SH3001驱动介绍及使用方法
18.2 硬件设计
18.3 程序设计
18.4 运行验证
18.1 SH3001驱动介绍及使用方法
有关SH3001驱动的介绍及使用方法，请见第17.1小节《SH3001驱动介绍及使用方法》。
18.2 硬件设计
18.2.1 例程功能
1. 初始化SH3001等外设后，在死循环里面不停读取：温度传感器、加速度传感器和陀螺仪，以及姿态解算后的欧拉角等数据，通过串口上报给上位机（温度不上报），利用上位机软件（ANO_TC匿名科创地面站v4.exe） ，可以实时显示SH3001的传感器状态曲线，并显示3D姿态。同时，在LCD模块上面显示温度和欧拉角等信息。
18.2.2 硬件资源
1. SH3001
IIC_SCL - IO22
IIC_SDA - IO23
18.2.3 原理图
本章实验内容，需要使用到板载的SH3001芯片，正点原子DNK210开发板上的SH3001芯片连接原理图，如下图所示：
图18.2.3.1 SH3001连接原理图
18.3 程序设计
18.3.1 姿态解算库介绍
有关SH3001驱动的介绍，请见第17.1小节《SH3001驱动介绍及使用方法》。
我们来介绍一下如何使用这个姿态库，需要调用imu_get_eulerian_angle这个函数来获取欧拉角，然后将读取到的数据用一些变量传入姿态算法库。本次实验我们直接使用sh3001_get_imu_compdata函数读取补偿后的加速度和陀螺仪数据，然后调用imu_data_calibration函数对数据进行校准，再调用imu_get_eulerian_angles函数就能得到姿态解算后的欧拉角：俯仰角（pitch）、横滚角（roll）和航向角（yaw）。
源码包括包括两个文件：imu.c和imu.h，imu.h文件只包含了函数的声明和两个结构体，我们这里不多介绍，下面的们介绍imu.c文件的内容。
/**
* @brief 数据转换
* @note 对加速度数据做一阶低通滤波(参考匿名)，对gyro转成弧度每秒(2000dps)
* @param gx, gy, gz : 3轴陀螺仪数据指针
* @param ax, ay, az : 3轴加速度数据指针
* @retval 无
*/
static void imu_data_transform(float *gx, float *gy, float *gz, float *ax, float *ay, float *az)
{
static double lastax = 0;
static double lastay = 0;
static double lastaz = 0;
*ax = *ax * IMU_NEW_WEIGHT + lastax * IMU_OLD_WEIGHT;
*ay = *ay * IMU_NEW_WEIGHT + lastay * IMU_OLD_WEIGHT;
*az = *az * IMU_NEW_WEIGHT + lastaz * IMU_OLD_WEIGHT;
lastax = *ax;
lastay = *ay;
lastaz = *az;
*gx = *gx * IMU_M_PI / 180 / 16.4f;
*gy = *gy * IMU_M_PI / 180 / 16.4f;
*gz = *gz * IMU_M_PI / 180 / 16.4f;
}
imu_data_transform函数用来数据转换，对校正后的原始数据ax（ay、az）进行低通滤波，对gx（gy、gz）转换成弧度每秒（2000°/s）。
/**
* @brief 姿态解算融合, 核心算法
* @note 使用的是互补滤波算法，没有使用Kalman滤波算法
* 尽量保证该函数的调用频率为: IMU_DELTA_T , 否则YAW会相应的偏大/偏小
* @param gx, gy, gz : 3轴陀螺仪数据
* @param ax, ay, az : 3轴加速度数据
* @retval 无
*/
static void imu_ahrsupdate_nomagnetic(float gx, float gy, float gz, float ax, float ay, float az)
{
static float i_ex, i_ey, i_ez; /* 误差积分 */
float half_t = 0.5f * IMU_DELTA_T;
float vx, vy, vz; /* 当前的机体坐标系上的重力单位向量 */
float ex, ey, ez; /* 四元数计算值与加速度计测量值的误差 */
float q0 = g_q_info.q0;
float q1 = g_q_info.q1;
float q2 = g_q_info.q2;
float q3 = g_q_info.q3;
float q0q0 = q0 * q0;
float q0q1 = q0 * q1;
float q0q2 = q0 * q2;
// float q0q3 = q0 * q3;
float q1q1 = q1 * q1;
// float q1q2 = q1 * q2;
float q1q3 = q1 * q3;
float q2q2 = q2 * q2;
float q2q3 = q2 * q3;
float q3q3 = q3 * q3;
float delta_2 = 0;
/* 对加速度数据进行归一化 得到单位加速度 */
float norm = imu_inv_sqrt(ax * ax + ay * ay + az * az);
ax = ax * norm;
ay = ay * norm;
az = az * norm;
vx = 2 * (q1q3 - q0q2);
vy = 2 * (q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3;
ex = ay * vz - az * vy;
ey = az * vx - ax * vz;
ez = ax * vy - ay * vx;
/* 用差乘误差来做PI修正陀螺仪零偏，
* 通过调节 g_param_kp，g_param_ki 两个参数，
* 可以控制加速度计修正陀螺仪积分姿态的速度。*/
i_ex += IMU_DELTA_T * ex; /* integral error scaled by Ki */
i_ey += IMU_DELTA_T * ey;
i_ez += IMU_DELTA_T * ez;
gx = gx + g_param_kp * ex + g_param_ki * i_ex;
gy = gy + g_param_kp * ey + g_param_ki * i_ey;
gz = gz + g_param_kp * ez + g_param_ki * i_ez;
/*数据修正完成，下面是四元数微分方程*/
/* 四元数微分方程，其中half_t为测量周期的1/4，gx gy gz为陀螺仪角速度，
以下都是已知量，这里使用了一阶龙哥库塔求解四元数微分方程 */
/* q0 = q0 + (-q1 * gx - q2 * gy - q3 * gz) * half_t;
q1 = q1 + ( q0 * gx + q2 * gz - q3 * gy) * half_t;
q2 = q2 + ( q0 * gy - q1 * gz + q3 * gx) * half_t;
q3 = q3 + ( q0 * gz + q1 * gy - q2 * gx) * half_t; */
delta_2 = (2 * half_t * gx) * (2 * half_t * gx) + (2 * half_t * gy) * (2 * half_t * gy) + (2 * half_t * gz) * (2 * half_t * gz);
/* 整合四元数率 四元数微分方程 四元数更新算法，二阶毕卡法 */
q0 = (1 - delta_2 / 8) * q0 + (-q1 * gx - q2 * gy - q3 * gz) * half_t;
q1 = (1 - delta_2 / 8) * q1 + (q0 * gx + q2 * gz - q3 * gy) * half_t;
q2 = (1 - delta_2 / 8) * q2 + (q0 * gy - q1 * gz + q3 * gx) * half_t;
q3 = (1 - delta_2 / 8) * q3 + (q0 * gz + q1 * gy - q2 * gx) * half_t;
/* normalise quaternion */
norm = imu_inv_sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
g_q_info.q0 = q0 * norm;
g_q_info.q1 = q1 * norm;
g_q_info.q2 = q2 * norm;
g_q_info.q3 = q3 * norm;
}
imu_ahrsupdate_nomagnetic函数是姿态解算融合，是crazepony的核心算法，使用了互补滤波算法。在姿态解算中运用了四元数来计算。首先对加速度数据进行归一化，得到单位加速度，再修正陀螺仪的偏差。修正完成后，使用二阶毕卡法更新四元数。
/**
* @brief 得到姿态解算后的欧拉角
* @param gx, gy, gz : 3轴陀螺仪数据
* @param ax, ay, az : 3轴加速度数据
* @retval 返回值 : 欧拉角
*/
eulerian_angles_t imu_get_eulerian_angles(float gx, float gy, float gz, float ax, float ay, float az)
{
eulerian_angles_t eulerangle;
imu_data_transform(&gx, &gy, &gz, &ax, &ay, &az); /* 数据转换 */
imu_ahrsupdate_nomagnetic(gx, gy, gz, ax, ay, az); /* 姿态解算 */
float q0 = g_q_info.q0;
float q1 = g_q_info.q1;
float q2 = g_q_info.q2;
float q3 = g_q_info.q3;
eulerangle.pitch = - asin(-2 * q1 * q3 + 2 * q0 * q2) * 190 / IMU_M_PI;
eulerangle.roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * 180 / IMU_M_PI;
eulerangle.yaw = - atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2 * q2 - 2 * q3 * q3 + 1) * 180 / IMU_M_PI;
/* 可以不用作姿态限度的限制 */
// if (eulerangle.roll > 90 || eulerangle.roll < -90)
// {
// if (eulerangle.pitch > 0)
// {
// eulerangle.pitch = 180 - eulerangle.pitch;
// }
// if (eulerangle.pitch < 0)
// {
// eulerangle.pitch = -(180 + eulerangle.pitch);
// }
// }
if (eulerangle.yaw > 180)
{
eulerangle.yaw -= 360;
}
else if (eulerangle.yaw < -180)
{
eulerangle.yaw += 360;
}
return eulerangle;
}
imu_get_eulerian_angles函数把四元数转换成欧拉角，并且对欧拉角的角度姿态范围做了限制。
18.3.2 main.c代码
main.c中的代码如下所示：
/**
* @brief 显示角度
* @note 以图片显示的方式更快
* @param x, y : 坐标
* @param title: 标题
* @param angle: 角度
* @retval 无
*/
void user_show_data(uint16_t x, uint16_t y, char * title, float angle)
{
char buf[15];
sprintf(buf,"%s%3.1fC", title, angle); /* 格式化输出 */
draw_fill_rectangle_image(lcd_gram, 320, x, y, x + 120, y + 16, WHITE);
draw_string_rgb565_image(lcd_gram, 320, 240, x, y, buf, BLUE);
// lcd_draw_fill_rectangle(x, y, x + 120, y + 16, WHITE);
/* 清除上次数据(最多显示15个字符,15*8=120) */
// lcd_draw_string(x, y, buf, BLUE); /* 显示字符串 */
}
/**
* @brief USB串口发送1个字符
* @param c:要发送的字符
* @retval 无
*/
void usart1_send_char(const uint8_t *c, size_t len)
{
uarths_send_data(c, len);
}
/**
* @brief 传送数据给 ANO_TC匿名科创地面站v4.exe
* @param fun:功能字. 0XA0~0XAF
* @param data:数据缓存区,最多28字节!!
* @param len:data区有效数据个数
* @retval 无
*/
void usart1_niming_report(uint8_t fun, uint8_t *data, uint8_t len)
{
uint8_t send_buf[32];
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]; /* 计算校验和 */
}
usart1_send_char((const uint8_t *)send_buf, (len + 5)); /* 发送数据到串口 */
}
/**
* @brief 发送加速度传感器数据和陀螺仪数据
* @param aacx,aacy,aacz:x,y,z三个方向上面的加速度值
* @param gyrox,gyroy,gyroz:x,y,z三个方向上面的陀螺仪值
* @retval 无
*/
void sh3001_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;
tbuf[12]=0; /* 因为开启MPL后,无法直接读取磁力计数据,所以这里直接屏蔽掉.用0替代. */
tbuf[13]=0;
tbuf[14]=0;
tbuf[15]=0;
tbuf[16]=0;
tbuf[17]=0;
usart1_niming_report(0X02, tbuf, 18); /* 自定义帧,0X02 */
}
/**
* @brief 通过USB串口上报结算后的姿态数据给电脑
* @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 usart1_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;
usart1_niming_report(0X01, tbuf, 12); /* 状态帧,0X01 */
}
/**
* @brief 硬件初始化，绑定GPIO口
* @param 无
* @retval 无
*/
void hardware_init(void)
{
fpioa_set_function(PIN_UART_USB_RX, FUNC_UART_USB_RX);
fpioa_set_function(PIN_UART_USB_TX, FUNC_UART_USB_TX);
}
int main(void)
{
uint8_t t = 0;
float temperature; /* 温度值 */
short acc_data[3]; /* 加速度传感器原始数据 */
short gyro_data[3]; /* 陀螺仪原始数据 */
eulerian_angles_t e_angles;
sysctl_pll_set_freq(SYSCTL_PLL0, 800000000);
sysctl_pll_set_freq(SYSCTL_PLL1, 400000000);
sysctl_pll_set_freq(SYSCTL_PLL2, 45158400);
sysctl_set_power_mode(SYSCTL_POWER_BANK6, SYSCTL_POWER_V18);
sysctl_set_power_mode(SYSCTL_POWER_BANK7, SYSCTL_POWER_V18);
sysctl_set_spi0_dvp_data(1);
lcd_init();
lcd_set_direction(DIR_YX_LRUD);
// 硬件引脚初始化
hardware_init();
uarths_init();
uarths_config(500000,UARTHS_STOP_1);
/* 初始化 IMU */
while (sh3001_init()) /* 检测不到SH3001 */
{
msleep(10);
}
msleep(500);
imu_init();
for (size_t i = 0; i < 320 * 240; i++)
{
lcd_gram[i] = 0xFFFF;
}
while (1)
{
usleep(3100); /* 调节延时，矫正YAW角 */
t++;
sh3001_get_imu_compdata(acc_data, gyro_data);
/* 数据校准 */
imu_data_calibration(&gyro_data[0], &gyro_data[1], &gyro_data[2],
&acc_data[0], &acc_data[1], &acc_data[2]);
/* 获取欧拉角 */
e_angles = imu_get_eulerian_angles(gyro_data[0], gyro_data[1], gyro_data[2],
acc_data[0], acc_data[1], acc_data[2]);
sh3001_send_data(acc_data[0], acc_data[1], acc_data[2], gyro_data[0], gyro_data[1], gyro_data[2]); /* 发送加速度+陀螺仪原始数据 */
usart1_report_imu((int)(e_angles.roll * 100), (int)(e_angles.pitch * 100), (int)(e_angles.yaw * 100), 0, 0, 0);
if (t == 50)
{
temperature = sh3001_get_tempdata(); /* 读取温度值 */
// printf("temp=%.2f\n", temperature);
user_show_data(30, 30, "Temp :", temperature);
user_show_data(30, 50, "Pitch:", e_angles.pitch);
user_show_data(30, 70, "Roll :", e_angles.roll);
user_show_data(30, 90, "Yaw :", e_angles.yaw);
lcd_draw_picture(0, 0, 320, 240, (uint16_t *)lcd_gram);
t = 0;
}
}
}
此部分代码除了main函数，还有几个函数，用于上报数据给上位机软件，可以通过上位机软件显示六轴传感器波形，以及3D姿态显示，有助于更好的调试SH3001。上位机软件使用ANO_TC匿名科创地面站v4.exe，该软件在：A盘à软件资料à软件à匿名科创地面站文件夹里可以找到，该软件的使用方法，见该文件夹下的：飞控通信协议v1.3-0720.pdf，这里我们不做介绍。其中，usart1_niming_report函数用于将数据打包、计算校验和，然后上报给匿名地面站上位机软件。sh3001_send_data函数用于上报加速度和陀螺仪的原始数据，可用于波形显示传感器数据，通过传感器帧（0X02）发送。而usart1_report_imu函数，则用于上报飞控显示帧，可以实时3D显示SH3001的姿态，传感器数据等，通过状态帧（0X01）发送。
下面我们来看main函数，利用上位机软件（ANO_TC匿名科创地面站v4.exe），可以实时显示SH3001的传感器状态曲线，并显示3D姿态。同时，在LCD模块上面显示温度和欧拉角等信息。
18.4 运行验证
将DNK210开发板连接到电脑主机，通过VSCode将固件烧录到开发板中，可以看到LCD显示的内容如图所示：
图18.4.2 传感器数据波形显示
图18.4.3 飞控状态显示