spdis/Loongson_2k0300_SmartCar
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

模型每类阈值+配置热重载+雷达纯距离触发+速度min不叠加+弯道参数化

Browse Source
This commit is contained in:
spdis
2026-06-21 14:15:53 +08:00
parent 95a46c130d
commit 40d5cb1604
11 changed files with 234 additions and 147 deletions
Download Patch File Download Diff File Expand all files Collapse all files
lib/global.h
+9 -1
View File
@@ -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 检测起始偏移
lib/vl53l0x.h
+2
View File
@@ -14,6 +14,8 @@ public:
~VL53L0X();
bool init();
bool startMeasure();
bool readResult(VL53L0X_RangingMeasurementData_t &data);
bool readRange(VL53L0X_RangingMeasurementData_t &data);
bool stop();
main/CMakeLists.txt
+4 -1
View File
@@ -1,3 +1,6 @@
# 主程序
add_executable(smartcar_demo1 main.cpp)
target_link_libraries(smartcar_demo1 common_lib ${OpenCV_LIBS})
target_link_libraries(smartcar_demo1 common_lib ${OpenCV_LIBS})
add_executable(lidar_test lidar_test.cpp)
target_link_libraries(lidar_test common_lib)
main/lidar_test.cpp
+49
View File
@@ -0,0 +1,49 @@
#include <cstdio>
#include <csignal>
#include <atomic>
#include <unistd.h>
#include "vl53l0x.h"
static std::atomic<bool> 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;
}
main/main.cpp
-16
View File
@@ -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;
src/camera.cpp
+118 -90
View File
@@ -15,6 +15,7 @@
#include <cstring>
#include <sstream>
#include <iomanip>
#include <malloc.h>
// ═══════════════════════════════════════════════════════════
// 模型检测类别
@@ -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;
src/control.cpp
+1 -7
View File
@@ -10,8 +10,6 @@
#include <atomic>
#include <cmath>
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);
}
src/global.cpp
+7
View File
@@ -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);
src/image_cv.cpp
+9 -27
View File
@@ -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<cv::Mat> hsvChannels(3);
// 1. BGR → HSV：分离色调(H)、饱和度(S)、明度(V) 三个通道
cv::cvtColor(frame, hsvImage, cv::COLOR_BGR2HSV);
std::vector<cv::Mat> 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));
src/model_v10.cpp
+5 -5
View File
@@ -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) {
src/vl53l0x.cpp
+30
View File
@@ -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) {