diff --git a/lib/global.h b/lib/global.h index df29b44..b5e7639 100644 --- a/lib/global.h +++ b/lib/global.h @@ -31,7 +31,11 @@ const std::string cone_hold_frames_file = "./cone_hold_frames"; const std::string brake_scale_file = "./brake_scale"; const std::string brake_max_file = "./brake_max"; +const std::string curve_slope_file = "./curve_slope"; +const std::string curve_min_file = "./curve_min"; + const std::string lidar_thresh_file = "./lidar_thresh"; +const std::string lidar_pre_file = "./lidar_pre"; const std::string lidar_near_file = "./lidar_near"; const std::string lidar_far_file = "./lidar_far"; const std::string lidar_near_start_file = "./lidar_near_start"; @@ -71,7 +75,11 @@ struct CfgCache { double brake_scale = 10; // 速度(pps) → 刹车占空比(ns) 缩放系数 int brake_max = 10000; // 最大刹车占空比 (ns) - int lidar_thresh = 300; // 激光障碍判定距离阈值 (mm) + double curve_slope = 0.4; // 弯道减速斜率 (越大越猛) + double curve_min = 0.8; // 弯道最低速度倍率 + + int lidar_thresh = 300; // 激光障碍判定距离阈值 (mm), 必须<此值才触发 + int lidar_pre = 800; // 预触发去抖窗口, <此值开始攒帧 int lidar_near = 50; // row=lt_h-1 对应的物理距离 (mm) int lidar_far = 1200; // row=10 对应的物理距离 (mm) int lidar_near_start = 1; // near_valid 检测起始偏移 diff --git a/lib/vl53l0x.h b/lib/vl53l0x.h index b943873..d423705 100644 --- a/lib/vl53l0x.h +++ b/lib/vl53l0x.h @@ -14,6 +14,8 @@ public: ~VL53L0X(); bool init(); + bool startMeasure(); + bool readResult(VL53L0X_RangingMeasurementData_t &data); bool readRange(VL53L0X_RangingMeasurementData_t &data); bool stop(); diff --git a/main/CMakeLists.txt b/main/CMakeLists.txt index 3b9a0b6..3144813 100644 --- a/main/CMakeLists.txt +++ b/main/CMakeLists.txt @@ -1,3 +1,6 @@ # 主程序 add_executable(smartcar_demo1 main.cpp) -target_link_libraries(smartcar_demo1 common_lib ${OpenCV_LIBS}) \ No newline at end of file +target_link_libraries(smartcar_demo1 common_lib ${OpenCV_LIBS}) + +add_executable(lidar_test lidar_test.cpp) +target_link_libraries(lidar_test common_lib) \ No newline at end of file diff --git a/main/lidar_test.cpp b/main/lidar_test.cpp new file mode 100644 index 0000000..2c563c2 --- /dev/null +++ b/main/lidar_test.cpp @@ -0,0 +1,49 @@ +#include +#include +#include +#include +#include "vl53l0x.h" + +static std::atomic running(true); +static void on_signal(int) { running.store(false); } + +int main() +{ + setvbuf(stdout, NULL, _IONBF, 0); + std::signal(SIGINT, on_signal); + std::signal(SIGTERM, on_signal); + + VL53L0X sensor; + if (!sensor.init()) { + fprintf(stderr, "VL53L0X init failed\n"); + return 1; + } + + sensor.startMeasure(); + + printf("lidar_test running, Ctrl+C to stop\n"); + printf("%-8s %-8s %-10s\n", "cnt", "range_mm", "status"); + + int count = 0; + VL53L0X_RangingMeasurementData_t data; + + while (running.load()) { + if (!sensor.readResult(data)) { + printf("%-8d %-8s %-10s\n", ++count, "ERR", "timeout"); + sensor.startMeasure(); + usleep(50000); + continue; + } + + int range = data.RangeMilliMeter; + const char* st = (data.RangeStatus == 0) ? "OK" : "SIGMA"; + printf("%-8d %-8d %-10s\n", ++count, range, st); + + sensor.startMeasure(); + usleep(30000); + } + + sensor.stop(); + printf("stopped\n"); + return 0; +} diff --git a/main/main.cpp b/main/main.cpp index 702d389..19e2a1c 100644 --- a/main/main.cpp +++ b/main/main.cpp @@ -20,22 +20,6 @@ int main(void) cfg_load_all(); - double avoid_gain_val = readDoubleFromFile(cone_avoid_gain_file); - int avoid_range_val = (int)readDoubleFromFile(cone_avoid_range_file); - int hold_frames_val = (int)readDoubleFromFile(cone_hold_frames_file); - if (avoid_gain_val > 0) g_cfg.cone_avoid_gain = avoid_gain_val; - if (avoid_range_val > 0) g_cfg.cone_avoid_range = avoid_range_val; - if (hold_frames_val > 0) g_cfg.cone_hold_frames = hold_frames_val; - - double lidar_gain_val = readDoubleFromFile(lidar_avoid_gain_file); - int lidar_range_val = (int)readDoubleFromFile(lidar_avoid_range_file); - int lidar_hold_val = (int)readDoubleFromFile(lidar_hold_frames_file); - int lidar_thresh_val = (int)readDoubleFromFile(lidar_thresh_file); - if (lidar_gain_val > 0) g_cfg.lidar_avoid_gain = lidar_gain_val; - if (lidar_range_val > 0) g_cfg.lidar_avoid_range = lidar_range_val; - if (lidar_hold_val > 0) g_cfg.lidar_hold_frames = lidar_hold_val; - if (lidar_thresh_val > 0) g_cfg.lidar_thresh = lidar_thresh_val; - if (CameraInit(0) < 0) { std::cerr << "CameraInit failed" << std::endl; return -1; diff --git a/src/camera.cpp b/src/camera.cpp index ec2ece4..6686947 100644 --- a/src/camera.cpp +++ b/src/camera.cpp @@ -15,6 +15,7 @@ #include #include #include +#include // ═══════════════════════════════════════════════════════════ // 模型检测类别 @@ -38,7 +39,7 @@ static constexpr int calc_scale = 2; // ═══════════════════════════════════════════════════════════ // 模型相关静态 // ═══════════════════════════════════════════════════════════ -static float g_thresh[4] = {0.90f, 0.75f, 0.80f, 0.90f}; +static float g_thresh[4] = {0.90f, 0.75f, 0.80f, 0.82f}; static DetectBoxV10 g_boxes[16]; static int g_box_count = 0; @@ -72,6 +73,8 @@ 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; // ═══════════════════════════════════════════════════════════ // 激光雷达挡板避障 (主循环每 5 帧同步读取一次) @@ -81,9 +84,8 @@ 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_ref_row = -1; -static int g_lidar_hold_ctr = 0; static int g_lidar_hold_src_row = -1; +static int g_lidar_hold_ctr = 0; static double g_lidar_hold_dir = 0; // ═══════════════════════════════════════════════════════════ @@ -196,9 +198,10 @@ int CameraInit(int camera_id) // 只在启用时才初始化激光,避免驱动内核定时器拖慢 CPU if (g_cfg.lidar_enable) { g_lidar_ok = g_lidar_sensor.init(); - if (g_lidar_ok) + if (g_lidar_ok) { printf("[LIDAR] VL53L0X 已初始化\n"); - else + g_lidar_sensor.startMeasure(); + } else printf("[LIDAR] VL53L0X 未连接, 避障禁用\n"); } else { printf("[LIDAR] 已禁用\n"); @@ -387,89 +390,62 @@ static bool lidar_is_active() { static void lidar_avoid_process() { - // 每 5 帧读取一次, 单次测距 ~20ms (高速模式) + // 每 2 帧一读 (15Hz), 预触发 static int lidar_skip = 0; if (!g_lidar_ok || !g_cfg.lidar_enable) return; - if (++lidar_skip < 5) return; + if (++lidar_skip < 2) return; lidar_skip = 0; - // ── 1. 单次激光测距 (高速模式 ~20ms) ── VL53L0X_RangingMeasurementData_t data; - if (!g_lidar_sensor.readRange(data)) return; + if (!g_lidar_sensor.readResult(data)) return; int d_mm = data.RangeMilliMeter; + g_lidar_sensor.startMeasure(); - if (d_mm >= g_cfg.lidar_thresh) { - if (g_lidar_frames > 0) - g_lidar_frames = std::max(0, g_lidar_frames - 1); - goto hold_decay; + // 超出预触发窗口 → 减帧(不归零), 传感器0不计数 + int lt_h = line_tracking_height; + bool skip = false; + if (d_mm == 0) { + skip = true; // 读失败, 本帧不动 + } else if (d_mm >= g_cfg.lidar_pre) { + if (g_lidar_frames > 0) g_lidar_frames--; + skip = true; } - // ── 2. 距离 → 图像行映射 ── - { - int lt_h = line_tracking_height; - 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; + if (!skip) { + // 距离 → 图像行 + 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; - // ── 3a. 近处边线有效检查 ── - bool near_valid = false; - int near_row = -1; - int near_start = std::min(row + g_cfg.lidar_near_start, lt_h - 1); - int near_end = std::min(row + g_cfg.lidar_near_end, lt_h - 1); - for (int r = near_start; r <= near_end; ++r) { - if (left_line[r] != -1 && right_line[r] != -1) { - near_valid = true; - near_row = r; - break; - } - } + // 纯距离触发, 不看赛道线 + g_lidar_frames++; + g_lidar_obstacle_row = row; - // ── 3b. 远处丢线检查 ── - bool far_lost = false; - int far_start = std::max(row - g_cfg.lidar_far_span, 10); - for (int r = row; r >= far_start; --r) { - if (left_line[r] == -1 || right_line[r] == -1) { - far_lost = true; - break; - } - } + g_lidar_confirmed = (d_mm < g_cfg.lidar_thresh) && (g_lidar_frames >= g_cfg.lidar_min_frames); - // ── 3c. 联合判定 ── - if (near_valid && far_lost) { - g_lidar_frames++; - g_lidar_obstacle_row = row; - g_lidar_ref_row = near_row; - } else { - if (g_lidar_frames > 0) - g_lidar_frames = std::max(0, g_lidar_frames - 1); - } + static int dbg = 0; + if (g_cfg.debug && ++dbg >= 5) { dbg = 0; + printf("[LIDAR] d=%d row=%d frames=%d confirm=%d\n", + d_mm, row, g_lidar_frames, g_lidar_confirmed); } - g_lidar_confirmed = (g_lidar_frames >= g_cfg.lidar_min_frames); - - // ── 保持/衰减 ── if (g_lidar_confirmed) { g_lidar_hold_ctr = 0; g_lidar_hold_src_row = g_lidar_obstacle_row; - // 在参考行判断左右空间，往宽侧推 - int r = g_lidar_ref_row; - double lspace = mid_line[r] - left_line[r]; - double rspace = right_line[r] - mid_line[r]; - g_lidar_hold_dir = (lspace > rspace) ? 1.0 : -1.0; - if (g_cfg.debug) - printf("[LIDAR] 触发 d=%d row=%d dir=%.0f\n", d_mm, g_lidar_obstacle_row, g_lidar_hold_dir); + 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) { + 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; - // ── 中线变形 ── + // 中线变形 — 固定朝右 int src_row = g_lidar_hold_src_row; double rng = (double)g_cfg.lidar_avoid_range; double half_w = line_tracking_width / 2.0; @@ -477,13 +453,13 @@ hold_decay: double decay = g_lidar_confirmed ? 1.0 : (1.0 - (double)g_lidar_hold_ctr / g_cfg.lidar_hold_frames); - for (int row = 10; row <= src_row; ++row) { - double t = std::clamp(((row - 10) / rng), 0.0, 1.0); + 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[row] + push, - (double)left_line[row] + 2.0, - (double)right_line[row] - 2.0); - mid_line[row] = (int)(nm + 0.5); + 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); } } @@ -492,7 +468,8 @@ hold_decay: // ═══════════════════════════════════════════════════════════ 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_hold_ctr > 0 && g_cone_hold_ctr <= g_cfg.cone_hold_frames) || + (g_cone_return_ctr > 0 && g_cone_return_ctr <= g_cfg.cone_hold_frames / 3); } static void cone_detect_and_deform() @@ -550,11 +527,19 @@ static void cone_detect_and_deform() // ── 保持/衰减 ── if (g_cone_confirmed) { - g_cone_hold_ctr = 0; + g_cone_hold_ctr = 0; g_cone_hold_src_row = cone_row; g_cone_hold_src_col = cone_col; + g_cone_return_ctr = 0; } else if (g_cone_hold_src_row > 0 && g_cone_hold_ctr <= g_cfg.cone_hold_frames) { g_cone_hold_ctr++; + if (g_cone_hold_ctr > g_cfg.cone_hold_frames) { + double ms = mid_line[g_cone_hold_src_row]; + g_cone_return_dir = (g_cone_hold_src_col < ms) ? -1.0 : 1.0; + g_cone_return_ctr = 1; + } + } else if (g_cone_return_ctr > 0 && g_cone_return_ctr <= g_cfg.cone_hold_frames / 3) { + g_cone_return_ctr++; } if (!cone_is_slow()) return; @@ -571,13 +556,33 @@ static void cone_detect_and_deform() double dir = (src_col < ms) ? 1.0 : -1.0; double decay = g_cone_confirmed ? 1.0 : (1.0 - (double)g_cone_hold_ctr / g_cfg.cone_hold_frames); - for (int row = 10; row <= src_row; ++row) { - 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); + // ── 避开阶段: 推离锥桶 ── + if (g_cone_return_ctr == 0) { + for (int row = 10; row <= src_row; ++row) { + 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); + } + } + + // ── 回弹: 锥桶消失后朝锥桶方向推, 把车带回赛道中心 ── + if (g_cone_return_ctr > 0 && g_cone_return_ctr <= g_cfg.cone_hold_frames / 3) { + int lt_h = line_tracking_height; + double ret_rng = rng * 1.2; + double ret_hw = half_w; + double ret_dir = g_cone_return_dir; + double ret_dec = 1.0 - (double)g_cone_return_ctr / (g_cfg.cone_hold_frames / 3); + 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_avoid_gain * 0.5 * 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); + } } } @@ -611,11 +616,27 @@ static void steering_update() static void motor_update(bool zebra_block, bool tl_block) { double spd = target_speed; - if (cone_is_slow() && g_zstate != Z_STOP && g_tl_state == TL_NORMAL) - spd *= g_cfg.cone_speed; - if (lidar_is_active() && g_zstate != Z_STOP && g_tl_state == TL_NORMAL) - spd *= g_cfg.lidar_speed; - ControlUpdate(spd, zebra_block || tl_block); + + // 弯道减速 + 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 (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; + + 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); + } + + ControlUpdate(final_spd, zebra_block || tl_block); } @@ -717,6 +738,11 @@ static void fps_log(struct timespec *ts) // ═══════════════════════════════════════════════════════════ int CameraHandler(void) { + static int reloadCnt = 0; + if (g_cfg.debug && ++reloadCnt >= 30) { reloadCnt = 0; cfg_load_all(); } + static int trimCnt = 0; + if (++trimCnt >= 150) { trimCnt = 0; malloc_trim(0); } + struct timespec ts[5]; // [0]=capture_end, [1]=vision_end, [2]=model_end, [3]=control_end, [4]=fps // 1. 取帧 + 解码 @@ -729,27 +755,29 @@ int CameraHandler(void) image_main(); clock_gettime(CLOCK_MONOTONIC, &ts[2]); - // 4. 模型推理 + // 4. 避障激光 (预触发: 读数+起测 ~1ms, 测距 20ms 后台跑) + lidar_avoid_process(); + + // 5. 模型推理 run_model_inference(); clock_gettime(CLOCK_MONOTONIC, &ts[3]); - // 5. 场景识别 + // 6. 场景识别 bool zebra_block = zebra_process(); bool tl_block = traffic_light_process(); - lidar_avoid_process(); cone_detect_and_deform(); - // 6. 舵机 + // 7. 舵机 steering_update(); - // 7. 电机 + // 8. 电机 motor_update(zebra_block, tl_block); clock_gettime(CLOCK_MONOTONIC, &ts[4]); - // 8. LCD + // 9. LCD lcd_render(); - // 9. FPS + // 10. FPS fps_log(ts); return 0; diff --git a/src/control.cpp b/src/control.cpp index 9772a28..7dbc68d 100644 --- a/src/control.cpp +++ b/src/control.cpp @@ -10,8 +10,6 @@ #include #include -extern double g_steer_deviation; - MotorController *motorController[2] = {nullptr, nullptr}; GPIO mortorEN(73); @@ -118,12 +116,8 @@ void ControlUpdate(double speed, bool zebra_block) return; } - double curve = 1.0 - std::abs(g_steer_deviation) * 0.4; - if (curve < 0.6) curve = 0.6; - double spd = speed * curve; - for (int i = 0; i < 2; ++i) - if (motorController[i]) motorController[i]->updateduty(spd); + if (motorController[i]) motorController[i]->updateduty(speed); mortorEN.setValue(1); } diff --git a/src/global.cpp b/src/global.cpp index 28d8ebf..2def5f7 100644 --- a/src/global.cpp +++ b/src/global.cpp @@ -36,11 +36,18 @@ void cfg_load_all() g_cfg.cone_min_frames = (int)readDoubleFromFile(cone_min_frames_file); g_cfg.cone_margin = (int)readDoubleFromFile(cone_margin_file); g_cfg.cone_thresh = readDoubleFromFile(cone_thresh_file); + g_cfg.cone_avoid_gain = readDoubleFromFile(cone_avoid_gain_file); + g_cfg.cone_avoid_range = (int)readDoubleFromFile(cone_avoid_range_file); + g_cfg.cone_hold_frames = (int)readDoubleFromFile(cone_hold_frames_file); g_cfg.brake_scale = readDoubleFromFile(brake_scale_file); g_cfg.brake_max = readDoubleFromFile(brake_max_file); + g_cfg.curve_slope = readDoubleFromFile(curve_slope_file); + g_cfg.curve_min = readDoubleFromFile(curve_min_file); + g_cfg.lidar_thresh = (int)readDoubleFromFile(lidar_thresh_file); + g_cfg.lidar_pre = (int)readDoubleFromFile(lidar_pre_file); g_cfg.lidar_near = (int)readDoubleFromFile(lidar_near_file); g_cfg.lidar_far = (int)readDoubleFromFile(lidar_far_file); g_cfg.lidar_near_start = (int)readDoubleFromFile(lidar_near_start_file); diff --git a/src/image_cv.cpp b/src/image_cv.cpp index bbe4b63..4c1fe65 100644 --- a/src/image_cv.cpp +++ b/src/image_cv.cpp @@ -58,29 +58,19 @@ int line_tracking_height; // 处理高度 = 60 // ============================================================ cv::Mat image_binerize(cv::Mat &frame) { - cv::Mat output; - cv::Mat binarizedFrame; - cv::Mat hsvImage; + static cv::Mat output; + static cv::Mat binarizedFrame; + static cv::Mat hsvImage; + static std::vector hsvChannels(3); - // 1. BGR → HSV：分离色调(H)、饱和度(S)、明度(V) 三个通道 cv::cvtColor(frame, hsvImage, cv::COLOR_BGR2HSV); - - std::vector hsvChannels; cv::split(hsvImage, hsvChannels); - // hsvChannels[0] = H (色调, 0~180) - // hsvChannels[1] = S (饱和度, 0~255) - // hsvChannels[2] = V (明度, 0~255) - // 2. H 通道 Otsu 二值化：区分蓝色与非蓝色 cv::threshold(hsvChannels[0], binarizedFrame, 0, 255, cv::THRESH_BINARY_INV | cv::THRESH_OTSU); - - // 3. S 通道 Otsu 二值化：区分高饱和(蓝色赛道)与低饱和(灰色路面) cv::threshold(hsvChannels[1], output, 0, 255, cv::THRESH_BINARY_INV | cv::THRESH_OTSU); - // 4. 双通道融合：任一通道认为"非赛道"就排除 - // bitwise_or: 两幅图同为赛道(255)才保留，否则变为背景(0) cv::bitwise_or(output, binarizedFrame, output); return output; @@ -111,15 +101,11 @@ cv::Mat image_binerize(cv::Mat &frame) // ============================================================ cv::Mat find_road(cv::Mat &frame) { - // 1. 形态学开运算去噪 - // MORPH_CROSS: 十字形结构元素，2×2 - // MORPH_OPEN: 先腐蚀(去除小白点) 再膨胀(恢复区域尺寸) static cv::Mat kernel = cv::getStructuringElement(cv::MORPH_CROSS, cv::Size(2, 2)); cv::morphologyEx(binarizedFrame, morphologyExFrame, cv::MORPH_OPEN, kernel); - // 2. 创建洪泛填充蒙版 - // 尺寸比原图各边大 2 像素 (floodFill 要求) - cv::Mat mask = cv::Mat::zeros(line_tracking_height + 2, line_tracking_width + 2, CV_8UC1); + static cv::Mat mask; + mask = cv::Mat::zeros(line_tracking_height + 2, line_tracking_width + 2, CV_8UC1); // 3. 种子点位置 // X = 图像水平中心 (line_tracking_width/2) @@ -143,10 +129,8 @@ cv::Mat find_road(cv::Mat &frame) cv::floodFill(morphologyExFrame, mask, seedPoint, newVal, nullptr, loDiff, upDiff, 8); - // 7. 从蒙版提取赛道区域 - // mask 的外扩边框(±1) 用于 floodFill 的内部计算，实际区域在(1,1)起 - // ROI 裁掉边框后即为赛道主体蒙版 - cv::Mat outputImage = cv::Mat::zeros(line_tracking_height, line_tracking_width, CV_8UC1); + static cv::Mat outputImage; + outputImage = cv::Mat::zeros(line_tracking_height, line_tracking_width, CV_8UC1); mask(cv::Rect(1, 1, line_tracking_width, line_tracking_height)).copyTo(outputImage); return outputImage; @@ -167,10 +151,8 @@ cv::Mat find_road(cv::Mat &frame) // ============================================================ void image_main() { - cv::Mat resizedFrame; + static cv::Mat resizedFrame; - // ── 1. 降采样 ────────────────────────────────────── - // 320×240 → 80×60，大幅缩减计算量 cv::resize(raw_frame, resizedFrame, cv::Size(line_tracking_width, line_tracking_height)); diff --git a/src/model_v10.cpp b/src/model_v10.cpp index b7d8f8f..df16510 100644 --- a/src/model_v10.cpp +++ b/src/model_v10.cpp @@ -305,7 +305,7 @@ static void model_compute() { nullptr, nullptr, 15, 20, 64, 9, 1, 1, 1, false); } -static int decode(DetectBoxV10* boxes, int max_boxes, float thr) { +static int decode(DetectBoxV10* boxes, int max_boxes, const float* thresh) { const int N = OH * OW; int cnt = 0; @@ -323,6 +323,7 @@ static int decode(DetectBoxV10* boxes, int max_boxes, float thr) { } for (int c = 0; c < NC && cnt < max_boxes; ++c) { + float thr = thresh ? thresh[c] : 0.6f; for (int gy = 0; gy < OH; ++gy) { for (int gx = 0; gx < OW; ++gx) { const int idx = gy * OW + gx; @@ -453,7 +454,7 @@ bool model_v10_init(const char* path) { } int model_v10_detect_fmt(const uint8_t* img, int w, int h, MildPixFmt fmt, - DetectBoxV10* boxes, int max_boxes, float thr) { + DetectBoxV10* boxes, int max_boxes, const float* thresh) { if (!g_rdy || !img || !boxes || max_boxes <= 0 || w <= 1 || h <= 1) return 0; const uint8_t* net_in = img; @@ -463,7 +464,7 @@ int model_v10_detect_fmt(const uint8_t* img, int w, int h, MildPixFmt fmt, } preproc_160(net_in, fmt); model_compute(); - const int n = decode(boxes, max_boxes, thr); + const int n = decode(boxes, max_boxes, thresh); const float sx = (float)w / (float)IN_W; const float sy = (float)h / (float)IN_H; @@ -475,8 +476,7 @@ int model_v10_detect_fmt(const uint8_t* img, int w, int h, MildPixFmt fmt, } int model_v10_detect(const uint8_t* img, int w, int h, DetectBoxV10* boxes, int max_boxes, const float* thresh) { - float thr = thresh ? thresh[0] : 0.6f; - return model_v10_detect_fmt(img, w, h, MILD_FMT_BGR, boxes, max_boxes, thr); + return model_v10_detect_fmt(img, w, h, MILD_FMT_BGR, boxes, max_boxes, thresh); } const float* model_v10_forward_debug(const float* chw) { diff --git a/src/vl53l0x.cpp b/src/vl53l0x.cpp index bba6e38..4887da9 100644 --- a/src/vl53l0x.cpp +++ b/src/vl53l0x.cpp @@ -133,6 +133,36 @@ bool VL53L0X::readRange(VL53L0X_RangingMeasurementData_t &data) return true; } +bool VL53L0X::startMeasure() +{ + if (!initialized) return false; + VL53L0X_ClearInterruptMask(&dev, 0); + VL53L0X_Error Status = VL53L0X_StartMeasurement(&dev); + return (Status == VL53L0X_ERROR_NONE); +} + +bool VL53L0X::readResult(VL53L0X_RangingMeasurementData_t &data) +{ + if (!initialized) return false; + + uint8_t ready = 0; + VL53L0X_Error Status; + int timeout = 25; // 25ms max, 读不到就跳过 + while (!ready && --timeout > 0) { + Status = VL53L0X_GetMeasurementDataReady(&dev, &ready); + if (Status != VL53L0X_ERROR_NONE) + return false; + if (!ready) + usleep(1000); + } + + if (!ready) return false; + + Status = VL53L0X_GetRangingMeasurementData(&dev, &data); + VL53L0X_ClearInterruptMask(&dev, 0); + return (Status == VL53L0X_ERROR_NONE); +} + bool VL53L0X::stop() { if (fd >= 0) {