四旋翼飞行器教学笔记2 —— 飞行器de姿态读取 1

• 小希 所用的是MPU6050+HMC5883所集成的GY87（6+3,9轴）
  大家可以只买MPU6050（六轴传感器），不过一般都会出现漂的问题（GY87也会存在，不过对于YAW方向的偏移误差会减小很多.

  1. 对于MPU6050，大家得先知道它是一个X，Y，Z 3个方向的加速度计，和3个方向的角速度计合成的6轴模块，而HMC5883是一个磁力计，可以处理Z轴方向上角度的偏差

• 正确读取飞行器的姿态的步骤

  1. 成功 用软件IIC实现和陀螺仪通信
  2. 成功 读取陀螺仪的数据
  3. 成功 对陀螺仪的数据进行误差处理（滤波分析）
  4. 对误差处理后的数据 进行四元数处理 再得到欧拉角（得到姿态）

说完大体的思路，我们正式开始！
步骤1： 接线我就不说啦。直接放代码：

#define IIC_SCL             PORT7.PODR.BIT.B5#define IIC_SDA_O           PORT7.PODR.BIT.B3#define IIC_SDA_I           PORT7.PIDR.BIT.B3#define SET_IIC_SDA_OUTPUT  PORT7.PDR.BIT.B3 = 1#define SET_IIC_SDA_INPUT   PORT7.PDR.BIT.B3 = 0#define SET_IIC_SDA_PULLUP  PORT7.PCR.BYTE = 0x02void IIC_Init(void);void IIC_Start(void);void IIC_Stop(void);void IIC_Ack(void);void IIC_NAck(void);void IIC_Send_Byte(uint8_t txd);uint8_t IIC_WaitAck(void);uint8_t IIC_Read_Byte(unsigned char ack);uint8_t IIC_Read_Bytes(uint8_t dev, uint8_t reg, uint8_t length, uint8_t *data);uint8_t IIC_Write_Bytes(uint8_t dev, uint8_t reg, uint8_t length, uint8_t* data);uint8_t IIC_Write_Byte(uint8_t dev, uint8_t reg, uint8_t data);uint8_t IIC_Write_Bits(uint8_t dev, uint8_t reg, uint8_t bitStart, uint8_t length, uint8_t data);uint8_t IIC_Write_Bit(uint8_t dev, uint8_t reg, uint8_t bitNum, uint8_t data);

