无人机底层开发-MPU6050六轴传感器+磁力计初始化

最新推荐文章于 2024-08-15 16:27:44 发布

飘飘仙儿__

最新推荐文章于 2024-08-15 16:27:44 发布

阅读量6.8k

点赞数 5

分类专栏：无人机开发应用

本文链接：https://blog.csdn.net/qq_41422043/article/details/83820114

版权

无人机开发应用专栏收录该内容

5 篇文章 9 订阅

订阅专栏

MPU6050简单初始化步骤如下（仅供参考）：

复位MPU6050。配置电源管理寄存器1（0x6B），复位：将bit7写1实现，复位后，默认值0x40，然后设置0x00-->唤醒MPU6050。
选择X/Y/Z其中一个轴作为参考坐标。同样也是配置电源管理寄存器1（0x6B）。
设置陀螺仪的输出频率。配置采样率分频寄存器（0x07），采样频率 = 陀螺仪输出频率/(1+SMPLRT_DIV) 。当DLPF is disable(DLPF_CFG=0 or 7)，陀螺仪输出频率=8kHz;当DLPF is enabled(寄存器26),陀螺仪输出频率=1kHz
设置低通滤波频率。配置寄存器（0x1A），当滤波频率超过90Hz时无明显滤波效果。
设置陀螺仪最大量程。配置陀螺仪配置寄存器（0x1B）。
设置加速计最大量程。配置加速度传感器配置寄存器（0x1C）；

//MPU6050初始化
//1：复位设备
//2：关闭复位模式，使用Z轴陀螺仪作为参考时钟
//3：设定陀螺仪的输出频率为1KHz
//4：设置低通滤波频率为42
//5：陀螺仪最大量程 +-2000度每秒
//6：加速度计最大量程 +-8G
void MPU6050_Init(void)
{
    MPU6050_Write_Byte(MPU6050_RA_PWR_MGMT_1, 0x80); // 复位MPU6050
    vTaskDelay(100/portTICK_PERIOD_MS);
    MPU6050_Write_Byte(MPU6050_RA_PWR_MGMT_1, MPU6050_CLOCK_PLL_ZGYRO);	// 使用Z轴参考时钟
    //采样频率 = 陀螺仪输出频率/(1+SMPLRT_DIV) 当DLPF is disable(DLPF_CFG=0 or 7)
    //陀螺仪输出频率=8kHz;当DLPF is enabled(寄存器26),陀螺仪输出频率=1kHz
    MPU6050_Write_Byte(MPU6050_RA_SMPLRT_DIV, 1000/(1000) - 1);	
    MPU6050_Write_Byte(MPU6050_RA_CONFIG, MPU6050_DLPF_BW_42);	//设置低通滤波频率，当滤波频率超过90Hz时无明显滤波效果
    MPU6050_Write_Byte(MPU6050_RA_GYRO_CONFIG, MPU6050_GYRO_FS);	//陀螺仪最大量程 + - 2000度每秒
    MPU6050_Write_Byte(MPU6050_RA_ACCEL_CONFIG, MPU6050_ACCEL_AFS); // 加速度计最大量程 +-8G
    MPU6050_Write_Byte(0x19, MPU6050_GYRO_FS); // 唤醒MPU6050
}

从MPU6050寄存器中读取数据：

这里只读取MPU6050的陀螺仪、加速度计、温度的值。
加速度计的值分布在6个字节的地址上，分为X轴的高8位和低8位数据，Y轴的高8位和低8位数据，Z轴的高8位和低8位数据，寄存器地址为0x3B ~ 0x40。
同样陀螺仪的值也分布在6个字节的地址上，分为X轴的高8位和低8位数据，Y轴的高8位和低8位数据，Z轴的高8位和低8位数据，寄存器地址为0x43 ~ 0x48。
温度的值分为2个字节的地址，也是高八位和低八位。寄存器地址为0x41 ~ 0x42。

//从MPU6050传感器中获取陀螺仪、加速度计的原始值
//acc : 加速度计的值
//gyro: 陀螺仪的值
bool MPU6050_Read(Vector3i* acc, Vector3i* gyro)
{
    bool staue = false;
    uint8_t buf[14] = {0};
    staue = MPU6050_Read_NByte(MPU6050_RA_ACCEL_XOUT_H,buf,14);
    if(staue == true)
    {
	acc->x = (int16_t)buf[0]<<8 | buf[1];
	acc->y = (int16_t)buf[2]<<8 | buf[3];
	acc->z = (int16_t)buf[4]<<8 | buf[5];
		
	gyro->x = (int16_t)buf[8]<<8 | buf[9];
	gyro->y = (int16_t)buf[10]<<8 | buf[11];
	gyro->z = (int16_t)buf[12]<<8 | buf[13];
	return true;
    }
    else
        return false;
}

MPU6050寄存器地址宏定义如下：

#define MPU6050_ADDRESS_AD0_LOW 0xD0 // address pin low (GND), default for InvenSense evaluation board
#define MPU6050_ADDRESS_AD0_HIGH 0xD2 // address pin high (VCC)

