基于I2C总线的IMU-磁力计融合算法与数据共享

本文涉及：

• ESPIDF的IIC通信示例
• 加速度+陀螺仪计算欧拉角
• 互补滤波融合稳定欧拉角
• 磁力计硬软铁校准
• 磁力计倾斜补偿
• 磁力计 偏航角359~1度跳变
• 磁力计与预测值之间的"最短路径误差"
• IMU：ICM42670P
• 磁力计： QMC5883P

ESPIDF旧版IIC通信

官方文档 ：https://docs.espressif.com/projects/esp-idf/zh_CN/v5.1/esp32/api-reference/peripherals/i2c.html

官方示例 ：esp-idf/examples/peripherals/i2c/i2c_simple/main/i2c_simple_main.c at v5.1 · espressif/esp-idf

头文件： i2c.h遗留 I2C API 的头文件（用于使用旧驱动程序的应用）

代码：D:\ESP_IDF_FILES\ESP32S3\ESP32_01\components\IIC\IIC.c

---

配置驱动程序(主机) ：建立 I2C 通信第一步是配置驱动程序，这需要设置 i2c_config_t 结构中的几个参数

• 设置 I2C 工作模式 - 从 i2c_mode_t 中选择主机模式或从机模式
• 指定 SDA 和 SCL 信号使用的 GPIO 管脚
• 是否启用 ESP32 的内部上拉电阻
• 设置 I2C 时钟速度(仅限主机模式)
• 选择频率

配置示例（主机）

//配置IIC参数
i2c_config_t conf = {
    .mode = I2C_MODE_MASTER, //配置为主机模式
    .sda_io_num = I2C_MASTER_GPIO_SDA, // 配置 SDA 的 GPIO，GPIO40
    .scl_io_num = I2C_MASTER_GPIO_SCL, // 配置 SCL 的 GPIO，GPIO39
    .sda_pullup_en = GPIO_PULLUP_ENABLE,//需要内部上拉，开漏输出，非推挽输出
    .scl_pullup_en = GPIO_PULLUP_ENABLE,//需要内部上拉，开漏输出，非推挽输出
    .master.clk_speed = I2C_MASTER_FREQ_HZ, //频率为400KHz
};

配置IIC端口号 ：i2c_port_t i2c_master_port = I2C_NUM_0;

• ESP32S3有两个端口号，分别为I2C_NUM_0 I2C_NUM_1

配置一个 I2C 总线 ：调用 i2c_param_config() 函数

配置总线示例（主机）

//配置IIC端口号
i2c_port_t i2c_master_port = I2C_MASTER_NUM;
//配置IIC参数
i2c_config_t conf = {
    .mode = I2C_MODE_MASTER, //配置为主机模式
    .sda_io_num = I2C_MASTER_GPIO_SDA, // 配置 SDA 的 GPIO，GPIO40
    .scl_io_num = I2C_MASTER_GPIO_SCL, // 配置 SCL 的 GPIO，GPIO39
    .sda_pullup_en = GPIO_PULLUP_ENABLE,//需要内部上拉，开漏输出，非推挽输出
    .scl_pullup_en = GPIO_PULLUP_ENABLE,//需要内部上拉，开漏输出，非推挽输出
    .master.clk_speed = I2C_MASTER_FREQ_HZ, //频率为400KHz
};
i2c_param_config(i2c_master_port, &conf);//配置一个 I2C 总线，配置给定的配置

安装驱动程序 ：配置好 I2C 驱动程序后，使用以下参数调用函数 i2c_driver_install() 安装驱动程序

• 端口号，从 i2c_port_t 中二选一

• 主机或从机模式，从 i2c_mode_t 中选择

• 分配用于在从机模式下发送和接收数据的缓存区大小。I2C 是一个以主机为中心的总线，数据只能根据主机的请求从从机传输到主机。因此，从机通常有一个发送缓存区，供从应用程序写入数据使用。数据保留在发送缓存区中，由主机自行读取（仅限从机模式）

• 用于分配中断的标志

安装示例

// 安装一个 I2C 驱动.参数： I2C 端口号，I2C 模式（主控或从），接收缓冲区大小，发送缓冲区大小，用于分配中断的标志
ESP_ERROR_CHECK(i2c_driver_install(i2c_master_port, conf.mode, I2C_MASTER_RX_BUF_DISABLE, I2C_MASTER_TX_BUF_DISABLE, 0));

主机模式下通信（主机写入数据）

• 使用 i2c_cmd_link_create() 创建一个命令链接

  • 然后，将一系列待发送给从机的数据填充命令链接：

    1. 启动位 - i2c_master_start()
    2. 从机地址 - i2c_master_write_byte()。提供单字节地址作为调用此函数的实参。
    3. 数据 - 一个或多个字节的数据作为 i2c_master_write() 的实参。
    4. 停止位 - i2c_master_stop()

    函数 i2c_master_write_byte() 和 i2c_master_write() 都有额外的实参，规定主机是否应确认其有无接受到 ACK 位。

• 通过调用 i2c_master_cmd_begin() 来触发 I2C 控制器执行命令链接。一旦开始执行，就不能再修改命令链接。

• 命令发送后，通过调用 i2c_cmd_link_delete() 释放命令链接使用的资源