void IIC_Delay(void){    uint8_t i;    for(i=0;i<2;i++)    {        nop();    }}void IIC_Init(void){    SET_IIC_SDA_OUTPUT;    IIC_SCL = 1;    IIC_Delay();    IIC_SDA_O = 0;    IIC_Delay();}//START:when CLK is high,DATA change form high to low void IIC_Start(void){      SET_IIC_SDA_OUTPUT;    IIC_SDA_O = 1;    IIC_SCL = 1;    IIC_Delay();    IIC_SDA_O = 0;    IIC_Delay();    IIC_SCL = 0;}//STOP:when CLK is high DATA change form low to highvoid IIC_Stop(void){    SET_IIC_SDA_OUTPUT;    IIC_SCL = 0;    IIC_SDA_O = 0;    IIC_Delay();    IIC_SCL = 1;     IIC_SDA_O = 1;    IIC_Delay();}uint8_t IIC_WaitAck(void){    uint8_t ucErrTime = 0;    SET_IIC_SDA_INPUT;     SET_IIC_SDA_PULLUP;    IIC_SCL = 1;    IIC_Delay();    while(IIC_SDA_I)    {        ucErrTime++;        if(ucErrTime>50)        {            IIC_Stop();            return 1;        }      IIC_Delay();    }    IIC_SCL = 0;    return 0;  } void IIC_Ack(void){    SET_IIC_SDA_OUTPUT;    IIC_SCL = 0;    IIC_SDA_O = 0;    IIC_Delay();    IIC_SCL = 1;    IIC_Delay();    IIC_SCL = 0;}void IIC_NAck(void){    SET_IIC_SDA_OUTPUT;    IIC_SCL = 0;    IIC_SDA_O = 1;    IIC_Delay();    IIC_SCL = 1;    IIC_Delay();    IIC_SCL = 0;}                                        void IIC_Send_Byte(uint8_t txd){                            uint8_t t;       SET_IIC_SDA_OUTPUT;    IIC_SCL = 0;    for(t=0;t<8;t++)    {                      IIC_SDA_O = (txd&0x80)>>7;        txd <<= 1;            IIC_Delay();        IIC_SCL = 1;        IIC_Delay();        IIC_SCL = 0;            IIC_Delay();    }    }    uint8_t IIC_Read_Byte(unsigned char ack){    uint8_t i, receive = 0;    SET_IIC_SDA_INPUT;    SET_IIC_SDA_PULLUP;    for(i=0;i<8;i++ )    {        IIC_SCL = 0;         IIC_Delay();        IIC_SCL = 1;        receive <<= 1;        if(IIC_SDA_I)receive++;        IIC_Delay();    }                        if (ack)            IIC_Ack();    else            IIC_NAck();    return receive;}uint8_t IIC_Read_Bytes(uint8_t dev, uint8_t reg, uint8_t length, uint8_t *data){    uint8_t count = 0;    IIC_Start();    IIC_Send_Byte(dev);       IIC_WaitAck();    IIC_Send_Byte(reg);      IIC_WaitAck();        IIC_Start();    IIC_Send_Byte(dev+1);    IIC_WaitAck();    for(count=0;count<length;count++)    {        if(count!=length-1)data[count] = IIC_Read_Byte(1);  //Read with ACK        else  data[count] = IIC_Read_Byte(0);   //Read without ACK    }    IIC_Stop();    return count;}uint8_t IIC_Write_Bytes(uint8_t dev, uint8_t reg, uint8_t length, uint8_t* data){    uint8_t count = 0;    IIC_Start();    IIC_Send_Byte(dev);       IIC_WaitAck();    IIC_Send_Byte(reg);       IIC_WaitAck();        for(count=0;count<length;count++)    {        IIC_Send_Byte(data[count]);         IIC_WaitAck();     }    IIC_Stop();  return 1; //status == 0;}uint8_t IIC_Write_Byte(uint8_t dev, uint8_t reg, uint8_t data){   return IIC_Write_Bytes(dev, reg, 1, &data);}uint8_t IIC_Write_Bits(uint8_t dev, uint8_t reg, uint8_t bitStart, uint8_t length, uint8_t data){    uint8_t b;    if (IIC_Read_Bytes(dev, reg, 1, &b) != 0)    {        uint8_t mask = (0xFF << (bitStart + 1)) | 0xFF >> ((8 - bitStart) + length - 1);        data <<= (8 - length);        data >>= (7 - bitStart);        b &= mask;        b |= data;        return IIC_Write_Byte(dev, reg, b);    }     else     {        return 0;    }}uint8_t IIC_Write_Bit(uint8_t dev, uint8_t reg, uint8_t bitNum, uint8_t data){    uint8_t b;    IIC_Read_Bytes(dev, reg, 1, &b);    b = (data != 0) ? (b | (1 << bitNum)) : (b & ~(1 << bitNum));    return IIC_Write_Byte(dev, reg, b);}

步骤2：首先需要对陀螺仪进行初始化