#define MPU6050_DEFAULT_ADDRESS MPU6050_ADDRESS_AD0_HIGH
#define MPU6050_RA_XG_OFFS_TC 0x00 //[7] PWRMODE, [6:1] XGOFFS_TC, [0] OTPBNKVLD
#define MPU6050_RA_YG_OFFS_TC 0x01 //[7] PWRMODE, [6:1] YGOFFS_TC, [0] OTPBNKVLD
#define MPU6050_RA_ZG_OFFS_TC 0x02 //[7] PWRMODE, [6:1] ZGOFFS_TC, [0] OTPBNKVLD
#define MPU6050_RA_X_FINE_GAIN 0x03 //[7:0] XFINEGAIN
#define MPU6050_RA_Y_FINE_GAIN 0x04 //[7:0] YFINEGAIN
#define MPU6050_RA_Z_FINE_GAIN 0x05 //[7:0] ZFINEGAIN
#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_PRODUCT_ID	0x0C
#define MPU6050_RA_XG_OFFS_USRH 0x13 //[15:0] XGOFFSUSR
#define MPU6050_RA_XG_OFFS_USRL 0x14
#define MPU6050_RA_YG_OFFS_USRH 0x15 //[15:0] YGOFFSUSR
#define MPU6050_RA_YG_OFFS_USRL 0x16
#define MPU6050_RA_ZG_OFFS_USRH 0x17 //[15:0] ZGOFFSUSR
#define MPU6050_RA_ZG_OFFS_USRL 0x18
#define MPU6050_RA_SMPLRT_DIV 0x19  //采样率分频寄存器:0x07(125Hz)
#define MPU6050_RA_CONFIG 0x1A	 //配置寄存器:0x06(5Hz)
#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	 // FIFO使能寄存器
#define MPU6050_RA_I2C_MST_CTRL 0x24 // IIC主机控制寄存器
#define MPU6050_RA_I2C_SLV0_ADDR 0x25 // IIC从机0器件地址寄存器
#define MPU6050_RA_I2C_SLV0_REG 0x26 // IIC从机0数据地址寄存器
#define MPU6050_RA_I2C_SLV0_CTRL 0x27 // IIC从机0控制寄存器
#define MPU6050_RA_I2C_SLV1_ADDR 0x28 // IIC从机1器件地址寄存器
#define MPU6050_RA_I2C_SLV1_REG 0x29 // IIC从机1数据地址寄存器
#define MPU6050_RA_I2C_SLV1_CTRL 0x2A // IIC从机1控制寄存器
#define MPU6050_RA_I2C_SLV2_ADDR 0x2B // IIC从机2器件地址寄存器
#define MPU6050_RA_I2C_SLV2_REG 0x2C // IIC从机2数据地址寄存器
#define MPU6050_RA_I2C_SLV2_CTRL 0x2D // IIC从机2控制寄存器
#define MPU6050_RA_I2C_SLV3_ADDR 0x2E // IIC从机3器件地址寄存器
#define MPU6050_RA_I2C_SLV3_REG 0x2F // IIC从机3数据地址寄存器
#define MPU6050_RA_I2C_SLV3_CTRL 0x30 // IIC从机3控制寄存器
#define MPU6050_RA_I2C_SLV4_ADDR 0x31 // IIC从机4器件地址寄存器
#define MPU6050_RA_I2C_SLV4_REG 0x32 // IIC从机4数据地址寄存器
#define MPU6050_RA_I2C_SLV4_DO 0x33 // IIC从机4写数据地址寄存器
#define MPU6050_RA_I2C_SLV4_CTRL 0x34 // IIC从机4控制寄存器
#define MPU6050_RA_I2C_SLV4_DI 0x35 // IIC从机4读数寄存器
#define MPU6050_RA_I2C_MST_STATUS 0x36 // IIC主机状态寄存器
#define MPU6050_RA_INTPINCFG 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 //0x3B~0x40 加速度传感器数据输出寄存器  H高8位 L低8位
#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 // 温度传感器的值  H高8位  L低8位
#define MPU6050_RA_TEMP_OUT_L 0x42
#define MPU6050_RA_GYRO_XOUT_H 0x43 // 0x43~0x48 陀螺仪数据输出寄存器 H高8位  L低8位
#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_I2C_SLV0_DO 0x63 // IIC从机0数据寄存器
#define MPU6050_RA_I2C_SLV1_DO 0x64 // IIC从机1数据寄存器
#define MPU6050_RA_I2C_SLV2_DO 0x65 // IIC从机2数据寄存器
#define MPU6050_RA_I2C_SLV3_DO 0x66 // IIC从机3数据寄存器
#define MPU6050_RA_I2C_MST_DELAY_CTRL 0x67 // IIC主机延时管理寄存器
#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  // 电源管理寄存器1，复位：将bit7写1实现，复位后，默认值0x40，然后设置0x00-->唤醒MPU6050
#define MPU6050_RA_PWR_MGMT_2 0x6C // 电源管理寄存器2