主机模式下通信（主机读取数据）

• 用 i2c_master_read_byte() 和/或 i2c_master_read() 填充命令链接读取数据

读写示例（参考：https://www.bilibili.com/video/BV1xv4yzHEhM?t=1560.0\&p=20）

void bh1750_send_cmd(uint8_t cmd_data)
{
    i2c_cmd_handle_t cmd = i2c_cmd_link_create();
    i2c_master_start(cmd);
    i2c_master_write_byte(cmd, bh1750_write_addr, true);
    i2c_master_write_byte(cmd, cmd_data, true);
    i2c_master_stop(cmd);
    i2c_master_cmd_begin(I2C_NUM_0, cmd, 1000);
    i2c_cmd_link_delete(cmd);
}

uint16_t bh1750_read_data(void)
{
    uint8_t light_high = 0 ,light_low = 0 ;
    i2c_cmd_handle_t cmd = i2c_cmd_link_create();
    i2c_master_start(cmd);
    i2c_master_write_byte(cmd, bh1750_read_addr, true);
    i2c_master_read_byte(cmd, &light_high, I2C_MASTER_ACK);
    i2c_master_read_byte(cmd, &light_low, I2C_MASTER_NACK);
    i2c_master_stop(cmd);
    i2c_master_cmd_begin(I2C_NUM_0, cmd, 1000);
    i2c_cmd_link_delete(cmd);
}

便捷读写i2c_master_write_to_device()

esp_err_t i2c_master_write_to_device(i2c_port_t i2c_num, uint8_t device_address, const uint8_t *write_buffer, size_t write_size, TickType_t ticks_to_wait)

对连接到特定 I2C 端口的设备执行写入作。该函数是 i2c_master_start（）、i2c_master_write（）、i2c_master_read（） 等的封装器,它只能在 I2C 主模式下调用。

参数

• i2c_num -- 用于传输的 I2C 端口号
• device_address -- I2C 设备的 7 位地址
• write_buffer -- 总线发送的字节数
• write_size -- 写缓冲区的大小（字节单位）
• ticks_to_wait -- 暂停前等待的最大计时数。

返回值

• ESP_OK 成功
• ESP_ERR_INVALID_ARG 参数误差
• ESP_FAIL 发送命令错误，从属服务器没有确认传输。
• ESP_ERR_INVALID_STATE I2C 驱动未安装或未进入主控模式
• ESP_ERR_TIMEOUT 由于公交车忙碌，作暂停

示例

static esp_err_t mpu9250_register_read(uint8_t reg_addr, uint8_t *data, size_t len)
{
    return i2c_master_write_read_device(I2C_MASTER_NUM, MPU9250_SENSOR_ADDR, &reg_addr, 1, data, len, I2C_MASTER_TIMEOUT_MS / portTICK_PERIOD_MS);
}


static esp_err_t mpu9250_register_write_byte(uint8_t reg_addr, uint8_t data)
{
    int ret;
    uint8_t write_buf[2] = {reg_addr, data};

    ret = i2c_master_write_to_device(I2C_MASTER_NUM, MPU9250_SENSOR_ADDR, write_buf, sizeof(write_buf), I2C_MASTER_TIMEOUT_MS / portTICK_PERIOD_MS);

    return ret;
}

ESPIDF新版IIC通信

官方文档 ：https://docs.espressif.com/projects/esp-idf/zh_CN/latest/esp32s3/api-reference/peripherals/i2c.html

官方示例 ：esp-idf/examples/peripherals/i2c/i2c_basic/main/i2c_basic_example_main.c at 12f36a021f511cd4de41d3fffff146c5336ac1e7 · espressif/esp-idf

本地代码：D:\ESP_IDF_FILES\ESP32S3\ESP32_01\components\IIC_MASTER\IIC_MASTER.c

头文件： i2c_master.h提供标准通信模式下特定 API 的头文件（用于使用主机模式的新驱动程序的应用）

---

IIC总线句柄和设备句柄

i2c_master_bus_handle_t bus_handle = NULL;//总线
i2c_master_dev_handle_t dev_handle = NULL;//设备

安装 I2C 主机总线和设备

I2C 主机总线是基于总线-设备模型设计的，因此需要分别使用 i2c_master_bus_config_t 和 i2c_device_config_t 来分配 I2C 主机总线实例和 I2C 设备实例

I2C 主机总线 需要 i2c_master_bus_config_t 指定的配置：

• i2c_master_bus_config_t::i2c_port 设置控制器使用的 I2C 端口。
• i2c_master_bus_config_t::sda_io_num 设置串行数据总线 (SDA) 的 GPIO 编号。
• i2c_master_bus_config_t::scl_io_num 设置串行时钟总线 (SCL) 的 GPIO 编号。
• i2c_master_bus_config_t::clk_source 选择 I2C 总线的时钟源。可用时钟列表见 i2c_clock_source_t。
• i2c_master_bus_config_t::glitch_ignore_cnt 设置主机总线的毛刺周期。如果线上的毛刺周期小于设置的值（通常设为 7），则可以被滤除。
• i2c_master_bus_config_t::intr_priority 设置中断的优先级。如果设置为 0，则驱动程序将使用低或中优先级的中断（优先级可设为 1、2 或 3 中的一个），若未设置，则将使用 i2c_master_bus_config_t::intr_priority 指示的优先级。请使用数字形式（1、2、3），不要用位掩码形式（(1<<1)、(1<<2)、(1<<3)）。
• i2c_master_bus_config_t::trans_queue_depth 设置内部传输队列的深度，但仅在异步传输中有效。
• i2c_master_bus_config_t::enable_internal_pullup 启用内部上拉电阻。注意：该设置无法在高速频率下拉高总线，此时建议使用合适的外部上拉电阻。

