feat(VL53L4CD): 支持透过玻璃串扰补偿、串口校准命令与校准数据Flash存储

2026-09-07 00:29:41 +08:00
parent ef37553b18
commit ec412b1ffd
13 changed files with 966 additions and 48 deletions
@@ -127,3 +127,63 @@ Src/VL53L4CD_Driver.c
 ### 说明

 固定安装场景使用单点偏移校准即可。`VL53L4CD_AUTO_CALIB = CFG_ENABLE` 时每次开机自动校准，适合重新标定；校准完成后建议改为 `CFG_DISABLE`，使用 `VL53L4CD_CALIB_OFFSET_MM` 中的固定值，避免每次开机都依赖目标存在。
+
+## 9. 透过玻璃串扰补偿（Crosstalk）
+
+### 改动
+
+- `Inc/VL53L4CD_Driver.h` / `Src/VL53L4CD_Driver.c`：新增 `vl53l4cd_set_xtalk()`、`vl53l4cd_get_xtalk()`、`vl53l4cd_calibrate_xtalk()`。
+- `Inc/board_config.h`：新增 `VL53L4CD_GLASS_XTALK`、`VL53L4CD_GLASS_XTALK_KCPS`、`VL53L4CD_XTALK_CALIB_TARGET_MM`、`VL53L4CD_XTALK_CALIB_SAMPLES` 开关。
+- `Src/main.c`：开机按开关写入固定串扰值。
+
+### 实现
+
+按 ST ULD 官方 `vl53l4cd_calibration.c` 的 `VL53L4CD_CalibrateXtalk()` 流程实现（清零补偿 → 热机 10 帧 → 采样有效帧 → 计算写回），浮点公式改为定点数以适配无 FPU 的 Cortex-M23：
+
+```text
+xtalk = (1 - 平均距离/目标距离) * 平均信号速率/平均SPAD数
+```
+
+上限 127 kcps，寄存器值 = `xtalk_kcps * 512`（XTALK_PLANE_OFFSET_KCPS 0x0016），X/Y 梯度寄存器（0x0018/0x001A）清零。
+
+## 10. 串口命令扩展
+
+### 改动
+
+- `Inc/command.h` / `Src/command.c`：新增命令处理与共享状态（最近一帧结果、上报开关）。
+- `Src/main.c`：主循环推送最近一帧结果、按上报开关输出。
+
+### 命令列表
+
+- 基础：`M1`（开上报）、`M2`（关上报）、`M3`（查最近一帧 `d=距离;s=状态`）、`M888`（重启）、`M999`（版本）、`M9999`（OTA）
+- 校准：`M101[S]`（读/写偏移）、`M102S`（偏移校准）、`M103[S]`（读/写串扰）、`M104S`（串扰校准）、`M105`（存 Flash）、`M106`（从 Flash 加载应用）、`M107`（擦除并清零）
+
+`M102`/`M104` 阻塞约 2 秒，期间连续上报暂停。带符号参数（如 `M101S-9`）通过新增的 `parse_int_dec()` 解析。
+
+## 11. 校准数据 Flash 持久化
+
+### 改动
+
+- 新增 `Inc/calib_store.h` / `Src/calib_store.c`。
+- `LD/gd32e23x_flash.ld`：代码区 16K → 15K，保留最后一页（1KB）。
+- `Inc/board_config.h`：新增 `VL53L4CD_PERSIST_CALIB`。
+- `Src/main.c`：开机优先从 Flash 加载校准数据。
+
+### 实现
+
+- 校准记录（偏移 + 串扰）16 字节：魔数 `0x344C5643`（"CVL4"）+ 版本 + 偏移 + 串扰 + 累加和。
+- 页地址按运行时检测的 Flash 容量取最后一页：16K→`0x0800 3C00`、32K→`0x0800 7C00`、64K→`0x0800 FC00`。
+- 擦写用 SDK 的 `fmc_page_erase()` / `fmc_doubleword_program()`，期间暂停串口接收中断防止丢字节。
+
+## 12. 体积优化与构建调整
+
+新增功能后固件超预算（16K 仅剩约 1.9K），做了两处优化：
+
+- `Src/main.c`：测距上报与启动信息改用手写格式化输出（`uart_write_str/u32/i32`），输出与原 printf 版本逐字节一致，不再链接 vfprintf/malloc 等 libc 组件，省约 2.7K。
+- `cmake/project.cmake`：Debug 优化等级 `-O0` → `-Og`（`-O0` 已无法放下，且旧 Debug 本就已用 15.6K/16K）。
+
+### 结果
+
+- Release：`text 12908 / data 24 / bss 1704`（改动前 `text 14316 / data 116 / bss 2012`）
+- Debug：`text 14876 / data 24 / bss 1712`
+- 两配置均在 15K 代码区内，编译无警告（Release）
@@ -37,6 +37,7 @@ set(TARGET_SRC
    Src/i2c.c
    Src/board_config.c
    Src/VL53L4CD_Driver.c
+    Src/calib_store.c
 )

 # 设置输出目录
