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

Ne0inhk

— 26 min read
【嵌入式】基于I2C总线的IMU-磁力计融合算法与数据共享
ICM42670P+QMC5883P
在这里插入图片描述


本文涉及：

  • 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_master_write_byte() 和 i2c_master_write() 都有额外的实参，规定主机是否应确认其有无接受到 ACK 位。

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

读写示例（参考：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 主机设备需要 i2c_device_config_t 指定的配置:

分配和初始化 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;
在这里插入图片描述

Read more

惊叹数据结构之美，品味排序算法之妙：对计排、桶排的详细介绍

惊叹数据结构之美，品味排序算法之妙：对计排、桶排的详细介绍

大家好，这里是小编的博客频道 小编的博客：就爱学编程 很高兴在ZEEKLOG这个大家庭与大家相识，希望能在这里与大家共同进步，共同收获更好的自己！！！ 本文目录 * 引言 * 正文 * 一、计数排序（Counting Sort） * 二、基数排序（Radix Sort） * 三、总结 * 快乐的时光总是短暂，咱们下篇博文再见啦！！！不要忘了，给小编点点赞和收藏支持一下，在此非常感谢！！！ 引言 排序算法中的基数排序和计数排序都是非基于传统比较的排序方法，它们各自有着独特的实现原理和应用场景。下面小编将从代码实现的角度对这两种排序算法进行详细介绍。 那接下来就让我们开始遨游在知识的海洋！ 正文 一、计数排序（Counting Sort） 原理概述： 计数排序是一种适用于元素范围较小的排序算法。它利用一个额外的计数数组来记录待排序数组中每个元素出现的次数，然后根据这些次数来确定每个元素在最终排序数组中的位置。 代码实现步骤： 1. 确定元素范围：找出待排序数组中的最小值和最大值，记为min和max。2. 创建计数数组：创建

By Ne0inhk
【C++进阶系列】：万字详解unordered_set和unordered_map，带你手搓一个哈希表！(附模拟实现unordered_set和unordered_map的源码）

【C++进阶系列】：万字详解unordered_set和unordered_map，带你手搓一个哈希表！(附模拟实现unordered_set和unordered_map的源码）

🔥 本文专栏：c++ 🌸作者主页：努力努力再努力wz 💪 今日博客励志语录：努力不是为了回报，而是不让自己留下任何遗憾 ★★★ 本文前置知识： map和set模拟实现 引入 那么在正式讲解STL的unordered_map以及unordered_set这两个容器之前，我们先来回顾一下，目前我们接触到能够高效查找数据的数据结构，那么首先我们可以想到的能够实现高效查找数据的数据结构便是数组，但是这里的数组不是简单的将元素直接存放到数组中的任意位置，而是会将存储在数组中的元素先进行一次排序，然后借助二分算法来进行查找，由于这里数组的排序只需要一次，那么排序付出的代价可以均摊到每一次的查找操作中，所以这里排序的代价可以忽略不计，而二分查找的时间复杂度则是logN，所以这种方式能够实现高效的数据查找，但是如果涉及到插入以及删除操作的话，如果插入以及删除元素不在数组末尾，那么必然就要移动大量的元素，意味着插入和删除的时间复杂度最坏情况下会到达O(N)，效率相比于查找就不那么高效 接着就是在二叉搜索树的基础上优化，压缩其高度的AVL树和红黑树这两个数据结构，这两种数据结

By Ne0inhk
【LeetCode原地复写零】：双指针+逆向填充，O(n)时间O(1)空间最优解！

【LeetCode原地复写零】：双指针+逆向填充，O(n)时间O(1)空间最优解！

🎁个人主页：User_芊芊君子 🎉欢迎大家点赞👍评论📝收藏⭐文章 🔍系列专栏：Java.数据结构 【前言】 本文聚焦 LeetCode“原地复写零”经典题目，核心需求是在固定长度数组中复写每个 0并右移其余元素，且需满足原地修改、不使用额外数组空间的约束。正向遍历易导致后续元素被覆盖，为此本文详解双指针+逆向填充的优雅解法，高效破解这一核心难点。 文章目录： * 一、复写零 * 二、思路分析 * 1.找到复写的最后一个数 * 2.开始从后往前复写 * 三、代码展示 * 四、时间和空间复杂度分析 * 五、总结 一、复写零 二、思路分析 复写零这道题是让在原数组修改，如果从前向后遍历，后面的元素会被覆盖，所以我们要找到被复写的最后一个元素，然后从后往前复写。运用双指针+逆向填充 1.

By Ne0inhk
❿⁄₁₃ ⟦ OSCP ⬖ 研记 ⟧ 密码攻击实践 ➱ 获取并破解Net-NTLMv2哈希（下）

❿⁄₁₃ ⟦ OSCP ⬖ 研记 ⟧ 密码攻击实践 ➱ 获取并破解Net-NTLMv2哈希（下）

郑重声明：本文所涉安全技术仅限用于合法研究与学习目的，严禁任何形式的非法利用。因不当使用所导致的一切法律与经济责任，本人概不负责。任何形式的转载均须明确标注原文出处，且不得用于商业目的。 🔋 点赞 | 能量注入 ❤️ 关注 | 信号锁定 🔔 收藏 | 数据归档 ⭐️ 评论 | 保持连接💬 🌌 立即前往 👉晖度丨安全视界🚀 ▶ 信息收集  ▶ 漏洞检测 ▶ 初始立足点  ▶ 权限提升 ▶ 横向移动 ➢ 密码攻击 ➢  获取并破解Net-NTLMv2哈希（下）🔥🔥🔥 ▶ 报告/分析 ▶ 教训/修复 目录 1.密码破解 1.1 破解Windows哈希实践 1.1.3 捕获Net-NTLMv2哈希实践 1.1.3.3 使用Netcat连接绑定 Shell（kali上） 1.连接流程 2.连接命令

By Ne0inhk