I2C 主机设备 需要 i2c_device_config_t 指定的配置:

• i2c_device_config_t::dev_addr_length 配置从机设备的地址位长度，可从枚举 I2C_ADDR_BIT_LEN_7 或 I2C_ADDR_BIT_LEN_10 （如果支持）中进行选择。
• i2c_device_config_t::device_address 设置 I2C 设备原始地址，请直接将设备地址解析到此成员。例如，若设备地址为 0x28，则将 0x28 解析到 i2c_device_config_t::device_address，不要带写入或读取位。
• i2c_device_config_t::scl_speed_hz 设置此设备的 SCL 线频率。
• i2c_device_config_t::scl_wait_us 设置 SCL 等待时间（以微秒为单位）。通常此值较大，因为从机延伸时间会很长（甚至可能延伸到 12 ms）。设置为 0 表示使用默认的寄存器值。

分配和初始化 I2C 主机总线 ：在 i2c_master_bus_config_t 中指定了配置，则可调用 i2c_new_master_bus()

配置示例

i2c_master_bus_handle_t bus_handle = NULL;
i2c_master_dev_handle_t dev_handle = NULL;

i2c_master_bus_config_t bus_config = {
    .i2c_port = I2C_MASTER_NUM, // 使用I2C0控制器
    .sda_io_num = I2C_MASTER_SDA_IO, //sda的IO口
    .scl_io_num = I2C_MASTER_SCL_IO, //scl的IO口
    .clk_source = I2C_CLK_SRC_DEFAULT, // 默认时钟源
    .glitch_ignore_cnt = 7, // 抗干扰配置（忽略7个时钟毛刺）
    .flags.enable_internal_pullup = true// 启用内部上拉
};

esp_err_t ret =  i2c_new_master_bus(&bus_config, &bus_handle); //将配置bus_config更新到句柄bus_handle
if (ret != ESP_OK) {
    ESP_LOGE(TAG, "Create I2C master bus failed: %s", esp_err_to_name(ret));
    return ret;
}

​ 一旦填充好 i2c_device_config_t 结构体的必要参数，就可调用 i2c_master_bus_add_device() 来分配 I2C 设备实例，并将设备挂载到主机总线上。如果函数运行正确，则将返回一个 I2C 设备句柄。若未正确初始化 I2C 总线，此函数将返回 ESP_ERR_INVALID_ARG 错误

配置示例

i2c_device_config_t dev_cfg = {
    .dev_addr_length = I2C_ADDR_BIT_LEN_7, // 7位I2C地址模式
    .device_address = 0x68,            // 设备I2C地址
    .scl_speed_hz = 100000,                // I2C时钟频率：100kHz（标准模式）
};
esp_err_t ret2 = i2c_master_bus_add_device(bus_handle, &dev_cfg, &dev_handle);
if (ret2 != ESP_OK) {
    ESP_LOGE(TAG, "Add device 0x%02x failed: %s", 0x68, esp_err_to_name(ret));
    return ret2;
}
ESP_LOGI(TAG, "I2C master init success (new i2c_master driver)");
return ESP_OK;

通过端口获取 I2C 主控句柄

使用辅助函数 i2c_master_get_bus_handle() 可通过端口获取已初始化的句柄。但请确保句柄已经提前初始化，否则可能会报错。

// 源文件 1
#include "driver/i2c_master.h"
i2c_master_bus_handle_t bus_handle;
i2c_master_bus_config_t i2c_mst_config = {
    ... // 与其他相同
};
ESP_ERROR_CHECK(i2c_new_master_bus(&i2c_mst_config, &bus_handle));

// 源文件 2
#include "driver/i2c_master.h"
i2c_master_bus_handle_t handle;
ESP_ERROR_CHECK(i2c_master_get_bus_handle(0, &handle));

I2C 主机写入

在成功安装 I2C 主机总线之后，可以通过调用 i2c_master_transmit() 来向从机设备写入数据。驱动程序用一系列命令填充了一个命令链 ，并将该命令链传递给 I2C 控制器执行，上述流程被封装到了函数i2c_master_transmit() 中

esp_err_t i2c_master_transmit(i2c_master_dev_handle_t i2c_dev, const uint8_t *write_buffer, size_t write_size, int xfer_timeout_ms)

参数

• i2c_dev ------ [in] I2C 主设备处理由 i2c_master_bus_add_device 创建的设备。
• write_buffer -- [in] 数据字节通过 I2C 总线发送。
• write_size -- [字节] 写缓冲区的大小，单位为字节。
• xfer_timeout_ms ------ [in] 等待暂停，按毫秒。注：-1 意味着永远等待

返回值

