四旋翼飛行器教學筆記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 = 0x02

void 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 high
void 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 >>3
static 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 MATLAB
const uint8_t filter_order = 4;
const float a_10Hz[5] = {1.0, -3.1806, 3.8612, -2.1122, 0.4383};    // Fs = 200Hz, Fcut = 10Hz
const float b_10Hz[5] = {0.0004, 0.0017, 0.0025, 0.0017, 0.0004};   // Fs = 200Hz, Fcut = 10Hz
const float a_20Hz[5] = {1.0, -2.3695, 2.3140, -1.0547, 0.1874};    // Fs = 200Hz, Fcut = 20Hz
const float b_20Hz[5] = {0.0048, 0.0193, 0.0289, 0.0193, 0.0048};   // Fs = 200Hz, Fcut = 20Hz
const float a_30Hz[5] = {1.0, -1.5704, 1.2756, -0.4844, 0.0762};    // Fs = 200Hz, Fcut = 30Hz
const float b_30Hz[5] = {0.0186, 0.0743, 0.1114, 0.0743, 0.0186};   // Fs = 200Hz, Fcut = 30Hz
static 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;
}

這裏小希也對得到的數據進行了誤差處理