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

Src/VL53L4CD_Driver.c

@@ -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;