#define MPU6050_CLOCK_INTERNAL 0x00 	// 内部8M晶振
#define MPU6050_CLOCK_PLL_XGYRO 0x01 	// 参考X轴陀螺仪
#define MPU6050_CLOCK_PLL_YGYRO 0x02  // 参考y轴陀螺仪
#define MPU6050_CLOCK_PLL_ZGYRO 0x03 	// 参考z轴陀螺仪
#define MPU6050_CLOCK_PLL_EXT32K 0x04 	// 外部32.768KHz
#define MPU6050_CLOCK_PLL_EXT19M 0x05 	// 外部19.2MHz
#define MPU6050_CLOCK_KEEP_RESET 0x07
// Register 26 ?Configuration
#define MPU6050_CFG_DLPF_CFG
#define MPU6050_DLPF_BW_256 0x00 // 当陀螺仪频率=8kHz时有效,

#define MPU6050_GYRO_YG_ST 0x40
#define MPU6050_GYRO_ZG_ST 0x20
#define MPU6050_GYRO_FS_250 0x00 // +-250°/s
#define MPU6050_GYRO_FS_500 0x08 // +-500°/s
#define MPU6050_GYRO_FS_1000 0x10 // +-1000°/s
#define MPU6050_GYRO_FS_2000 0x18 // +-2000°/s
// Register 28 - Accelerometer Configuration
#define MPU6050_GYRO_XA_ST 0x80 // Self-test response = Sensor output with self-test enabled - Sensor output without self-test enabled
#define MPU6050_GYRO_YA_ST 0x40
#define MPU6050_GYRO_ZA_ST 0x20
#define MPU6050_ACCEL_AFS_2 0x00 // +-2g
#define MPU6050_ACCEL_AFS_4 0x08 // +-4g
#define MPU6050_ACCEL_AFS_8 0x10 // +-8g
#define MPU6050_ACCEL_AFS_16 0x18 // +-16g

HMC5883L磁力计初始化步骤如下（仅供参考）：

设置采样平均数，输出速率，模式。配置寄存器A(地址0)。
设置周围磁场强度及高斯增益（地磁场强度大约0.5~0.6高斯）。配置寄存器B(地址1)。
设置测量模式。配置模式寄存器（地址2）。

//函数功能 ： 磁力计初始化程序，初始化步骤为：
//1.设置采样平均数为0 ， 输出速率为75Hz，正常模式
//2.设置周围磁场强度+-4.3Ga，高斯增益1090 （地磁场强度大约0.5~0.6高斯）
//3.设置为连续测量模式
void HMC5883L_Init(void)
{
    HMC5883L_Write_Byte(HMC58X3_R_CONFA, 0x78);	// 采样平均数默认，频率75Hz，正常模式
	
    HMC5883L_Write_Byte(HMC58X3_R_CONFB,0x20); // 设置周围磁场强度+-4.3Ga，高斯增益1090 （地磁场强度大约0.5~0.6高斯）
    HMC5883L_Write_Byte(HMC58X3_R_MODE,0x00); // 设置为连续测量模式
}

从HMC5883L中读取数据：

磁力计的原始数据分布在6个字节的地址上，分为分为X轴的高8位和低8位数据，Y轴的高8位和低8位数据，Z轴的高8位和低8位数据，寄存器地址为3 ~8。（其中 7和8地址为Y轴的高八位和低八位）

bool statt = false;
//函数名称 ： HMC5883L_Read
//函数输入 ： 
//1.mag : 读取到的三轴磁力计数据
//函数返回 ： bool : 成功 返回true 失败 返回false
//函数功能 ： 读取三轴磁力计的原始数据
bool HMC5883L_Read(Vector3i* mag)
{
    uint8_t buf[6] = {0};
    statt = HMC5883L_Read_NByte(HMC58X3_R_XM, buf, 6);
    if(statt == true)
    {
	mag->x = (int16_t)buf[0] << 8 | buf[1];
	mag->y = (int16_t)buf[4] << 8 | buf[5];
	mag->z = (int16_t)buf[2] << 8 | buf[3];
	return true;
    }
    else
	return false;
}

磁力计寄存器地址如下：

#define HMC58X3_R_CONFA	0
#define HMC58X3_R_CONFB 1
#define HMC58X3_R_MODE 2
#define HMC58X3_R_XM 3
#define HMC58X3_R_XL 4
#define HMC58X3_R_YM 7 //!< Register address for YM.
#define HMC58X3_R_YL 8 //!< Register address for YL.
#define HMC58X3_R_ZM 5 //!< Register address for ZM.
#define HMC58X3_R_ZL 6 //!< Register address for ZL.
#define HMC58X3_R_STATUS 9
#define HMC58X3_R_IDA 10
#define HMC58X3_R_IDB 11
#define HMC58X3_R_IDC 12
#define HMC5583l_ADDR 0x3C
#define CALIBRATING_MAG_CYCLES 3000 // n*20ms
#define HMC5583l_GAIN	1090	// 高斯增益