逐飞DL1B TOF问题排查记录

逐飞DL1B TOF问题排查记录

背景

本次调试对象是基于 STM32F407 的小车，敌人检测中使用逐飞 DL1B TOF 模块。DL1B 在 IIC 通信和寄存器行为上可以按 VL53L1X 兼容思路处理。

当前传感器设计中，前、左、右三个方向使用 TOF 检测敌人。K230、红外和边缘检测在不同阶段会参与主循环，但为了排查 TOF 问题，曾多次只保留 TOF 敌人检测。

最终需要满足的目标是：

  • TOF 不阻塞红外和主循环。
  • 无目标时不误触发。
  • 遮挡后能够快速响应，不能需要 2 到 3 秒。
  • 可以按方向单独开启 TOF，逐路排查安装和误测问题。

现象

调试过程中主要出现过四类现象。

上电无目标仍然转动

在前、左、右 TOF 同时参与敌人检测时，小车在无遮挡情况下仍然进入敌人动作，表现为启动后左右旋转。

这说明 Enemy 层实际收到了满足条件的 TOF 数据：

1
有效状态帧 + 距离小于阈值 + 连续帧确认

也就是说，问题不是简单的“代码乱进 if”，而是某一路 TOF 被读成了有效近距离目标。

禁用左右 TOF 后前方能正常工作

为了隔离问题，先只开启前方 TOF。前方 TOF 在安装上基本不可能扫到车体结构。测试发现前方逻辑正常，遮挡后能触发，不遮挡不会误转。

这个结果排除了很多系统性问题，例如：

  • IIC 总线完全不可用。
  • TOF 初始化完全失败。
  • Enemy 判断链路完全错误。
  • 多 TOF 地址分配必然错误。

前方 TOF 响应需要 2 到 3 秒

前方 TOF 单路测试正常后，又发现响应时间很慢。遮挡后需要稳定保持 2 到 3 秒才触发动作。

一开始怀疑过 Enemy 层的确认参数，例如：

1
2
#define ENEMY_TOF_UPDATE_PERIOD_MS  80
#define ENEMY_TOF_CONFIRM_COUNT     3

但按这个配置计算，理论响应时间大约是：

1
80ms * 3 = 240ms

不应该达到 2 到 3 秒。因此真正问题不在 Enemy 层确认逻辑，而在 TOF 底层有效帧周期。

只开左 TOF 仍然误转

前方 TOF 修复后，重新打开多路 TOF，又出现乱转。随后只开启左侧 TOF，仍然会误触发。

这说明左侧 TOF 本身读到了有效近距离目标。原因可能来自安装、视场、阈值或左侧模块本身，而不是前方 TOF 的问题。

关键代码结构

Enemy 层的 TOF 检测入口主要在 EnemyControlTask()：

1
2
3
4
enemy_direction = GetTOFEnemyDirection();
if (enemy_direction == ENEMY_NONE) {
    enemy_direction = GetInfraredEnemyDirection();
}

TOF 方向判断由 GetTOFEnemyDirection() 完成：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
static int GetTOFEnemyDirection(void)
{
    EnemyTOF_UpdateTask();

    if (EnemyDistanceValid(enemy_front_mm, enemy_front_update_ms)) {
        return ENEMY_FRONT;
    }
    if (EnemyDistanceValid(enemy_left_mm, enemy_left_update_ms)) {
        return ENEMY_LEFT;
    }
    if (EnemyDistanceValid(enemy_right_mm, enemy_right_update_ms)) {
        return ENEMY_RIGHT;
    }

    return ENEMY_NONE;
}