@@ -140,6 +140,22 @@ bool vl53l4cd_set_range_timing(uint8_t timing_budget_ms, uint32_t inter_measurem
 */
 bool vl53l4cd_set_offset(int16_t offset_mm);

+/**
+ * @brief   写入串扰（玻璃反射）补偿值，透过玻璃测距时使用
+ * @details 写 XTALK_PLANE_OFFSET_KCPS (0x0016) 并将 X/Y 梯度寄存器清零，
+ *          寄存器值为 xtalk_kcps * 512
+ * @param[in]  xtalk_kcps: 串扰补偿值，单位 kcps，范围 0..127
+ * @retval   true: 成功; false: 参数无效或写入失败
+ */
+bool vl53l4cd_set_xtalk(uint16_t xtalk_kcps);
+
+/**
+ * @brief   读取当前生效的串扰补偿值
+ * @param[out] xtalk_kcps: 返回串扰值，单位 kcps
+ * @retval   true: 成功; false: 失败
+ */
+bool vl53l4cd_get_xtalk(uint16_t *xtalk_kcps);
+
 /**
 * @brief   读取当前生效的距离偏移量
 * @param[out] offset_mm: 返回偏移量，单位 mm
@@ -157,6 +173,20 @@ bool vl53l4cd_get_offset(int16_t *offset_mm);
 bool vl53l4cd_calibrate_offset(uint16_t target_distance_mm, uint16_t num_samples,
                               int16_t *measured_offset_mm);

+/**
+ * @brief   执行一次串扰校准（透过玻璃安装时使用）
+ * @details 参照 ST ULD vl53l4cd_calibration.c 的 CalibrateXtalk 流程，定点数实现：
+ *          先清零串扰补偿，热机 10 帧，然后采样有效测距帧，
+ *          xtalk = (1 - 平均距离/目标距离) * 平均信号/平均SPAD数，上限 127 kcps。
+ *          校准期间传感器被独占，主循环上报会暂停。
+ * @param[in]  target_distance_mm: 玻璃后方目标的实际距离，单位 mm，范围 10..5000
+ * @param[in]  num_samples: 采样次数，范围 5..255，ST 建议至少 10
+ * @param[out] measured_xtalk_kcps: 计算得到的串扰值，单位 kcps
+ * @retval   true: 校准并写入成功; false: 失败（含有效样本不足或串扰超限）
+ */
+bool vl53l4cd_calibrate_xtalk(uint16_t target_distance_mm, uint16_t num_samples,
+                              uint16_t *measured_xtalk_kcps);
+
 /**
 * @brief   读一个 8 位寄存器
 * @param[in]  reg: 16 位寄存器地址
@@ -55,6 +55,14 @@
 #define VL53L4CD_AUTO_CALIB        CFG_DISABLE  // CFG_ENABLE: 每次开机自动校准; CFG_DISABLE: 使用下面固定偏移
 #define VL53L4CD_CALIB_OFFSET_MM   (-9)  // 校准后保存的固定偏移量，单位 mm，可为负

+/* >>>>>>>>>>>>>>>>>>>>[VL53L4CD GLASS XTALK / FLASH PERSIST]<<<<<<<<<<<<<<<<<<<< */
+
+#define VL53L4CD_GLASS_XTALK            CFG_DISABLE  // CFG_ENABLE: 透过玻璃测距，开机写入串扰补偿
+#define VL53L4CD_GLASS_XTALK_KCPS       20U  // 编译期固定串扰值，单位 kcps，范围 0..127
+#define VL53L4CD_XTALK_CALIB_TARGET_MM  500U // M104 串扰校准默认目标距离（玻璃后方），单位 mm
+#define VL53L4CD_XTALK_CALIB_SAMPLES    20U  // M104 串扰校准默认采样次数，ST 建议至少 10
+#define VL53L4CD_PERSIST_CALIB          CFG_ENABLE  // CFG_ENABLE: 开机优先从 Flash 末页加载校准数据
+
 /* >>>>>>>>>>>>>>>>>>>>[RS485 MODE]<<<<<<<<<<<<<<<<<<<< */

 #define RS485_MODE            CFG_ENABLE  // CFG_DISABLE: RS485 off; CFG_ENABLE: RS485 on
@@ -0,0 +1,37 @@
+/**
+ * @file calib_store.h
+ * @brief VL53L4CD 校准数据 Flash 持久化存储接口
+ * @details 利用主 Flash 最后一页（1KB，地址按运行时检测到的 Flash 容量计算）
+ *          保存偏移校准与串扰校准结果，掉电不丢失。
+ *          注意：链接脚本已将代码区缩小 1KB，为 16K 容量器件保留该页。
+ */
+#ifndef CALIB_STORE_H
+#define CALIB_STORE_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+/**
+ * @brief   从 Flash 读取校准数据
+ * @param[out] offset_mm: 返回保存的偏移量，单位 mm
+ * @param[out] xtalk_kcps: 返回保存的串扰值，单位 kcps
+ * @retval   true: 校准数据有效; false: 无数据、版本不匹配或校验失败
+ */
+bool calib_store_load(int16_t *offset_mm, uint16_t *xtalk_kcps);
+
+/**
+ * @brief   保存校准数据到 Flash（整页擦除后写入）
+ * @details 操作期间会短暂暂停串口接收中断，耗时约几十毫秒
+ * @param[in]  offset_mm: 偏移量，单位 mm
+ * @param[in]  xtalk_kcps: 串扰值，单位 kcps，0..127
+ * @retval   true: 保存成功; false: Flash 操作失败
+ */
+bool calib_store_save(int16_t offset_mm, uint16_t xtalk_kcps);
+
+/**
+ * @brief   擦除 Flash 中的校准数据页
+ * @retval   true: 擦除成功; false: Flash 操作失败
+ */
+bool calib_store_erase(void);
+
+#endif /* CALIB_STORE_H */
@@ -9,6 +9,7 @@

 #include <stdint.h>
 #include <stdbool.h>
+#include "VL53L4CD_Driver.h"

 /**
 * @defgroup Command 命令处理模块
@@ -95,6 +96,27 @@ void command_execute(const char *cmd_str);
 */
 void command_m737_timer_finished_callback(void);

+/**
+ * @brief 由主循环推送最近一帧测距结果，供 M3 命令立即查询
+ * @param result 最近一帧测距结果
+ * @ingroup Command
+ */
+void command_set_last_result(const vl53l4cd_result_t *result);
+
+/**
+ * @brief 查询测距上报是否开启（M1/M2 控制）
+ * @retval true 上报开启；false 上报关闭
+ * @ingroup Command
+ */
+bool get_sensor_report_enabled(void);
+
+/**
+ * @brief 开启/关闭测距上报（同时控制 LED），供 M1/M2 命令调用
+ * @param enabled true 开启上报并点亮 LED；false 关闭上报并熄灭 LED
+ * @ingroup Command
+ */
+void set_sensor_report_enabled(bool enabled);
+
 /** @} */ // end of Command group

 #endif // COMMAND_H
@@ -12,7 +12,8 @@ _Min_Stack_Size = 0x400; /* required amount of stack */
 /* Memories definition */
 MEMORY
 {
-    FLASH (rx)  : ORIGIN = 0x08000000, LENGTH = 16K
+    /* 代码区保留 1KB：最后一页 Flash 用于保存校准数据（calib_store，页地址按实际容量取末页） */
+    FLASH (rx)  : ORIGIN = 0x08000000, LENGTH = 15K
    RAM (xrw)   : ORIGIN = 0x20000000, LENGTH = 4K
 }

@@ -7,10 +7,14 @@
 - [硬件连接](#硬件连接)
 - [功能开关](#功能开关)
 - [编译与烧录](#编译与烧录)
+- [串口命令（Mx）](#串口命令mx)
 - [串口输出](#串口输出)
 - [VL53L4CD 驱动](#vl53l4cd-驱动)
 - [测距结果字段](#测距结果字段)
 - [测距状态码](#测距状态码)
+- [偏移校准](#偏移校准)
+- [透过玻璃测距（Crosstalk）](#透过玻璃测距crosstalk)
+- [校准数据 Flash 存储](#校准数据-flash-存储)
 - [I2C 16 位寄存器说明](#i2c-16-位寄存器说明)
 - [Flash 体积说明](#flash-体积说明)
 - [已知限制](#已知限制)
@@ -64,6 +68,11 @@
 | `VL53L4CD_CALIB_SAMPLES` | `20` | 校准采样次数 |
 | `VL53L4CD_AUTO_CALIB` | `CFG_DISABLE` | 开机自动校准开关 |
 | `VL53L4CD_CALIB_OFFSET_MM` | `-9` | 当前固定的距离偏移量 |
+| `VL53L4CD_GLASS_XTALK` | `CFG_DISABLE` | 透过玻璃测距：开机写入串扰补偿 |
+| `VL53L4CD_GLASS_XTALK_KCPS` | `20` | 编译期固定串扰值，单位 kcps，0..127 |
+| `VL53L4CD_XTALK_CALIB_TARGET_MM` | `500` | M104 串扰校准默认目标距离（玻璃后方） |
+| `VL53L4CD_XTALK_CALIB_SAMPLES` | `20` | M104 串扰校准默认采样次数 |
+| `VL53L4CD_PERSIST_CALIB` | `CFG_ENABLE` | 开机优先从 Flash 末页加载校准数据 |
 | `RS485_MODE` | `CFG_ENABLE` | 使用 RS485 串口输出 |
 | `SOFTWARE_IIC` | `CFG_DISABLE` | 使用硬件 I2C |

@@ -108,6 +117,41 @@ D:/toolchain/openocd/bin/openocd.exe \

 也可以在 VSCode 中使用已配置的 Flash 任务。

+## 串口命令（Mx）
+
+命令帧格式 `D5 01 LEN [命令] CRC`（LEN 为命令字节数，CRC 为帧内累加和），响应帧 `B5 TYPE LEN [数据] CRC`，`TYPE=0xF0` 表示成功。波特率 115200，8N1。
+
+### 基础命令
+
+| 命令 | 功能 | 成功响应 |
+|---|---|---|
+| `M1` | 开启测距上报，LED 亮 | `ok` |
+| `M2` | 关闭测距上报，LED 灭 | `ok` |
+| `M3` | 立即返回最近一帧结果（上报关闭时也可查询） | `d=830;s=0` |
+| `M888` | 软件重启 | `ok` 后复位 |
+| `M999` | 输出固件版本号 | `v20.0.0.1` |
+| `M9999` | 复位进入 OTA/Bootloader | 复位 |
+
+### 校准命令
+
+| 命令 | 功能 | 成功响应 |
+|---|---|---|
+| `M101` | 读取当前偏移量 | `o=-9` |
+| `M101S<mm>` | 写入偏移量并立即生效，如 `M101S-9` | `o=-9` |
+| `M102S<mm>` | 执行偏移校准，`S` 为目标实际距离（默认 44） | `o=-9` |
+| `M103` | 读取当前串扰补偿值 | `x=12` |
+| `M103S<kcps>` | 写入串扰补偿值 0..127，如 `M103S12` | `x=12` |
+| `M104S<mm>` | 执行串扰校准，`S` 为玻璃后方目标距离（默认 500） | `x=12` |
+| `M105` | 保存当前偏移+串扰到 Flash（掉电保持） | `ok` |
+| `M106` | 从 Flash 读取并应用校准数据 | `o=-9` |
+| `M107` | 擦除 Flash 校准数据，偏移/串扰清零 | `ok` |
+
+说明：
+
+- `M102`/`M104` 为阻塞执行，耗时约 2 秒，期间连续上报暂停。
+- `M101S`/`M103S`/`M102`/`M104` 只改寄存器，掉电丢失；需要掉电保持时接一条 `M105`。
+- 参数错误返回 `0xFD`，未知命令返回 `0xFE`，校准/Flash 失败返回 `0xFF`。
+
 ## 串口输出

 连接串口后使用 115200 8N1 查看，例如 COM17：
@@ -139,6 +183,9 @@ Distance: 830 mm | status: 0 | signal: 1128 kcps | ambient: 72 kcps | sigma: 3 m
 | `vl53l4cd_set_offset(offset_mm)` | 写入手动指定的距离偏移量 |
 | `vl53l4cd_get_offset(&offset_mm)` | 读取当前生效的距离偏移量 |
 | `vl53l4cd_calibrate_offset(target_mm, samples, &offset)` | 对当前目标执行偏移校准并写回 |
+| `vl53l4cd_set_xtalk(xtalk_kcps)` | 写入串扰补偿值（透过玻璃），0..127 kcps |
+| `vl53l4cd_get_xtalk(&xtalk_kcps)` | 读取当前串扰补偿值 |
+| `vl53l4cd_calibrate_xtalk(target_mm, samples, &xtalk)` | 执行串扰校准并写回 |
 | `vl53l4cd_read_reg8/16()` | 读 8/16 位寄存器 |
 | `vl53l4cd_write_reg8/16()` | 写 8/16 位寄存器 |

@@ -155,10 +202,56 @@ VL53L4CD 出厂带有默认偏移，但器件之间存在差异，固定安装
 3. 上电后串口会打印 `VL53L4CD offset calibrated: X mm`。
 4. 把打印出的 `X` 填入 `VL53L4CD_CALIB_OFFSET_MM`，再将 `VL53L4CD_AUTO_CALIB` 改回 `CFG_DISABLE`。

+也可以不发固件、直接用串口命令在线校准：目标放好后依次发送 `M102S44`（校准）、`M105`（保存到 Flash）。`M101` 可随时读回当前偏移。
+
 也可以直接调用 `vl53l4cd_calibrate_offset()`，该函数会先清零偏移、热机 10 帧，再统计有效测距样本并计算 `offset = 实际距离 - 平均距离`，最后写入 `RANGE_OFFSET_MM (0x001E)`。

 注意：单点偏移只修正当前固定距离的绝对误差。近距离高反射金属目标存在非线性，建议使用哑光目标。

+## 透过玻璃测距（Crosstalk）
+
+传感器透过盖板玻璃测距时，玻璃上下表面的反射会产生串扰（crosstalk），表现为读数偏小、近距离出现固定虚假目标。VL53L4CD 通过 XTALK 寄存器组（0x0016..0x001D）做信号级补偿。
+
+固件按 ST ULD 官方算法（`vl53l4cd_calibration.c`）实现，并改为定点数运算以省 Flash：
+
+```text
+xtalk = (1 - 平均距离/目标距离) * 平均信号速率/平均SPAD数
+```
+
+上限 127 kcps，超出判定为校准失败；寄存器值为 `xtalk_kcps * 512`。
+
+### 使用方式
+
+**方式一：编译期固定值（不使用串口）**
+
+1. 测得或校准出串扰值后，打开 `VL53L4CD_GLASS_XTALK`，把值填入 `VL53L4CD_GLASS_XTALK_KCPS`。
+2. Flash 中无校准数据时，开机自动写入该值。
+
+**方式二：串口命令（推荐）**
+
+1. 装上玻璃，目标放在玻璃后方已知距离（如 500mm），确保目标与玻璃之间无遮挡。
+2. 先完成偏移校准：`M102S44`。
+3. 执行串扰校准：`M104S500`，返回如 `x=12`。
+4. `M105` 保存到 Flash，掉电不丢。
+5. `M103` 可随时读回当前串扰值。
+
+注意：偏移校准与串扰校准相互影响，重新校准其中一项后建议两项都重新校准并再次 `M105` 保存。
+
+## 校准数据 Flash 存储
+
+校准数据（偏移 + 串扰）保存在主 Flash 的最后一页（1KB），由 `Src/calib_store.c` 管理：
+
+| 器件 | 校准页地址 |
+|---|---|
+| 16K（F4） | `0x0800 3C00` |
+| 32K（F6） | `0x0800 7C00` |
+| 64K（F8） | `0x0800 FC00` |
+
+- 页地址按上电检测到的 Flash 容量动态计算，一份固件适配三种容量。
+- 记录 16 字节，含魔数 `CVL4`、版本号、累加和校验；空白 Flash（全 `0xFF`）不会误判。
+- 链接脚本代码区已缩至 15K，避免代码长入校准页；擦写通过 GD32 SDK 的 `fmc_page_erase()` / `fmc_doubleword_program()` 完成，操作期间短暂暂停串口接收中断。
+- 上电加载顺序（`VL53L4CD_PERSIST_CALIB = CFG_ENABLE` 时）：Flash 有有效数据 → 用 Flash 值；否则用 `VL53L4CD_CALIB_OFFSET_MM`（及开启 `VL53L4CD_GLASS_XTALK` 时的固定串扰值）。
+
 ### 修改时序预算

 在 `Src/VL53L4CD_Driver.c` 的 `vl53l4cd_init()` 末尾：
@@ -179,6 +272,8 @@ delay_ms(500);

 改为需要的毫秒数即可。

+## I2C 16 位寄存器说明
+
 ## 测距结果字段

 `vl53l4cd_get_result()` 返回 `vl53l4cd_result_t`：
@@ -216,21 +311,22 @@ VL53L4CD 的寄存器地址是 16 位。本项目的 `i2c_read()` / `i2c_write()

 ## Flash 体积说明

-当前 Release 固件约：
+链接脚本代码区为 15K（16K 容量器件预留最后 1KB 页存放校准数据）。当前 Release 固件约：

 ```text
-text   14316
-data     116
-bss     2012
+text   12908
+data      24
+bss     1704
 ```

-16K Flash 下仍有约 2K 余量。保留 `printf` 后体积会增加约 2.5K，因此不建议同时开启全部 IIC 调试打印。
+15K 内仍有约 2.4K 余量。为省体积，主循环测距上报与启动信息改为手写格式化输出（不再链接 vfprintf，省约 2.7K），输出格式与旧版本逐字节一致。Debug 构建优化等级由 `-O0` 调整为 `-Og`（-O0 已无法放下），text 约 14876。保留 `printf` 会增加约 2.5K，因此 `DEBUG_VERBOSE` 全开仍会让 Flash 非常紧张。

 ## 已知限制

 - 驱动只支持连续测距模式
- 尚未实现 Crosstalk 校准、距离阈值中断、自主低功耗模式、ULP 模式
+- 距离阈值中断、自主低功耗模式、ULP 模式尚未实现
 - VL53L4CD 是单区传感器，不是多区测距
+- 串扰补偿为单区平面补偿（梯度寄存器清零），玻璃需正对传感器且尽量平行
 - `DEBUG_VERBOSE` 全开时 IIC 调试打印会让 Flash 非常紧张

 ## 变更记录
@@ -300,6 +300,122 @@ bool vl53l4cd_calibrate_offset(uint16_t target_distance_mm, uint16_t num_samples
    return true;
 }

+bool vl53l4cd_set_xtalk(uint16_t xtalk_kcps)
+{
+    /* ST 固件上限：127 kcps（65536/512），寄存器值单位为 1/512 kcps */
+    if (xtalk_kcps > 127U) {
+        return false;
+    }
+    if (!vl53l4cd_write_reg16(VL53L4CD_REG_XTALK_X_PLANE_GRADIENT_KCPS, 0U) ||
+        !vl53l4cd_write_reg16(VL53L4CD_REG_XTALK_Y_PLANE_GRADIENT_KCPS, 0U)) {
+        return false;
+    }
+    return vl53l4cd_write_reg16(VL53L4CD_REG_XTALK_PLANE_OFFSET_KCPS,
+                                (uint16_t)(xtalk_kcps << 9));
+}
+
+bool vl53l4cd_get_xtalk(uint16_t *xtalk_kcps)
+{
+    uint16_t reg_value;
+
+    if (xtalk_kcps == NULL) {
+        return false;
+    }
+    if (!vl53l4cd_read_reg16(VL53L4CD_REG_XTALK_PLANE_OFFSET_KCPS, &reg_value)) {
+        return false;
+    }
+    *xtalk_kcps = (uint16_t)(reg_value >> 9);
+    return true;
+}
+
+bool vl53l4cd_calibrate_xtalk(uint16_t target_distance_mm, uint16_t num_samples,
+                              uint16_t *measured_xtalk_kcps)
+{
+    vl53l4cd_result_t result;
+    uint32_t distance_sum = 0U;
+    uint32_t signal_sum = 0U;
+    uint32_t spad_sum = 0U;
+    uint32_t valid_samples = 0U;
+    uint16_t avg_distance;
+    uint32_t avg_signal;
+    uint32_t avg_spad;
+    uint32_t signal_ratio;
+    uint16_t xtalk_kcps;
+    uint16_t i;
+
+    if (measured_xtalk_kcps == NULL ||
+        target_distance_mm < 10U || target_distance_mm > 5000U ||
+        num_samples < 5U || num_samples > 255U) {
+        return false;
+    }
+
+    /* 先禁用串扰补偿再采样 */
+    if (!vl53l4cd_set_xtalk(0U)) {
+        return false;
+    }
+
+    if (!vl53l4cd_stop_ranging() || !vl53l4cd_start_ranging()) {
+        return false;
+    }
+
+    /* 热机 10 帧，丢弃 */
+    for (i = 0U; i < 10U; i++) {
+        if (!vl53l4cd_wait_for_data_ready(VL53L4CD_DATA_TIMEOUT_MS)) {
+            (void)vl53l4cd_stop_ranging();
+            return false;
+        }
+        (void)vl53l4cd_clear_interrupt();
+        (void)vl53l4cd_get_result(&result);
+    }
+
+    /* 统计有效测距样本（ST 参考实现：跳过首个采样帧） */
+    for (i = 0U; i < num_samples; i++) {
+        if (!vl53l4cd_wait_for_data_ready(VL53L4CD_DATA_TIMEOUT_MS)) {
+            (void)vl53l4cd_stop_ranging();
+            return false;
+        }
+        (void)vl53l4cd_clear_interrupt();
+        if (i > 0U &&
+            vl53l4cd_get_result(&result) &&
+            result.range_status == VL53L4CD_STATUS_RANGE_VALID) {
+            distance_sum += result.distance_mm;
+            signal_sum += result.signal_rate_kcps;
+            spad_sum += result.number_of_spad;
+            valid_samples++;
+        }
+    }
+
+    (void)vl53l4cd_stop_ranging();
+
+    if (valid_samples == 0U) {
+        return false;
+    }
+
+    /* 定点数计算 xtalk = (1 - 平均距离/目标距离) * 平均信号/平均SPAD数 */
+    avg_distance = (uint16_t)(distance_sum / valid_samples);
+    avg_signal = signal_sum / valid_samples;
+    avg_spad = spad_sum / valid_samples;
+    if (avg_spad == 0U || avg_distance >= target_distance_mm) {
+        return false;
+    }
+
+    /* 乘法上界安全：avg_signal<=524280, (target-avg_distance)<=4990, 乘积 < 2^32 */
+    signal_ratio = (uint32_t)((avg_signal * (uint32_t)(target_distance_mm - avg_distance))
+                              / target_distance_mm);
+    xtalk_kcps = (uint16_t)((signal_ratio + (avg_spad / 2U)) / avg_spad);
+    if (xtalk_kcps > 127U) {
+        /* 超出传感器固件支持的最大串扰值，判定为校准失败 */
+        return false;
+    }
+
+    if (!vl53l4cd_set_xtalk(xtalk_kcps)) {
+        return false;
+    }
+
+    *measured_xtalk_kcps = xtalk_kcps;
+    return true;
+}
+
 bool vl53l4cd_init(void)
 {
    uint16_t model_id = 0U;
@@ -0,0 +1,137 @@
+/**
+ * @file calib_store.c
+ * @brief VL53L4CD 校准数据 Flash 持久化存储实现
+ * @details 校准记录保存在主 Flash 的最后一页（1KB）。页地址根据上电检测到的
+ *          Flash 容量（16K/32K/64K）动态计算，同一份固件可适配不同容量器件：
+ *            16K: 0x0800 3C00    32K: 0x0800 7C00    64K: 0x0800 FC00
+ *          记录为 16 字节（2 个双字），带魔数、版本与累加和校验；
+ *          空白 Flash 全 0xFF，不会误判为有效记录。
+ *          GD32E230 页擦除为 1KB，编程以 64 位双字为单位。
+ */
+#include "gd32e23x.h"
+#include "calib_store.h"
+#include "board_config.h"
+#include "gd32e23x_fmc.h"
+#include "uart.h"
+#include <string.h>
+
+/** 校准记录魔数，"CVL4" 的 ASCII 小端编码 */
+#define CALIB_RECORD_MAGIC        0x344C5643U
+#define CALIB_RECORD_VERSION      1U
+#define CALIB_RECORD_SIZE         16U
+
+typedef struct {
+    uint32_t magic;       /**< 魔数，用于识别有效记录 */
+    uint16_t version;     /**< 记录格式版本 */
+    uint16_t flags;       /**< 保留标志位 */
+    int16_t  offset_mm;   /**< 偏移校准值，单位 mm */
+    uint16_t xtalk_kcps;  /**< 串扰校准值，单位 kcps */
+    uint16_t checksum;    /**< 前 12 字节累加和 */
+    uint16_t pad;         /**< 填充到 16 字节 */
+} calib_record_t;
+
+/**
+ * @brief   计算校准记录所在页的起始地址（Flash 最后一页）
+ * @retval   uint32_t 页起始地址
+ */
+static uint32_t calib_page_address(void)
+{
+    uint32_t size_kb = get_flash_size();
+
+    if (size_kb == 0U) {
+        size_kb = 16U; /* 读取异常时按最小容量处理 */
+    }
+    return 0x08000000U + (size_kb * 1024U) - 1024U;
+}
+
+/**
+ * @brief   计算记录前 12 字节的 16 位累加和
+ * @param[in]  rec: 校准记录指针
+ * @retval   uint16_t 累加和
+ */
+static uint16_t calib_checksum(const calib_record_t *rec)
+{
+    const uint8_t *p = (const uint8_t *)rec;
+    uint16_t sum = 0U;
+    uint8_t i;
+
+    for (i = 0U; i < 12U; i++) {
+        sum += p[i];
+    }
+    return sum;
+}
+
+bool calib_store_load(int16_t *offset_mm, uint16_t *xtalk_kcps)
+{
+    const calib_record_t *rec = (const calib_record_t *)(uintptr_t)calib_page_address();
+
+    if (offset_mm == NULL || xtalk_kcps == NULL) {
+        return false;
+    }
+    if (rec->magic != CALIB_RECORD_MAGIC || rec->version != CALIB_RECORD_VERSION) {
+        return false;
+    }
+    if (rec->checksum != calib_checksum(rec)) {
+        return false;
+    }
+
+    *offset_mm = rec->offset_mm;
+    *xtalk_kcps = rec->xtalk_kcps;
+    return true;
+}
+
+bool calib_store_save(int16_t offset_mm, uint16_t xtalk_kcps)
+{
+    calib_record_t rec;
+    uint64_t data[2];
+    uint32_t page_addr;
+    fmc_state_enum fmc_state;
+
+    rec.magic = CALIB_RECORD_MAGIC;
+    rec.version = CALIB_RECORD_VERSION;
+    rec.flags = 0U;
+    rec.offset_mm = offset_mm;
+    rec.xtalk_kcps = xtalk_kcps;
+    rec.checksum = 0U;
+    rec.pad = 0U;
+    rec.checksum = calib_checksum(&rec);
+
+    /* GD32E230 按双字（64 位）编程，直接按内存布局拆成两个 64 位 */
+    memcpy(&data[0], &rec, 8U);
+    memcpy(&data[1], (const uint8_t *)&rec + 8U, 8U);
+
+    page_addr = calib_page_address();
+
+    /* Flash 操作期间总线停顿，暂停串口接收避免丢字节 */
+    uart_rx_irq_pause();
+    fmc_unlock();
+
+    fmc_state = fmc_page_erase(page_addr);
+    if (fmc_state == FMC_READY) {
+        fmc_state = fmc_doubleword_program(page_addr, data[0]);
+        if (fmc_state == FMC_READY) {
+            fmc_state = fmc_doubleword_program(page_addr + 8U, data[1]);
+        }
+    }
+
+    fmc_lock();
+    uart_rx_irq_resume();
+
+    return (fmc_state == FMC_READY);
+}
+
+bool calib_store_erase(void)
+{
+    uint32_t page_addr = calib_page_address();
+    fmc_state_enum fmc_state;
+
+    uart_rx_irq_pause();
+    fmc_unlock();
+
+    fmc_state = fmc_page_erase(page_addr);
+
+    fmc_lock();
+    uart_rx_irq_resume();
+
+    return (fmc_state == FMC_READY);
+}
@@ -21,6 +21,7 @@
 #include "gd32e23x_usart.h"
 #include "i2c.h"
 #include "core_cm23.h"
+#include "calib_store.h"

 /* ============================================================================
 * 协议格式说明
@@ -88,6 +89,13 @@ static const uint8_t s_report_status_ok[] __attribute__((unused)) = { 'o', 'k' }
 static const uint8_t s_report_status_err[] = { 'e','r','r' }; /**< 错误响应数据 */
 /** @} */

+/** @name 运行状态共享数据
+ * @{ */
+static bool s_report_enabled = true;                 /**< 测距上报开关，M1/M2 控制 */
+static vl53l4cd_result_t s_last_result;              /**< 最近一帧测距结果，主循环推送 */
+static bool s_last_result_valid = false;             /**< 最近一帧结果是否有效 */
+/** @} */
+
 void system_software_reset(void)
 {
    // 确保所有待处理的内存访问完成
@@ -111,6 +119,42 @@ void system_software_reset(void)
 * 公共接口函数
 * ============================================================================ */

+bool get_sensor_report_enabled(void)
+{
+    return s_report_enabled;
+}
+
+void set_sensor_report_enabled(bool enabled)
+{
+    s_report_enabled = enabled;
+    if (enabled) {
+        led_on();
+    } else {
+        led_off();
+    }
+}
+
+void command_set_last_result(const vl53l4cd_result_t *result)
+{
+    if (result != NULL) {
+        s_last_result = *result;
+        s_last_result_valid = true;
+    }
+}
+
+/**
+ * @brief 发送 "前缀=数值" 格式的成功响应，如 "off=-9"、"xt=12"。
+ * @param prefix 数值前缀字符。
+ * @param value 有符号数值。
+ */
+static void send_resp_prefixed(char prefix, int32_t value);
+
+/**
+ * @brief 发送 "d=<距离>;s=<状态>" 格式的测距结果响应。
+ * @param result 测距结果指针。
+ */
+static void send_resp_result(const vl53l4cd_result_t *result);
+
 /**
 * @brief 计算协议包的 8 位累加校验值（Checksum）。
 * @details 对输入缓冲区逐字节累加并取低 8 位，累加范围为 data[1] 至 data[len-2]，
@@ -150,7 +194,7 @@ static uint8_t command_sum_crc_calc(const uint8_t *data, uint8_t len)
 static void send_response(uint8_t type, const uint8_t *payload, uint8_t len)
 {
    uint8_t buf_len = (uint8_t)(3 + len + 1);
-    uint8_t buf[16]; // 简单场景足够，必要时可增大
+    uint8_t buf[20]; // 简单场景足够，必要时可增大
    if (buf_len > sizeof(buf)) return; // 防御

    buf[0] = RESP_HEADER;
@@ -322,6 +366,62 @@ static uint8_t parse_uint_dec(const uint8_t *s, uint8_t n, uint32_t *out)
    return i;
 }

+/**
+ * @brief 从缓冲区解析十进制有符号整数。
+ * @details 支持可选的正负号前缀（'-' 或 '+'），数字部分同 parse_uint_dec()。
+ * @param s 指向待解析字符缓冲区的起始位置。
+ * @param n 允许解析的最大字符数。
+ * @param out 输出参数，存储解析结果，可为NULL。
+ * @return uint8_t 实际消耗的字符数（含符号位），首字符不是数字时返回 0。
+ * @ingroup Command
+ */
+static uint8_t parse_int_dec(const uint8_t *s, uint8_t n, int32_t *out)
+{
+    uint8_t used = 0;
+    bool negative = false;
+    uint32_t magnitude = 0;
+    uint8_t digits;
+
+    if (n > 0 && (s[0] == '-' || s[0] == '+')) {
+        negative = (s[0] == '-');
+        used = 1U;
+    }
+
+    digits = parse_uint_dec(&s[used], (uint8_t)(n - used), &magnitude);
+    if (digits == 0U) {
+        return 0;
+    }
+    used = (uint8_t)(used + digits);
+
+    if (out != NULL) {
+        *out = negative ? -(int32_t)magnitude : (int32_t)magnitude;
+    }
+    return used;
+}
+
+/**
+ * @brief 在命令字符串中查找带符号参数的值
+ * @param cmd 指向命令起始位置（Mxxx 之后）
+ * @param cmd_len 命令剩余长度
+ * @param key 要查找的参数字符（如 'S'）
+ * @param value 输出参数，存储解析到的值
+ * @return bool 是否找到该参数
+ */
+static bool find_int_parameter_value(const uint8_t *cmd, uint8_t cmd_len, char key, int32_t *value)
+{
+    for (uint8_t i = 0; i < cmd_len; i++) {
+        if (cmd[i] == (uint8_t)key) {
+            uint8_t param_idx = (uint8_t)(i + 1);
+            if (param_idx >= cmd_len) return false;
+            if (parse_int_dec(&cmd[param_idx], (uint8_t)(cmd_len - param_idx), value) > 0) {
+                return true;
+            }
+            return false;
+        }
+    }
+    return false;
+}
+
 /**
 * @brief 通用T参数解析函数
 * @details 解析命令中的T参数（定时参数），格式为T<数字>
@@ -390,6 +490,41 @@ static bool __attribute__((unused)) find_parameter_value(const uint8_t *cmd, uin
    return false;
 }

+/**
+ * @brief 发送 "前缀=数值" 格式的成功响应，如 "off=-9"、"xt=12"。
+ * @param prefix 数值前缀字符。
+ * @param value 有符号数值。
+ */
+static void send_resp_prefixed(char prefix, int32_t value)
+{
+    char payload[13];
+    uint8_t n = 0;
+
+    payload[n++] = prefix;
+    payload[n++] = '=';
+    n += int_to_str(value, &payload[n]);
+    send_response(RESP_TYPE_OK, (const uint8_t *)payload, n);
+}
+
+/**
+ * @brief 发送 "d=<距离>;s=<状态>" 格式的测距结果响应。
+ * @param result 测距结果指针。
+ */
+static void send_resp_result(const vl53l4cd_result_t *result)
+{
+    char payload[13];
+    uint8_t n = 0;
+
+    payload[n++] = 'd';
+    payload[n++] = '=';
+    n += uint_to_str(result->distance_mm, &payload[n]);
+    payload[n++] = ';';
+    payload[n++] = 's';
+    payload[n++] = '=';
+    n += uint_to_str(result->range_status, &payload[n]);
+    send_response(RESP_TYPE_OK, (const uint8_t *)payload, n);
+}
+
 /* ============================================================================
 * 命令处理函数
 * ============================================================================ */
@@ -397,18 +532,30 @@ static bool __attribute__((unused)) find_parameter_value(const uint8_t *cmd, uin
 /**
 * @brief 解析并处理完整的命令帧。
 * @details 处理经过协议校验的完整命令帧，支持以下命令格式：
- *          - 无参数命令：M<数字>（如 M1、M2、M10、M201）
- *          - 带参数命令：M<数字>S<参数>（如 M100S123，参数为十进制）
+ *          - 无参数命令：M<数字>（如 M1、M2、M999）
+ *          - 带参数命令：M<数字>S<参数>（如 M101S-9、M102S44，参数为十进制）
 *
 *          支持的命令：
- *          - M1: 开启LED，启用传感器上报
- *          - M2: 关闭LED，禁用传感器上报  
- *          - M100S<value>: 设置PWM值（示例）
+ *          - M1      开启测距上报（LED 亮）
+ *          - M2      关闭测距上报（LED 灭）
+ *          - M3      立即返回最近一帧测距结果 "d=830;s=0"
+ *          - M101    读取当前偏移量 "off=-9"
+ *          - M101S   写入偏移量（mm，立即生效）
+ *          - M102S   执行偏移校准（S=目标实际距离 mm，默认 44）
+ *          - M103    读取当前串扰补偿 "xt=12"
+ *          - M103S   写入串扰补偿（kcps，0..127，立即生效）
+ *          - M104S   执行串扰校准（S=玻璃后方目标距离 mm，默认 500）
+ *          - M105    保存当前偏移+串扰到 Flash
+ *          - M106    从 Flash 读取并应用校准数据
+ *          - M107    擦除 Flash 校准数据并清零偏移/串扰
+ *          - M888    软件重启
+ *          - M999    输出固件版本号
+ *          - M9999   进入 OTA/Bootloader 模式
 *
 * @param frame 指向完整命令帧的缓冲区（从包头0xD5开始）。
 * @param len 命令帧总长度（字节）。
 * @note 函数内部进行帧格式校验，格式错误时自动发送错误响应。
- * @warning 假设输入帧已通过基本协议校验（包头、类型、CRC等）。
+ * @warning M102/M104 校准命令为阻塞执行，耗时约 2 秒，期间主循环上报暂停。
 * @ingroup Command
 */

@@ -440,9 +587,164 @@ void handle_command(const uint8_t *frame, uint8_t len) {
    cmd_index = (uint8_t)(cmd_index + used_base_cmd); // 更新索引到命令后

    switch (base_cmd) {
-            case 1u: // M1
+        /* ==========================================
+         * M1 开启测距上报（LED 亮）
+         * M2 关闭测距上报（LED 灭）
+         * ========================================== */
+        case 1u:
+            set_sensor_report_enabled(true);
+            send_response(RESP_TYPE_OK, s_report_status_ok, sizeof(s_report_status_ok));
+            return;
+
+        case 2u:
+            set_sensor_report_enabled(false);
+            send_response(RESP_TYPE_OK, s_report_status_ok, sizeof(s_report_status_ok));
+            return;
+
+        /* ==========================================
+         * M3 立即返回最近一帧测距结果 "d=830;s=0"
+         * （上报关闭时也可用于查询）
+         * ========================================== */
+        case 3u:
+            if (s_last_result_valid) {
+                send_resp_result(&s_last_result);
+            } else {
+                send_response(RESP_TYPE_ERR, s_report_status_err, sizeof(s_report_status_err));
+            }
+            return;
+
+        /* ==========================================
+         * M101 读取当前偏移量 "off=-9"
+         * M101S<mm> 写入偏移量（立即生效，如 M101S-9）
+         * ========================================== */
+        case 101u: {
+            int32_t offset_param;
+            int16_t offset_mm;
+
+            if (find_int_parameter_value(&cmd[cmd_index], (uint8_t)(cmd_len - cmd_index), 'S', &offset_param)) {
+                if (offset_param < -4095 || offset_param > 4095) {
+                    send_response(RESP_TYPE_PARAM_ERR, s_report_status_err, sizeof(s_report_status_err));
+                    return;
+                }
+                if (!vl53l4cd_set_offset((int16_t)offset_param)) {
+                    send_response(RESP_TYPE_ERR, s_report_status_err, sizeof(s_report_status_err));
+                    return;
+                }
+            }
+            if (vl53l4cd_get_offset(&offset_mm)) {
+                send_resp_prefixed('o', offset_mm);
+                COMMAND_DEBUG("offset %d mm", offset_mm);
+            } else {
+                send_response(RESP_TYPE_ERR, s_report_status_err, sizeof(s_report_status_err));
+            }
+            return;
+        }
+
+        /* ==========================================
+         * M102[S<mm>] 执行偏移校准
+         * 目标放在已知距离，可选 S 指定距离（默认 VL53L4CD_CALIB_TARGET_MM），
+         * 校准需数秒，期间上报暂停，成功返回 "off=-9"
+         * ========================================== */
+        case 102u: {
+            uint32_t target_param;
+            int16_t measured_offset;
+
+            if (!find_parameter_value(&cmd[cmd_index], (uint8_t)(cmd_len - cmd_index), 'S', &target_param)) {
+                target_param = VL53L4CD_CALIB_TARGET_MM;
+            }
+            if (vl53l4cd_calibrate_offset((uint16_t)target_param, VL53L4CD_CALIB_SAMPLES, &measured_offset)) {
+                send_resp_prefixed('o', measured_offset);
+            } else {
+                send_response(RESP_TYPE_ERR, s_report_status_err, sizeof(s_report_status_err));
+            }
+            return;
+        }
+
+        /* ==========================================
+         * M103 读取当前串扰补偿值 "xt=12"
+         * M103S<kcps> 写入串扰补偿值 0..127（立即生效）
+         * ========================================== */
+        case 103u: {
+            uint32_t xtalk_param;
+            uint16_t xtalk_kcps;
+
+            if (find_parameter_value(&cmd[cmd_index], (uint8_t)(cmd_len - cmd_index), 'S', &xtalk_param)) {
+                if (xtalk_param > 127U) {
+                    send_response(RESP_TYPE_PARAM_ERR, s_report_status_err, sizeof(s_report_status_err));
+                    return;
+                }
+                if (!vl53l4cd_set_xtalk((uint16_t)xtalk_param)) {
+                    send_response(RESP_TYPE_ERR, s_report_status_err, sizeof(s_report_status_err));
+                    return;
+                }
+            }
+            if (vl53l4cd_get_xtalk(&xtalk_kcps)) {
+                send_resp_prefixed('x', (int32_t)xtalk_kcps);
+                COMMAND_DEBUG("xtalk %u kcps", xtalk_kcps);
+            } else {
+                send_response(RESP_TYPE_ERR, s_report_status_err, sizeof(s_report_status_err));
+            }
+            return;
+        }
+
+        /* ==========================================
+         * M104[S<mm>] 执行串扰校准（透过玻璃）
+         * 玻璃后方目标放在已知距离（默认 VL53L4CD_XTALK_CALIB_TARGET_MM），
+         * 建议先完成 M102 偏移校准；成功返回 "xt=12"
+         * ========================================== */
+        case 104u: {
+            uint32_t target_param;
+            uint16_t measured_xtalk;
+
+            if (!find_parameter_value(&cmd[cmd_index], (uint8_t)(cmd_len - cmd_index), 'S', &target_param)) {
+                target_param = VL53L4CD_XTALK_CALIB_TARGET_MM;
+            }
+            if (vl53l4cd_calibrate_xtalk((uint16_t)target_param, VL53L4CD_XTALK_CALIB_SAMPLES, &measured_xtalk)) {
+                send_resp_prefixed('x', (int32_t)measured_xtalk);
+            } else {
+                send_response(RESP_TYPE_ERR, s_report_status_err, sizeof(s_report_status_err));
+            }
+            return;
+        }
+
+        /* ==========================================
+         * M105 保存当前偏移+串扰到 Flash（掉电保持）
+         * M106 从 Flash 读取并应用校准数据
+         * M107 擦除 Flash 校准数据并清零当前偏移/串扰
+         * ========================================== */
+        case 105u: {
+            int16_t offset_mm;
+            uint16_t xtalk_kcps;
+
+            if (vl53l4cd_get_offset(&offset_mm) && vl53l4cd_get_xtalk(&xtalk_kcps) &&
+                calib_store_save(offset_mm, xtalk_kcps)) {
                send_response(RESP_TYPE_OK, s_report_status_ok, sizeof(s_report_status_ok));
-                return;
+            } else {
+                send_response(RESP_TYPE_ERR, s_report_status_err, sizeof(s_report_status_err));
+            }
+            return;
+        }
+
+        case 106u: {
+            int16_t offset_mm;
+            uint16_t xtalk_kcps;
+
+            if (calib_store_load(&offset_mm, &xtalk_kcps) &&
+                vl53l4cd_set_offset(offset_mm) && vl53l4cd_set_xtalk(xtalk_kcps)) {
+                send_resp_prefixed('o', offset_mm);
+            } else {
+                send_response(RESP_TYPE_ERR, s_report_status_err, sizeof(s_report_status_err));
+            }
+            return;
+        }
+
+        case 107u:
+            if (calib_store_erase() && vl53l4cd_set_offset(0) && vl53l4cd_set_xtalk(0)) {
+                send_response(RESP_TYPE_OK, s_report_status_ok, sizeof(s_report_status_ok));
+            } else {
+                send_response(RESP_TYPE_ERR, s_report_status_err, sizeof(s_report_status_err));
+            }
+            return;

        /* ==========================================
         * M888 软件重启命令
@@ -22,14 +22,14 @@ are permitted provided that the following conditions are met:

    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 
-WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. 
-IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 
-INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT 
-NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 
-PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 
-WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 
-ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY 
-OF SUCH DAMAGE.
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

 #include "gd32e23x.h"
@@ -38,10 +38,100 @@ OF SUCH DAMAGE.
 #include "uart_ring_buffer.h"
 #include "led.h"
 #include "command.h"
-#include <stdio.h>
 #include "i2c.h"
 #include "board_config.h"
 #include "VL53L4CD_Driver.h"
+#include "calib_store.h"
+
+/*!
+    \brief      向测距串口发送一个字节
+    \param[in]  byte: 待发送字节
+    \param[out] none
+    \retval     none
+*/
+static void uart_put_byte(uint8_t byte)
+{
+    while (usart_flag_get(UART_PHY, USART_FLAG_TBE) == RESET) {}
+    usart_data_transmit(UART_PHY, byte);
+}
+
+/*!
+    \brief      向测距串口发送以 NUL 结尾的字符串
+    \param[in]  s: 字符串指针
+    \param[out] none
+    \retval     none
+*/
+static void uart_write_str(const char *s)
+{
+    while (*s != '\0') {
+        uart_put_byte((uint8_t)*s++);
+    }
+}
+
+/*!
+    \brief      以十进制输出无符号整数（替代 printf，节省 libc 体积）
+    \param[in]  value: 待输出的数值
+    \param[out] none
+    \retval     none
+*/
+static void uart_write_u32(uint32_t value)
+{
+    char temp[10];
+    uint8_t i = 0U;
+
+    if (value == 0U) {
+        uart_put_byte('0');
+        return;
+    }
+    while (value > 0U) {
+        temp[i++] = (char)('0' + (value % 10U));
+        value /= 10U;
+    }
+    while (i > 0U) {
+        uart_put_byte((uint8_t)temp[--i]);
+    }
+}
+
+/*!
+    \brief      以十进制输出有符号整数
+    \param[in]  value: 待输出的数值
+    \param[out] none
+    \retval     none
+*/
+static void uart_write_i32(int32_t value) __attribute__((unused));
+static void uart_write_i32(int32_t value)
+{
+    uint32_t magnitude;
+
+    if (value < 0) {
+        uart_put_byte('-');
+        magnitude = (uint32_t)(-value);
+    } else {
+        magnitude = (uint32_t)value;
+    }
+    uart_write_u32(magnitude);
+}
+
+/*!
+    \brief      输出一帧测距结果，格式与原 printf 版本逐字节一致
+    \param[in]  r: 测距结果指针
+    \param[out] none
+    \retval     none
+*/
+static void report_result(const vl53l4cd_result_t *r)
+{
+    uart_write_str("Distance: ");
+    uart_write_u32(r->distance_mm);
+    uart_write_str(" mm | status: ");
+    uart_write_u32(r->range_status);
+    uart_write_str(" | signal: ");
+    uart_write_u32(r->signal_rate_kcps);
+    uart_write_str(" kcps | ambient: ");
+    uart_write_u32(r->ambient_rate_kcps);
+    uart_write_str(" kcps | sigma: ");
+    uart_write_u32(r->sigma_mm);
+    uart_write_str(" mm\r\n");
+}

 /*!
    \brief      main function
@@ -84,17 +174,37 @@ int main(void)
        if (vl53l4cd_calibrate_offset(VL53L4CD_CALIB_TARGET_MM,
                                      VL53L4CD_CALIB_SAMPLES,
                                      &cal_offset)) {
-            printf("VL53L4CD offset calibrated: %d mm\r\n", cal_offset);
+            uart_write_str("VL53L4CD offset calibrated: ");
+            uart_write_i32(cal_offset);
+            uart_write_str(" mm\r\n");
        } else {
-            printf("VL53L4CD offset calibration failed\r\n");
+            uart_write_str("VL53L4CD offset calibration failed\r\n");
        }
 #else
-        (void)vl53l4cd_set_offset(VL53L4CD_CALIB_OFFSET_MM);
+        int16_t stored_offset = 0;
+        uint16_t stored_xtalk = 0;
+        bool applied = false;
+
+#if VL53L4CD_PERSIST_CALIB == CFG_ENABLE
+        /* Flash 末页有有效校准数据时优先生效（M105 保存 / M107 擦除） */
+        if (calib_store_load(&stored_offset, &stored_xtalk)) {
+            vl53l4cd_set_offset(stored_offset);
+            vl53l4cd_set_xtalk(stored_xtalk);
+            applied = true;
+        }
+#endif
+        if (!applied) {
+            vl53l4cd_set_offset(VL53L4CD_CALIB_OFFSET_MM);
+#if VL53L4CD_GLASS_XTALK == CFG_ENABLE
+            /* 透过玻璃安装：写入编译期固定串扰补偿 */
+            vl53l4cd_set_xtalk(VL53L4CD_GLASS_XTALK_KCPS);
+#endif
+        }
 #endif
        vl53l4cd_start_ranging();
-        printf("VL53L4CD ready\r\n");
+        uart_write_str("VL53L4CD ready\r\n");
    } else {
-        printf("VL53L4CD init failed\r\n");
+        uart_write_str("VL53L4CD init failed\r\n");
    }

    /* ========== Command Testing ========== */
@@ -109,12 +219,10 @@ int main(void)

            vl53l4cd_clear_interrupt();
            if (vl53l4cd_get_result(&result)) {
-                printf("Distance: %u mm | status: %u | signal: %lu kcps | ambient: %lu kcps | sigma: %u mm\r\n",
-                       result.distance_mm,
-                       result.range_status,
-                       (unsigned long)result.signal_rate_kcps,
-                       (unsigned long)result.ambient_rate_kcps,
-                       result.sigma_mm);
+                command_set_last_result(&result);  /* 供 M3 命令查询 */
+                if (get_sensor_report_enabled()) {
+                    report_result(&result);
+                }
            }
            delay_ms(500);
        }
@@ -14,8 +14,8 @@ target_compile_options(${TARGET_NAME} PRIVATE
    "$<$<AND:$<NOT:$<CONFIG:Debug>>,$<COMPILE_LANGUAGE:C>>:-g0>"
    "$<$<AND:$<NOT:$<CONFIG:Debug>>,$<COMPILE_LANGUAGE:CXX>>:-g0>"

-    "$<$<AND:$<CONFIG:Debug>,$<COMPILE_LANGUAGE:C>>:-O0>"
-    "$<$<AND:$<CONFIG:Debug>,$<COMPILE_LANGUAGE:CXX>>:-O0>"
+    "$<$<AND:$<CONFIG:Debug>,$<COMPILE_LANGUAGE:C>>:-Og>"
+    "$<$<AND:$<CONFIG:Debug>,$<COMPILE_LANGUAGE:CXX>>:-Og>"
    "$<$<AND:$<NOT:$<CONFIG:Debug>>,$<COMPILE_LANGUAGE:C>>:-Os>"
    "$<$<AND:$<NOT:$<CONFIG:Debug>>,$<COMPILE_LANGUAGE:CXX>>:-Os>"