使用四元数计算俯仰角和横滚角

系列文章目录

---

前言

本文主要是将采集到的ICM20602的加速度计和陀螺仪原始数据，通过四元数法进行姿态解算，得到相对准确的俯仰角和横滚角数据

---

一、原始数据

ICM20602传感器和MPU6050传感器的数据几乎相同都位于0x3B到0x48这14个字节的寄存器中，每两个寄存器代表加速度计或者陀螺仪一个轴的数据，每个数据都是两个字节。

因为每两个寄存器代表一个轴的加速度计或者陀螺仪数据，所以使用IIC采集到原始数据后需要对数据进行整理，下面是从ICM20602的0x3B寄存器读取原始数据进行整理的代码。

    acc[0] = (int16)((datared[0]<<8) | datared[1]);  //加速度计的X轴数据ACC_X
	acc[1] = (int16)((datared[2]<<8) | datared[3]);  //加速度计的Y轴数据ACC_Y
	acc[2] = (int16)((datared[4]<<8) | datared[5]);  //加速度计的Z轴数据ACC_Z   
	gyr[0] = (int16)((datared[8]<<8) | datared[9] ); //角速度计的X轴数据GYR_X 
	gyr[1] = (int16)((datared[10]<<8)| datared[11]); //角速度计的Y轴数据GYR_Y 
	gyr[2]