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

最新推荐文章于 2024-02-29 10:13:20 发布
最新推荐文章于 2024-02-29 10:13:20 发布
阅读量1.8k 收藏 21
点赞数 3
分类专栏： 四旋翼飞行器 文章标签： 四旋翼飞行器 姿态读取 MPU6050
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/jerryandtony/article/details/77715530
版权

  • 小希 所用的是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;
}

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

【四旋翼｜B站笔记】四旋翼飞行器姿态解算算法入门学习
【人工智障 | 计算鸡视觉 | 机器孩】
09-17 412
【教程】四旋翼飞行器姿态解算算法入门学习-Rick Grimes
stm32读取gy87
09-12
stm32读取gy87，包括mpu6050、hmc5883、bmp180，可直接使用
MPU6050通过XDA和XCL读取HMC5883数据
z659941528的专栏
06-09 1万+
MPU6050作为I2C MASTER读取HMC5883数据，MPU6050的XDA和XCL分别与HMC5883的SDA和SCL相连。 下面引用MultiWii中配置MPU6050 I2C MASTER的代码段： #if defined(MPU6050_I2C_AUX_MASTER) //at this stage, the MAG is configured via the or
四旋翼飞行器教学笔记2.2——姿态读取的计算
致终将努力的我们
09-11 1142
在能够成功读取并且已经对读取到的数据进行误差处理后，就要对得到的数据进行处理，进行计算，小希是通过得到四元数，计算出欧拉角来进行读取飞行器姿态的。 具体的公式小希就不去搜了，po在这上面了，网上有很多，大家随便搜一下就知道了。 小希就直接po源码了。 通过三个加速度和三个角速度先计算出四元数，再得到欧拉角！ void IMU_Get_Attitude(float ax, float ay,
MPU6050-扫盲
zuoheizhu的博客
07-21 2万+
文章部分内容转载于:https://blog.csdn.net/zmjames2000/article/details/88379640 管脚 名称 说明 VCC 3.3-5V （内部有稳压芯片） GND 地线 SCL MPU6050 作为从机时 IIC 时钟线 SDA MPU6050 作为从机时 IIC 数据线 XCL MPU6050 作为主机时 IIC 时钟线...
STM32通过MPU6050读取HMC5883数据时遇到的问题
u010030591的专栏
10-22 9483
上图为所用模块，为市场常见的MPU6050模块（GY-521）和HMC5883模块（GY-271） 遇到的问题： 1、模块共用STM32的I2C接口时的连接方法？ 答： 
GY-87资料.rar_87gy_GY 87_GY...87_TKNZ_gy资料
07-15
gy87详细资料，原理图，各个模块的说明以及Ardino的控制程序
微型四旋翼飞行器设计经验之瞎扯淡
Rick_Grimes的博客
07-19 3900
在正式开始记录微型四旋翼飞行器设计的学习笔记之前，感觉自己很有必要先给自己一个总结，也希望能够帮到更多的朋友少走弯路（虽然不知道帮助大不大）。 去年九月下旬开始了微型四旋翼飞行器的学习与设计，在12月底的时候初步实现了稳定的遥控与飞行，后续又增加了一些常规的辅助的功能，失控保护、姿态微调等等，之后又利用OLED屏幕，设计了一个三级菜单，把各种参数的设置，飞控锁定与通讯的状态等等一些信息都整合在一
四旋翼飞行器知识详解：导线电阻影响与关键技术点
最后，资源中还包含了关于解决四旋翼飞行器高度保持问题、硬件选型（电池、电机、螺旋桨、电调和机架）、无人机操控和DIY四轴飞行器的指导，这些都是构建和维护无人机时需要考虑的实际问题。例如，选择适合的电机、...
Optitrack视觉定位下基于ROS及PX4搭建四旋翼多机飞行平台
Lftree的博客
10-02 1918
单机平台搭建介绍 基于单机组建ROS网络介绍 控制代码分享
GY-87数据手册
08-09
GY-87详细数据手册，官方资料包含内置硬件资料超详细。
Arduino-GY-85.zip
09-18
Arduino-GY-85.zip,GY-85（ADXL345加速度计、ITG3200陀螺仪和HMC5883L磁强计）GY-85的Arduino实现,Arduino是一家开源软硬件公司和制造商社区。Arduino始于21世纪初，深受电子制造商的欢迎，Arduino通过开源系统提供了很多灵活性。
MPU6500+QMC5883L驱动程序，基于STM32F3x编写，测试可用，内有注解
08-22
MPU6500+QMC5883L驱动程序，基于STM32F3x编写，测试可用，内有注解。 使用IO模拟IIC接口。
HMC5883 QMC5883L指南+针罗盘模块+三轴磁场传感器测试代码和数据手册
03-12
HMC5883L是进口的，而QMC5883L是国产的。两款模块使用方法完全不一样。HMC5883和QMC5883的测试程序，以及数据手册。测试程序有STC51的，Arduino。
MPU6050和HMC5983的I2C接口连接
08-11
MPU6050通过I2C接口连接到主控制器，我这里就是单片机，用的是STM32F103，在初始化阶段，要设置MPU6050的测量感度和裁量频率等等，如果要连接HMC5983，还得开通AUXI2C。
BMS电池管理系统笔记——持续补充
最新发布
weixin_52232783的博客
02-29 4405
检测电池包的正极对壳体和负极对壳体的绝缘阻值，防止电池包漏电导致安全事故发生。
小白入门笔记：CMake编译过程详解
3D视觉工坊
04-12 1145
作者丨Sky Shaw@知乎点击进入—>3D视觉工坊学习交流群1、你好，CMake1.1 CMake是什么？我觉得针对这个问题最简单（但不是最正确的）的回答应该是：“CMake是服务于将源代111码转换成可执行的文件的工具”。将源码转换为可工作应用会比较神奇。不仅是效果本身(即设计并赋予生命的工作机制)，而且是将理念付诸于过程的行为本身。CMake本身是一个工具集，由五个可执行的程序组成：c...
MPU6050+HMC5883+BMP180+GPS导航系统设计
boboelec的博客
03-13 7122
老师有个项目，让我搞惯导这一块,虽然最后也没用廉价的MPU6050，而是用了一两万的Xsens。但是本人还是想写一下MPU6050，虽然技术含量不高，但是写下来，做个纪念吧。
基于STM32的四旋翼无人机项目（二）：MPU6050姿态解算（含上位机3D姿态显示教学）
热门推荐
black_sneak的博客
06-20 6万+
基于STM32的四旋翼无人机项目之MPU6050姿态解算教程，作者手把手教学MPU6050的姿态解算（含卡尔曼滤波教学），同时包含匿名上位机通讯代码教学和使用！
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值