单路 TOF 更新时，必须连续满足距离窗口：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
static void EnemyTOF_UpdateOne(uint32_t distance_mm,
                               uint32_t *cached_mm,
                               uint32_t *update_ms,
                               uint8_t *confirm_count,
                               uint32_t *candidate_mm,
                               uint32_t *candidate_ms)
{
    if (EnemyTOF_DistanceInWindow(distance_mm)) {
        if (*candidate_mm == TOF_INVALID_DISTANCE_MM ||
            (uwTick - *candidate_ms) > ENEMY_TOF_CONFIRM_GAP_MS) {
            *confirm_count = 1;
        } else {
            if (*confirm_count < ENEMY_TOF_CONFIRM_COUNT) {
                (*confirm_count)++;
            }
        }

        *candidate_mm = distance_mm;
        *candidate_ms = uwTick;

        if (*confirm_count >= ENEMY_TOF_CONFIRM_COUNT) {
            *cached_mm = distance_mm;
            *update_ms = uwTick;
        }
    } else {
        *confirm_count = 0;
        *cached_mm = TOF_INVALID_DISTANCE_MM;
        *candidate_mm = TOF_INVALID_DISTANCE_MM;
    }
}

这里的 ENEMY_TOF_CONFIRM_GAP_MS 和 ENEMY_TOF_CACHE_TIMEOUT_MS 容易被误解为等待时间。实际它们不是主动延时：

  • ENEMY_TOF_CONFIRM_GAP_MS 是两次有效帧之间允许的最大间隔，超过才重新计数。
  • ENEMY_TOF_CACHE_TIMEOUT_MS 是已确认结果的缓存有效期，不会拖慢首次触发。

真正影响首次响应的是：

  • TOF 是否有新帧：TOF_CheckDataReady()
  • 状态码是否有效：TOF_RangeStatusValid()
  • TOF 出帧周期
  • Enemy 层确认帧数

第一次误判：只改 Enemy 采样周期

曾尝试把 Enemy 层采样周期从 80ms 降到 20ms，并把确认帧数从 3 帧降到 2 帧。但测试后出现“完全不动”的现象。

复盘后确认，问题不在于 20ms 本身，而是轮询变快并不会让 TOF 更快产生有效帧。如果底层 TOF 一秒才出一次有效 data ready，Enemy 每 20ms 查询也只是不断读到 not ready 或 invalid status。

因此，压缩响应时间不能只改 Enemy 层参数，必须检查 TOF 底层测距周期。

核心根因：Inter-measurement Period 被配置为约 1000ms

查阅 VL53L1X Ultra Lite Driver 文档后，确认 VL53L1X/DL1B 的测距频率主要由两个参数决定：

  • Timing budget
  • Inter-measurement period

其中 inter-measurement period 决定连续测距模式下两次测距之间的间隔。ST 的 ULD 中通常通过 VL53L1X_SetInterMeasurementInMs() 配置。

当前 TOF.c 的配置数组中，相关字节原本是：

1
2
3
4
0x006C: 0x00
0x006D: 0x00
0x006E: 0x99
0x006F: 0xC8

也就是：

1
0x000099C8

该值对应的测距周期接近 1000ms。由于 Enemy 层当时要求 3 帧确认，所以表现就是：

1
约 1s/帧 * 3 帧 = 约 2 到 3 秒触发

这和现场现象完全一致。

关键修改：将测距周期压缩到约 100ms

将 DL1B_ConfigFile 中 inter-measurement period 相关字节改为 ST ULD 常见的 100ms 配置：

1
2
3
4
5
// 修改前
0x38, 0x00, 0x00, 0x00, 0x00, 0x99, 0xC8, 0x00,

// 修改后
0x38, 0x00, 0x00, 0x00, 0x00, 0x0F, 0x89, 0x00,

对应寄存器值变为：

1
2
3
4
0x006C: 0x00
0x006D: 0x00
0x006E: 0x0F
0x006F: 0x89

修改后，TOF 有效帧周期从约 1000ms 降到约 100ms。

连续帧确认改为 2 帧

在前方 TOF 单路测试稳定后，将确认帧数从 3 帧改为 2 帧：

1
#define ENEMY_TOF_CONFIRM_COUNT     2