• ESP_OK：I2C 主传输成功。
• ESP_ERR_INVALID_RESPONSE：I2C 主传输接收 NACK。
• ESP_ERR_INVALID_ARG：I2C 主传输参数无效。
• ESP_ERR_TIMEOUT：作超时（大于 xfer_timeout_ms），因为总线忙或硬件崩溃。

写入示例

数据包通常用数组表示

esp_err_t qmc5883p_write_register(qmc5883p_dev_t *dev, uint8_t reg, uint8_t value) {
    // 参数校验：设备/设备句柄不能为空
    if (!dev || !dev->dev_handle) return ESP_ERR_INVALID_ARG;
    uint8_t tx[2] = {reg, value}; // 待发送数据：[寄存器地址, 数值]
    // I2C写操作：发送2字节，超时1000ms
    return i2c_master_transmit(dev->dev_handle, tx, 2, 1000);
}

esp_err_t II2C_Master_Write(uint8_t addr, uint8_t reg, uint16_t len, uint8_t* data)
{
    if (bus_handle == NULL || data == NULL || len == 0) {
        ESP_LOGE(TAG, "Invalid param: bus=%p, data=%p, len=%d", bus_handle, data, len);
        return ESP_ERR_INVALID_ARG;
    }
    esp_err_t ret;
    uint8_t *buf = (uint8_t *)malloc(len+1);
    if (buf == NULL) {
        ESP_LOGE(TAG, "Malloc failed for write buf, len=%d", len + 1);
        return ESP_ERR_NO_MEM;
    }
    buf[0] = reg;// 把寄存器地址放到缓冲区第1个字节（I2C写规则：先送寄存器地址，再送数据）
    for (int i = 0; i < len; i++) //把要写入的数据拷贝到缓冲区（从第2个字节开始）
    {
        buf[i+1] = data[i];
    }
     // 发送总长度：1(寄存器)+len(数据)
    ret = i2c_master_transmit(dev_handle, buf, len+1, I2C_MASTER_TIMEOUT_MS / portTICK_PERIOD_MS);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Write failed: addr=0x%02x, reg=0x%02x, len=%d, err=%s", addr, reg, len, esp_err_to_name(ret));
    }
    free(buf);
    return ret;
}

I2C 主机读取

在成功安装 I2C 主机总线后，可以通过调用 i2c_master_receive() 从从机设备读取数据

I2C 主机写入后读取

一些 I2C 设备中读取数据之前需要进行写入配置，可通过 i2c_master_transmit_receive() 接口进行配置

esp_err_t i2c_master_transmit_receive(i2c_master_dev_handle_t i2c_dev, const uint8_t *write_buffer, size_t write_size, uint8_t *read_buffer, size_t read_size, int xfer_timeout_ms)

参数

• i2c_dev ------ [in] I2C 主设备处理由 i2c_master_bus_add_device 创建的设备。
• write_buffer -- [in] 数据字节通过 I2C 总线发送。
• write_size -- [字节] 写缓冲区的大小，单位为字节。
• read_buffer -- [输出] 从 i2c 总线接收的数据字节。
• read_size -- [字节] 读取缓冲区的大小。
• xfer_timeout_ms ------ [in] 等待暂停，按毫秒。注：-1 意味着永远等待。

返回值

• ESP_OK：I2C 主控发送-接收成功。
• ESP_ERR_INVALID_RESPONSE：I2C 主发送-接收接收 NACK。
• ESP_ERR_INVALID_ARG：I2C 主传输参数无效。
• ESP_ERR_TIMEOUT：作超时（大于 xfer_timeout_ms），因为总线忙或硬件崩溃。

读取示例

esp_err_t II2C_Master_Read(uint8_t addr, uint8_t reg, uint16_t len, uint8_t* data)
{
    if (bus_handle == NULL || data == NULL || len == 0) {
        ESP_LOGE(TAG, "Invalid param: bus=%p, data=%p, len=%d", bus_handle, data, len);
        return ESP_ERR_INVALID_ARG;
    }
    // 一次完成「写寄存器地址+读数据」
   esp_err_t ret = i2c_master_transmit_receive(dev_handle, &reg, 1, data, len, I2C_MASTER_TIMEOUT_MS / portTICK_PERIOD_MS);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Read failed: addr=0x%02x, reg=0x%02x, len=%d, err=%s", addr, reg, len, esp_err_to_name(ret));
    }
    return ret;
}

I2C 主机探测

I2C 驱动程序可以使用 i2c_master_probe() 来检测设备是否已经连接到 I2C 总线上。如果该函数返回 ESP_OK，则表示该设备已经被检测到

在调用该函数时，必须将上拉电阻连接到 SCL 和 SDA 管脚。如果在正确解析 xfer_timeout_ms 时收到 ESP_ERR_TIMEOUT ，则应检查上拉电阻。若暂无合适的电阻，也可将 flags.enable_internal_pullup 设为 true。

esp_err_t i2c_master_probe(i2c_master_bus_handle_t bus_handle, uint16_t address, int xfer_timeout_ms)

参数

• bus_handle ------ [in] I2C 主设备处理由 i2c_master_bus_add_device 创建的设备。
• address ------[in] 你想探测的 I2C 设备地址。
• xfer_timeout_ms ------ [in] 等待暂停，按毫秒。注意：-1 表示等待非常久（此函数不推荐）

