【嵌入式】基于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_0I2C_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_cmd_begin() 来触发 I2C 控制器执行命令链接。一旦开始执行，就不能再修改命令链接。
• 命令发送后，通过调用 i2c_cmd_link_delete() 释放命令链接使用的资源

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

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

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

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

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

voidbh1750_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_tbh1750_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_ti2c_master_write_to_device(i2c_port_t i2c_num,uint8_t device_address,constuint8_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 由于公交车忙碌，作暂停

示例

staticesp_err_tmpu9250_register_read(uint8_t reg_addr,uint8_t*data,size_t len){returni2c_master_write_read_device(I2C_MASTER_NUM, MPU9250_SENSOR_ADDR,&reg_addr,1, data, len, I2C_MASTER_TIMEOUT_MS / portTICK_PERIOD_MS);}staticesp_err_tmpu9250_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_handleif(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_ti2c_master_transmit(i2c_master_dev_handle_t i2c_dev,constuint8_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_tqmc5883p_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字节，超时1000msreturni2c_master_transmit(dev->dev_handle, tx,2,1000);}

esp_err_tII2C_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_ti2c_master_transmit_receive(i2c_master_dev_handle_t i2c_dev,constuint8_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_tII2C_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_ti2c_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）保护全局变量

全局缓存+互斥锁

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

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

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

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

voidimu_shared_update_data(constimu_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初始化时候对互斥锁初始化

// 初始化ICM42670PvoidIcm42670p_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

staticvoidimu_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 和 Pitchfloat 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

staticvoidqmc5883p_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

代码示例

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

// 获取偏移测量值staticfloat x_max = X_MAX_OFFSET;staticfloat x_min = X_MIN_OFFSET;staticfloat y_max = Y_MAX_OFFSET;staticfloat y_min = Y_MIN_OFFSET;staticfloat z_max = Z_MAX_OFFSET;staticfloat 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.015float offset_y =(y_max+y_min)/2.0f;//Y轴偏移：(-0.02 + (-0.32)) / 2 = -0.34 / 2 =0.017float 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主控代码-ZEEKLOG博客

代码

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);staticfloat filtered_sin =0.0f;// 静态变量保存滤波后的sin值staticfloat 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低通滤波

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

归一化

// 归一化到 0-360if(filtered_deg <0.0f){ filtered_deg +=360.0f;}elseif(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;}elseif(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-360if(filtered_deg <0.0f){ filtered_deg +=360.0f;}elseif(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;}elseif(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;