这样理论触发时间约为：

1
100ms * 2 = 约 200ms

这比原来的 2 到 3 秒明显更符合比赛中的快速检测需求。

状态码过滤的取舍

调试中曾短暂放开 range_status 过滤，只根据距离判断是否有效。这样会让遮挡更容易触发，但也会带来严重副作用：无遮挡时的错误距离值可能被当成敌人。

最终恢复状态码过滤：

1
2
3
4
5
static uint8_t TOF_RangeStatusValid(uint8_t range_status)
{
    range_status &= 0x1F;
    return range_status == 0 || range_status == 9;
}

这里兼容 0 和 9 两种有效状态，是为了适配 VL53L1X/DL1B 可能出现的驱动转换状态和原始寄存器状态。

结论是：

  • 调试“有没有读到距离”时，可以临时放宽状态码。
  • 正式检测敌人时，必须保留状态码过滤，否则会增加误触发。

动作后的阻塞延时问题

早期为了看清动作，曾在触发后加入长延时：

1
2
3
4
Speed(30, -30);
Delay_ms(250);
Speed(0, 0);
Delay_ms(5000);

这个写法会让主循环完全停在动作里，TOF、红外、K230 都不再更新。调试时会造成误判：看起来像检测慢或检测失效。

后续去掉了动作后的额外阻塞停顿，仅保留动作本身：

1
2
3
4
5
Speed(30, -30);
Delay_ms(250);
Speed(0, 0);
EnemyTOF_AfterAction();
return 1;

这能保证动作完成后尽快回到主循环。

单路排查策略

为了排查多路 TOF 误触发，使用宏逐路开启：

1
2
3
#define ENEMY_TOF_FRONT_ENABLE      1
#define ENEMY_TOF_LEFT_ENABLE       0
#define ENEMY_TOF_RIGHT_ENABLE      0

前方 TOF 单独开启时，测试正常，说明核心读取链路和测距周期配置已经正确。

之后打开三路：

1
2
3
#define ENEMY_TOF_FRONT_ENABLE      1
#define ENEMY_TOF_LEFT_ENABLE       1
#define ENEMY_TOF_RIGHT_ENABLE      1

如果出现乱转，再逐路排查：

1
2
3
#define ENEMY_TOF_FRONT_ENABLE      0
#define ENEMY_TOF_LEFT_ENABLE       1
#define ENEMY_TOF_RIGHT_ENABLE      0

只开左侧仍然误触发，说明左侧 TOF 当前确实被读成了有效近距离目标。

左侧 TOF 误触发的可能原因

只开左 TOF 仍然转动时，不应优先怀疑多 TOF 初始化，因为前方 TOF 在同样初始化逻辑下已经验证正常。

更可能的原因包括：

视场扫到车体结构

TOF 不是理想单线激光，存在视场角。左侧安装时可能扫到轮子、挡板、车壳、线束或固定件。

安装角度朝下

如果左侧 TOF 有轻微俯角，可能测到地面、台面边缘或赛台结构。只要距离落入阈值范围，就会被判断为敌人。

侧向阈值过大

当前全方向使用：

1
#define ENEMY_DISTANCE_THRESHOLD_MM 600

前方 600mm 比较合理，但左/右侧 600mm 可能过大。侧向 TOF 更适合单独设置较小阈值，例如 300mm 或 400mm。

模块或镜片问题

镜片灰尘、保护膜、热熔胶、反光线材都会造成近距离有效回波。此时状态码可能仍然有效，代码无法区分“敌人”和“结构反射”。

参考资料

Embedded > STM32 Robot > Sensor
#STM32 #TOF #DL1B #VL53L1X #IIC #Debug
逐飞DL1B TOF问题排查记录
http://ruak.github.io/2026/05/24/逐飞DL1B-TOF问题排查记录/
作者
HUANGDAN
发布于
2026年5月24日
许可协议