返回值

• ESP_OK：I2C 设备成功探测。
• ESP_ERR_NOT_FOUND：I2C 探头失败，找不到你给的具体地址的设备。
• ESP_ERR_TIMEOUT：作超时（大于 xfer_timeout_ms），因为总线忙或硬件崩溃。

注：旧驱动程序与新驱动程序无法共存

IMU数据多线程共享

需求分析：由于MPU6050 的偏航角没有重力参考，所以数据会产生较大的漂移，为了解决这个问题，采用外接三轴磁力计QMC5883 构成9轴陀螺仪来纠正零偏，在ESPIDF中，IMU和磁力计的数据读取示两个不同的任务(线程)，为了将两者的数据融合，就必须使用线程间通信 同时获取两个传感器的输出，此处我使用的用互斥锁（Mutex）保护全局变量

全局缓存+互斥锁

// 全局缓存+互斥锁
static imu_data_t g_imu_shared_data = {0};
static SemaphoreHandle_t g_imu_shared_mutex = NULL;

初始化互斥锁,用于imu数据共享

void imu_shared_init(void) {
    if (g_imu_shared_mutex == NULL) {
        g_imu_shared_mutex = xSemaphoreCreateMutex();
    }
}

更新IMU数据（供回调函数调用）

void imu_shared_update_data(const imu_data_t *in_data) {
    if (in_data == NULL || g_imu_shared_mutex == NULL) return;
    
    // 加锁写入
    if (xSemaphoreTake(g_imu_shared_mutex, portMAX_DELAY) == pdTRUE)//获取锁的控制权，portMAX_DELAY永久等待
    {
        g_imu_shared_data = *in_data; // 拷贝数据
        xSemaphoreGive(g_imu_shared_mutex);//释放锁的控制权
    }
}

获取最新IMU数据（线程安全）

bool imu_shared_get_data(imu_data_t *out_data){
	if (out_data == NULL || g_imu_shared_mutex == NULL) return false;
    
    // 加锁读取
    if (xSemaphoreTake(g_imu_shared_mutex, pdMS_TO_TICKS(10)) == pdTRUE) {
        *out_data = g_imu_shared_data; // 拷贝数据
        xSemaphoreGive(g_imu_shared_mutex);
        return true;
    }
    return false;
}

在IMU初始化时候对互斥锁初始化

// 初始化ICM42670P
void Icm42670p_Init(void)
{
    ...// 初始化I2C接口 ，IIC.c配置主机参数
    imu_shared_init();   // 初始化imu共享数据
    ...// 开启IMU任务 ，Icm42670p_Task任务函数
}

由于对于6轴IMU芯片ICM42670P来说，程序中imu数据的获取在 imu_callback()中，所以在该函数中，将读取到的数据写入全局变量，并通过互斥锁保护全局变量

本地地址：D:\ESP_IDF_FILES\ESP32S3\ESP32_01\components\ICM42670P\ICM42670P.c

static void imu_callback(inv_imu_sensor_event_t *event){
    ...
    // 获取角速度 (由于开启了 DMP，这里的 gyro 是经过芯片内部自动去偏置的)
    float gx = (float)event->gyro[0] * icm_gyro_fsr_dps / (float)INT16_MAX;
    float gy = (float)event->gyro[1] * icm_gyro_fsr_dps / (float)INT16_MAX;
    float gz = (float)event->gyro[2] * icm_gyro_fsr_dps / (float)INT16_MAX;

    // 3. 获取加速度用于修正 Roll 和 Pitch
    float ax = (float)event->accel[0];
    float ay = (float)event->accel[1];
    float az = (float)event->accel[2];

    imu_data_t imu_data;
    //写入imu结构体，用于数据共享
    imu_data.gyro[0] = gx;
    imu_data.gyro[1] = gy;
    imu_data.gyro[2] = gz;
    imu_data.accel[0] = ax;
    imu_data.accel[1] = ay;
    imu_data.accel[2] = az;
    
    imu_shared_update_data(&imu_data);//更新IMU数据
    ...
}

在读取磁力计QMC5883P数据的任务中，获取共享的IMU数据

**本地地址：**D:\ESP_IDF_FILES\ESP32S3\ESP32_01\components\IMU_MAG\IMU_MAG.c

static void qmc5883p_test_task(void *arg){
	...//磁力计初始化
	 // 无限循环：持续读取磁力计数据
    while (1){
    	...
        //线程程间通信获取imu数据
        bool is_imu_ok = imu_shared_get_data(&imu_data);
        if (is_imu_ok){
        ESP_LOGI(TAG, "gyro: X=%.2f, Y=%.2f , Z=%.2f ",imu_data.gyro[0],imu_data.gyro[1],imu_data.gyro[2]); //共享的imu角速度数据
        ESP_LOGI(TAG, "accel: X=%.2f, Y=%.2f , Z=%.2f ",imu_data.accel[0],imu_data.accel[1],imu_data.accel[2]);//共享imu加速度数据
        ESP_LOGI(TAG, "Mag: X=%.3f Gauss, Y=%.3f Gauss, Z=%.3f Gauss",  data.x, data.y, data.z); // 打印磁力计三轴高斯值（单位：高斯） 
        ...
        }
    }
}

