峰岹51单片机+ICM42670+四元数解算+互补滤波计算欧拉角

        常用的IMU陀螺仪计算欧拉角方式为：Cortex M0/M3/M4的单片机+六轴陀螺仪如MPU6050，通过MPU6050自带的DMP库，直接输出四元数q0~q4，然后通过欧拉角转换公式直接转换,如：

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; //横滚角
yaw   = atan2(2 * (q1 * q2 + q0 * q3), q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * 57.3; //航向角

 若采用51核的峰岹FU6572N+不带DMP库的ICM42670，可否也能计算出欧拉角呢?

结果当然是OK！并且占用MCU资源少，程序运行快，上电即稳态！

+

关于MCU用IIC或SPI驱动ICM42670的代码网上非常多，可以从别的文章摘录，这里只分享四元数解算欧拉角的方法：

1、第一步配置全局变量：

#include <FU6522.H>
#include "Angle.h"
// 参数配置
#define Kp  10.0f    // 比例增益
#define Ki  0.008f  // 积分增益
#define halfT 0.01f // 半采样周期（采样率50Hz）
// 全局/静态变量声明
float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;  // 初始四元数
float exInt = 0.0f, eyInt = 0.0f, ezInt = 0.0f;    // 积分误差
float Q_ANGLE_X = 0.0f, Q_ANGLE_Y = 0.0f, Q_ANGLE_Z = 0.0f; // 输出姿态角

2.配置IMU的量程及采样周期等：

       因51单片机主频低，采样周期设置小点，50Hz即可；

void ICM42670_Init(void)
{  
	MPU42670_WriteReg(0x