#define MPU6050_ADDR  0xD0#define MPU6050_ADDRESS_AD0_LOW     0x68 // address pin low (GND), default for InvenSense evaluation board#define MPU6050_ADDRESS_AD0_HIGH    0x69 // address pin high (VCC)#define MPU6050_DEFAULT_ADDRESS     MPU6050_ADDRESS_AD0_LOW#define MPU6050_RA_XG_OFFS_TC       0x00 //[7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD#define MPU6050_RA_YG_OFFS_TC       0x01 //[7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD#define MPU6050_RA_ZG_OFFS_TC       0x02 //[7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD#define MPU6050_RA_X_FINE_GAIN      0x03 //[7:0] X_FINE_GAIN#define MPU6050_RA_Y_FINE_GAIN      0x04 //[7:0] Y_FINE_GAIN#define MPU6050_RA_Z_FINE_GAIN      0x05 //[7:0] Z_FINE_GAIN#define MPU6050_RA_XA_OFFS_H        0x06 //[15:0] XA_OFFS#define MPU6050_RA_XA_OFFS_L_TC     0x07#define MPU6050_RA_YA_OFFS_H        0x08 //[15:0] YA_OFFS#define MPU6050_RA_YA_OFFS_L_TC     0x09#define MPU6050_RA_ZA_OFFS_H        0x0A //[15:0] ZA_OFFS#define MPU6050_RA_ZA_OFFS_L_TC     0x0B#define MPU6050_RA_XG_OFFS_USRH     0x13 //[15:0] XG_OFFS_USR#define MPU6050_RA_XG_OFFS_USRL     0x14#define MPU6050_RA_YG_OFFS_USRH     0x15 //[15:0] YG_OFFS_USR#define MPU6050_RA_YG_OFFS_USRL     0x16#define MPU6050_RA_ZG_OFFS_USRH     0x17 //[15:0] ZG_OFFS_USR#define MPU6050_RA_ZG_OFFS_USRL     0x18#define MPU6050_RA_SMPLRT_DIV       0x19#define MPU6050_RA_CONFIG           0x1A#define MPU6050_RA_GYRO_CONFIG      0x1B#define MPU6050_RA_ACCEL_CONFIG     0x1C#define MPU6050_RA_FF_THR           0x1D#define MPU6050_RA_FF_DUR           0x1E#define MPU6050_RA_MOT_THR          0x1F#define MPU6050_RA_MOT_DUR          0x20#define MPU6050_RA_ZRMOT_THR        0x21#define MPU6050_RA_ZRMOT_DUR        0x22#define MPU6050_RA_FIFO_EN          0x23#define MPU6050_RA_IIC_MST_CTRL     0x24#define MPU6050_RA_IIC_SLV0_ADDR    0x25#define MPU6050_RA_IIC_SLV0_REG     0x26#define MPU6050_RA_IIC_SLV0_CTRL    0x27#define MPU6050_RA_IIC_SLV1_ADDR    0x28#define MPU6050_RA_IIC_SLV1_REG     0x29#define MPU6050_RA_IIC_SLV1_CTRL    0x2A#define MPU6050_RA_IIC_SLV2_ADDR    0x2B#define MPU6050_RA_IIC_SLV2_REG     0x2C#define MPU6050_RA_IIC_SLV2_CTRL    0x2D#define MPU6050_RA_IIC_SLV3_ADDR    0x2E#define MPU6050_RA_IIC_SLV3_REG     0x2F#define MPU6050_RA_IIC_SLV3_CTRL    0x30#define MPU6050_RA_IIC_SLV4_ADDR    0x31#define MPU6050_RA_IIC_SLV4_REG     0x32#define MPU6050_RA_IIC_SLV4_DO      0x33#define MPU6050_RA_IIC_SLV4_CTRL    0x34#define MPU6050_RA_IIC_SLV4_DI      0x35#define MPU6050_RA_IIC_MST_STATUS   0x36#define MPU6050_RA_INT_PIN_CFG      0x37#define MPU6050_RA_INT_ENABLE       0x38#define MPU6050_RA_DMP_INT_STATUS   0x39#define MPU6050_RA_INT_STATUS       0x3A#define MPU6050_RA_ACCEL_XOUT_H     0x3B#define MPU6050_RA_ACCEL_XOUT_L     0x3C#define MPU6050_RA_ACCEL_YOUT_H     0x3D#define MPU6050_RA_ACCEL_YOUT_L     0x3E#define MPU6050_RA_ACCEL_ZOUT_H     0x3F#define MPU6050_RA_ACCEL_ZOUT_L     0x40#define MPU6050_RA_TEMP_OUT_H       0x41#define MPU6050_RA_TEMP_OUT_L       0x42#define MPU6050_RA_GYRO_XOUT_H      0x43#define MPU6050_RA_GYRO_XOUT_L      0x44#define MPU6050_RA_GYRO_YOUT_H      0x45#define MPU6050_RA_GYRO_YOUT_L      0x46#define MPU6050_RA_GYRO_ZOUT_H      0x47#define MPU6050_RA_GYRO_ZOUT_L      0x48#define MPU6050_RA_EXT_SENS_DATA_00 0x49#define MPU6050_RA_EXT_SENS_DATA_01 0x4A#define MPU6050_RA_EXT_SENS_DATA_02 0x4B#define MPU6050_RA_EXT_SENS_DATA_03 0x4C#define MPU6050_RA_EXT_SENS_DATA_04 0x4D#define MPU6050_RA_EXT_SENS_DATA_05 0x4E#define MPU6050_RA_EXT_SENS_DATA_06 0x4F#define MPU6050_RA_EXT_SENS_DATA_07 0x50#define MPU6050_RA_EXT_SENS_DATA_08 0x51#define MPU6050_RA_EXT_SENS_DATA_09 0x52#define MPU6050_RA_EXT_SENS_DATA_10 0x53#define MPU6050_RA_EXT_SENS_DATA_11 0x54#define MPU6050_RA_EXT_SENS_DATA_12 0x55#define MPU6050_RA_EXT_SENS_DATA_13 0x56#define MPU6050_RA_EXT_SENS_DATA_14 0x57#define MPU6050_RA_EXT_SENS_DATA_15 0x58#define MPU6050_RA_EXT_SENS_DATA_16 0x59#define MPU6050_RA_EXT_SENS_DATA_17 0x5A#define MPU6050_RA_EXT_SENS_DATA_18 0x5B#define MPU6050_RA_EXT_SENS_DATA_19 0x5C#define MPU6050_RA_EXT_SENS_DATA_20 0x5D#define MPU6050_RA_EXT_SENS_DATA_21 0x5E#define MPU6050_RA_EXT_SENS_DATA_22 0x5F#define MPU6050_RA_EXT_SENS_DATA_23 0x60#define MPU6050_RA_MOT_DETECT_STATUS    0x61#define MPU6050_RA_IIC_SLV0_DO      0x63#define MPU6050_RA_IIC_SLV1_DO      0x64#define MPU6050_RA_IIC_SLV2_DO      0x65#define MPU6050_RA_IIC_SLV3_DO      0x66#define MPU6050_RA_IIC_MST_DELAY_CTRL   0x67#define MPU6050_RA_SIGNAL_PATH_RESET    0x68#define MPU6050_RA_MOT_DETECT_CTRL      0x69#define MPU6050_RA_USER_CTRL        0x6A#define MPU6050_RA_PWR_MGMT_1       0x6B#define MPU6050_RA_PWR_MGMT_2       0x6C#define MPU6050_RA_BANK_SEL         0x6D#define MPU6050_RA_MEM_START_ADDR   0x6E#define MPU6050_RA_MEM_R_W          0x6F#define MPU6050_RA_DMP_CFG_1        0x70#define MPU6050_RA_DMP_CFG_2        0x71#define MPU6050_RA_FIFO_COUNTH      0x72#define MPU6050_RA_FIFO_COUNTL      0x73#define MPU6050_RA_FIFO_R_W         0x74#define MPU6050_RA_WHO_AM_I         0x75#define MPU6050_TC_PWR_MODE_BIT     7#define MPU6050_TC_OFFSET_BIT       6#define MPU6050_TC_OFFSET_LENGTH    6#define MPU6050_TC_OTP_BNK_VLD_BIT  0#define MPU6050_VDDIO_LEVEL_VLOGIC  0#define MPU6050_VDDIO_LEVEL_VDD     1#define MPU6050_CFG_EXT_SYNC_SET_BIT    5#define MPU6050_CFG_EXT_SYNC_SET_LENGTH 3#define MPU6050_CFG_DLPF_CFG_BIT    2#define MPU6050_CFG_DLPF_CFG_LENGTH 3#define MPU6050_EXT_SYNC_DISABLED       0x0#define MPU6050_EXT_SYNC_TEMP_OUT_L     0x1#define MPU6050_EXT_SYNC_GYRO_XOUT_L    0x2#define MPU6050_EXT_SYNC_GYRO_YOUT_L    0x3#define MPU6050_EXT_SYNC_GYRO_ZOUT_L    0x4#define MPU6050_EXT_SYNC_ACCEL_XOUT_L   0x5#define MPU6050_EXT_SYNC_ACCEL_YOUT_L   0x6#define MPU6050_EXT_SYNC_ACCEL_ZOUT_L   0x7#define MPU6050_DLPF_BW_256         0x00#define MPU6050_DLPF_BW_188         0x01#define MPU6050_DLPF_BW_98          0x02#define MPU6050_DLPF_BW_42          0x03#define MPU6050_DLPF_BW_20          0x04#define MPU6050_DLPF_BW_10          0x05#define MPU6050_DLPF_BW_5           0x06#define MPU6050_GCONFIG_FS_SEL_BIT      4#define MPU6050_GCONFIG_FS_SEL_LENGTH   2#define MPU6050_GYRO_FS_250         0x00#define MPU6050_GYRO_FS_500         0x01#define MPU6050_GYRO_FS_1000        0x02#define MPU6050_GYRO_FS_2000        0x03#define MPU6050_ACONFIG_XA_ST_BIT           7#define MPU6050_ACONFIG_YA_ST_BIT           6#define MPU6050_ACONFIG_ZA_ST_BIT           5#define MPU6050_ACONFIG_AFS_SEL_BIT         4#define MPU6050_ACONFIG_AFS_SEL_LENGTH      2#define MPU6050_ACONFIG_ACCEL_HPF_BIT       2#define MPU6050_ACONFIG_ACCEL_HPF_LENGTH    3#define MPU6050_ACCEL_FS_2          0x00#define MPU6050_ACCEL_FS_4          0x01#define MPU6050_ACCEL_FS_8          0x02#define MPU6050_ACCEL_FS_16         0x03#define MPU6050_DHPF_RESET          0x00#define MPU6050_DHPF_5              0x01#define MPU6050_DHPF_2P5            0x02#define MPU6050_DHPF_1P25           0x03#define MPU6050_DHPF_0P63           0x04#define MPU6050_DHPF_HOLD           0x07#define MPU6050_TEMP_FIFO_EN_BIT    7#define MPU6050_XG_FIFO_EN_BIT      6#define MPU6050_YG_FIFO_EN_BIT      5#define MPU6050_ZG_FIFO_EN_BIT      4#define MPU6050_ACCEL_FIFO_EN_BIT   3#define MPU6050_SLV2_FIFO_EN_BIT    2#define MPU6050_SLV1_FIFO_EN_BIT    1#define MPU6050_SLV0_FIFO_EN_BIT    0#define MPU6050_MULT_MST_EN_BIT     7#define MPU6050_WAIT_FOR_ES_BIT     6#define MPU6050_SLV_3_FIFO_EN_BIT   5#define MPU6050_IIC_MST_P_NSR_BIT   4#define MPU6050_IIC_MST_CLK_BIT     3#define MPU6050_IIC_MST_CLK_LENGTH  4#define MPU6050_CLOCK_DIV_348       0x0#define MPU6050_CLOCK_DIV_333       0x1#define MPU6050_CLOCK_DIV_320       0x2#define MPU6050_CLOCK_DIV_308       0x3#define MPU6050_CLOCK_DIV_296       0x4#define MPU6050_CLOCK_DIV_286       0x5#define MPU6050_CLOCK_DIV_276       0x6#define MPU6050_CLOCK_DIV_267       0x7#define MPU6050_CLOCK_DIV_258       0x8#define MPU6050_CLOCK_DIV_500       0x9#define MPU6050_CLOCK_DIV_471       0xA#define MPU6050_CLOCK_DIV_444       0xB#define MPU6050_CLOCK_DIV_421       0xC#define MPU6050_CLOCK_DIV_400       0xD#define MPU6050_CLOCK_DIV_381       0xE#define MPU6050_CLOCK_DIV_364       0xF#define MPU6050_IIC_SLV_RW_BIT      7#define MPU6050_IIC_SLV_ADDR_BIT    6#define MPU6050_IIC_SLV_ADDR_LENGTH 7#define MPU6050_IIC_SLV_EN_BIT      7#define MPU6050_IIC_SLV_BYTE_SW_BIT 6#define MPU6050_IIC_SLV_REG_DIS_BIT 5#define MPU6050_IIC_SLV_GRP_BIT     4#define MPU6050_IIC_SLV_LEN_BIT     3#define MPU6050_IIC_SLV_LEN_LENGTH  4#define MPU6050_IIC_SLV4_RW_BIT         7#define MPU6050_IIC_SLV4_ADDR_BIT       6#define MPU6050_IIC_SLV4_ADDR_LENGTH    7#define MPU6050_IIC_SLV4_EN_BIT         7#define MPU6050_IIC_SLV4_INT_EN_BIT     6#define MPU6050_IIC_SLV4_REG_DIS_BIT    5#define MPU6050_IIC_SLV4_MST_DLY_BIT    4#define MPU6050_IIC_SLV4_MST_DLY_LENGTH 5#define MPU6050_MST_PASS_THROUGH_BIT    7#define MPU6050_MST_IIC_SLV4_DONE_BIT   6#define MPU6050_MST_IIC_LOST_ARB_BIT    5#define MPU6050_MST_IIC_SLV4_NACK_BIT   4#define MPU6050_MST_IIC_SLV3_NACK_BIT   3#define MPU6050_MST_IIC_SLV2_NACK_BIT   2#define MPU6050_MST_IIC_SLV1_NACK_BIT   1#define MPU6050_MST_IIC_SLV0_NACK_BIT   0#define MPU6050_INTCFG_INT_LEVEL_BIT        7#define MPU6050_INTCFG_INT_OPEN_BIT         6#define MPU6050_INTCFG_LATCH_INT_EN_BIT     5#define MPU6050_INTCFG_INT_RD_CLEAR_BIT     4#define MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT  3#define MPU6050_INTCFG_FSYNC_INT_EN_BIT     2#define MPU6050_INTCFG_IIC_BYPASS_EN_BIT    1#define MPU6050_INTCFG_CLKOUT_EN_BIT        0#define MPU6050_INTMODE_ACTIVEHIGH  0x00#define MPU6050_INTMODE_ACTIVELOW   0x01#define MPU6050_INTDRV_PUSHPULL     0x00#define MPU6050_INTDRV_OPENDRAIN    0x01#define MPU6050_INTLATCH_50USPULSE  0x00#define MPU6050_INTLATCH_WAITCLEAR  0x01#define MPU6050_INTCLEAR_STATUSREAD 0x00#define MPU6050_INTCLEAR_ANYREAD    0x01#define MPU6050_INTERRUPT_FF_BIT            7#define MPU6050_INTERRUPT_MOT_BIT           6#define MPU6050_INTERRUPT_ZMOT_BIT          5#define MPU6050_INTERRUPT_FIFO_OFLOW_BIT    4#define MPU6050_INTERRUPT_IIC_MST_INT_BIT   3#define MPU6050_INTERRUPT_PLL_RDY_INT_BIT   2#define MPU6050_INTERRUPT_DMP_INT_BIT       1#define MPU6050_INTERRUPT_DATA_RDY_BIT      0#define MPU6050_DMPINT_5_BIT            5#define MPU6050_DMPINT_4_BIT            4#define MPU6050_DMPINT_3_BIT            3#define MPU6050_DMPINT_2_BIT            2#define MPU6050_DMPINT_1_BIT            1#define MPU6050_DMPINT_0_BIT            0#define MPU6050_MOTION_MOT_XNEG_BIT     7#define MPU6050_MOTION_MOT_XPOS_BIT     6#define MPU6050_MOTION_MOT_YNEG_BIT     5#define MPU6050_MOTION_MOT_YPOS_BIT     4#define MPU6050_MOTION_MOT_ZNEG_BIT     3#define MPU6050_MOTION_MOT_ZPOS_BIT     2#define MPU6050_MOTION_MOT_ZRMOT_BIT    0#define MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT   7#define MPU6050_DELAYCTRL_IIC_SLV4_DLY_EN_BIT   4#define MPU6050_DELAYCTRL_IIC_SLV3_DLY_EN_BIT   3#define MPU6050_DELAYCTRL_IIC_SLV2_DLY_EN_BIT   2#define MPU6050_DELAYCTRL_IIC_SLV1_DLY_EN_BIT   1#define MPU6050_DELAYCTRL_IIC_SLV0_DLY_EN_BIT   0#define MPU6050_PATHRESET_GYRO_RESET_BIT    2#define MPU6050_PATHRESET_ACCEL_RESET_BIT   1#define MPU6050_PATHRESET_TEMP_RESET_BIT    0#define MPU6050_DETECT_ACCEL_ON_DELAY_BIT       5#define MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH    2#define MPU6050_DETECT_FF_COUNT_BIT             3#define MPU6050_DETECT_FF_COUNT_LENGTH          2#define MPU6050_DETECT_MOT_COUNT_BIT            1#define MPU6050_DETECT_MOT_COUNT_LENGTH         2#define MPU6050_DETECT_DECREMENT_RESET  0x0#define MPU6050_DETECT_DECREMENT_1      0x1#define MPU6050_DETECT_DECREMENT_2      0x2#define MPU6050_DETECT_DECREMENT_4      0x3#define MPU6050_USERCTRL_DMP_EN_BIT             7#define MPU6050_USERCTRL_FIFO_EN_BIT            6#define MPU6050_USERCTRL_IIC_MST_EN_BIT         5#define MPU6050_USERCTRL_IIC_IF_DIS_BIT         4#define MPU6050_USERCTRL_DMP_RESET_BIT          3#define MPU6050_USERCTRL_FIFO_RESET_BIT         2#define MPU6050_USERCTRL_IIC_MST_RESET_BIT      1#define MPU6050_USERCTRL_SIG_COND_RESET_BIT     0#define MPU6050_PWR1_DEVICE_RESET_BIT   7#define MPU6050_PWR1_SLEEP_BIT          6#define MPU6050_PWR1_CYCLE_BIT          5#define MPU6050_PWR1_TEMP_DIS_BIT       3#define MPU6050_PWR1_CLKSEL_BIT         2#define MPU6050_PWR1_CLKSEL_LENGTH      3#define MPU6050_CLOCK_INTERNAL          0x00#define MPU6050_CLOCK_PLL_XGYRO         0x01#define MPU6050_CLOCK_PLL_YGYRO         0x02#define MPU6050_CLOCK_PLL_ZGYRO         0x03#define MPU6050_CLOCK_PLL_EXT32K        0x04#define MPU6050_CLOCK_PLL_EXT19M        0x05#define MPU6050_CLOCK_KEEP_RESET        0x07#define MPU6050_PWR2_LP_WAKE_CTRL_BIT       7#define MPU6050_PWR2_LP_WAKE_CTRL_LENGTH    2#define MPU6050_PWR2_STBY_XA_BIT            5#define MPU6050_PWR2_STBY_YA_BIT            4#define MPU6050_PWR2_STBY_ZA_BIT            3#define MPU6050_PWR2_STBY_XG_BIT            2#define MPU6050_PWR2_STBY_YG_BIT            1#define MPU6050_PWR2_STBY_ZG_BIT            0#define MPU6050_WAKE_FREQ_1P25      0x0#define MPU6050_WAKE_FREQ_2P5       0x1#define MPU6050_WAKE_FREQ_5         0x2#define MPU6050_WAKE_FREQ_10        0x3#define MPU6050_BANKSEL_PRFTCH_EN_BIT       6#define MPU6050_BANKSEL_CFG_USER_BANK_BIT   5#define MPU6050_BANKSEL_MEM_SEL_BIT         4#define MPU6050_BANKSEL_MEM_SEL_LENGTH      5#define MPU6050_WHO_AM_I_BIT        6#define MPU6050_WHO_AM_I_LENGTH     6#define GYRO_GA 0.0609756           // 1/16.4    (measurement scale: 2000deg/s) unit: deg/s#define GYRO_GR 0.0010642           // 1/16.4/180*pi     (measurement scale: 2000deg/s) unit: rad/s#define ACC_G   6.1035e-5   // 1/16384 (measurement scale: 2g) unit: g//#define GYRO_GAIN 32.8            // 32.8  (measurement scale: 1000deg/s)#define ACC_GAIN    16284.0 // 16284 (measurement scale: 2g)extern volatile int16_t Acc_Gyro[6];//void MPU6050_Init(void); //uint8_t MPU6050_TestConnection(void); //void MPU6050_Get_Data6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz);uint8_t MPU6050_Get_DeviceID(void); //void MPU6050_Init_GyroOffset(void);//void MPU6050_Get_Gyro(int16_t* gx, int16_t* gy, int16_t* gz); //

