MPU6500

# MPU6500 驱动代码例程

## 一、Arduino平台例程

### 1.1 基础读取（I2C通信，使用Wire库）

```cpp
#include <Wire.h>

#define MPU6500_ADDR    0x68    // AD0接GND时为0x68，接VCC为0x69
#define PWR_MGMT_1      0x6B
#define ACCEL_XOUT_H    0x3B
#define GYRO_XOUT_H     0x43

void setup() {
    Serial.begin(115200);
    Wire.begin(21, 22);         // ESP32: SDA=21, SCL=22
    
    // 唤醒MPU6500（退出睡眠模式）
    Wire.beginTransmission(MPU6500_ADDR);
    Wire.write(PWR_MGMT_1);
    Wire.write(0x00);           // 清零SLEEP位
    Wire.endTransmission();
    
    Serial.println("MPU6500 初始化完成");
}

void loop() {
    int16_t ax, ay, az, gx, gy, gz;
    readMPU6500(&ax, &ay, &az, &gx, &gy, &gz);
    
    // 加速度计默认量程±2g，换算为 g 值
    float accelX = ax / 16384.0;
    float accelY = ay / 16384.0;
    float accelZ = az / 16384.0;
    
    // 陀螺仪默认量程±250°/s，换算为 °/s
    float gyroX = gx / 131.0;
    float gyroY = gy / 131.0;
    float gyroZ = gz / 131.0;
    
    Serial.printf("Accel: X=%.2f Y=%.2f Z=%.2f g\n", accelX, accelY, accelZ);
    Serial.printf("Gyro:  X=%.2f Y=%.2f Z=%.2f °/s\n", gyroX, gyroY, gyroZ);
    
    delay(100);
}

void readMPU6500(int16_t *ax, int16_t *ay, int16_t *az,
                 int16_t *gx, int16_t *gy, int16_t *gz) {
    Wire.beginTransmission(MPU6500_ADDR);
    Wire.write(ACCEL_XOUT_H);       // 起始寄存器地址
    Wire.endTransmission(false);
    
    Wire.requestFrom(MPU6500_ADDR, 14); // 读取14字节
    *ax = (Wire.read() << 8) | Wire.read();
    *ay = (Wire.read() << 8) | Wire.read();
    *az = (Wire.read() << 8) | Wire.read();
    // 跳过温度寄存器(TEMP_OUT_H, TEMP_OUT_L)
    Wire.read(); Wire.read();
    *gx = (Wire.read() << 8) | Wire.read();
    *gy = (Wire.read() << 8) | Wire.read();
    *gz = (Wire.read() << 8) | Wire.read();
}
```

### 1.2 配置量程与数字低通滤波器

```cpp
#define GYRO_CONFIG      0x1B
#define ACCEL_CONFIG     0x1C
#define CONFIG           0x1A

// 设置陀螺仪量程: 0=±250, 1=±500, 2=±1000, 3=±2000 dps
void setGyroRange(uint8_t range) {
    Wire.beginTransmission(MPU6500_ADDR);
    Wire.write(GYRO_CONFIG);
    Wire.write(range << 3);         // FS_SEL在bit[4:3]
    Wire.endTransmission();
}

// 设置加速度计量程: 0=±2, 1=±4, 2=±8, 3=±16 g
void setAccelRange(uint8_t range) {
    Wire.beginTransmission(MPU6500_ADDR);
    Wire.write(ACCEL_CONFIG);
    Wire.write(range << 3);         // AFS_SEL在bit[4:3]
    Wire.endTransmission();
}

// 设置DLPF带宽: 0-6 (越小滤波越强，延迟越大)
void setDLPF(uint8_t bw) {
    Wire.beginTransmission(MPU6500_ADDR);
    Wire.write(CONFIG);
    Wire.write(bw & 0x07);
    Wire.endTransmission();
}

// 使用示例
void setupWithConfig() {
    Serial.begin(115200);
    Wire.begin(21, 22);
    
    // 唤醒
    Wire.beginTransmission(MPU6500_ADDR);
    Wire.write(PWR_MGMT_1);
    Wire.write(0x00);
    Wire.endTransmission();
    delay(100);
    
    setGyroRange(1);      // ±500 dps
    setAccelRange(2);     // ±8 g
    setDLPF(3);           // DLPF带宽 ~44Hz
    
    Serial.println("MPU6500 配置完成");
}
```

## 二、ESP-IDF平台例程

### 2.1 完整IMU驱动模块

```c
// mpu6500.h
#ifndef MPU6500_H
#define MPU6500_H

#include "driver/i2c.h"

#define MPU6500_I2C_ADDR        0x68
#define I2C_MASTER_NUM          I2C_NUM_0
#define I2C_MASTER_FREQ_HZ      400000

// 寄存器定义
#define REG_WHO_AM_I            0x75
#define REG_PWR_MGMT_1          0x6B
#define REG_ACCEL_XOUT_H        0x3B
#define REG_GYRO_XOUT_H         0x43
#define REG_GYRO_CONFIG         0x1B
#define REG_ACCEL_CONFIG        0x1C
#define REG_CONFIG              0x1A

typedef struct {
    float accel_x, accel_y, accel_z;    // g
    float gyro_x, gyro_y, gyro_z;       // °/s
    float temp_c;                        // °C
} mpu6500_data_t;

esp_err_t mpu6500_init(i2c_port_t i2c_port);
esp_err_t mpu6500_read_data(mpu6500_data_t *data);
esp_err_t mpu6500_configure(uint8_t gyro_range, uint8_t accel_range, uint8_t dlpf);

#endif
```