成功

imu磁力计数据融合

ICM42670数据手册 ：ICM-42670-P -PDF数据手册-参考资料-立创商城

Qmc5883p数据手册 ： QMC5883P -PDF数据手册-参考资料-立创商城

磁力计校准

磁力计测出来的磁场 = 地球磁场 + 干扰磁场

硬铁补偿

硬铁 = 永磁体、固定磁场源（电机磁铁，喇叭磁铁，电池、电路板上的永磁材料等），它们会在磁力计周围产生一个固定方向、固定大小的偏置磁场。

表现

• 理想磁力计绕一圈，数据应该是以原点为中心的圆 / 球（例 -0.5 ~ 0 ~ 0.5）

• 有硬铁干扰 → 整个圆整体平移偏移，中心不在原点（例 -0.1 ~ 0 ~ 0.6）

补偿方法

• 转8字校准：减去一个固定偏移量 (mx, my, mz)
• 记录磁力计x,y,z轴 输出的数据的 最大，最小值(单位高斯)
• 计算偏移量：(最大+最小)/2

代码示例

由于最大最小值可能不会固定，所以需要再代码中添加自动更新

// 获取偏移测量值
static float x_max = X_MAX_OFFSET;
static float x_min = X_MIN_OFFSET;

static float y_max = Y_MAX_OFFSET;
static float y_min = Y_MIN_OFFSET;

static float z_max = Z_MAX_OFFSET;
static float z_min = Z_MIN_OFFSET;

//转8字校准后的值
if (data.x > x_max) x_max = data.x;
if (data.x < x_min) x_min = data.x;

if (data.y > y_max) y_max = data.y;
if (data.y < y_min) y_min = data.y;

if (data.z > z_max) z_max = data.z;
if (data.z < z_min) z_min = data.z;

// 根据x，y轴最大最小数据进行手动补偿
float offset_x = (x_max+x_min)/2.0f;//x轴偏移：(0.19 + (-0.16)) / 2 = 0.03 / 2 =0.015
float offset_y = (y_max+y_min)/2.0f;//Y轴偏移：(-0.02 + (-0.32)) / 2 = -0.34 / 2 =0.017
float offset_z = (z_max+z_min)/2.0f;

// 消除硬铁干扰（归零）
float cal_x = data.x - offset_x;
float cal_y = data.y - offset_y;
float cal_z = data.z - offset_z;

软铁补偿

软铁 = 导磁材料，但本身不带磁(铁壳,螺丝、金属支架等)，它们不会自己产生磁场 ，但会扭曲、拉伸、改变地球磁场的分布

表现

• 理想是圆 → 软铁干扰后变成椭圆
• 三轴灵敏度不一致、方向畸变

补偿方法：把磁力计三轴的磁场幅值（半径）归一化到同一个目标值，消除三轴因软铁干扰导致的 "拉伸 / 压缩" 差异，让椭圆变回正圆。

代码示例

// 软铁补偿
//  计算三轴各自的半径（半长轴）
float x_r = (x_max-x_min)/2.0f;  // X轴磁场最大值-最小值的一半 → X轴方向的"有效半径"
float y_r = (y_max-y_min)/2.0f;  // Y轴同理
float z_r = (z_max-z_min)/2.0f;  // Z轴同理

//  计算三轴半径的平均值 → 目标半径（理想正圆的半径）
float target_r = (x_r+y_r+z_r)/3.0f;

// 防止除以0（避免分母为0导致程序崩溃）
if(x_r > 0 && y_r > 0 && z_r > 0){
    // 软铁补偿核心：将校准后的三轴数据归一化到目标半径
    cal_x *= (target_r / x_r);  // X轴缩放：如果x_r偏大，就缩小；偏小就放大
    cal_y *= (target_r / y_r);  // Y轴同理
    cal_z *= (target_r / z_r);  // Z轴同理
}

确认imu坐标系和磁力计坐标系统一

硬件安装软件修正

• imu硬件安装：imu的x轴朝向小车的左方 ，y轴朝向后方
• 磁力计硬件安装：磁力计的x轴朝向小车的右方 ，y轴朝向前方

软件修正

// 坐标系重映射：将 IMU 原始轴向映射到小车标准轴向（X前，Y左）
float _ax = -imu_data.accel[1]; // 物理前方 (原Y是后，取反就是前)
float _ay =  imu_data.accel[0]; // 物理左方 (原X就是左)
float _az =  imu_data.accel[2]; // 物理上方 (保持不变)

float _gx = -imu_data.gyro[1];  // 绕小车X轴转动（对应现在的侧倾）
float _gy =  imu_data.gyro[0];  // 绕小车Y轴转动（对应现在的抬头）
float _gz =  imu_data.gyro[2];  // 绕小车Z轴转动（原地转圈）

//纠正磁力计方向为小车方向(qmc5883p丝印xy轴错误)
float mag_x = cal_y;
float mag_y = -cal_x;
float mag_z = cal_z;

磁力计QMC5883p模块坐标系丝印致命错误❌

• 经过倾斜补偿实测，确认该磁力计模块的坐标系标注错误，我说倾斜补偿怎么一直调试的有问题😈

读取IMU数据

见IMU数据多线程共享

