📚 课程目录第3章温度补偿算法实现

3. 温度补偿算法实现

任务说明

本案例实现ToF（飞行时间）深度传感器的温度补偿算法。ToF传感器因激光二极管与CMOS电路的温度漂移，会导致深度测量值随温度变化产生系统性偏差（典型漂移率约0.5~2 mm/°C）。目标是根据传感器内置温度传感器的读数，对原始深度测量值进行实时补偿，使补偿后的深度值在不同温度下保持稳定。

温度补偿模型

采用分段线性插值补偿模型：在出厂标定阶段，记录若干温度点下的深度偏移量，形成温度-偏移查找表；运行时根据当前温度在查找表中进行线性插值，得到补偿偏移量并从原始深度中减去。该模型计算量小、无需浮点协处理器，适合嵌入式实时处理。

代码实现


#include <stdint.h>

/* 温度-偏移查找表（出厂标定数据）
 * 温度单位：°C；偏移单位：mm
 * 偏移 = 原始深度 - 真实深度（即需要从原始深度中减去的值）
 */
typedef struct {
    float temp_c;    /* 温度采样点 */
    float offset_mm; /* 该温度下的深度偏移量 */
} temp_calib_t;

/* 标定查找表：5个温度点（示例数据，实际需按传感器实测标定） */
static const temp_calib_t calib_table[] = {
    {-10.0f, -12.5f},
    { 25.0f,   0.0f},
    { 50.0f,   8.3f},
    { 70.0f,  15.1f},
    { 85.0f,  21.7f}
};

#define CALIB_POINTS  (sizeof(calib_table) / sizeof(calib_table[0]))

/* 温度传感器原始值转摄氏温度
 * 假设：12-bit ADC，分辨率0.0625°C/LSB，-40°C偏移
 */
static float raw_to_celsius(uint16_t raw_adc)
{
    return (float)raw_adc * 0.0625f - 40.0f;
}

/* 分段线性插值：根据当前温度查表得到补偿偏移量 */
static float interp_offset(float temp_c)
{
    uint8_t i;

    /* 低于最低温度点：使用第一个区间斜率外推 */
    if (temp_c <= calib_table[0].temp_c) {
        float slope = (calib_table[1].offset_mm - calib_table[0].offset_mm) /
                      (calib_table[1].temp_c - calib_table[0].temp_c);
        return calib_table[0].offset_mm + slope * (temp_c - calib_table[0].temp_c);
    }

    /* 高于最高温度点：使用最后一个区间斜率外推 */
    if (temp_c >= calib_table[CALIB_POINTS - 1].temp_c) {
        float slope = (calib_table[CALIB_POINTS - 1].offset_mm -
                       calib_table[CALIB_POINTS - 2].offset_mm) /
                      (calib_table[CALIB_POINTS - 1].temp_c -
                       calib_table[CALIB_POINTS - 2].temp_c);
        return calib_table[CALIB_POINTS - 1].offset_mm +
               slope * (temp_c - calib_table[CALIB_POINTS - 1].temp_c);
    }

    /* 正常区间内线性插值 */
    for (i = 0; i < CALIB_POINTS - 1; i++) {
        if (temp_c >= calib_table[i].temp_c && temp_c <= calib_table[i + 1].temp_c) {
            float ratio = (temp_c - calib_table[i].temp_c) /
                          (calib_table[i + 1].temp_c - calib_table[i].temp_c);
            return calib_table[i].offset_mm +
                   ratio * (calib_table[i + 1].offset_mm - calib_table[i].offset_mm);
        }
    }

    return 0.0f; /* 不应到达 */
}

/* 温度补偿主函数：输入原始深度与ADC温度值，输出补偿后深度 */
float tof_temp_compensate(uint16_t raw_depth_mm, uint16_t temp_adc_raw)
{
    float temp_c = raw_to_celsius(temp_adc_raw);
    float offset = interp_offset(temp_c);
    float compensated = (float)raw_depth_mm - offset;

    /* 防止补偿后出现负值 */
    if (compensated < 0.0f) {
        compensated = 0.0f;
    }

    return compensated;
}

调用示例


/* 示例：25°C时ADC原始值 = (25+40)/0.0625 = 1040 */
uint16_t adc_25c = 1040;
uint16_t raw_depth = 1000;  /* 原始深度 1000mm */

float compensated = tof_temp_compensate(raw_depth, adc_25c);
/* 预期输出：compensated = 1000.0mm（25°C时偏移为0） */

/* 示例：70°C时ADC原始值 = (70+40)/0.0625 = 1760 */
uint16_t adc_70c = 1760;
compensated = tof_temp_compensate(raw_depth, adc_70c);
/* 预期输出：compensated = 1000 - 15.1 = 984.9mm */

验证方法

单元测试路径：将上述代码编译为独立测试程序（如gcc），在main函数中调用tof_temp_compensate()并打印结果。验证以下判定条件：

输入温度ADC值对应25°C时，输出深度等于原始深度（偏移为0）；
输入温度ADC值对应70°C时，输出深度 = 原始深度 - 15.1mm；
输入温度ADC值对应-10°C时，输出深度 = 原始深度 + 12.5mm（偏移为负，补偿后增大）；
输入温度ADC值对应100°C（超出表范围），输出深度按85°C区间斜率外推，不产生异常值。

系统级验证：将传感器置于温控箱中，在-10°C至85°C范围内每5°C记录一次补偿前后的深度测量值（目标靶距固定为1000mm）。判定标准：补偿后深度值在全温区内的最大偏差≤±3mm，且无跳变。