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

feat: 斑马线停车LCD图片提示 + 冷却期bang-bang转向

Browse Source 
- 斑马线Z_STOP期间LCD显示pedestrian_stop.png图片，不受showImg控制
- 进入/离开Z_STOP时清屏，仅状态切换时操作LCD，无每帧开销
- Z_COOLDOWN 3.3~5s内：一次方向锁定打满机会（偏差出现->打满->方向翻转/回正即退出）
- Z_COOLDOWN 3.3~5s降速上限13，foresee缩短至24
- 程序退出前自动清屏
- CMake: OpenCV_DIR改为本地交叉编译4.13.0路径
- 主程序改名为smartcar_test
This commit is contained in:
spdis
2026-07-04 20:42:47 +08:00
parent 2cab70791e
commit 06bb64ad26
3 changed files with 233 additions and 62 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CMakeLists.txt
+1 -1
View File
@@ -14,7 +14,7 @@ set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -O3 -Wall")
project(smartcar_demo2 VERSION 0.1.0 LANGUAGES C CXX)
# OpenCV 设备端路径
set(OpenCV_DIR /home/ilikara/loongson/opencv-4.13.0/loongson/lib/cmake/opencv4)
set(OpenCV_DIR /home/spdis/loongson/opencv-4.13.0/loongson/lib/cmake/opencv4)
find_package(OpenCV REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS})
message(STATUS "OpenCV Include Directories: ${OpenCV_INCLUDE_DIRS}")
main/CMakeLists.txt
+5 -2
View File
@@ -1,9 +1,12 @@
# 主程序
add_executable(smartcar_demo1 main.cpp)
target_link_libraries(smartcar_demo1 common_lib ${OpenCV_LIBS})
add_executable(smartcar_test main.cpp)
target_link_libraries(smartcar_test common_lib ${OpenCV_LIBS})
add_executable(lidar_test lidar_test.cpp)
target_link_libraries(lidar_test common_lib)
add_executable(remote_control remote_control.cpp)
target_link_libraries(remote_control common_lib)
add_executable(lcd_test lcd_test.cpp)
target_link_libraries(lcd_test common_lib ${OpenCV_LIBS})
src/camera.cpp
+225 -57
View File
@@ -25,6 +25,7 @@
#include "camera.h"
#include "model_v10.hpp"
#include "vl53l0x.h"
#include "font_bitmap.h"
#include <fcntl.h>
#include <unistd.h>
@@ -58,6 +59,8 @@ int screenWidth, screenHeight; // LCD 屏幕物理分辨率 (从 /dev/f
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
// 巡线空间到显示空间的缩放因子
@@ -72,7 +75,7 @@ static constexpr int calc_scale = 2;
// g_boxes: 检测结果缓冲区 (最多 16 个框)
// g_box_count: 当前帧有效检测框数
// ═══════════════════════════════════════════════════════════
static float g_thresh[4] = {0.80f, 0.75f, 0.80f, 0.82f};
static float g_thresh[4] = {0.80f, 0.70f, 0.80f, 0.85f};
static DetectBoxV10 g_boxes[16];
static int g_box_count = 0;
@@ -94,6 +97,9 @@ 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=左满
// ═══════════════════════════════════════════════════════════
// 红绿灯状态机
@@ -212,6 +218,76 @@ static void play_zebra_audio()
}
// ═══════════════════════════════════════════════════════════
// LCD 中文文字渲染
//
// 使用 src/font_bitmap.h 中嵌入的 16x16 字模
// ═══════════════════════════════════════════════════════════
static const uint8_t* find_glyph(uint32_t codepoint)
{
for (int i = 0; i < FONT_GLYPH_COUNT; ++i)
if (FONT_GLYPHS[i].unicode == codepoint) return FONT_GLYPHS[i].bitmap;
return nullptr;
}
static uint32_t utf8_decode(const char*& p)
{
unsigned char c = static_cast<unsigned char>(*p);
if (c == 0) return 0;
if ((c & 0x80) == 0) { ++p; return c; }
if ((c & 0xE0) == 0xC0) {
uint32_t v = c & 0x1F; ++p;
v = (v << 6) | (static_cast<unsigned char>(*p++) & 0x3F);
return v;
}
if ((c & 0xF0) == 0xE0) {
uint32_t v = c & 0x0F; ++p;
v = (v << 6) | (static_cast<unsigned char>(*p++) & 0x3F);
v = (v << 6) | (static_cast<unsigned char>(*p++) & 0x3F);
return v;
}
if ((c & 0xF8) == 0xF0) {
uint32_t v = c & 0x07; ++p;
v = (v << 6) | (static_cast<unsigned char>(*p++) & 0x3F);
v = (v << 6) | (static_cast<unsigned char>(*p++) & 0x3F);
v = (v << 6) | (static_cast<unsigned char>(*p++) & 0x3F);
return v;
}
++p;
return '?';
}
static void lcd_draw_glyph(cv::Mat& img, int x0, int y0, const uint8_t* glyph, const cv::Scalar& color)
{
for (int row = 0; row < FONT_SIZE; ++row) {
for (int col = 0; col < FONT_SIZE; ++col) {
if (glyph[row * FONT_SIZE + col] > 128) {
int px = x0 + col;
int py = y0 + row;
if (px >= 0 && px < img.cols && py >= 0 && py < img.rows)
img.at<cv::Vec3b>(py, px) = cv::Vec3b(static_cast<uchar>(color[0]),
static_cast<uchar>(color[1]),
static_cast<uchar>(color[2]));
}
}
}
}
static void lcd_draw_chinese_text(cv::Mat& img, const char* text, int x, int y, const cv::Scalar& color)
{
const char* p = text;
int cx = x;
while (*p) {
uint32_t cp = utf8_decode(p);
const uint8_t* glyph = find_glyph(cp);
if (glyph) {
lcd_draw_glyph(img, cx, y, glyph, color);
cx += FONT_SIZE;
}
}
}
// ═══════════════════════════════════════════════════════════
// CameraInit — 系统初始化
//
@@ -240,12 +316,19 @@ int CameraInit(int camera_id)
std::cerr << "无法获取帧缓冲区信息" << std::endl; close(fb); return -1;
}
screenWidth = vinfo.xres; screenHeight = vinfo.yres;
size_t fb_size = vinfo.yres_virtual * vinfo.xres_virtual * vinfo.bits_per_pixel / 8;
fb_buffer = (uint16_t *)mmap(NULL, fb_size, PROT_READ | PROT_WRITE, MAP_SHARED, fb, 0);
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
@@ -257,7 +340,7 @@ int CameraInit(int camera_id)
cap.set(cv::CAP_PROP_CONVERT_RGB, 0); // 关键: 不让 V4L2 转 RGB
if (!cap.isOpened()) {
printf("无法打开摄像头\n");
munmap(fb_buffer, fb_size); close(fb); return -1;
munmap(fb_buffer, g_fb_size); close(fb); return -1;
}
int camW = cap.get(cv::CAP_PROP_FRAME_WIDTH);
@@ -305,16 +388,18 @@ int CameraInit(int camera_id)
return 0;
}
// 前置声明: LCD 辅助函数定义在后方
static void lcd_clear_screen();
// ═══════════════════════════════════════════════════════════
// cameraDeInit — 释放所有资源
// ═══════════════════════════════════════════════════════════
void cameraDeInit(void)
{
cap.release(); // 释放摄像头
struct fb_var_screeninfo vinfo;
if (ioctl(fb, FBIOGET_VSCREENINFO, &vinfo) != -1) {
size_t fb_size = vinfo.yres_virtual * vinfo.xres_virtual * vinfo.bits_per_pixel / 8;
munmap(fb_buffer, fb_size); // 解除 LCD mmap
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 音频
@@ -685,8 +770,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_return_ctr > 0 && g_cone_return_ctr <= g_cfg.cone_return_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_return_frames);
}
static void cone_detect_and_deform()
@@ -760,7 +845,7 @@ static void cone_detect_and_deform()
g_cone_frames, g_cone_confirmed, g_cone_hold_ctr, g_cone_return_ctr);
}
// ── 3. 三阶段生命周期: 确认 → 保持衰减 → 回弹 ──
// ── 3. 两阶段生命周期: 确认 → 保持衰减 ──
if (g_cone_confirmed) {
g_cone_hold_ctr = 1;
g_cone_return_ctr = 0;
@@ -769,26 +854,26 @@ static void cone_detect_and_deform()
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;
// 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) {
// 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;
}
// 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;
@@ -818,9 +903,9 @@ static void cone_detect_and_deform()
// 保持阶段衰减: 从 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 <= src_row; ++row) {
// ── 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;
@@ -829,31 +914,31 @@ static void cone_detect_and_deform()
(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);
}
}
// 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);
// }
// }
}
@@ -900,11 +985,12 @@ static void detect_sharp_turn()
static void steering_update()
{
if (!g_cfg.start) return;
// 斑马线起步后延迟1.5s, 再5s内缩短前瞻, 先走稳再改
// 斑马线起步后延迟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 = 35;
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; // 丢线行 → 跳过
@@ -915,6 +1001,38 @@ static void steering_update()
// 归一化到 [-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;
}
}
}
}
if (std::abs(deviation) < g_cfg.deadband) {
// 死区内 → 直行
servo.setDutyCycle(1500000);
@@ -945,13 +1063,17 @@ static void steering_update()
// 起步弹射: 启动/斑马线恢复后 1.5x 速度持续 0.5s 快速起步
// ═══════════════════════════════════════════════════════════
static struct timespec g_boost_end = {0, 0};
static double g_boost_mul = 2.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()
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)(BOOST_SECS * 1e9);
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;
@@ -972,13 +1094,23 @@ static void motor_update(bool zebra_block, bool tl_block)
// 起步弹射检测
static int prev_start = 0;
static ZState prev_zstate = Z_NORMAL;
if (g_cfg.start && !prev_start) trigger_boost(); // 启动
if (prev_zstate == Z_STOP && g_zstate == Z_COOLDOWN) trigger_boost(); // 斑马线起步
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 = target_speed * boost_factor();
// 斑马线起步后 3.3~5s 降速走稳
if (g_zstate == Z_COOLDOWN) {
time_t elapsed = time(nullptr) - g_ztime;
if (elapsed >= 3.3 && elapsed < 5)
spd = std::min(spd, 13.0);
}
// 弯道减速: 舵机偏差越大 → 速度越低
double curve = 1.0 - std::abs(g_steer_deviation) * g_cfg.curve_slope;
if (curve < g_cfg.curve_min) curve = g_cfg.curve_min;
@@ -1019,16 +1151,37 @@ static void motor_update(bool zebra_block, bool tl_block)
// ═══════════════════════════════════════════════════════════
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);
@@ -1211,6 +1364,21 @@ int CameraHandler(void)
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 日志