• 地面坐标系（earth-surface inertial reference frame）Sg
  
  ①在地面上选一点Og
  ②使xg轴在水平面内并指向某一方向
  ③zg轴垂直于地面并指向地心
  ④yg轴在水平面内垂直于xg轴，其指向按右手定则确定
• 机体坐标系(Aircraft-body coordinate frame) Sb
  
  ①原点O取在飞机质心处，坐标系与飞机固连
  ②x轴在飞机对称平面内并平行于飞机的设计轴线指向机头
  ③y轴垂直于飞机对称平面指向机身右方
  ④z轴在飞机对称平面内，与x轴垂直并指向机身下方

• 俯仰角θ（pitch）：载体坐标系X轴与水平面的夹角。当X轴的正半轴位于过坐标原点的水平面之上（抬头）时，俯仰角为正，否则为负。
  

• 偏航角ψ（yaw）：载体坐标系xb轴在水平面上投影与地面坐标系xg轴（在水平面上，指向目标为正）之间的夹角，由xg轴逆时针转至机体xb的投影线时，偏航角为正，即机头右偏航为正，反之为负。
  

• 滚转角Φ（roll）：机体坐标系zb轴与通过机体xb轴的铅垂面间的夹角，机体向右滚为正，反之为负。
  

ICM20602是一个六轴运动跟踪装置，它结合了三轴陀螺仪和三轴加速度计

• 基础介绍
    一般用陀螺仪检测偏航角ψ（yaw），但是它实际检测的是角速度，换算角度时需要对角速度积分，这样就会存在积分误差，误差值与积分时间Dt成正比。此外，陀螺仪还存在零点漂移现象，每次上电时需要重新校准偏移值，后续检测时减去偏移值即可消除此误差。

零点漂移：由于各种原因，陀螺仪上往往被作用有人们所不希望的各种干扰力矩，在这些很小干扰力矩的作用下，陀螺仪的陀螺会产生进动，从而使角动量慢慢偏离原来的方向。

• 规格参数
    从官方规格表可以看出，陀螺仪量程为±250 dps、±500 dps、±1000 dps和±2000 dps（degree per second，即°/s），分别对应的灵敏度为131、65.5、32.8和16.4 LSB/dps。
  量程与灵敏度解释：
    ICM20602数据寄存器是一个16位的，由于最高位是符号位，故数据寄存器的输出范围是-7FFF~7FFF， 即-32767~32767。所以当灵敏度为16.4 LSB/dps、读取的陀螺仪值为32767时，对应的角速度为2000°/s。在姿态解算时，需要把角度换算成弧度，即:$$1°/s=\frac{\pi }{180}=57.3rad/s$$

• 基础介绍
    一般用加速度计检测俯仰角θ（pitch），它通过检测器件在各个方向的形变情况而采用得到受力数据，根据F = ma转换，传感器直接输出加速度数据。由于地球存在重力场，重力任何时刻都会作用与传感器，所以静止时检测出加速度为1g，只有当传感器做自由落体运动时才会输出0g。
    加速度计不会区分重力加速度与外力加速度，当物体运动时，它也会在运动的方向上检测出加速度，所以加速度计对振动之类的噪声比较敏感。
• 规格参数
  
    从官方规格表可以看出，加速度计量程为±2 g、±4 g、±8 g和±16 g，分别对应的灵敏度为16384、8192、4096和2048 LSB/g。上述已解释含义，不再赘述。

参考资料：什么是姿态角

  加速度计的静态稳定性更好，而在运动时其数据相对不可靠；陀螺仪的动态稳定性更好，但是静止时数据相对不可靠。因此，我们可以短期相信陀螺仪，长期相信加速度计，使用互补滤波算法可以很好的解决这个问题，即通过加速度计的输出来修正陀螺仪的积分累积误差。

参考：四旋翼姿态解算——互补滤波算法及理论推导

笔者使用的是硬件SPI通信，SPI底层代码就不贴出了。

• 陀螺仪自检函数

void icm20602_self3_check(void)
{ 
        
    uint8 dat=0;
    while(0x12 != dat)   //读取ICM20602 ID
    { 
        
        icm_spi_r_reg_byte(ICM20602_WHO_AM_I, &dat);
        systick_delay_ms(10);
        //卡在这里原因有以下几点
        //1 ICM20602坏了，如果是新的这样的概率极低
        //2 接线错误或者没有接好
        //3 可能你需要外接上拉电阻，上拉到3.3V
    }
}

说明：陀螺仪初始化成功的话，可以从ICM20602_WHO_AM_I寄存器(0x75)读出8 bits 设备ID（与设备地址不是同一个概念），默认值即可0x12，反之初始化失败。

• 初始化函数

void icm20602_init_spi(void)
{ 
        
    uint8 val = 0x0;

    systick_delay_ms(10);  //上电延时
    
    (void)spi_init(SPI_NUM, SPI_SCK_PIN, SPI_MOSI_PIN, SPI_MISO_PIN, SPI_CS_PIN, 3, 10*1000*1000);//硬件SPI初始化

    icm20602_self3_check();//检测
    
    icm_spi_w_reg_byte(ICM20602_PWR_MGMT_1,0x80);//复位设备
    systick_delay_ms(2);
    do
    { 
        //等待复位成功
        icm_spi_r_reg_byte(ICM20602_PWR_MGMT_1,&val);
    }while(0x41 != val);
    
    icm_spi_w_reg_byte(ICM20602_PWR_MGMT_1,     0x01);      //时钟设置
    icm_spi_w_reg_byte(ICM20602_PWR_MGMT_2,     0x00);      //开启陀螺仪和加速度计
    icm_spi_w_reg_byte(ICM20602_CONFIG,         0x01);      //176HZ 1KHZ
    icm_spi_w_reg_byte(ICM20602_SMPLRT_DIV,     0x07);      //! FCHOICE_B[1:0]位为0, 且SMPLRT_DIV > 7，所以不分频。即分频系数不起作用,所以采样频率 = 1K
                                                            //! 采样速率 SAMPLE_RATE = INTERNAL_SAMPLE_RATE / (1 + SMPLRT_DIV)
    icm_spi_w_reg_byte(ICM20602_GYRO_CONFIG,    0x18);      //±2000 dps
    icm_spi_w_reg_byte(ICM20602_ACCEL_CONFIG,   0x10);      //±8g
    icm_spi_w_reg_byte(ICM20602_ACCEL_CONFIG_2, 0x03);      //Average 4 samples 44.8HZ //0x23 Average 16 samples
}

说明：六轴采样频率初始化为1K（采样周期为0.001s），陀螺仪测量范围为±2000 dps ，对应灵敏度为16.4 LSB/dps；加速度计测量范围为±8g， 对应灵敏度为4096 LSB/g。

• 读取陀螺仪

void get_icm20602_accdata_spi(void)
{ 
        
    struct
    { 
        
        uint8 reg;
        uint8 dat[6];
    }buf;

    buf.reg = ICM20602_ACCEL_XOUT_H | ICM20602_SPI_R;
    
    icm_spi_r_reg_bytes(&buf.reg, 7);
    icm_acc_x = (int16)(((uint16)buf.dat[0]<<8 | buf.dat[1]));
    icm_acc_y = (int16)(((uint16)buf.dat[2]<<8 | buf.dat[3]));
    icm_acc_z = (int16)(((uint16)buf.dat[4]<<8 | buf.dat[5]));
}

• 读取加速度计数据