在imu数据来临时添加积分的时间

//获取积分时间微秒级
uint64_t current_time = inv_imu_get_time_us();
float dt = (last_imu_time != 0) ? (current_time - last_imu_time) / 1000000.0f : 0.0f;
last_imu_time = current_time;

加速度计算 翻滚角Roll 俯仰角Pitch

通过重力投影解算角度，无漂移

公式
ϕ = arctan ⁡ 2 ( a y ′ , a z ′ ) 翻滚角 θ = arctan ⁡ 2 ( − a x ′ , a y ′ 2 + a z ′ 2 ) 俯仰角 \phi = \arctan2\left( a_y', a_z' \right)翻滚角\\ \theta = \arctan2\left( -a_x', \sqrt{a_y'^2 + a_z'^2} \right) 俯仰角 ϕ=arctan2(ay′,az′)翻滚角θ=arctan2(−ax′,ay′2+az′2 )俯仰角
公式推导 ：见 【嵌入式】ESP32S3主控底盘驱动代码及其原理(ESPIDF)_esp-idf esp32 s3 modbus主控代码-CSDN博客

代码

float accel_roll = atan2f(_ay, _az)* 180.0f / M_PI;
float accel_pitch =  -atan2f(_ax, sqrt(_ay*_ay + _az*_az)) * 180.0f / M_PI;

• 优点：长期稳定（是角度的 "绝对参考"）；
• 缺点：响应慢，运动时（如设备快速移动）会受线加速度干扰，角度瞬间跳变

无法计算偏航角yaw原因

• 俯仰角和翻滚角的计算都是依靠重力加速度的，在小车呈现出不同的姿态时，重力加速度会在x,y轴上产生分量，从而计算出角度
• 偏航角是绕z轴旋转产生的角度，而重力加速度只会在x,y轴上产生分量，它与z轴方向平行，所以无法计算。

陀螺仪积分计算翻滚角Roll 俯仰角Pitch

角速度 × 时间 = 角度变化量 ，因此 roll + _gx * dt 是通过陀螺仪 "积分" 得到的当前角度；

代码示例

//获取积分时间微秒级
uint64_t current_time = inv_imu_get_time_us();
float dt = (last_imu_time != 0) ? (current_time - last_imu_time) / 1000000.0f : 0.0f;
last_imu_time = current_time;
roll = roll + _gx * dt;
pitch = pitch + _gy * dt;

• 优点：响应快，不受运动（如晃动、平移）影响；
• 缺点 ：存在积分漂移（微小误差随时间累加，角度会慢慢偏离真实值）静止时表现突出

互补滤波横滚俯仰角

用陀螺仪 解决加速度计 "易受运动干扰" 的问题，用加速度计解决陀螺仪 "长时间漂移" 的问题

代码示例

// 互补滤波：98% 信任陀螺仪积分，2% 信任加速度计修正 (消除漂移的关键)
roll = 0.98f * (roll + _gx * dt) + 0.02f * (accel_roll);
pitch = 0.98f * (pitch + _gy * dt) + 0.02f * (accel_pitch);

磁力计倾斜补偿

• 已知校准后的磁力计三轴数据：mx,my,mz（硬铁 + 软铁补偿后）；
• 已知融合后的欧拉角：横滚角 ϕ（Roll）、俯仰角 θ（Pitch）（弧度制）

公式
{ m x h = m x ⋅ cos ⁡ θ + m y ⋅ sin ⁡ ϕ ⋅ sin ⁡ θ + m z ⋅ cos ⁡ ϕ ⋅ sin ⁡ θ m y h = m y ⋅ cos ⁡ ϕ − m z ⋅ sin ⁡ ϕ % 磁力计倾斜补偿核心公式 \begin{cases} m_{xh} = m_x \cdot \cos\theta + m_y \cdot \sin\phi \cdot \sin\theta + m_z \cdot \cos\phi \cdot \sin\theta \\ m_{yh} = m_y \cdot \cos\phi - m_z \cdot \sin\phi \end{cases} {mxh=mx⋅cosθ+my⋅sinϕ⋅sinθ+mz⋅cosϕ⋅sinθmyh=my⋅cosϕ−mz⋅sinϕ
公式推导：

用树莓派创建数字指南针 -- 第二部分 -- "倾斜补偿" |ozzmaker.com --- Create a Digital Compass with the Raspberry Pi -- Part 2 -- "Tilt Compensation" | ozzmaker.com

Using LSM303DLH for a tilt compensated electronic compass

代码示例

// 磁力计倾斜补偿:对于atan2f(-cal_y, cal_x)它默认x和y始终处于水平面，当roll和pitch变化时，yaw会产生剧烈跳变
float xMag = mag_x*cosf(pitch_rad) + mag_y*sinf(roll_rad)*sinf(pitch_rad) + mag_z*cosf(roll_rad)*sinf(pitch_rad);
float yMag = mag_y*cosf(roll_rad) - mag_z*sinf(roll_rad); //减号还是加号，取决z轴是朝上还是朝下

参考视频：磁力计偏航角跳变问题

Z轴角速度积分与磁力计计算稳定yaw

校准后磁力计数据计算弧度

// 计算弧度(逆时针转动yaw增加，原来是顺时针，所以加负号)
float heading_rad = atan2f(-yMag, xMag);

三角滤波+一阶低通滤波消除 360 ~ 0度跳变非线性问题与噪声

问题分析：假设直接对角度进行 一阶低通滤波(EMA)，角度的变化范围是 0~360，当角度从359 ~ 1度时，根据EMA 滤波计算（filtered = 0.5× 新值 + 0.5× 旧值）

得到的滤波结果是 0.5×1 + 0.5×359 = 0.5 + 179.5 = 180° ，此时滤波值从 359° 突变到 180°，完全偏离实际旋转方向！，在图形化界面展示就是角度不会平滑的在坐标轴上到1度 的转变，而是会发生剧烈的跳变。

问题本质 ：角度的 "不连续性" ，角度的取值范围是[0°, 360°)，但数值上：

• 当角度从 359°→360°（=0°）时，数值从 359→0，跳变了 359；
• 数学上角度是 "模 360° 的循环量"，但数值本身是线性的，EMA 滤波无法识别这种循环特性。
• 物理旋转：连续的：设备只旋转了 1°
• 数值表示：断点式跳变：数值从 359 直接掉到 0，差值是 - 359（而非 - 1），

解决方法：三角函数滤波 ， 正弦 / 余弦的 "连续性"， sin/cos 是光滑的周期函数（没有断点）

• 359度的sin值(-0.01745) 和 1度的sin值(0.01745) 是连续变化的（差值仅 0.0349），cos 值几乎不变（0.99985），滤波还原的角度自然平滑，无跳变
• 0.5×0.01745 + 0.5×(-0.01745) = 0

代码示例

//  计算当前角度的正弦余弦值(三角滤波)
float sin_rad = sinf(heading_rad);
float cos_rad = cosf(heading_rad);

static float filtered_sin = 0.0f;  // 静态变量保存滤波后的sin值
static float filtered_cos = 1.0f;  // 静态变量保存滤波后的cos值

filtered_sin = EMA(sin_rad, filtered_sin);
filtered_cos = EMA(cos_rad, filtered_cos);

// 从滤波后的正弦余弦值计算角度（弧度）
float filtered_rad = atan2f(filtered_sin, filtered_cos);
//转角度
float filtered_deg = filtered_rad * (180.0f / M_PI);

EMA低通滤波

float EMA(float rawValue, float filteredValue)
{
    return ALPHA * rawValue + (1 - ALPHA) * filteredValue;
}

归一化

// 归一化到 0-360
if (filtered_deg < 0.0f) {
    filtered_deg += 360.0f;
} 
else if (filtered_deg >= 360.0f) {
    filtered_deg -= 360.0f;
}

磁力计与预测值之间的"最短路径误差"

问题分析：比如磁力计 Yaw 是 10°，陀螺仪预测是 350°，直接算误差是 - 340°，但实际最短误差是 20°（350°→360°→10°）；

解决问题 ：把磁力计和陀螺仪积分后的差值限制在[-180, 180]，避免融合时出现 "反向修正" 的错误。

代码示例

// 计算陀螺仪积分的预测值，短期精准的 Yaw 预测值（但长期会漂移）
float predicted_yaw = yaw +_gz * dt;

// 计算磁力计（已滤波）与预测值之间的"最短路径误差"
float yaw_error = filtered_deg - predicted_yaw;

// 确保误差在 [-180, 180] 之间
if (yaw_error > 180.0f) {
    yaw_error -= 360.0f;
} else if (yaw_error < -180.0f) {
    yaw_error += 360.0f;
}

互补滤波为稳定偏航角

// 融合：在预测值的基础上修正误差的 5%
yaw = predicted_yaw + 0.05f * yaw_error;
// 保持输出在 0-360 范围内
if (yaw >= 360.0f) yaw -= 360.0f;
if (yaw < 0.0f) yaw += 360.0f;

归一化

// 归一化到 0-360
if (filtered_deg < 0.0f) {
    filtered_deg += 360.0f;
} 
else if (filtered_deg >= 360.0f) {
    filtered_deg -= 360.0f;
}

磁力计与预测值之间的"最短路径误差"

问题分析：比如磁力计 Yaw 是 10°，陀螺仪预测是 350°，直接算误差是 - 340°，但实际最短误差是 20°（350°→360°→10°）；

解决问题 ：把磁力计和陀螺仪积分后的差值限制在[-180, 180]，避免融合时出现 "反向修正" 的错误。

代码示例

// 计算陀螺仪积分的预测值，短期精准的 Yaw 预测值（但长期会漂移）
float predicted_yaw = yaw +_gz * dt;

// 计算磁力计（已滤波）与预测值之间的"最短路径误差"
float yaw_error = filtered_deg - predicted_yaw;

// 确保误差在 [-180, 180] 之间
if (yaw_error > 180.0f) {
    yaw_error -= 360.0f;
} else if (yaw_error < -180.0f) {
    yaw_error += 360.0f;
}

互补滤波为稳定偏航角

// 融合：在预测值的基础上修正误差的 5%
yaw = predicted_yaw + 0.05f * yaw_error;
// 保持输出在 0-360 范围内
if (yaw >= 360.0f) yaw -= 360.0f;
if (yaw < 0.0f) yaw += 360.0f;