```c
// mpu6500.c
#include "mpu6500.h"
#include "esp_log.h"

static const char *TAG = "MPU6500";
static i2c_port_t g_i2c_port;

static esp_err_t mpu6500_write_reg(uint8_t reg, uint8_t val) {
    uint8_t buf[2] = {reg, val};
    return i2c_master_write_to_device(g_i2c_port, MPU6500_I2C_ADDR, buf, 2, 
                                       pdMS_TO_TICKS(100));
}

static esp_err_t mpu6500_read_regs(uint8_t reg, uint8_t *data, size_t len) {
    return i2c_master_write_read_device(g_i2c_port, MPU6500_I2C_ADDR, 
                                        &reg, 1, data, len, pdMS_TO_TICKS(100));
}

esp_err_t mpu6500_init(i2c_port_t i2c_port) {
    g_i2c_port = i2c_port;
    
    // 配置I2C
    i2c_config_t conf = {
        .mode = I2C_MODE_MASTER,
        .sda_io_num = 21,
        .scl_io_num = 22,
        .sda_pullup_en = GPIO_PULLUP_ENABLE,
        .scl_pullup_en = GPIO_PULLUP_ENABLE,
        .master.clk_speed = I2C_MASTER_FREQ_HZ,
    };
    ESP_ERROR_CHECK(i2c_param_config(i2c_port, &conf));
    ESP_ERROR_CHECK(i2c_driver_install(i2c_port, conf.mode, 0, 0, 0));
    
    // 检查WHO_AM_I
    uint8_t whoami = 0;
    mpu6500_read_regs(REG_WHO_AM_I, &whoami, 1);
    if (whoami != 0x70) {
        ESP_LOGE(TAG, "WHO_AM_I mismatch: 0x%02X (expected 0x70)", whoami);
        return ESP_ERR_NOT_FOUND;
    }
    
    // 唤醒设备
    mpu6500_write_reg(REG_PWR_MGMT_1, 0x00);
    vTaskDelay(pdMS_TO_TICKS(100));
    
    ESP_LOGI(TAG, "MPU6500 initialized successfully");
    return ESP_OK;
}

esp_err_t mpu6500_read_data(mpu6500_data_t *data) {
    uint8_t buf[14];
    esp_err_t ret = mpu6500_read_regs(REG_ACCEL_XOUT_H, buf, 14);
    if (ret != ESP_OK) return ret;
    
    int16_t ax = (buf[0] << 8) | buf[1];
    int16_t ay = (buf[2] << 8) | buf[3];
    int16_t az = (buf[4] << 8) | buf[5];
    int16_t temp_raw = (buf[6] << 8) | buf[7];
    int16_t gx = (buf[8] << 8) | buf[9];
    int16_t gy = (buf[10] << 8) | buf[11];
    int16_t gz = (buf[12] << 8) | buf[13];
    
    // 默认量程换算
    data->accel_x = ax / 16384.0f;
    data->accel_y = ay / 16384.0f;
    data->accel_z = az / 16384.0f;
    data->gyro_x  = gx / 131.0f;
    data->gyro_y  = gy / 131.0f;
    data->gyro_z  = gz / 131.0f;
    data->temp_c  = (temp_raw / 340.0f) + 36.53f;
    
    return ESP_OK;
}

esp_err_t mpu6500_configure(uint8_t gyro_range, uint8_t accel_range, uint8_t dlpf) {
    mpu6500_write_reg(REG_GYRO_CONFIG, (gyro_range & 0x03) << 3);
    mpu6500_write_reg(REG_ACCEL_CONFIG, (accel_range & 0x03) << 3);
    mpu6500_write_reg(REG_CONFIG, dlpf & 0x07);
    return ESP_OK;
}
```

### 2.2 主程序使用示例

```c
// main.c
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "mpu6500.h"

void app_main(void) {
    mpu6500_data_t imu_data;
    
    // 初始化
    ESP_ERROR_CHECK(mpu6500_init(I2C_NUM_0));
    mpu6500_configure(1, 2, 3);  // ±500dps, ±8g, DLPF=44Hz
    
    while (1) {
        if (mpu6500_read_data(&imu_data) == ESP_OK) {
            printf("Accel: %.2f, %.2f, %.2f g | ", 
                   imu_data.accel_x, imu_data.accel_y, imu_data.accel_z);
            printf("Gyro: %.2f, %.2f, %.2f °/s | ", 
                   imu_data.gyro_x, imu_data.gyro_y, imu_data.gyro_z);
            printf("Temp: %.1f°C\n", imu_data.temp_c);
        }
        vTaskDelay(pdMS_TO_TICKS(50));
    }
}
```

## 三、注意事项

1. **I2C上拉电阻**：SDA和SCL需外接4.7kΩ上拉电阻到3.3V
2. **SPI模式**：若使用SPI，需将nCS拉低，AD0变为SDO引脚
3. **量程换算因子**：修改量程后需更新换算系数
4. **采样率**：默认采样率为1kHz，配合DLPF使用可有效抑制噪声
5. **温度漂移**：上电前几分钟传感器会有温漂，建议预热30秒后开始采集