MPU6050 主模式(Master)拓展外挂I2C设备 :QMC5883P示例
使用中断触发的;
- qmc5883p.h
//
// Created by root on 26-3-9.
//
#ifndef QMC5883P_H
#define QMC5883P_H
#include "main.h"
//实际是七位地址0x2C << 1
#define QMC5883P_ADDR 0x58
#define QMC5883P_AXIS 0x29
#define QMC5883P_CONTROL_2 0x0B
#define QMC5883P_CONTROL_1 0x0A
//数据寄存器起始地址
#define QMC5883P_DATA_REG 0x01
// 定义结构体
typedef struct {
int16_t mx;
int16_t my;
int16_t mz;
} QMC5883P_DATA;
//初始化
HAL_StatusTypeDef QMC5883P_Init(I2C_HandleTypeDef* handle);
//阻塞式序列更新数据
void QMC5883P_loadDataSync();
#endif //QMC5883P_H
- mpu6050.h
//
// Created by root on 26-3-4.
//
#ifndef MPU6050_H
#define MPU6050_H
#include "main.h"
//地址:b110100X
#define MPU6050_ADDRESS 0xD0
#define MPU6050_SMPLRT_DIV 0x19
#define MPU6050_CONFIG 0x1A
#define MPU6050_GYRO_CONFIG 0x1B
#define MPU6050_ACCEL_CONFIG 0x1C
#define MPU6050_INT_STATUS 0x3A
#define MPU6050_ACCEL_XOUT_H 0x3B
#define MPU6050_ACCEL_XOUT_L 0x3C
#define MPU6050_ACCEL_YOUT_H 0x3D
#define MPU6050_ACCEL_YOUT_L 0x3E
#define MPU6050_ACCEL_ZOUT_H 0x3F
#define MPU6050_ACCEL_ZOUT_L 0x40
#define MPU6050_TEMP_OUT_H 0x41
#define MPU6050_TEMP_OUT_L 0x42
#define MPU6050_GYRO_XOUT_H 0x43
#define MPU6050_GYRO_XOUT_L 0x44
#define MPU6050_GYRO_YOUT_H 0x45
#define MPU6050_GYRO_YOUT_L 0x46
#define MPU6050_GYRO_ZOUT_H 0x47
#define MPU6050_GYRO_ZOUT_L 0x48
#define MPU6050_PWR_MGMT_1 0x6B
#define MPU6050_PWR_MGMT_2 0x6C
#define MPU6050_WHO_AM_I 0x75
#define MPU6050_INT_PIN_CFG 0x37
#define MPU6050_INT_ENABLE 0x38
#define MPU6050_USER_CTRL 0x6A
#define MPU6050_I2C_MST_CTRL 0x24
//SLAVE的配置地址
#define MPU6050_SLV0_ADDR 0x25
#define MPU6050_SLV0_REG 0x26
#define MPU6050_SLV0_CTRL 0x27
#define MPU6050_MST_STATUS 0x36
// 定义姿态结构体
typedef struct
{
float pitch; // 俯仰角
float roll; // 横滚角
float yaw; // 偏航角
} EulerAngle;
//加速度计和陀螺仪的数据结构
typedef struct
{
//加速度
int16_t ax;
int16_t ay;
int16_t az;
int16_t temp;
//角速度
int16_t gx;
int16_t gy;
int16_t gz;
} AGData;
//初始化
HAL_StatusTypeDef MPU6050_Init(I2C_HandleTypeDef* handle);
//写单字节寄存器
void MPU6050_WriteReg(uint8_t RegAddress, uint8_t Data);
//读单字节寄存器
uint8_t MPU6050_ReadReg(uint8_t RegAddress);
//读坐标信息.更新数据 3步走
void MPU6050_loadData();
void MPU6050_startLoad();
void MPU6050_sendReadSeq();
void MPU6050_parsingAndCalculate();
//校准陀螺仪
void simpleCalibrateGyroscope();
//解算姿态
void attitudeUpdate();
#endif //MPU6050_H
- mpu6050.c
//
// Created by root on 26-3-4.
//
#include "mpu6050.h"
#include <tgmath.h>
#include "i2c.h"
#include "qmc5883p.h"
static I2C_HandleTypeDef* i2cHandle;
EulerAngle attitude; // 全局姿态变量
AGData agData;//当前传感器值
uint8_t mpuId;
//是否使用mater模式配置QMC5883P
uint8_t materQMC5883P = 1;
extern QMC5883P_DATA qmcData;
//阻塞式写,因为这里的写都是配置式的,初始化写即可
void MPU6050_WriteReg(uint8_t RegAddress, uint8_t Data)
{
HAL_StatusTypeDef status = HAL_I2C_Mem_Write(i2cHandle,MPU6050_ADDRESS,RegAddress,I2C_MEMADD_SIZE_8BIT,&Data,1,HAL_MAX_DELAY);
// 写入失败执行的逻辑 TODO
if (status != HAL_OK) {
mpuId = 999;
}
}
//同步读
uint8_t MPU6050_ReadReg(uint8_t RegAddress)
{
uint8_t Data;
HAL_I2C_Mem_Read(i2cHandle,MPU6050_ADDRESS,RegAddress,I2C_MEMADD_SIZE_8BIT,&Data,1,100);
// HAL_I2C_Master_Seq_Receive_IT()
// HAL_I2C_Mem_Read_IT(i2cHandle,MPU6050_ADDRESS,RegAddress,I2C_MEMADD_SIZE_8BIT,&Data,1);
// HAL_I2C_Mem_Read_DMA(i2cHandle,MPU6050_ADDRESS,RegAddress,I2C_MEMADD_SIZE_8BIT,&Data,1);
return Data;
}
//获取设备id
void MPU6050_GetID(void)
{
HAL_I2C_Mem_Read(i2cHandle,MPU6050_ADDRESS,MPU6050_WHO_AM_I,I2C_MEMADD_SIZE_8BIT,&mpuId,1,100);
}
HAL_StatusTypeDef MPU6050_Init(I2C_HandleTypeDef* handle)
{
i2cHandle = handle;
// 1. 电源管理1 (0x6B)
// 0x01: 解除睡眠，选择 X轴陀螺仪作为系统时钟源
MPU6050_WriteReg(MPU6050_PWR_MGMT_1, 0x01);
// 2. 电源管理2 (0x6C)
// 0x00: 启用所有传感器(加速度计+陀螺仪)，不进入循环模式
MPU6050_WriteReg(MPU6050_PWR_MGMT_2, 0x00);
// 3. 【关键】采样率分频器 (0x19)
// 0x09: 分频值为 9
// 公式: Sample Rate = 1kHz / (1 + 9) = 100Hz
// 意味着: MPU6050 将以每秒100次的速度更新数据寄存器
MPU6050_WriteReg(MPU6050_SMPLRT_DIV, 0x09);
// 4. 【关键】配置寄存器 (0x1A)
// 0x06: DLPF_CFG = 6 (带宽5Hz)
// 作用1: 滤除高频噪声，输出更平滑
// 作用2: 决定了内部陀螺仪输出频率为 1kHz (这是上面公式中 1kHz 的来源)
MPU6050_WriteReg(MPU6050_CONFIG, 0x06);
// 5. 陀螺仪配置 (0x1B)
// 0x00: FS_SEL = 0 -> 量程 ±250°/s
// 灵敏度: 131 LSB/(°/s)
MPU6050_WriteReg(MPU6050_GYRO_CONFIG, 0x00);
// 6. 加速度计配置 (0x1C)
// 0x00: AFS_SEL = 0 -> 量程 ±2g
// 灵敏度: 16384 LSB/g
MPU6050_WriteReg(MPU6050_ACCEL_CONFIG, 0x00);
// 【新增】配置 I2C Master 控制
// WAIT_FOR_ES=1, I2C_MST_P_NSR=1, I2C_MST_CLK=13 (400kHz)
MPU6050_WriteReg(MPU6050_I2C_MST_CTRL, 0b01011101);
// 4. 【新增】配置中断
// 配置 INT 引脚行为: 低电平有效, 脉冲模式,任意读取清除中断状态位
if (materQMC5883P)
{
// 先禁止 MPU 的 Master 模式
MPU6050_WriteReg(MPU6050_USER_CTRL,0x00);
//第二位为1时多路复用开启,通向外接i2c设备
MPU6050_WriteReg(MPU6050_INT_PIN_CFG, 0b10010010);
//开启直通后可以,直接调用QMC的初始化方法,进行初始化
QMC5883P_Init(handle);
// 关闭 Bypass(多路复用),回到master模式
MPU6050_WriteReg(MPU6050_INT_PIN_CFG, 0b10010000);
// 配置 Slave 0 读取 QMC5883P 数据,注意,高位表示RW,和一般不同
//QMC5883P 的7位地址是0x2C,前面补1是0XAC; 不可用HAL库表示的值哦
MPU6050_WriteReg(MPU6050_SLV0_ADDR,0xAC);
MPU6050_WriteReg(MPU6050_SLV0_REG,QMC5883P_DATA_REG);
// 4. 配置 Slave 0 控制 (Register 39)
// Bit 7 (EN) = 1 (使能)
// Bit 6 (BYTE_SW) = 1 (使能字节交换，将 LSB/MSB 转换为 MSB/LSB)
// Bit 5 (REG_DIS) = 0 (每次读之前发送寄存器地址)
// Bit 4 (GRP) = 1 (奇/偶分组，因为起始地址是 0x01)
// Bit 3-0 (LEN) = 6 (读取 6 字节: X, Y, Z)
// 二进制: 1101 0110 = 0xD6
MPU6050_WriteReg(MPU6050_SLV0_CTRL,0b11010110);
// 使能 I2C Master 模式 (Register 106)
MPU6050_WriteReg(MPU6050_USER_CTRL,0x20);
}
else
{
MPU6050_WriteReg(MPU6050_INT_PIN_CFG, 0b10010000);
}
// 使能数据就绪中断 (0x01)
MPU6050_WriteReg(MPU6050_INT_ENABLE, 0x01);
//获取设备ID
MPU6050_GetID();
HAL_Delay(100);
//等待一段时间后自动校准陀螺仪
simpleCalibrateGyroscope();
return HAL_OK;
}
//阻塞式读取
void MPU6050_loadData()
{
// 接收缓冲区：1标志位+ 14(内传感器) + 6(外传感器)
uint8_t i2c_rx_buffer[39]={0};
HAL_I2C_Mem_Read(i2cHandle,MPU6050_ADDRESS,MPU6050_INT_STATUS,1,i2c_rx_buffer,39,100);
agData.ax = (int16_t)((i2c_rx_buffer[1] << 8) | i2c_rx_buffer[2]);
agData.ay = (int16_t)((i2c_rx_buffer[3] << 8) | i2c_rx_buffer[4]);
agData.az = (int16_t)((i2c_rx_buffer[5] << 8) | i2c_rx_buffer[6]);
agData.temp = (int16_t)((i2c_rx_buffer[7] << 8) | i2c_rx_buffer[8]);
agData.gx = (int16_t)((i2c_rx_buffer[9] << 8) | i2c_rx_buffer[10]);
agData.gy = (int16_t)((i2c_rx_buffer[11] << 8) | i2c_rx_buffer[12]);
agData.gz = (int16_t)((i2c_rx_buffer[13] << 8) | i2c_rx_buffer[14]);
qmcData.mx = (int16_t)((i2c_rx_buffer[15] << 8) | i2c_rx_buffer[16]);
qmcData.my = (int16_t)((i2c_rx_buffer[17] << 8) | i2c_rx_buffer[18]);
qmcData.mz = (int16_t)((i2c_rx_buffer[19] << 8) | i2c_rx_buffer[20]);
attitudeUpdate();
}
uint8_t rx_buffer[15];
uint8_t reg = MPU6050_INT_STATUS;
/* 第一步,执行写请求,由ready中断调用*/
void MPU6050_startLoad()
{
i2cSendAsync(MPU6050_ADDRESS, ®, 1,2);
}
/* 第二步：开始接收15字节数据，作为顺序传输的最后一帧 由i2c送成功中断调用 */
void MPU6050_sendReadSeq()
{
HAL_I2C_Master_Seq_Receive_IT(&hi2c1, MPU6050_ADDRESS, rx_buffer, 15, I2C_LAST_FRAME);
}
/* 第三步,由rx成功中断回调调用*/
void MPU6050_parsingAndCalculate()
{
/* 解析接收到的14字节数据（MPU6050为大端格式：高字节在前，低字节在后） */
agData.ax = (int16_t)((rx_buffer[1] << 8) | rx_buffer[2]);
agData.ay = (int16_t)((rx_buffer[3] << 8) | rx_buffer[4]);
agData.az = (int16_t)((rx_buffer[4] << 8) | rx_buffer[6]);
agData.temp = (int16_t)((rx_buffer[7] << 8) | rx_buffer[8]);
agData.gx = (int16_t)((rx_buffer[9] << 8) | rx_buffer[10]);
agData.gy = (int16_t)((rx_buffer[11] << 8) | rx_buffer[12]);
agData.gz = (int16_t)((rx_buffer[13] << 8) | rx_buffer[14]);
attitudeUpdate();
}
// 互补滤波系数 (0.0 - 1.0)
// 值越小越信任加速度计，值越大越信任陀螺仪
// 小车应用推荐 0.95 - 0.98
#define Kp 0.02f // 加速度计权重
#define Ki 0.001f // 积分项权重 (可选，用于消除零偏)
#define dt 0.01f // 采样周期，你配置的 SMPLRT_DIV=0x09 对应 100Hz，即 0.01s
// 陀螺仪零偏校准值 (需要在初始化时计算)
int16_t gyro_x_offset = 0;
int16_t gyro_y_offset = 0;
int16_t gyro_z_offset = 0;
/**
* 校准陀螺仪(简单,只校准一次)
*/
void simpleCalibrateGyroscope()
{
MPU6050_loadData();
gyro_x_offset = agData.gx;
gyro_y_offset = agData.gy;
gyro_z_offset = agData.gz;
}
/**
* @brief 互补滤波姿态解算函数
* @param ax, ay, az: 加速度计原始值 (int16)
* @param gx, gy, gz: 陀螺仪原始值 (int16)
* @note 此函数应在 100Hz 定时器中断或循环中被调用
*/
void attitudeUpdate()
{
// 1. 将原始值转换为物理单位
// 加速度计 ±2g: 灵敏度 16384 LSB/g
float acc_x = agData.ax / 16384.0f;
float acc_y = agData.ay / 16384.0f;
float acc_z = agData.az / 16384.0f;
// 陀螺仪 ±250dps: 灵敏度 131 LSB/(°/s)
// 减去零偏得到真实角速度
float gyro_x = (agData.gx - gyro_x_offset) / 131.0f;
float gyro_y = (agData.gy - gyro_y_offset) / 131.0f;
float gyro_z = (agData.gz - gyro_z_offset) / 131.0f;
// 2. 加速度计计算姿态角 (仅受重力影响，无运动加速度时才准确)
// Roll (绕 X 轴旋转) -> 由 Y 和 Z 轴加速度计算
float acc_roll = atan2(acc_y, acc_z) * 57.3f;
// Pitch (绕 Y 轴旋转) -> 由 X 和 Z 轴加速度计算
// 注意：atan2(-acc_x, acc_z) 确保抬头为正
float acc_pitch = atan2(-acc_x, acc_z) * 57.3f;
// 3. 陀螺仪积分计算姿态角
// 注意坐标系对应关系：
// Roll 角速度对应 Gyro X (gx)
// Pitch 角速度对应 Gyro Y (gy)
// Yaw 角速度对应 Gyro Z (gz)
// 4. 互补滤波融合
// Roll 融合
// 角度 = (上次角度 + 陀螺仪变化量) * 高通权重 + 加速度计角度 * 低通权重
attitude.roll = 0.98f * (attitude.roll + gyro_x * dt) + 0.02f * acc_roll;
// Pitch 融合
attitude.pitch = 0.98f * (attitude.pitch + gyro_y * dt) + 0.02f * acc_pitch;
// Yaw 计算 (仅陀螺仪积分，无修正，会漂移)
attitude.yaw += gyro_z * dt;
// 限制 Yaw 角范围 (-180 ~ 180)，防止数值溢出
if(attitude.yaw > 180.0f) attitude.yaw -= 360.0f;
if(attitude.yaw < -180.0f) attitude.yaw += 360.0f;
}
- qmc5883p.c
//
// Created by root on 26-3-9.
//
#include "qmc5883p.h"
static I2C_HandleTypeDef* i2cHandle;
QMC5883P_DATA qmcData;
//阻塞式写,因为这里的写都是配置式的,初始化写即可
void QMC5883P_WriteReg(uint8_t RegAddress, uint8_t Data)
{
HAL_StatusTypeDef status = HAL_I2C_Mem_Write(i2cHandle,QMC5883P_ADDR, RegAddress,I2C_MEMADD_SIZE_8BIT, &Data, 1,
HAL_MAX_DELAY);
// 写入失败执行的逻辑 TODO
if (status != HAL_OK)
{
}
}
HAL_StatusTypeDef QMC5883P_Init(I2C_HandleTypeDef* handle)
{
i2cHandle = handle;
// 用于定义 X、Y、Z 轴的符号
QMC5883P_WriteReg(QMC5883P_AXIS, 0x06);
//量程±8G(±8G 时为 2048 LSB/G),Set偏置消除,不复位
QMC5883P_WriteReg(QMC5883P_CONTROL_2, 0x08);
//ODR = 200Hz,OSR1 = 8,Normal Mode 允许芯片在测量间隙进入低功耗状态,
QMC5883P_WriteReg(QMC5883P_CONTROL_1, 0xCD);
return HAL_OK;
}
void QMC5883P_loadDataSync()
{
uint8_t buffer[6];
HAL_StatusTypeDef status = HAL_I2C_Mem_Read(i2cHandle, QMC5883P_ADDR, QMC5883P_DATA_REG, 1, buffer, 6, 100);
if (status == HAL_OK)
{
qmcData.mx = (int16_t)((buffer[1] << 8) | buffer[0]);
qmcData.my = (int16_t)((buffer[3] << 8) | buffer[2]);
qmcData.mz = (int16_t)((buffer[5] << 8) | buffer[4]);
}
}
浙公网安备 33010602011771号