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

模型推理移至管道末尾降舵机延迟60→16ms + 视觉Mat复用 + 端到端延迟计时 + OpenCV 4.13 + WASD遥控

Browse Source
This commit is contained in:
spdis
2026-06-29 16:43:41 +08:00
parent 40d5cb1604
commit 017a827b18
16 changed files with 1201 additions and 709 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/camera.cpp
+525 -159
View File
File diff suppressed because it is too large Load Diff
src/control.cpp
+70 -16
View File
@@ -1,8 +1,15 @@
/*
* control — 电机控制层
*
* 开环占空比控制 + 斑马线编码器比例刹车。
* 开环占空比控制 + 编码器比例刹车（斑马线/红绿灯停车时触发）。
* 编码器线程: 高速轮询 gpio67 → 算速度(脉冲/秒) → 原子变量共享。
*
* 硬件映射:
* motor[0] = 左电机 pwmchip8/pwm2, GPIO12 方向
* motor[1] = 右电机 pwmchip8/pwm1, GPIO13 方向
* GPIO73 = 电机使能 (mortorEN)
* GPIO67 = 编码器 LSB 脉冲 (输入)
* GPIO72 = 编码器方向 (输入)
*/
#include "control.h"
#include "global.h"
@@ -10,17 +17,27 @@
#include <atomic>
#include <cmath>
// ═══════════════════════════════════════════════════════════
// 全局 & 静态变量
// ═══════════════════════════════════════════════════════════
MotorController *motorController[2] = {nullptr, nullptr};
GPIO mortorEN(73);
GPIO mortorEN(73); // 电机使能引脚
static GPIO encoderLSB(67);
static GPIO encoderDIR(72);
static GPIO encoderLSB(67); // 编码器 LSB 脉冲
static GPIO encoderDIR(72); // 编码器方向
static std::thread g_enc_thread;
static std::atomic<bool> g_enc_running{false};
static std::atomic<double> g_enc_speed{0.0};
static std::atomic<int> g_enc_dir{0};
static std::atomic<double> g_enc_speed{0.0}; // 编码器速度 (脉冲/秒)
static std::atomic<int> g_enc_dir{0}; // 编码器方向 (0/1)
// ═══════════════════════════════════════════════════════════
// 编码器线程 — 高频轮询测速
//
// ~2kHz (500µs sleep) 轮询 GPIO67 边沿计脉冲，
// 每 100ms 窗口计算一次速度写入原子变量 g_enc_speed。
// 仅在刹车时被 ControlUpdate 读取。
// ═══════════════════════════════════════════════════════════
static long enc_now_ns() {
struct timespec ts;
clock_gettime(CLOCK_MONOTONIC, &ts);
@@ -36,43 +53,58 @@ static void encoder_thread() {
int cur_lsb = encoderLSB.readValue() ? 1 : 0;
int cur_dir = encoderDIR.readValue() ? 1 : 0;
// 检测 LSB 边沿变化 → 计脉冲
if (cur_lsb != prev_lsb) {
pulse_cnt++;
prev_lsb = cur_lsb;
}
g_enc_dir.store(cur_dir);
// 每 100ms 窗口输出一次速度
long t1 = enc_now_ns();
long dt = t1 - t0;
if (dt >= 100000000L) {
if (dt >= 100000000L) { // 100ms
g_enc_speed.store(pulse_cnt / (dt / 1e9));
pulse_cnt = 0;
t0 = t1;
}
// 500µs 间隔，防止 CPU 饿死其他线程
std::this_thread::sleep_for(std::chrono::microseconds(500));
}
}
// ═══════════════════════════════════════════════════════════
// ControlInit — 初始化电机 + 编码器线程
//
// 调用时机: main.cpp 启动时，CameraInit 之后。
// ═══════════════════════════════════════════════════════════
void ControlInit()
{
// 电机使能引脚
mortorEN.setDirection("out");
mortorEN.setValue(1);
// 编码器引脚
encoderLSB.setDirection("in");
encoderDIR.setDirection("in");
// 启动编码器测速线程
g_enc_running.store(true);
g_enc_thread = std::thread(encoder_thread);
// GPIO13 (左电机 IN2) 设为高电平，配合 PWM 实现 H 桥正/反转
GPIO leftIn2(13);
leftIn2.setDirection("out");
leftIn2.setValue(1);
// 创建双电机控制器
// motor[0]: pwmchip8/pwm2 + GPIO12 方向 (左电机)
// motor[1]: pwmchip8/pwm1 + GPIO13 方向 (右电机)
const int pwmchip[2] = {8, 8};
const int pwmnum[2] = {2, 1};
const int gpioNum[2] = {12, 13};
const unsigned int period_ns = 50000;
const unsigned int period_ns = 50000; // PWM 周期 50µs = 20kHz
for (int i = 0; i < 2; ++i)
{
@@ -82,55 +114,77 @@ void ControlInit()
}
}
// ═══════════════════════════════════════════════════════════
// ControlUpdate — 每帧电机控制入口
//
// speed: 目标速度 (占空比 %)，已经过弯道减速处理
// zebra_block: true = 斑马线 STOP 或红绿灯 STOP/WAIT_GREEN
//
// 三种模式:
// 1. g_cfg.start == 0 → 全部停转，拉低使能
// 2. zebra_block → 编码器比例刹车 (反向 PWM)
// 3. 正常 → 开环占空比驱动
// ═══════════════════════════════════════════════════════════
void ControlUpdate(double speed, bool zebra_block)
{
// ── 模式 1: 未启动 → 停转 ──
if (!g_cfg.start)
{
for (int i = 0; i < 2; ++i)
if (motorController[i]) motorController[i]->updateduty(0);
mortorEN.setValue(0);
mortorEN.setValue(0); // 拉低使能
return;
}
// ── 模式 2: 刹车 (斑马线/红绿灯停车) ──
if (zebra_block)
{
double cur_speed = g_enc_speed.load();
int cur_dir = g_enc_dir.load();
double cur_speed = g_enc_speed.load(); // 原子读: 编码器速度 (pps)
int cur_dir = g_enc_dir.load(); // 原子读: 编码器方向
if (cur_speed > 0.5)
if (cur_speed > 0.5) // 仍在运动
{
// 比例刹车: 速度越快 → 刹车占空比越大
int brake_ns = (int)(cur_speed * g_cfg.brake_scale);
if (brake_ns > g_cfg.brake_max) brake_ns = g_cfg.brake_max;
double brake_pct = brake_ns / 500.0; // ns → % (period=50000ns)
double brake_pct = brake_ns / 500.0; // ns → % (period=50000ns, 50000/100=500)
int dir = cur_dir ? 1 : 0;
// 施加反向 PWM: 方向取反实现刹车
for (int i = 0; i < 2; ++i)
if (motorController[i])
motorController[i]->updateduty(dir ? -brake_pct : brake_pct);
}
else
else // 已停止 (< 0.5 pps)
{
// 不再施加刹车，防止反冲
for (int i = 0; i < 2; ++i)
if (motorController[i]) motorController[i]->updateduty(0);
}
return;
}
// ── 模式 3: 正常行驶 → 开环占空比 ──
for (int i = 0; i < 2; ++i)
if (motorController[i]) motorController[i]->updateduty(speed);
mortorEN.setValue(1);
mortorEN.setValue(1); // 使能拉高
}
// ═══════════════════════════════════════════════════════════
// ControlExit — 停止编码器线程 + 释放电机资源
// ═══════════════════════════════════════════════════════════
void ControlExit()
{
// 停止编码器线程
g_enc_running.store(false);
if (g_enc_thread.joinable()) g_enc_thread.join();
// 释放电机控制器
for (int i = 0; i < 2; ++i)
{
delete motorController[i];
motorController[i] = nullptr;
}
mortorEN.setValue(0);
mortorEN.setValue(0); // 使能拉低
}
src/global.cpp
+4
View File
@@ -24,6 +24,8 @@ void cfg_load_all()
{
g_cfg.speed = readDoubleFromFile(speed_file);
g_cfg.foresee = readDoubleFromFile(foresee_file);
g_cfg.foresee_lost_scale = readDoubleFromFile(foresee_lost_scale_file);
g_cfg.sharp_turn_scale = readDoubleFromFile(sharp_turn_scale_file);
g_cfg.zebrasee = readDoubleFromFile(zebrasee_file);
g_cfg.deadband = readDoubleFromFile(deadband_file);
g_cfg.steer_gain = readDoubleFromFile(steer_gain_file);
@@ -39,6 +41,8 @@ void cfg_load_all()
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.cone_return_gain = readDoubleFromFile(cone_return_gain_file);
g_cfg.cone_return_frames = (int)readDoubleFromFile(cone_return_frames_file);
g_cfg.brake_scale = readDoubleFromFile(brake_scale_file);
g_cfg.brake_max = readDoubleFromFile(brake_max_file);
src/image_cv.cpp
+10 -2
View File
@@ -30,6 +30,9 @@ cv::Mat track;
std::vector<int> left_line;
std::vector<int> right_line;
std::vector<int> mid_line;
std::vector<int> mid_line_raw;
int g_lost_rows = 0;
int line_tracking_width; // 处理宽度 = 80
int line_tracking_height; // 处理高度 = 60
@@ -105,7 +108,8 @@ cv::Mat find_road(cv::Mat &frame)
cv::morphologyEx(binarizedFrame, morphologyExFrame, cv::MORPH_OPEN, kernel);
static cv::Mat mask;
mask = cv::Mat::zeros(line_tracking_height + 2, line_tracking_width + 2, CV_8UC1);
if (mask.empty()) mask.create(line_tracking_height + 2, line_tracking_width + 2, CV_8UC1);
mask.setTo(0);
// 3. 种子点位置
// X = 图像水平中心 (line_tracking_width/2)
@@ -130,7 +134,8 @@ cv::Mat find_road(cv::Mat &frame)
nullptr, loDiff, upDiff, 8);
static cv::Mat outputImage;
outputImage = cv::Mat::zeros(line_tracking_height, line_tracking_width, CV_8UC1);
if (outputImage.empty()) outputImage.create(line_tracking_height, line_tracking_width, CV_8UC1);
outputImage.setTo(0);
mask(cv::Rect(1, 1, line_tracking_width, line_tracking_height)).copyTo(outputImage);
return outputImage;
@@ -244,11 +249,13 @@ void image_main()
// ── 6. 中线计算 + 丢线补全 ─────
// 从倒数第二行开始向上，底行(row=height-1)单独兜底避免 mid_line[row+1] 越界
g_lost_rows = 0;
for (int row = line_tracking_height - 2; row >= 10; --row)
{
// ── 6a. 丢线补全 ──────────────────────────────
if (left_line[row] == -1 && right_line[row] == -1)
{
g_lost_rows++;
// 当前行完全丢线: 用下行(row+1)的中线来虚拟补线
mid_line[row] = mid_line[row + 1];
@@ -278,6 +285,7 @@ void image_main()
const int row = line_tracking_height - 1;
if (left_line[row] == -1 && right_line[row] == -1)
{
g_lost_rows++;
mid_line[row] = line_tracking_width / 2;
left_line[row] = 0;
right_line[row] = line_tracking_width - 1;