spdis
1324 lines
61 KiB
C++
1324 lines
61 KiB
C++
/*
|
||
* camera.cpp — 每帧感知-决策-执行流水线
|
||
*
|
||
* 入口: CameraHandler() 由 main.cpp 主循环每帧调用一次。
|
||
*
|
||
* 10 步流水线:
|
||
* 1. capture_frame MJPEG 取帧 → 1/4 解码 160×120 BGR
|
||
* 2. save_image_if_requested debug 存图
|
||
* 3. image_main 视觉巡线 → left/right/mid_line[]
|
||
* 4. lidar_avoid_process VL53L0X 激光避障 → 中线变形 (已禁用)
|
||
* 5. zebra/tl/cone 场景识别状态机 (用上一帧 g_boxes[])
|
||
* 6. steering_update 舵机比例控制
|
||
* 7. motor_update 电机开环 + 编码器刹车 + 弯道减速
|
||
* 8. run_model_inference Mild v12 每2帧推理 → g_boxes[] (放最后, 不阻塞舵机)
|
||
* 9. lcd_render LCD RGB565 渲染
|
||
* 10. fps_log 分步耗时日志
|
||
*
|
||
* 硬件依赖:
|
||
* - 摄像头: /dev/video0 (640×480 MJPEG, V4L2)
|
||
* - LCD: /dev/fb0 (RGB565, mmap)
|
||
* - 舵机: pwmchip1/pwm0 (3ms 周期, 1.2~1.8ms 占空比)
|
||
* - 语音: /dev/i2c-2 从地址 0x34 (斑马线播报)
|
||
* - 激光: VL53L0X via /dev/stmvl53l0x_ranging
|
||
*/
|
||
#include "camera.h"
|
||
#include "model_v10.hpp"
|
||
#include "vl53l0x.h"
|
||
|
||
#include <fcntl.h>
|
||
#include <unistd.h>
|
||
#include <sys/ioctl.h>
|
||
#include <sys/mman.h>
|
||
#include <linux/i2c-dev.h>
|
||
#include <linux/i2c.h>
|
||
#include <linux/fb.h>
|
||
#include <algorithm>
|
||
#include <chrono>
|
||
#include <thread>
|
||
#include <cstring>
|
||
#include <sstream>
|
||
#include <iomanip>
|
||
#include <malloc.h>
|
||
|
||
// ═══════════════════════════════════════════════════════════
|
||
// 模型检测类别索引
|
||
// Mild v12 输出 4 类 + 背景, 对应 g_thresh[] 阈值
|
||
// ═══════════════════════════════════════════════════════════
|
||
enum { MD_CONE = 0, MD_RED = 1, MD_GREEN = 2, MD_ZEBRA = 3 };
|
||
|
||
// ═══════════════════════════════════════════════════════════
|
||
// 摄像头 & 帧缓冲全局变量
|
||
// ═══════════════════════════════════════════════════════════
|
||
cv::VideoCapture cap; // V4L2 摄像头句柄
|
||
cv::Mat raw_mat, decoded_frame; // raw_mat=V4L2原始帧, decoded_frame=MJPEG解码结果
|
||
static int g_decode_mode = -1; // -1=未检测, 1=MJPEG 1/4解码, 0=全尺寸回退
|
||
|
||
int screenWidth, screenHeight; // LCD 屏幕物理分辨率 (从 /dev/fb0 读取)
|
||
int newWidth, newHeight; // 自适应显示分辨率 = 摄像头分辨率 × 缩放因子
|
||
int fb; // /dev/fb0 文件描述符
|
||
uint16_t *fb_buffer; // LCD 帧缓冲 mmap 地址
|
||
static size_t g_fb_size = 0; // 帧缓冲字节数
|
||
static cv::Mat g_stop_image; // 斑马线停车提示图片
|
||
PwmController servo(1, 0); // 舵机: pwmchip1/pwm0
|
||
|
||
// 巡线空间到显示空间的缩放因子
|
||
// line_tracking_width = newWidth / calc_scale
|
||
// line_tracking_height = newHeight / calc_scale
|
||
static constexpr int calc_scale = 2;
|
||
|
||
// ═══════════════════════════════════════════════════════════
|
||
// 模型推理相关
|
||
//
|
||
// g_thresh: 四类各自置信度阈值 (锥桶/红灯/绿灯/斑马线)
|
||
// g_boxes: 检测结果缓冲区 (最多 16 个框)
|
||
// g_box_count: 当前帧有效检测框数
|
||
// ═══════════════════════════════════════════════════════════
|
||
static float g_thresh[4] = {0.80f, 0.70f, 0.80f, 0.85f};
|
||
static DetectBoxV10 g_boxes[16];
|
||
static int g_box_count = 0;
|
||
|
||
// ═══════════════════════════════════════════════════════════
|
||
// 斑马线状态机
|
||
//
|
||
// 状态: Z_NORMAL → Z_STOP(4s) → Z_COOLDOWN(5s) → Z_NORMAL
|
||
// 触发条件: 连续 5 帧 + 起源于远(cy≤50) + 当前近(cy>zebrasee)
|
||
//
|
||
// g_zc_frames: 去抖连续帧计数 (未检测时每帧 -2 衰减)
|
||
// g_zc_min_cy: 本轮追踪中斑马线出现时的最小 cy (越小=越远)
|
||
// g_ztime: 进入 Z_STOP / Z_COOLDOWN 的时间戳
|
||
// ═══════════════════════════════════════════════════════════
|
||
static constexpr int ZEBRA_MIN_FRAMES = 3; // 连续确认帧数
|
||
static constexpr int ZEBRA_FAR_CY = 80; // 远处 cy 阈值: 斑马线必须曾出现在 cy≤80
|
||
static int g_zc_frames = 0;
|
||
static int g_zc_min_cy = 120; // 初始设远 (120=从未检测到)
|
||
|
||
enum ZState { Z_NORMAL, Z_STOP, Z_COOLDOWN };
|
||
static ZState g_zstate = Z_NORMAL;
|
||
static time_t g_ztime = 0;
|
||
static bool g_bangbang_used = false; // 本轮 Z_COOLDOWN 打满机会是否已用
|
||
static bool g_bangbang_active = false; // 是否处于打满->等回正序列中
|
||
static int g_bangbang_dir = 0; // 锁定方向: +1=右满, -1=左满
|
||
|
||
// ═══════════════════════════════════════════════════════════
|
||
// 红绿灯状态机
|
||
//
|
||
// 状态: TL_NORMAL → TL_STOP → TL_WAIT_GREEN → TL_NORMAL
|
||
// 红灯≥3帧确认停车, 红灯消失→等绿灯, 绿灯≥3帧确认通行
|
||
//
|
||
// g_tl_red_frames: 红灯连续帧计数 (未检测时 -1 衰减)
|
||
// g_tl_green_frames: 绿灯连续帧计数
|
||
// ═══════════════════════════════════════════════════════════
|
||
enum TLState { TL_NORMAL, TL_STOP, TL_WAIT_GREEN };
|
||
static TLState g_tl_state = TL_NORMAL;
|
||
static int g_tl_red_frames = 0;
|
||
static int g_tl_green_frames = 0;
|
||
|
||
// ═══════════════════════════════════════════════════════════
|
||
// 锥桶检测 & 中线变形状态
|
||
//
|
||
// 生命周期: 确认(avoid) → 保持衰减(hold) → 回弹(return) → 空闲
|
||
//
|
||
// g_cone_frames: 去抖连续帧计数 (未检测时 -1 衰减)
|
||
// g_cone_last_row/col: 上帧锥桶位置 (用于位置容差判定)
|
||
// g_cone_confirmed: 是否通过去抖确认
|
||
// g_cone_hold_ctr: 保持衰减计数器 (确认时=1, 每帧++)
|
||
// g_cone_hold_src_row/col: 保持阶段使用的锥桶位置 (确认时冻结)
|
||
// g_cone_return_ctr: 回弹计数器 (保持结束后=1, 每帧++)
|
||
// g_cone_return_dir: 回弹方向 (+1=推右, -1=推左, 与避让方向相反)
|
||
// ═══════════════════════════════════════════════════════════
|
||
static int g_cone_frames = 0;
|
||
static int g_cone_last_row = -1;
|
||
static int g_cone_last_col = -1;
|
||
static bool g_cone_confirmed = false;
|
||
static int g_cone_hold_ctr = 0;
|
||
static int g_cone_hold_src_row = -1;
|
||
static int g_cone_hold_src_col = -1;
|
||
static int g_cone_return_ctr = 0;
|
||
static double g_cone_return_dir = 0;
|
||
|
||
// ═══════════════════════════════════════════════════════════
|
||
// 激光雷达挡板避障
|
||
//
|
||
// 每 2 帧非阻塞读取 VL53L0X 距离, 纯距离触发(不检查视觉边线)。
|
||
// 绕行方向固定朝右 (g_lidar_hold_dir = 1.0)。
|
||
//
|
||
// g_lidar_sensor: VL53L0X 驱动实例
|
||
// g_lidar_ok: 硬件是否初始化成功
|
||
// g_lidar_frames: 去抖连续帧计数
|
||
// g_lidar_confirmed: 是否通过去抖确认
|
||
// g_lidar_obstacle_row: 障碍物对应的巡线空间行号
|
||
// g_lidar_hold_src_row: 保持阶段使用的障碍物行号 (确认时冻结)
|
||
// g_lidar_hold_ctr: 保持衰减计数器
|
||
// g_lidar_hold_dir: 绕行方向 (固定 1.0 = 朝右)
|
||
// ═══════════════════════════════════════════════════════════
|
||
static VL53L0X g_lidar_sensor;
|
||
static bool g_lidar_ok = false;
|
||
static int g_lidar_frames = 0;
|
||
static bool g_lidar_confirmed = false;
|
||
static int g_lidar_obstacle_row = -1;
|
||
static int g_lidar_hold_src_row = -1;
|
||
static int g_lidar_hold_ctr = 0;
|
||
static double g_lidar_hold_dir = 0;
|
||
|
||
// ═══════════════════════════════════════════════════════════
|
||
// LCD 开关 & 舵机偏差
|
||
// ═══════════════════════════════════════════════════════════
|
||
static bool g_lcd_on = true; // LCD 渲染开关 (每 10 帧从 g_cfg.showImg 刷新)
|
||
double g_steer_deviation = 0; // 归一化舵机偏差 [-1, 1], 供 motor_update 弯道减速用
|
||
|
||
|
||
// ═══════════════════════════════════════════════════════════
|
||
// I2C 音频 — 斑马线语音播报
|
||
//
|
||
// 硬件: I2C-2 总线, 从地址 0x34, SMBus 块写
|
||
// 协议: command=0x6E, data={0xFF, 0x10} → 触发语音
|
||
// 调用时机: 斑马线状态机进入 Z_STOP 时
|
||
// ═══════════════════════════════════════════════════════════
|
||
static int i2c_audio_fd = -1;
|
||
|
||
// 打开 I2C 音频设备 (懒初始化, 只打开一次)
|
||
static bool i2c_audio_open()
|
||
{
|
||
if (i2c_audio_fd >= 0) return true; // 已打开
|
||
i2c_audio_fd = open("/dev/i2c-2", O_RDWR);
|
||
if (i2c_audio_fd < 0) {
|
||
fprintf(stderr, "[ZEBRA] 无法打开 I2C-2: %s\n", strerror(errno));
|
||
return false;
|
||
}
|
||
if (ioctl(i2c_audio_fd, I2C_SLAVE, 0x34) < 0) {
|
||
fprintf(stderr, "[ZEBRA] 无法设置 I2C 从地址 0x34: %s\n", strerror(errno));
|
||
close(i2c_audio_fd); i2c_audio_fd = -1;
|
||
return false;
|
||
}
|
||
return true;
|
||
}
|
||
|
||
// 触发斑马线语音播报: SMBus I2C 块写 → 音频模块
|
||
static void play_zebra_audio()
|
||
{
|
||
if (!i2c_audio_open()) { printf("[ZEBRA] 语音失败: I2C 未打开\n"); return; }
|
||
ioctl(i2c_audio_fd, I2C_SLAVE, 0x34); // 确保从地址
|
||
|
||
// SMBus I2C 块写: command=0x6E, data=[0xFF, 0x10]
|
||
union i2c_smbus_data d;
|
||
struct i2c_smbus_ioctl_data a;
|
||
__u8 v[] = {0xFF, 0x10};
|
||
d.block[0] = 2; d.block[1] = v[0]; d.block[2] = v[1];
|
||
a.read_write = I2C_SMBUS_WRITE;
|
||
a.command = 0x6E;
|
||
a.size = I2C_SMBUS_I2C_BLOCK_DATA;
|
||
a.data = &d;
|
||
|
||
if (ioctl(i2c_audio_fd, I2C_SMBUS, &a) < 0)
|
||
printf("[ZEBRA] 语音失败: %s\n", strerror(errno));
|
||
else
|
||
printf("[ZEBRA] 语音播报已触发\n");
|
||
}
|
||
|
||
|
||
// ═══════════════════════════════════════════════════════════
|
||
// CameraInit — 系统初始化
|
||
//
|
||
// 初始化顺序:
|
||
// 1. 舵机 → 中位
|
||
// 2. LCD 帧缓冲 → mmap
|
||
// 3. 摄像头 → 640×480 MJPEG, CONVERT_RGB=0
|
||
// 4. 自适应分辨率计算 → newWidth/newHeight, line_tracking_*
|
||
// 5. Mild v12 模型加载
|
||
// 6. I2C 音频设备
|
||
// 7. VL53L0X 激光 (可选, lidar_enable 控制)
|
||
// ═══════════════════════════════════════════════════════════
|
||
int CameraInit(int camera_id)
|
||
{
|
||
// ── 1. 舵机初始化: 3ms 周期, 1.5ms 中位 ──
|
||
servo.setPeriod(3000000); // 3,000,000 ns = 3ms
|
||
servo.setDutyCycle(1500000); // 1,500,000 ns = 1.5ms (中位)
|
||
servo.enable();
|
||
|
||
// ── 2. LCD 帧缓冲初始化 ──
|
||
fb = open("/dev/fb0", O_RDWR);
|
||
if (fb == -1) { std::cerr << "无法打开帧缓冲区设备" << std::endl; return -1; }
|
||
|
||
struct fb_var_screeninfo vinfo;
|
||
if (ioctl(fb, FBIOGET_VSCREENINFO, &vinfo) == -1) {
|
||
std::cerr << "无法获取帧缓冲区信息" << std::endl; close(fb); return -1;
|
||
}
|
||
screenWidth = vinfo.xres; screenHeight = vinfo.yres;
|
||
g_fb_size = vinfo.yres_virtual * vinfo.xres_virtual * vinfo.bits_per_pixel / 8;
|
||
fb_buffer = (uint16_t *)mmap(NULL, g_fb_size, PROT_READ | PROT_WRITE, MAP_SHARED, fb, 0);
|
||
if (fb_buffer == MAP_FAILED) {
|
||
std::cerr << "无法映射帧缓冲区到内存" << std::endl; close(fb); return -1;
|
||
}
|
||
|
||
// 加载斑马线停车提示图片（与 LCD 同分辨率 160x128）
|
||
g_stop_image = cv::imread("./pedestrian_stop.png");
|
||
if (!g_stop_image.empty())
|
||
printf("[LCD] 已加载停车提示图片: %dx%d\n", g_stop_image.cols, g_stop_image.rows);
|
||
else
|
||
printf("[LCD] 警告: 未找到 ./pedestrian_stop.png\n");
|
||
|
||
// ── 3. 摄像头初始化 ──
|
||
// CONVERT_RGB=0: 让 V4L2 返回原始 MJPEG 字节流 (单通道)
|
||
// 后续用 imdecode(IMREAD_REDUCED_COLOR_4) 做 1/4 解码 → 160×120
|
||
cap.open(0, cv::CAP_V4L2);
|
||
if (!cap.isOpened()) cap.open(0); // V4L2 失败则尝试默认后端
|
||
cap.set(cv::CAP_PROP_FOURCC, cv::VideoWriter::fourcc('M', 'J', 'P', 'G'));
|
||
cap.set(cv::CAP_PROP_FRAME_WIDTH, 640);
|
||
cap.set(cv::CAP_PROP_FRAME_HEIGHT, 480);
|
||
cap.set(cv::CAP_PROP_CONVERT_RGB, 0); // 关键: 不让 V4L2 转 RGB
|
||
if (!cap.isOpened()) {
|
||
printf("无法打开摄像头\n");
|
||
munmap(fb_buffer, g_fb_size); close(fb); return -1;
|
||
}
|
||
|
||
int camW = cap.get(cv::CAP_PROP_FRAME_WIDTH);
|
||
int camH = cap.get(cv::CAP_PROP_FRAME_HEIGHT);
|
||
printf("摄像头分辨率: %d x %d\n", camW, camH);
|
||
|
||
// ── 4. 自适应分辨率: 在 LCD 屏幕上等比缩放 ──
|
||
// newWidth × newHeight = 摄像头画面等比放入 LCD 后的像素尺寸
|
||
double ws = (double)screenWidth / camW;
|
||
double hs = (double)screenHeight / camH;
|
||
double s = std::min(ws, hs); // 取小边缩放因子, 保证不超出屏幕
|
||
newWidth = (int)(camW * s);
|
||
newHeight = (int)(camH * s);
|
||
printf("自适应分辨率: %d x %d\n", newWidth, newHeight);
|
||
|
||
double fps = cap.get(cv::CAP_PROP_FPS);
|
||
printf("Camera fps: %f\n", fps);
|
||
|
||
// 巡线空间 = 显示空间 / calc_scale (典型 80×60)
|
||
line_tracking_width = newWidth / calc_scale;
|
||
line_tracking_height = newHeight / calc_scale;
|
||
|
||
// ── 5. 加载 Mild v12 模型 ──
|
||
if (!model_v10_init("./mild_v12.bin"))
|
||
printf("[MODEL] 警告: 模型加载失败\n");
|
||
else
|
||
printf("[MODEL] Mild v12 模型已加载\n");
|
||
|
||
// ── 6. I2C 音频设备 ──
|
||
i2c_audio_open();
|
||
|
||
// ── 7. VL53L0X 激光测距 (可选) ──
|
||
// 仅在 lidar_enable=1 时初始化, 避免驱动内核定时器拖慢 CPU
|
||
// if (g_cfg.lidar_enable) {
|
||
// g_lidar_ok = g_lidar_sensor.init();
|
||
// if (g_lidar_ok) {
|
||
// printf("[LIDAR] VL53L0X 已初始化\n");
|
||
// g_lidar_sensor.startMeasure(); // 启动首次后台测量 (~20ms)
|
||
// } else
|
||
// printf("[LIDAR] VL53L0X 未连接, 避障禁用\n");
|
||
// } else {
|
||
// printf("[LIDAR] 已禁用\n");
|
||
// }
|
||
|
||
return 0;
|
||
}
|
||
|
||
// 前置声明: LCD 辅助函数定义在后方
|
||
static void lcd_clear_screen();
|
||
|
||
// ═══════════════════════════════════════════════════════════
|
||
// cameraDeInit — 释放所有资源
|
||
// ═══════════════════════════════════════════════════════════
|
||
void cameraDeInit(void)
|
||
{
|
||
cap.release(); // 释放摄像头
|
||
lcd_clear_screen(); // 程序退出前清屏
|
||
if (fb_buffer && g_fb_size > 0) {
|
||
munmap(fb_buffer, g_fb_size); // 解除 LCD mmap
|
||
}
|
||
close(fb); // 关闭 LCD fd
|
||
if (i2c_audio_fd >= 0) close(i2c_audio_fd); // 关闭 I2C 音频
|
||
if (g_lidar_ok) g_lidar_sensor.stop(); // 停止 VL53L0X
|
||
model_v10_deinit(); // 释放模型内存
|
||
}
|
||
|
||
|
||
// ═══════════════════════════════════════════════════════════
|
||
// Step 2: 图像保存 (debug 模式)
|
||
//
|
||
// 条件: debug=1 且 ./saveImg 文件内容为 1
|
||
// 保存路径: ./image/image_XXXXX.jpg (自增编号)
|
||
// ═══════════════════════════════════════════════════════════
|
||
static int saved_frame_count = 0;
|
||
|
||
static void save_image_if_requested()
|
||
{
|
||
if (!g_cfg.debug) return;
|
||
if (!readFlag(saveImg_file)) return; // 每帧读 ./saveImg 文件
|
||
std::ostringstream filename;
|
||
filename << "./image/image_" << std::setw(5) << std::setfill('0') << saved_frame_count << ".jpg";
|
||
if (cv::imwrite(filename.str(), raw_frame)) {
|
||
printf("图像%d已保存\n", saved_frame_count);
|
||
saved_frame_count++;
|
||
}
|
||
}
|
||
|
||
|
||
// ═══════════════════════════════════════════════════════════
|
||
// Step 1: 帧捕获 + MJPEG 解码
|
||
//
|
||
// 正常模式 (CONVERT_RGB=0 生效):
|
||
// V4L2 返回单通道 MJPEG → imdecode(IMREAD_REDUCED_COLOR_4)
|
||
// → 1/4 解码 → raw_frame = 160×120 BGR
|
||
//
|
||
// 回退模式 (CONVERT_RGB=0 未生效):
|
||
// V4L2 返回三通道 BGR → 直用 → raw_frame = 640×480 BGR
|
||
//
|
||
// 首帧自动检测: raw_mat.channels()==1 → 正常模式, 否则回退
|
||
// ═══════════════════════════════════════════════════════════
|
||
static int capture_frame(struct timespec *t0, struct timespec *t1)
|
||
{
|
||
clock_gettime(CLOCK_MONOTONIC, t0); // 计时: 取帧开始
|
||
cap.read(raw_mat);
|
||
clock_gettime(CLOCK_MONOTONIC, t1); // 计时: 取帧结束
|
||
if (raw_mat.empty()) return -1;
|
||
|
||
// 首帧自动检测解码模式
|
||
if (g_decode_mode == -1) {
|
||
g_decode_mode = (raw_mat.channels() == 1) ? 1 : 0;
|
||
printf("[CAM] CONVERT_RGB=0 %s, mode=%d\n",
|
||
g_decode_mode == 1 ? "生效->1/4解码" : "未生效->全解码回退",
|
||
g_decode_mode);
|
||
}
|
||
|
||
if (g_decode_mode == 1) {
|
||
// 正常: MJPEG 字节流 → 1/4 解码 → 160×120 BGR
|
||
cv::imdecode(raw_mat, cv::IMREAD_REDUCED_COLOR_4, &decoded_frame);
|
||
if (decoded_frame.empty()) return -1;
|
||
raw_frame = decoded_frame;
|
||
} else {
|
||
// 回退: 直接使用 640×480 BGR (性能较差)
|
||
raw_frame = raw_mat;
|
||
}
|
||
return 0;
|
||
}
|
||
|
||
// ═══════════════════════════════════════════════════════════
|
||
// Step 5: 模型推理 (每 2 帧一次)
|
||
//
|
||
// 输入: raw_frame (160×120 BGR)
|
||
// 输出: g_boxes[0..g_box_count-1], 每个框含 cx,cy,w,h,cls,conf
|
||
// 阈值: g_thresh = {0.80, 0.75, 0.80, 0.87} (锥桶/红灯/绿灯/斑马线)
|
||
// 跳帧: 每 2 帧推理一次, 奇数帧保留上次结果
|
||
// ═══════════════════════════════════════════════════════════
|
||
static void run_model_inference()
|
||
{
|
||
static int infer_skip = 0;
|
||
if (++infer_skip < 2) return; // 跳帧
|
||
infer_skip = 0;
|
||
g_box_count = 0;
|
||
if (model_v10_ready()) {
|
||
g_box_count = model_v10_detect(raw_frame.data, raw_frame.cols, raw_frame.rows,
|
||
g_boxes, 16, g_thresh);
|
||
}
|
||
}
|
||
|
||
|
||
// ═══════════════════════════════════════════════════════════
|
||
// Step 6a: 斑马线检测 + 状态机
|
||
//
|
||
// 检测逻辑:
|
||
// 遍历 g_boxes 找 cls=3 (斑马线), 只取第一个 (break)
|
||
// 记录 cy, 更新 g_zc_min_cy (本轮最远出现位置)
|
||
// 未检测到时 g_zc_frames 每帧 -2 衰减, 归零则重置 min_cy
|
||
//
|
||
// 触发条件 (仅 Z_NORMAL 时):
|
||
// 1. zebra_near: 当前 cy > g_cfg.zebrasee (足够近)
|
||
// 2. g_zc_frames >= 5 (连续确认)
|
||
// 3. g_zc_min_cy <= 50 (曾从远处检测到, 排除误触发)
|
||
// → 播报语音 → Z_STOP
|
||
//
|
||
// 状态转移:
|
||
// Z_NORMAL → Z_STOP (停车 4 秒) → Z_COOLDOWN (冷却 5 秒) → Z_NORMAL
|
||
// 冷却期间不检测斑马线, 防止重复触发
|
||
//
|
||
// 返回值: true = Z_STOP (通知 motor_update 刹车)
|
||
// ═══════════════════════════════════════════════════════════
|
||
static bool zebra_process()
|
||
{
|
||
// 双边丢线超过50% → 不采纳任何斑马线检测
|
||
if (g_lost_rows > line_tracking_height / 2) {
|
||
if (g_cfg.debug) printf("[ZEBRA] 忽略: 丢线%d/%d行\n", g_lost_rows, line_tracking_height);
|
||
return false;
|
||
}
|
||
|
||
bool zebra_seen = false;
|
||
bool zebra_near = false;
|
||
int zebra_cy = 0;
|
||
float zebra_cf = 0;
|
||
|
||
// 遍历检测框, 找第一个斑马线
|
||
for (int i = 0; i < g_box_count; ++i) {
|
||
if (g_boxes[i].cls != MD_ZEBRA) continue;
|
||
// ── 赛道内占比检查: 框80%以上在赛道外 → 忽略 ──
|
||
{
|
||
float bx = (float)line_tracking_width / raw_frame.cols;
|
||
float by = (float)line_tracking_height / raw_frame.rows;
|
||
int bx1 = (int)((g_boxes[i].cx - g_boxes[i].w/2) * bx);
|
||
int by1 = (int)((g_boxes[i].cy - g_boxes[i].h/2) * by);
|
||
int bx2 = (int)((g_boxes[i].cx + g_boxes[i].w/2) * bx);
|
||
int by2 = (int)((g_boxes[i].cy + g_boxes[i].h/2) * by);
|
||
int overlap = 0, area = (bx2 - bx1) * (by2 - by1);
|
||
if (area > 0) {
|
||
int y0 = std::max(0, std::min(by1, line_tracking_height - 1));
|
||
int y1 = std::min(by2, line_tracking_height - 1);
|
||
for (int y = y0; y <= y1 && y >= 0; ++y) {
|
||
if (left_line[y] == -1 || right_line[y] == -1) continue;
|
||
int ol = std::max(0, std::min(bx2, right_line[y]) - std::max(bx1, left_line[y]));
|
||
overlap += ol;
|
||
}
|
||
}
|
||
if (area > 0 && (float)overlap / area < 0.2f) {
|
||
if (g_cfg.debug) printf("[ZEBRA] 忽略: 赛道内仅%.0f%% cf=%.2f\n",
|
||
(float)overlap / area * 100, g_boxes[i].conf);
|
||
continue; // 赛道内<20% → 忽略
|
||
}
|
||
}
|
||
zebra_cy = (int)g_boxes[i].cy;
|
||
zebra_cf = g_boxes[i].conf;
|
||
zebra_seen = true;
|
||
zebra_near = (zebra_cy > g_cfg.zebrasee); // cy > 阈值 = 足够近
|
||
if (g_zstate == Z_NORMAL) {
|
||
if (zebra_cy < g_zc_min_cy) g_zc_min_cy = zebra_cy; // 追踪最远 cy
|
||
g_zc_frames++;
|
||
}
|
||
break; // 只取第一个斑马线框
|
||
}
|
||
|
||
// 未检测到 → 衰减去抖计数
|
||
if (!zebra_seen) {
|
||
if (g_zc_frames > 0) g_zc_frames = std::max(0, g_zc_frames - 1); // 每帧 -1 衰减
|
||
if (g_zc_frames == 0) g_zc_min_cy = 120; // 完全衰减 → 重置远处追踪
|
||
}
|
||
|
||
time_t now = time(nullptr);
|
||
switch (g_zstate) {
|
||
case Z_NORMAL:
|
||
// 触发: 近 + 连续帧够 + 曾从远处出现
|
||
if (zebra_near && g_zc_frames >= ZEBRA_MIN_FRAMES && g_zc_min_cy <= ZEBRA_FAR_CY) {
|
||
play_zebra_audio(); // I2C 语音播报
|
||
g_zstate = Z_STOP; g_ztime = now;
|
||
if (g_cfg.debug) printf("[ZEBRA] cy=%d cf=%.2f 停车3s 冷却5s\n", zebra_cy, zebra_cf);
|
||
g_zc_frames = 0; g_zc_min_cy = 120; // 重置, 为下次检测做准备
|
||
} else if (zebra_seen && g_cfg.debug) {
|
||
if (!zebra_near)
|
||
printf("[ZEBRA] 忽略: 太远 cy=%d <= %d\n", zebra_cy, (int)g_cfg.zebrasee);
|
||
else if (g_zc_frames < ZEBRA_MIN_FRAMES)
|
||
printf("[ZEBRA] 忽略: 帧数不足 %d/%d cy=%d\n", g_zc_frames, ZEBRA_MIN_FRAMES, zebra_cy);
|
||
else if (g_zc_min_cy > ZEBRA_FAR_CY)
|
||
printf("[ZEBRA] 忽略: 未曾远距出现 min_cy=%d > %d\n", g_zc_min_cy, ZEBRA_FAR_CY);
|
||
}
|
||
break;
|
||
case Z_STOP:
|
||
if (now - g_ztime >= 3) { // 停车 3 秒
|
||
g_zstate = Z_COOLDOWN; g_ztime = now;
|
||
if (g_cfg.debug) printf("[ZEBRA] 起步\n");
|
||
}
|
||
break;
|
||
case Z_COOLDOWN:
|
||
if (now - g_ztime >= 5) { // 冷却 5 秒
|
||
g_zstate = Z_NORMAL;
|
||
if (g_cfg.debug) printf("[ZEBRA] 恢复\n");
|
||
}
|
||
break;
|
||
}
|
||
return (g_zstate == Z_STOP);
|
||
}
|
||
|
||
// ═══════════════════════════════════════════════════════════
|
||
// Step 6b: 红绿灯状态机
|
||
//
|
||
// 去抖: 红灯/绿灯各自独立计数, 连续 3 帧确认, 未检测时 -1 衰减
|
||
//
|
||
// 状态转移:
|
||
// TL_NORMAL → TL_STOP (红灯连续 ≥3 帧)
|
||
// TL_STOP → TL_WAIT_GREEN (红灯消失)
|
||
// TL_WAIT_GREEN → TL_NORMAL (绿灯连续 ≥3 帧)
|
||
//
|
||
// 返回值: true = 非 TL_NORMAL (通知 motor_update 刹车)
|
||
// ═══════════════════════════════════════════════════════════
|
||
static bool traffic_light_process()
|
||
{
|
||
// 遍历检测框, 统计红/绿灯是否出现
|
||
bool red_seen = false, green_seen = false;
|
||
for (int i = 0; i < g_box_count; ++i) {
|
||
if (g_boxes[i].cls == MD_RED) red_seen = true;
|
||
if (g_boxes[i].cls == MD_GREEN) green_seen = true;
|
||
}
|
||
|
||
// 去抖: 出现 +1, 消失 -1
|
||
if (red_seen) g_tl_red_frames++;
|
||
else g_tl_red_frames = std::max(0, g_tl_red_frames - 1);
|
||
if (green_seen) g_tl_green_frames++;
|
||
else g_tl_green_frames = std::max(0, g_tl_green_frames - 1);
|
||
|
||
switch (g_tl_state) {
|
||
case TL_NORMAL:
|
||
if (g_tl_red_frames >= 3) { // 红灯确认 → 停车
|
||
g_tl_state = TL_STOP;
|
||
if (g_cfg.debug) printf("[TL] 红灯停车\n");
|
||
}
|
||
break;
|
||
case TL_STOP:
|
||
if (!red_seen) { // 红灯消失 → 等绿灯
|
||
g_tl_state = TL_WAIT_GREEN;
|
||
if (g_cfg.debug) printf("[TL] 等待绿灯\n");
|
||
}
|
||
break;
|
||
case TL_WAIT_GREEN:
|
||
if (g_tl_green_frames >= 3) { // 绿灯确认 → 通行
|
||
g_tl_state = TL_NORMAL;
|
||
g_tl_red_frames = g_tl_green_frames = 0; // 清零, 重新开始
|
||
if (g_cfg.debug) printf("[TL] 绿灯通行\n");
|
||
}
|
||
break;
|
||
}
|
||
return (g_tl_state != TL_NORMAL);
|
||
}
|
||
|
||
// ═══════════════════════════════════════════════════════════
|
||
// Step 4: 激光雷达挡板避障
|
||
//
|
||
// lidar_is_active(): 判断是否处于确认或保持衰减状态
|
||
//
|
||
// lidar_avoid_process() 流程:
|
||
// 1. 每 2 帧非阻塞读取 VL53L0X (readResult + startMeasure)
|
||
// 2. 预触发去抖: d_mm >= lidar_pre → 帧数衰减; d_mm == 0 → 跳过
|
||
// 3. 距离→行映射: 线性映射 [lidar_near, lidar_far] → [lt_h-1, 10]
|
||
// 4. 触发判定: d_mm < lidar_thresh && frames >= min_frames → 确认
|
||
// 5. 保持衰减: 确认后 hold_ctr 递增, 超过 hold_frames 则失效
|
||
// 6. 中线变形: 固定朝右推, 斜坡从 row=10 上升到障碍行, 衰减渐弱
|
||
// ═══════════════════════════════════════════════════════════
|
||
static bool lidar_is_active() {
|
||
return g_lidar_confirmed ||
|
||
(g_lidar_hold_ctr > 0 && g_lidar_hold_ctr <= g_cfg.lidar_hold_frames);
|
||
}
|
||
|
||
static void lidar_avoid_process()
|
||
{
|
||
// ── 跳帧 + 前置检查 ──
|
||
static int lidar_skip = 0;
|
||
if (!g_lidar_ok || !g_cfg.lidar_enable) return; // 硬件未就绪或已禁用
|
||
if (++lidar_skip < 2) return; // 每 2 帧一次 (~15Hz)
|
||
lidar_skip = 0;
|
||
|
||
// ── 非阻塞读取上次测量结果, 立即启动下次测量 ──
|
||
VL53L0X_RangingMeasurementData_t data;
|
||
if (!g_lidar_sensor.readResult(data)) return; // 读取失败 → 跳过
|
||
int d_mm = data.RangeMilliMeter;
|
||
g_lidar_sensor.startMeasure(); // 后台测量 ~20ms
|
||
|
||
// ── 预触发去抖窗口 ──
|
||
int lt_h = line_tracking_height;
|
||
bool skip = false;
|
||
if (d_mm == 0) {
|
||
skip = true; // 传感器读数为 0 = 读失败
|
||
} else if (d_mm >= g_cfg.lidar_pre) {
|
||
if (g_lidar_frames > 0) g_lidar_frames--; // 超出窗口 → 衰减 (不归零)
|
||
skip = true;
|
||
}
|
||
|
||
if (!skip) {
|
||
// ── 距离 → 图像行映射 (线性) ──
|
||
// lidar_near(50mm) → row=lt_h-1 (最近), lidar_far(1200mm) → row=10 (最远)
|
||
int row = lt_h - 1 - (d_mm - g_cfg.lidar_near) * (lt_h - 11)
|
||
/ (g_cfg.lidar_far - g_cfg.lidar_near);
|
||
if (row < 10) row = 10;
|
||
if (row >= lt_h) row = lt_h - 1;
|
||
|
||
// ── 纯距离触发, 不检查视觉边界线 ──
|
||
g_lidar_frames++;
|
||
g_lidar_obstacle_row = row;
|
||
|
||
g_lidar_confirmed = (d_mm < g_cfg.lidar_thresh) && (g_lidar_frames >= g_cfg.lidar_min_frames);
|
||
|
||
static int dbg = 0;
|
||
if (g_cfg.debug && ++dbg >= 30) { dbg = 0;
|
||
printf("[LIDAR] d=%d row=%d frames=%d confirm=%d\n",
|
||
d_mm, row, g_lidar_frames, g_lidar_confirmed);
|
||
}
|
||
|
||
// ── 确认 → 记录保持状态 ──
|
||
if (g_lidar_confirmed) {
|
||
g_lidar_hold_ctr = 0;
|
||
g_lidar_hold_src_row = g_lidar_obstacle_row;
|
||
g_lidar_hold_dir = 1.0; // 固定朝右绕行
|
||
if (g_cfg.debug) printf("[LIDAR] 触发 d=%d row=%d dir=右\n", d_mm, g_lidar_obstacle_row);
|
||
g_lidar_frames = 0; // 重置计数
|
||
}
|
||
}
|
||
|
||
// ── 保持计数器递增 (确认中和消失后都递增) ──
|
||
hold_decay:
|
||
if (g_lidar_hold_src_row > 0 && g_lidar_hold_ctr <= g_cfg.lidar_hold_frames)
|
||
g_lidar_hold_ctr++;
|
||
|
||
if (!lidar_is_active()) return; // 非活跃 → 不变形
|
||
|
||
// ── 中线变形: 固定朝右推 ──
|
||
// 从 row=10 到障碍行, 斜坡线性上升; 障碍行以下维持最大推力
|
||
// 消失后 decay 线性衰减到 0
|
||
int src_row = g_lidar_hold_src_row;
|
||
double rng = (double)g_cfg.lidar_avoid_range; // 斜坡陡峭度
|
||
double half_w = line_tracking_width / 2.0;
|
||
double dir = g_lidar_hold_dir; // 1.0 = 朝右
|
||
double decay = g_lidar_confirmed ? 1.0 :
|
||
(1.0 - (double)g_lidar_hold_ctr / g_cfg.lidar_hold_frames);
|
||
|
||
for (int i = 10; i < lt_h; ++i) {
|
||
// 障碍行及以上: 斜坡上升; 障碍行以下: 满推力
|
||
double t = (i <= src_row) ? std::clamp((i - 10) / rng, 0.0, 1.0) : 1.0;
|
||
double push = t * g_cfg.lidar_avoid_gain * half_w * dir * decay;
|
||
double nm = std::clamp(mid_line[i] + push,
|
||
(double)left_line[i] + 2.0,
|
||
(double)right_line[i] - 2.0);
|
||
mid_line[i] = (int)(nm + 0.5);
|
||
}
|
||
}
|
||
|
||
// ═══════════════════════════════════════════════════════════
|
||
// Step 6c: 锥桶检测 + 中线变形
|
||
//
|
||
// cone_is_slow(): 判断是否处于避让/保持/回弹任一阶段
|
||
// (供 motor_update 减速用, 但减速代码当前已注释)
|
||
//
|
||
// cone_detect_and_deform() 流程:
|
||
// 1. 仅在 Z_NORMAL && TL_NORMAL 时运行
|
||
// 2. 遍历 g_boxes 找 cls=0 (锥桶), 置信度 ≥ cone_thresh
|
||
// 3. 模型坐标→巡线空间, 边界过滤 (row 有效、赛道内)
|
||
// 4. cone_margin: 0=用中心点判断, 1=用框左右边缘判断
|
||
// 5. 取 cy 最大(最近)的锥桶
|
||
// 6. 去抖: 位置容差内连续帧计数, 突变则重计
|
||
// 7. 确认 → 保持衰减 → 回弹, 三阶段生命周期
|
||
// 8. 避让阶段: 推离锥桶方向的中线变形 (row 10 ~ 锥桶行)
|
||
// 9. 回弹阶段: 朝锥桶方向反推 (帮助车回到中央)
|
||
// ═══════════════════════════════════════════════════════════
|
||
static bool cone_is_slow() {
|
||
return g_cone_confirmed ||
|
||
(g_cone_hold_ctr > 0 && g_cone_hold_ctr <= g_cfg.cone_hold_frames);
|
||
// (g_cone_return_ctr > 0 && g_cone_return_ctr <= g_cfg.cone_return_frames);
|
||
}
|
||
|
||
static void cone_detect_and_deform()
|
||
{
|
||
// 斑马线停车或红绿灯非正常时, 不做锥桶检测
|
||
if (g_zstate == Z_STOP || g_tl_state != TL_NORMAL) return;
|
||
|
||
// ── 1. 找最近锥桶 (cy 最大 = 最近) ──
|
||
bool cone_seen = false;
|
||
int cone_row = -1, cone_col = -1;
|
||
|
||
for (int i = 0; i < g_box_count; ++i) {
|
||
if (g_boxes[i].cls != MD_CONE) continue;
|
||
if (g_boxes[i].conf < g_cfg.cone_thresh) continue;
|
||
|
||
// 模型空间 → 巡线空间坐标映射
|
||
int cl = g_boxes[i].cx * line_tracking_width / raw_frame.cols;
|
||
int rl = g_boxes[i].cy * line_tracking_height / raw_frame.rows;
|
||
|
||
// 边界过滤: row 0~9 无可靠巡线数据
|
||
if (rl < 10 || rl >= line_tracking_height) continue;
|
||
if (left_line[rl] == -1 || right_line[rl] == -1) continue; // 丢线行
|
||
|
||
// 赛道内判断
|
||
if (g_cfg.cone_margin) {
|
||
// margin=1: 用框的左右边缘判断是否在赛道内
|
||
int bl = (g_boxes[i].cx - g_boxes[i].w/2) * line_tracking_width / raw_frame.cols;
|
||
int br = (g_boxes[i].cx + g_boxes[i].w/2) * line_tracking_width / raw_frame.cols;
|
||
if (br < left_line[rl] || bl > right_line[rl]) continue; // 框完全在赛道外
|
||
} else {
|
||
// margin=0: 用中心点判断
|
||
if (cl < left_line[rl] || cl > right_line[rl]) continue; // 中心在赛道外
|
||
}
|
||
|
||
// 取 cy 最大的 (最近的锥桶)
|
||
if (rl > cone_row) { cone_row = rl; cone_col = cl; }
|
||
cone_seen = true;
|
||
}
|
||
|
||
// ── 2. 去抖确认 ──
|
||
{
|
||
// 位置容差: 动态计算, 典型 row_tol=5, col_tol=10
|
||
int row_tol = std::max(2, line_tracking_height / 12);
|
||
int col_tol = std::max(3, line_tracking_width / 8);
|
||
if (cone_seen) {
|
||
if (g_cone_frames == 0) {
|
||
// 首次出现 → 记录位置, 开始计数
|
||
g_cone_last_row = cone_row; g_cone_last_col = cone_col;
|
||
g_cone_frames = 1;
|
||
} else if (std::abs(cone_row - g_cone_last_row) <= row_tol &&
|
||
std::abs(cone_col - g_cone_last_col) <= col_tol) {
|
||
// 位置接近 → 继续计数
|
||
g_cone_frames++;
|
||
g_cone_last_row = cone_row; g_cone_last_col = cone_col;
|
||
} else {
|
||
// 位置突变 → 视为新锥桶, 重新开始计数
|
||
g_cone_frames = 1;
|
||
g_cone_last_row = cone_row; g_cone_last_col = cone_col;
|
||
}
|
||
} else {
|
||
// 未检测到 → 衰减
|
||
if (g_cone_frames > 0) g_cone_frames = std::max(0, g_cone_frames - 1);
|
||
}
|
||
}
|
||
|
||
g_cone_confirmed = (g_cone_frames >= g_cfg.cone_min_frames);
|
||
|
||
static int cdbg = 0;
|
||
if (g_cfg.debug && ++cdbg >= 15) { cdbg = 0;
|
||
printf("[CONE] frames=%d confirm=%d hold=%d return=%d\n",
|
||
g_cone_frames, g_cone_confirmed, g_cone_hold_ctr, g_cone_return_ctr);
|
||
}
|
||
|
||
// ── 3. 两阶段生命周期: 确认 → 保持衰减 ──
|
||
if (g_cone_confirmed) {
|
||
g_cone_hold_ctr = 1;
|
||
g_cone_return_ctr = 0;
|
||
// 只在锥桶实际可见时更新冻结位置和回弹方向,
|
||
// 防止 cone_row=-1 覆盖有效值 + mid_line[-1] 越界
|
||
if (cone_seen) {
|
||
g_cone_hold_src_row = cone_row;
|
||
g_cone_hold_src_col = cone_col;
|
||
// double ms = mid_line_raw[cone_row];
|
||
// g_cone_return_dir = (cone_col < ms) ? -1.0 : 1.0;
|
||
}
|
||
} else if (g_cone_hold_ctr > 0 && g_cone_hold_ctr <= g_cfg.cone_hold_frames) {
|
||
// 保持衰减阶段: 锥桶消失, 变形量逐帧衰减
|
||
g_cone_hold_ctr++;
|
||
// if (g_cone_hold_ctr > g_cfg.cone_hold_frames)
|
||
// g_cone_return_ctr = 1; // 保持结束 → 启动回弹
|
||
} /* else if (g_cone_return_ctr > 0 && g_cone_return_ctr <= g_cfg.cone_return_frames) {
|
||
// 回弹阶段: 朝锥桶方向反推, 帮助车回到赛道中央
|
||
g_cone_return_ctr++;
|
||
} */
|
||
|
||
// 回弹结束 → 清零所有状态, 避免计数器卡死
|
||
// if (g_cone_return_ctr > g_cfg.cone_return_frames) {
|
||
// printf("[CONE] ★ RETURN END\n");
|
||
// g_cone_hold_src_row = -1;
|
||
// g_cone_hold_ctr = 0;
|
||
// g_cone_return_ctr = 0;
|
||
// }
|
||
|
||
// 生命周期诊断 (debug 模式下每 5 帧打印)
|
||
// static int cone_ldbg = 0;
|
||
// if ((g_cone_confirmed || g_cone_hold_ctr > 0 || g_cone_return_ctr > 0) && ++cone_ldbg >= 5) {
|
||
// cone_ldbg = 0;
|
||
// printf("[CONE-DBG] seen=%d frm=%d conf=%d hold=%d/%d ret=%d/%d src_row=%d ret_dir=%.0f\n",
|
||
// cone_seen, g_cone_frames, g_cone_confirmed,
|
||
// g_cone_hold_ctr, g_cfg.cone_hold_frames,
|
||
// g_cone_return_ctr, g_cfg.cone_return_frames,
|
||
// g_cone_hold_src_row, g_cone_return_dir);
|
||
// }
|
||
|
||
// 三个阶段都不活跃 → 不做变形
|
||
if (!cone_is_slow()) return;
|
||
|
||
// ── 4. 中线变形 ──
|
||
// confirmed+可见 → 实时位置; 否则(confirmed+不可见 / 保持 / 回弹) → 冻结位置
|
||
int src_row = (g_cone_confirmed && cone_seen) ? cone_row : g_cone_hold_src_row;
|
||
int src_col = (g_cone_confirmed && cone_seen) ? cone_col : g_cone_hold_src_col;
|
||
if (src_row <= 10) return;
|
||
|
||
double rng = (double)g_cfg.cone_avoid_range; // 斜坡陡峭度
|
||
double half_w = line_tracking_width / 2.0;
|
||
double ms = mid_line_raw[src_row];
|
||
// 推离方向: 锥桶在原始中线左侧 → 推右(+1), 右侧 → 推左(-1)
|
||
double dir = (src_col < ms) ? 1.0 : -1.0;
|
||
// 保持阶段衰减: 从 1.0 线性降到 0.0
|
||
double decay = g_cone_confirmed ? 1.0 : (1.0 - (double)g_cone_hold_ctr / g_cfg.cone_hold_frames);
|
||
|
||
// ── 4a. 避开阶段: 推离锥桶 (row 10 ~ 底部, 确保前瞻行一定被覆盖) ──
|
||
// if (g_cone_return_ctr == 0) {
|
||
for (int row = 10; row < line_tracking_height; ++row) {
|
||
// 斜坡: row=10 处 t=0(不推), 随行号增大 t→1(满推)
|
||
double t = std::clamp(((row - 10) / rng), 0.0, 1.0);
|
||
double push = t * g_cfg.cone_avoid_gain * half_w * dir * decay;
|
||
double nm = std::clamp(mid_line[row] + push,
|
||
(double)left_line[row] + 2.0,
|
||
(double)right_line[row] - 2.0);
|
||
mid_line[row] = (int)(nm + 0.5);
|
||
}
|
||
// }
|
||
|
||
// ── 4b. 回弹: 锥桶消失后朝锥桶方向反推 ──
|
||
// if (g_cone_return_ctr > 0 && g_cone_return_ctr <= g_cfg.cone_return_frames) {
|
||
// static int rdbg = 0;
|
||
// if (++rdbg >= 10) { rdbg = 0;
|
||
// printf("[CONE] return frame=%d dir=%.0f gain=%.1f\n",
|
||
// g_cone_return_ctr, g_cone_return_dir, g_cfg.cone_return_gain);
|
||
// }
|
||
// int lt_h = line_tracking_height;
|
||
// double ret_rng = rng * 1.2; // 回弹斜坡比避让略缓
|
||
// double ret_hw = half_w;
|
||
// double ret_dir = g_cone_return_dir; // 与避让方向相反
|
||
// // 回弹衰减: 从 1.0 线性降到 0.0
|
||
// double ret_dec = 1.0 - (double)g_cone_return_ctr / g_cfg.cone_return_frames;
|
||
// // 回弹覆盖全部有效行 (row 10 ~ lt_h-1), 不限于锥桶位置
|
||
// for (int row = 10; row < lt_h; ++row) {
|
||
// double t = std::clamp((row - 10) / ret_rng, 0.0, 1.0);
|
||
// double push = t * g_cfg.cone_return_gain * ret_hw * ret_dir * ret_dec;
|
||
// double nm = std::clamp(mid_line[row] + push,
|
||
// (double)left_line[row] + 2.0,
|
||
// (double)right_line[row] - 2.0);
|
||
// mid_line[row] = (int)(nm + 0.5);
|
||
// }
|
||
// }
|
||
}
|
||
|
||
|
||
// ═══════════════════════════════════════════════════════════
|
||
// 急弯检测 — 扫描左右边界纵向梯度
|
||
//
|
||
// 5 行窗口内边界水平位移 > 15 像素 → 直角弯
|
||
// ═══════════════════════════════════════════════════════════
|
||
static bool g_sharp_turn = false;
|
||
|
||
static void detect_sharp_turn()
|
||
{
|
||
g_sharp_turn = false;
|
||
const int W = 3; // 窗口大小(行)
|
||
const int THR = 20; // 位移阈值(像素)
|
||
int lt_h = line_tracking_height;
|
||
for (int r = lt_h - 1; r >= 10 + W; --r) {
|
||
if (left_line[r] == -1 || left_line[r-W] == -1) continue;
|
||
if (std::abs(left_line[r] - left_line[r-W]) > THR) { g_sharp_turn = true; return; }
|
||
}
|
||
for (int r = lt_h - 1; r >= 10 + W; --r) {
|
||
if (right_line[r] == -1 || right_line[r-W] == -1) continue;
|
||
if (std::abs(right_line[r] - right_line[r-W]) > THR) { g_sharp_turn = true; return; }
|
||
}
|
||
}
|
||
|
||
// ═══════════════════════════════════════════════════════════
|
||
// Step 7: 舵机控制
|
||
//
|
||
// 读取 mid_line[foresee/calc_scale] 处的中线位置,
|
||
// 转换为显示空间偏差 → 归一化 → 乘增益 → PWM 占空比。
|
||
//
|
||
// 数学:
|
||
// deviation = mid_line[check_row] × calc_scale - newWidth/2 (像素)
|
||
// deviation -= center_bias (偏置修正)
|
||
// g_steer_deviation = deviation / (newWidth/2) (归一化 [-1,1])
|
||
//
|
||
// |deviation| < deadband → 直行 (1,500,000 ns)
|
||
// 否则: offset = norm × steer_gain × 300,000
|
||
// duty = clamp(1,500,000 + offset, 1,200,000, 1,800,000)
|
||
//
|
||
// 舵机量程: 1,200,000 ~ 1,800,000 ns, 中位 1,500,000 ns
|
||
// ═══════════════════════════════════════════════════════════
|
||
static void steering_update()
|
||
{
|
||
if (!g_cfg.start) return;
|
||
|
||
// 斑马线起步后延迟2s, 再3s内缩短前瞻, 先走稳再改
|
||
double eff_foresee = g_cfg.foresee;
|
||
if (g_zstate == Z_COOLDOWN && time(nullptr) - g_ztime >= 2
|
||
&& time(nullptr) - g_ztime < 5)
|
||
eff_foresee = 24;
|
||
int check_row = (int)eff_foresee / calc_scale;
|
||
if (check_row < 0 || check_row >= line_tracking_height) return;
|
||
if (mid_line[check_row] == -1) return; // 丢线行 → 跳过
|
||
|
||
// 巡线空间中线 → 显示空间偏差
|
||
double deviation = mid_line[check_row] * calc_scale - newWidth / 2;
|
||
deviation -= g_cfg.center_bias; // 机械偏置修正
|
||
// 归一化到 [-1, 1], 供弯道减速使用
|
||
g_steer_deviation = deviation / (newWidth / 2.0);
|
||
|
||
// 斑马线冷却后 3.3~5 秒：一次机会，偏差出现->方向锁定打满->方向翻转/回正即退出
|
||
if (g_zstate == Z_COOLDOWN) {
|
||
time_t elapsed = time(nullptr) - g_ztime;
|
||
if (elapsed >= 3.3 && elapsed < 5 && !g_bangbang_used) {
|
||
if (!g_bangbang_active) {
|
||
// 序列未开始，等偏差出现才启动
|
||
if (std::abs(deviation) < g_cfg.deadband) {
|
||
g_steer_deviation = 0.0;
|
||
servo.setDutyCycle(1500000);
|
||
return;
|
||
}
|
||
g_bangbang_active = true;
|
||
g_bangbang_dir = (deviation > 0) ? 1 : -1;
|
||
} else {
|
||
// 序列已启动：检查方向是否翻转或回到死区
|
||
bool same_dir = (deviation > 0 && g_bangbang_dir > 0) ||
|
||
(deviation < 0 && g_bangbang_dir < 0);
|
||
if (!same_dir || std::abs(deviation) < g_cfg.deadband) {
|
||
// 方向翻转或回正 -> 机会用完，退出，后续走正常线性转向
|
||
g_bangbang_active = false;
|
||
g_bangbang_used = true;
|
||
} else {
|
||
// 同方向，继续打满
|
||
unsigned int duty = (g_bangbang_dir > 0) ? 1800000 : 1200000;
|
||
servo.setDutyCycle(duty);
|
||
g_steer_deviation = (g_bangbang_dir > 0) ? 1.0 : -1.0;
|
||
return;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
// 平滑线性转向: 偏差从死区边界开始缩放，消除死区断崖
|
||
double effective = std::abs(deviation) - g_cfg.deadband;
|
||
if (effective <= 0) {
|
||
servo.setDutyCycle(1500000);
|
||
} else {
|
||
double half_w = newWidth / 2.0;
|
||
double norm = effective / (half_w - g_cfg.deadband);
|
||
double sign = (deviation > 0) ? 1.0 : -1.0;
|
||
double offset = norm * g_cfg.steer_gain * 300000.0 * sign;
|
||
double duty_ns = std::clamp(1500000.0 + offset, 1200000.0, 1800000.0);
|
||
servo.setDutyCycle((unsigned int)duty_ns);
|
||
}
|
||
}
|
||
|
||
// ═══════════════════════════════════════════════════════════
|
||
// Step 8: 电机控制
|
||
//
|
||
// 弯道减速:
|
||
// factor = 1.0 - |g_steer_deviation| × curve_slope
|
||
// factor = max(factor, curve_min)
|
||
// final_spd = target_speed × factor
|
||
//
|
||
// 锥桶/挡板减速 (代码存在但已注释):
|
||
// cone_is_slow() → factor = min(factor, cone_speed)
|
||
// lidar_is_active() → factor = min(factor, lidar_speed)
|
||
//
|
||
// 调用 ControlUpdate(final_spd, block) 执行:
|
||
// block = zebra_block || tl_block → 编码器比例刹车
|
||
// 否则 → 开环占空比驱动
|
||
//
|
||
// 起步弹射: 启动/斑马线恢复后 1.5x 速度持续 0.5s 快速起步
|
||
// ═══════════════════════════════════════════════════════════
|
||
static struct timespec g_boost_end = {0, 0};
|
||
static double g_boost_mul = 1.0;
|
||
static constexpr double BOOST_SECS = 0.7;
|
||
static constexpr double BOOST_MUL = 1.6;
|
||
static constexpr double BOOST_GREEN_SECS = 0.2;
|
||
static constexpr double BOOST_GREEN_MUL = 2.0;
|
||
|
||
static void trigger_boost(double secs, double mul)
|
||
{
|
||
g_boost_mul = mul;
|
||
clock_gettime(CLOCK_MONOTONIC, &g_boost_end);
|
||
g_boost_end.tv_nsec += (long)(secs * 1e9);
|
||
if (g_boost_end.tv_nsec >= 1000000000L) {
|
||
g_boost_end.tv_sec += 1;
|
||
g_boost_end.tv_nsec -= 1000000000L;
|
||
}
|
||
}
|
||
|
||
static double boost_factor()
|
||
{
|
||
struct timespec now;
|
||
clock_gettime(CLOCK_MONOTONIC, &now);
|
||
if (now.tv_sec > g_boost_end.tv_sec) return 1.0;
|
||
if (now.tv_sec == g_boost_end.tv_sec && now.tv_nsec >= g_boost_end.tv_nsec) return 1.0;
|
||
return g_boost_mul;
|
||
}
|
||
|
||
static void motor_update(bool zebra_block, bool tl_block)
|
||
{
|
||
// 起步弹射检测
|
||
static int prev_start = 0;
|
||
static ZState prev_zstate = Z_NORMAL;
|
||
static TLState prev_tlstate = TL_NORMAL;
|
||
if (g_cfg.start && !prev_start) trigger_boost(BOOST_SECS, BOOST_MUL); // 启动
|
||
if (prev_zstate == Z_STOP && g_zstate == Z_COOLDOWN) trigger_boost(BOOST_SECS, BOOST_MUL); // 斑马线起步
|
||
if (prev_tlstate == TL_WAIT_GREEN && g_tl_state == TL_NORMAL) trigger_boost(BOOST_GREEN_SECS, BOOST_GREEN_MUL); // 绿灯起步
|
||
prev_start = g_cfg.start;
|
||
prev_zstate = g_zstate;
|
||
prev_tlstate = g_tl_state;
|
||
|
||
double spd;
|
||
if (boost_factor() > 1.0) {
|
||
spd = 26.0; // 起步弹射
|
||
} else if (g_zstate == Z_COOLDOWN) {
|
||
spd = 16.0; // 冷却期基础速度
|
||
time_t elapsed = time(nullptr) - g_ztime;
|
||
if (elapsed >= 3.3 && elapsed < 5)
|
||
spd = std::min(spd, 13.0); // 3.3~5s 降速
|
||
} else {
|
||
spd = target_speed; // 正常行驶
|
||
}
|
||
|
||
// 弯道减速: 舵机偏差越大 → 速度越低
|
||
double curve = 1.0 - std::abs(g_steer_deviation) * g_cfg.curve_slope;
|
||
if (curve < g_cfg.curve_min) curve = g_cfg.curve_min;
|
||
|
||
// 取最低倍率: 弯道 / 锥桶 / 挡板 三者不叠加, 取最小值
|
||
double factor = curve;
|
||
// if (g_lost_rows > 15)
|
||
// factor = std::min(factor, 0.6);
|
||
// if (cone_is_slow() && g_zstate != Z_STOP && g_tl_state == TL_NORMAL)
|
||
// factor = std::min(factor, g_cfg.cone_speed);
|
||
// if (lidar_is_active() && g_zstate != Z_STOP && g_tl_state == TL_NORMAL)
|
||
// factor = std::min(factor, g_cfg.lidar_speed);
|
||
|
||
double final_spd = spd * factor;
|
||
|
||
// 每 30 帧打印一次速度调试信息
|
||
static int dbg = 0;
|
||
if (++dbg >= 30) { dbg = 0;
|
||
printf("[SPD] base=%.0f dev=%.2f curve=%.2f final=%.1f\n",
|
||
spd, g_steer_deviation, curve, final_spd);
|
||
}
|
||
|
||
// 传递给 control.cpp: 刹车信号 = 斑马线停车 OR 红绿灯停车
|
||
ControlUpdate(final_spd, zebra_block || tl_block);
|
||
}
|
||
|
||
|
||
// ═══════════════════════════════════════════════════════════
|
||
// Step 9: LCD 渲染
|
||
//
|
||
// 流程:
|
||
// 1. 每 10 帧从 g_cfg.showImg 刷新 g_lcd_on 开关
|
||
// 2. track (128/0 灰度) → resize → GRAY2BGR → 居中拷贝到 lcd_fbImage
|
||
// 3. 绘制边界线: 红=左边界, 绿=右边界, 蓝=中线
|
||
// 4. 绘制检测框: 橙=锥桶, 紫=斑马线 (跳过红绿灯框)
|
||
// 5. 绘制状态指示: L=激光避障, R=红灯, G=等绿灯, S=斑马线停, C=冷却, N=正常
|
||
// 6. BGR → RGB565 → 写入 /dev/fb0 mmap
|
||
// ═══════════════════════════════════════════════════════════
|
||
static cv::Mat lcd_fbImage, lcd_resized, lcd_colored;
|
||
|
||
static void lcd_clear_screen()
|
||
{
|
||
if (fb_buffer && g_fb_size > 0)
|
||
std::memset(fb_buffer, 0, g_fb_size);
|
||
}
|
||
|
||
static void lcd_show_stop_image()
|
||
{
|
||
if (g_stop_image.empty() || !fb_buffer) return;
|
||
// 若图片尺寸与屏幕不一致则缩放
|
||
cv::Mat display_img = g_stop_image;
|
||
if (display_img.cols != screenWidth || display_img.rows != screenHeight)
|
||
cv::resize(display_img, display_img, cv::Size(screenWidth, screenHeight));
|
||
convertMatToRGB565(display_img, fb_buffer, screenWidth, screenHeight);
|
||
}
|
||
|
||
static void lcd_render()
|
||
{
|
||
// 斑马线停车期间由 CameraHandler 单独控制清屏+图片, 此处不渲染
|
||
if (g_zstate == Z_STOP) return;
|
||
|
||
// 每 10 帧刷新 LCD 开关, 避免每帧读文件
|
||
static int lcd_check = 0;
|
||
if (--lcd_check < 0) { g_lcd_on = g_cfg.showImg; lcd_check = 10; }
|
||
if (!g_lcd_on) return;
|
||
|
||
// 创建黑底画布
|
||
lcd_fbImage.create(screenHeight, screenWidth, CV_8UC3);
|
||
lcd_fbImage.setTo(cv::Scalar(0, 0, 0));
|
||
|
||
// 赛道灰度图居中显示
|
||
cv::resize(track, lcd_resized, cv::Size(newWidth, newHeight));
|
||
cv::cvtColor(lcd_resized, lcd_colored, cv::COLOR_GRAY2BGR);
|
||
|
||
cv::Rect roi((screenWidth - newWidth) / 2, (screenHeight - newHeight) / 2, newWidth, newHeight);
|
||
lcd_colored.copyTo(lcd_fbImage(roi));
|
||
|
||
// ── 边界线: 逐行绘制 ──
|
||
for (int y = 0; y < line_tracking_height; ++y) {
|
||
int lx = (int)(left_line[y] * calc_scale), ly = y * calc_scale;
|
||
int rx = (int)(right_line[y] * calc_scale);
|
||
int mx = (int)(mid_line[y] * calc_scale);
|
||
cv::line(lcd_fbImage(roi), cv::Point(lx, ly), cv::Point(lx, ly), cv::Scalar(0, 0, 255), calc_scale); // 红=左
|
||
cv::line(lcd_fbImage(roi), cv::Point(rx, ly), cv::Point(rx, ly), cv::Scalar(0, 255, 0), calc_scale); // 绿=右
|
||
cv::line(lcd_fbImage(roi), cv::Point(mx, ly), cv::Point(mx, ly), cv::Scalar(255, 0, 0), calc_scale); // 蓝=中
|
||
}
|
||
|
||
// ── 检测框 (跳过红绿灯, 不画框) ──
|
||
float bx = (float)newWidth / raw_frame.cols, by = (float)newHeight / raw_frame.rows;
|
||
for (int i = 0; i < g_box_count; ++i) {
|
||
if (g_boxes[i].cls == MD_RED || g_boxes[i].cls == MD_GREEN) continue;
|
||
// 模型空间 → 显示空间
|
||
int x1 = (int)((g_boxes[i].cx - g_boxes[i].w/2) * bx);
|
||
int y1 = (int)((g_boxes[i].cy - g_boxes[i].h/2) * by);
|
||
int x2 = (int)((g_boxes[i].cx + g_boxes[i].w/2) * bx);
|
||
int y2 = (int)((g_boxes[i].cy + g_boxes[i].h/2) * by);
|
||
x1 = std::max(0, std::min(newWidth - 1, x1));
|
||
y1 = std::max(0, std::min(newHeight - 1, y1));
|
||
x2 = std::max(0, std::min(newWidth - 1, x2));
|
||
y2 = std::max(0, std::min(newHeight - 1, y2));
|
||
|
||
// 颜色: 锥桶=橙, 斑马线=紫, 其他=绿
|
||
cv::Scalar color(0, 255, 0);
|
||
if (g_boxes[i].cls == MD_ZEBRA) color = cv::Scalar(255, 0, 255); // 紫
|
||
else if (g_boxes[i].cls == MD_CONE) color = cv::Scalar(0, 165, 255); // 橙
|
||
cv::rectangle(lcd_fbImage(roi), cv::Point(x1, y1), cv::Point(x2, y2), color, 2);
|
||
|
||
// 标注: 类别编号 + 置信度百分比
|
||
char lab[16]; snprintf(lab, 16, "%d %.0f", g_boxes[i].cls, g_boxes[i].conf * 100);
|
||
cv::putText(lcd_fbImage(roi), lab, cv::Point(x1 + 2, y1 + 10),
|
||
cv::FONT_HERSHEY_SIMPLEX, 0.3, color, 1);
|
||
}
|
||
|
||
// ── 状态指示 (左下角) ──
|
||
// L=激光避障, R=红灯停, G=等绿灯, S=斑马线停, C=冷却, N=正常
|
||
char ztxt[8]; int zi = 0;
|
||
// if (lidar_is_active()) ztxt[zi++] = 'L';
|
||
if (g_tl_state == TL_STOP) ztxt[zi++] = 'R';
|
||
else if (g_tl_state == TL_WAIT_GREEN) ztxt[zi++] = 'G';
|
||
if (g_zstate == Z_STOP) ztxt[zi++] = 'S';
|
||
else if (g_zstate == Z_COOLDOWN) ztxt[zi++] = 'C';
|
||
else ztxt[zi++] = 'N';
|
||
ztxt[zi] = 0;
|
||
cv::putText(lcd_fbImage(roi), ztxt, cv::Point(2, newHeight - 4),
|
||
cv::FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(0, 255, 255), 1);
|
||
|
||
// BGR → RGB565 → /dev/fb0
|
||
convertMatToRGB565(lcd_fbImage, fb_buffer, screenWidth, screenHeight);
|
||
}
|
||
|
||
// ═══════════════════════════════════════════════════════════
|
||
// Step 10: FPS 统计
|
||
//
|
||
// 每 15 帧输出一行:
|
||
// fps: 总帧率
|
||
// cap: 取帧耗时(ms)
|
||
// vis: 视觉巡线耗时(ms)
|
||
// mdl: 模型推理耗时(ms) (管道末尾, 不阻塞舵机)
|
||
// ctl: 控制耗时(ms) (状态机+舵机+电机)
|
||
// lat: 端到端延时(ms) (取帧开始→舵机输出)
|
||
//
|
||
// ts[] 时间戳由 CameraHandler 在各步骤间采集
|
||
// ═══════════════════════════════════════════════════════════
|
||
static void fps_log(struct timespec *ts)
|
||
{
|
||
static int fc = 0;
|
||
static struct timespec tf0;
|
||
if (fc == 0) clock_gettime(CLOCK_MONOTONIC, &tf0);
|
||
fc++;
|
||
if (fc % 15 != 0) return; // 每 15 帧输出一次
|
||
|
||
struct timespec tf1;
|
||
clock_gettime(CLOCK_MONOTONIC, &tf1);
|
||
|
||
double dt = (tf1.tv_sec - tf0.tv_sec) + (tf1.tv_nsec - tf0.tv_nsec) * 1e-9;
|
||
double ms_cap = (ts[1].tv_sec - ts[0].tv_sec) * 1000.0 + (ts[1].tv_nsec - ts[0].tv_nsec) * 1e-6;
|
||
double ms_vis = (ts[2].tv_sec - ts[1].tv_sec) * 1000.0 + (ts[2].tv_nsec - ts[1].tv_nsec) * 1e-6;
|
||
double ms_mdl = (ts[5].tv_sec - ts[4].tv_sec) * 1000.0 + (ts[5].tv_nsec - ts[4].tv_nsec) * 1e-6;
|
||
double ms_ctl = (ts[4].tv_sec - ts[2].tv_sec) * 1000.0 + (ts[4].tv_nsec - ts[2].tv_nsec) * 1e-6;
|
||
double ms_lat = (ts[3].tv_sec - ts[0].tv_sec) * 1000.0 + (ts[3].tv_nsec - ts[0].tv_nsec) * 1e-6;
|
||
|
||
printf("fps=%.1f | cap=%.0f vis=%.0f mdl=%.0f ctl=%.0f lat=%.0f ms\r",
|
||
fc / dt, ms_cap, ms_vis, ms_mdl, ms_ctl, ms_lat);
|
||
fflush(stdout);
|
||
}
|
||
|
||
|
||
// ═══════════════════════════════════════════════════════════
|
||
// CameraHandler — 每帧主流程 (10 步流水线)
|
||
//
|
||
// 由 main.cpp 主循环调用, 单线程顺序执行。
|
||
//
|
||
// 附加逻辑:
|
||
// - debug 模式每 30 帧重载全部配置 (cfg_load_all)
|
||
//
|
||
// ts[6] 时间戳分布 (按执行顺序):
|
||
// ts[0]: capture_frame 取帧开始
|
||
// ts[1]: capture_frame 取帧结束
|
||
// ts[2]: image_main 视觉巡线结束
|
||
// ts[3]: steering_update 舵机输出结束 (端到端延时终点)
|
||
// ts[4]: motor_update 电机控制结束
|
||
// ts[5]: run_model_inference 模型推理结束 (管道末尾)
|
||
// ═══════════════════════════════════════════════════════════
|
||
int CameraHandler(void)
|
||
{
|
||
// debug 模式: 每 30 帧热重载配置文件
|
||
static int reloadCnt = 0;
|
||
if (g_cfg.debug && ++reloadCnt >= 30) { reloadCnt = 0; cfg_load_all(); }
|
||
// 嵌入式优化: 每 150 帧回收 glibc malloc 缓存的空闲页
|
||
static int trimCnt = 0;
|
||
// if (++trimCnt >= 150) { trimCnt = 0; malloc_trim(0); }
|
||
|
||
struct timespec ts[6];
|
||
|
||
// 1. 取帧 + MJPEG 解码
|
||
if (capture_frame(&ts[0], &ts[1]) < 0) return -1;
|
||
|
||
// 2. 保存图像 (debug)
|
||
save_image_if_requested();
|
||
|
||
// 3. 视觉巡线 → left/right/mid_line[]
|
||
image_main();
|
||
mid_line_raw = mid_line;
|
||
clock_gettime(CLOCK_MONOTONIC, &ts[2]);
|
||
|
||
// 4. 激光避障 → 修改 mid_line[] (在模型推理之前, 优先级高于锥桶)
|
||
// lidar_avoid_process();
|
||
|
||
// 5. 场景识别状态机 (使用上一帧的模型检测结果 g_boxes[])
|
||
bool zebra_block = zebra_process(); // 5a. 斑马线
|
||
bool tl_block = traffic_light_process(); // 5b. 红绿灯
|
||
cone_detect_and_deform(); // 5c. 锥桶 → 修改 mid_line[]
|
||
|
||
// detect_sharp_turn(); // 急弯检测
|
||
|
||
// 6. 舵机控制 (读取变形后的 mid_line)
|
||
steering_update();
|
||
clock_gettime(CLOCK_MONOTONIC, &ts[3]); // 舵机输出结束 = 端到端延时终点
|
||
// ── 临时测试: 锥桶确认后 1s 强制右转 0.5s ──
|
||
// {
|
||
// static int cone_test_timer = -1;
|
||
// static bool cone_test_need_clr = false;
|
||
//
|
||
// if (cone_test_need_clr && !g_cone_confirmed)
|
||
// cone_test_need_clr = false;
|
||
//
|
||
// if (g_cone_confirmed && cone_test_timer < 0 && !cone_test_need_clr) {
|
||
// cone_test_timer = 0;
|
||
// printf("[CONE-TEST] 锥桶确认, 开始计时\n");
|
||
// }
|
||
// if (cone_test_timer >= 0) {
|
||
// cone_test_timer++;
|
||
// if (cone_test_timer == 30)
|
||
// printf("[CONE-TEST] 强制右转开始\n");
|
||
// if (cone_test_timer >= 30 && cone_test_timer < 38)
|
||
// servo.setDutyCycle(1700000);
|
||
// if (cone_test_timer >= 38) {
|
||
// printf("[CONE-TEST] 强制右转结束\n");
|
||
// cone_test_timer = -1;
|
||
// cone_test_need_clr = true;
|
||
// }
|
||
// }
|
||
// }
|
||
|
||
// 7. 电机控制 (弯道减速 + 刹车)
|
||
motor_update(zebra_block, tl_block);
|
||
clock_gettime(CLOCK_MONOTONIC, &ts[4]);
|
||
|
||
// 8. 模型推理 → g_boxes[] (放到最后, 供下一帧状态机使用, 不阻塞舵机)
|
||
run_model_inference();
|
||
clock_gettime(CLOCK_MONOTONIC, &ts[5]);
|
||
|
||
// 9. LCD 渲染
|
||
// 斑马线停车状态切换时清屏/显示提示图片，仅切换时操作，不每帧准备缓存
|
||
static ZState prev_zstate = Z_NORMAL;
|
||
if (g_zstate == Z_STOP && prev_zstate != Z_STOP) {
|
||
lcd_clear_screen();
|
||
lcd_show_stop_image();
|
||
} else if (g_zstate != Z_STOP && prev_zstate == Z_STOP) {
|
||
lcd_clear_screen();
|
||
}
|
||
// 进入 Z_COOLDOWN 时重置打满状态机
|
||
if (g_zstate == Z_COOLDOWN && prev_zstate != Z_COOLDOWN) {
|
||
g_bangbang_used = false;
|
||
g_bangbang_active = false;
|
||
}
|
||
prev_zstate = g_zstate;
|
||
|
||
lcd_render();
|
||
|
||
// 10. FPS 日志
|
||
fps_log(ts);
|
||
|
||
return 0;
|
||
}
|