volatile int16_t Acc_Gyro[6];static uint8_t MPU6050_buffer[14];static int16_t MPU6050_FIFO[6][11];  // change to 9 in total, so that the average can be calculated by >>3static int16_t gx_offset = 0, gy_offset = 0, gz_offset = 0;static int16_t ax_new, ay_new, az_new, gx_new, gy_new, gz_new;// IIR Butterworth Filtering, calculated by MATLABconst uint8_t filter_order = 4;const float a_10Hz[5] = {1.0, -3.1806, 3.8612, -2.1122, 0.4383};    // Fs = 200Hz, Fcut = 10Hzconst float b_10Hz[5] = {0.0004, 0.0017, 0.0025, 0.0017, 0.0004};   // Fs = 200Hz, Fcut = 10Hzconst float a_20Hz[5] = {1.0, -2.3695, 2.3140, -1.0547, 0.1874};    // Fs = 200Hz, Fcut = 20Hzconst float b_20Hz[5] = {0.0048, 0.0193, 0.0289, 0.0193, 0.0048};   // Fs = 200Hz, Fcut = 20Hzconst float a_30Hz[5] = {1.0, -1.5704, 1.2756, -0.4844, 0.0762};    // Fs = 200Hz, Fcut = 30Hzconst float b_30Hz[5] = {0.0186, 0.0743, 0.1114, 0.0743, 0.0186};   // Fs = 200Hz, Fcut = 30Hzstatic float ax_x[5], ax_y[5];static float ay_x[5], ay_y[5];static float az_x[5], az_y[5];static float gx_x[5], gx_y[5];static float gy_x[5], gy_y[5];static float gz_x[5], gz_y[5];uint8_t count;/******************************************************************************* * CLK_SEL | Clock Source * --------+-------------------------------------- * 0       | Internal oscillator * 1       | PLL with X Gyro reference * 2       | PLL with Y Gyro reference * 3       | PLL with Z Gyro reference * 4       | PLL with external 32.768kHz reference * 5       | PLL with external 19.2MHz reference * 6       | Reserved * 7       | Stops the clock and keeps the timing generator in reset*******************************************************************************/void MPU6050_Set_ClockSource(uint8_t source){   IIC_Write_Bits(MPU6050_ADDR, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, source);}void MPU6050_Set_FullScaleGyroRange(uint8_t range) {  IIC_Write_Bits(MPU6050_ADDR, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, range);}void MPU6050_Set_FullScaleAccelRange(uint8_t range) {    IIC_Write_Bits(MPU6050_ADDR, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, range);}void MPU6050_Set_SleepEnabled(uint8_t enabled) {  IIC_Write_Bit(MPU6050_ADDR, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, enabled);}void MPU6050_Set_IICMasterModeEnabled(uint8_t enabled) {  IIC_Write_Bit(MPU6050_ADDR, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_IIC_MST_EN_BIT, enabled);}void MPU6050_Set_IICBypassEnabled(uint8_t enabled) {  IIC_Write_Bit(MPU6050_ADDR, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_IIC_BYPASS_EN_BIT, enabled);}/*******************************************************************************/uint8_t MPU6050_Get_DeviceID(void) {  IIC_Read_Bytes(MPU6050_ADDR, MPU6050_RA_WHO_AM_I, 1, MPU6050_buffer);  return MPU6050_buffer[0];}uint8_t MPU6050_TestConnection(void) {  if(MPU6050_Get_DeviceID() == 0x68)       return 1;  else return 0;}void MPU6050_Init(void) {    int16_t temp[6];    unsigned char i;    // set MPU6050 regesters    MPU6050_Set_ClockSource(MPU6050_CLOCK_PLL_XGYRO);     Delay_ms(10);    MPU6050_Set_FullScaleGyroRange(MPU6050_GYRO_FS_2000);    Delay_ms(10);    MPU6050_Set_FullScaleAccelRange(MPU6050_ACCEL_FS_2);        Delay_ms(10);    MPU6050_Set_SleepEnabled(0);     Delay_ms(10);    MPU6050_Set_IICMasterModeEnabled(0);        Delay_ms(10);    MPU6050_Set_IICBypassEnabled(1);     //1    Delay_ms(10);    IIC_Write_Bit(MPU6050_ADDR, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_LEVEL_BIT, 0);    Delay_ms(10);    IIC_Write_Bit(MPU6050_ADDR, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_OPEN_BIT, 0);    Delay_ms(10);    IIC_Write_Bit(MPU6050_ADDR, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_LATCH_INT_EN_BIT, 1);    Delay_ms(10);    IIC_Write_Bit(MPU6050_ADDR, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_RD_CLEAR_BIT, 1);    Delay_ms(10);    IIC_Write_Bit(MPU6050_ADDR, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DATA_RDY_BIT, 1);    Delay_ms(10);    IIC_Write_Byte(MPU6050_ADDR, MPU6050_RA_CONFIG, MPU6050_DLPF_BW_42);    Delay_ms(10);    // Get initial gyro offset                                                                           MPU6050_Init_GyroOffset();}void MPU6050_Get_Data6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz){       uint8_t i;    for(i=0;i<14;i++)    {        IIC_Read_Bytes(MPU6050_ADDR, MPU6050_RA_ACCEL_XOUT_H+i, 1, &MPU6050_buffer[i]);    }    ax_new = (((int16_t)MPU6050_buffer[0]) << 8) | MPU6050_buffer[1];    ay_new = (((int16_t)MPU6050_buffer[2]) << 8) | MPU6050_buffer[3];    az_new = (((int16_t)MPU6050_buffer[4]) << 8) | MPU6050_buffer[5];    // Skip temperature    gx_new = (((int16_t)MPU6050_buffer[8]) << 8) | MPU6050_buffer[9];    gy_new = (((int16_t)MPU6050_buffer[10]) << 8) | MPU6050_buffer[11];    gz_new = (((int16_t)MPU6050_buffer[12]) << 8) | MPU6050_buffer[13];    gx_new -= gx_offset;    gy_new -= gy_offset;    gz_new -= gz_offset;    *ax = IIR_Butterworth_LPF(ax_new, ax_x, ax_y, a_20Hz, b_20Hz, filter_order);    *ay = IIR_Butterworth_LPF(ay_new, ay_x, ay_y, a_20Hz, b_20Hz, filter_order);    *az = IIR_Butterworth_LPF(az_new, az_x, az_y, a_20Hz, b_20Hz, filter_order);    *gx = IIR_Butterworth_LPF(gx_new, gx_x, gx_y, a_30Hz, b_30Hz, filter_order);    *gy = IIR_Butterworth_LPF(gy_new, gy_x, gy_y, a_30Hz, b_30Hz, filter_order);    *gz = IIR_Butterworth_LPF(gz_new, gz_x, gz_y, a_30Hz, b_30Hz, filter_order);}void MPU6050_Get_Gyro(int16_t* gx, int16_t* gy, int16_t* gz) // raw data used for getting offset{    uint8_t i;    for(i=0;i<14;i++)    {        IIC_Read_Bytes(MPU6050_ADDR, MPU6050_RA_ACCEL_XOUT_H+i, 1, &MPU6050_buffer[i]);    }    *gx = (((int16_t)MPU6050_buffer[8]) << 8) | MPU6050_buffer[9];    *gy = (((int16_t)MPU6050_buffer[10]) << 8) | MPU6050_buffer[11];    *gz = (((int16_t)MPU6050_buffer[12]) << 8) | MPU6050_buffer[13];}void MPU6050_Init_GyroOffset(void){    unsigned char i;    int16_t temp[3];    int32_t tempgx=0,tempgy=0,tempgz=0;    gx_offset = 0;    gy_offset = 0;    gz_offset = 0;    for(i=0;i<100;i++)    {        Delay_us(200);//!!!        MPU6050_Get_Gyro(&temp[0], &temp[1], &temp[2]);        tempgx += temp[0];        tempgy += temp[1];        tempgz += temp[2];    }    gx_offset = tempgx/100; // !!!DO NOT TRANSFER TO int16_t    gy_offset = tempgy/100;    gz_offset = tempgz/100;}

这里小希也对得到的数据进行了误差处理