commit 28d9c6da588819f834e30151f40b6ea40286de67 Author: spdis Date: Mon May 25 10:31:55 2026 +0800 Initial commit: SmartCar Framework v0.1 — 龙芯2K0300智能车开发框架\n\n- HAL: GPIO/PWM/Encoder/Framebuffer 驱动\n- Control: PID/IMU/Motor/Servo 控制\n- Vision: HSV双Otsu→4点标定IPM→洪泛填充→逐行搜线\n- Strategy: 三区前瞻偏差+速度策略\n- Debug: 文件热调参+LCD预览+cv截帧\n- Scheduler: 5ms timerfd+epoll 中央调度器 diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..e2b639e --- /dev/null +++ b/.gitignore @@ -0,0 +1,7 @@ +build/ +*.o +*.a +smartcar_framework +bench/bench +.vscode/ +*~ diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..abbe9f7 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,32 @@ +include(cross.cmake) +cmake_minimum_required(VERSION 3.5.0) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -pthread -Wall") +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -O3 -Wall") + +project(smartcar_framework VERSION 0.1.0 LANGUAGES C CXX) + +set(OpenCV_DIR /home/ilikara/loongson/opencv-4.11.0/loongson) +find_package(OpenCV REQUIRED) +include_directories(${OpenCV_INCLUDE_DIRS}) + +include_directories( + . + hal + vision + control + strategy + debug + model +) + +aux_source_directory(hal DIR_SRCS) +aux_source_directory(vision DIR_SRCS) +aux_source_directory(control DIR_SRCS) +aux_source_directory(strategy DIR_SRCS) +aux_source_directory(debug DIR_SRCS) +aux_source_directory(model DIR_SRCS) + +add_executable(smartcar_framework main.cpp scheduler.cpp ${DIR_SRCS}) +target_link_libraries(smartcar_framework ${OpenCV_LIBS} pthread) diff --git a/bench/bench.cpp b/bench/bench.cpp new file mode 100644 index 0000000..bb6f2d2 --- /dev/null +++ b/bench/bench.cpp @@ -0,0 +1,128 @@ +#include +#include +#include +#include + +using Clock = std::chrono::high_resolution_clock; + +static volatile int32_t g_sink = 0; + +static constexpr int BLK = 512; +static int8_t A[BLK*BLK] __attribute__((aligned(64))); +static int8_t B[BLK*BLK] __attribute__((aligned(64))); +static int32_t C[BLK*BLK] __attribute__((aligned(64))); + +static void init() +{ + for (int i = 0; i < BLK*BLK; ++i) { + A[i] = (int8_t)((i * 13 + 7) % 127 - 63); + B[i] = (int8_t)((i * 29 + 3) % 127 - 63); + } +} + +// 运行足够 reps 使时长 >= target_sec +static double bench(int M, int N, int K, double target_sec) +{ + auto run = [&](int reps) { + auto t0 = Clock::now(); + for (int r = 0; r < reps; ++r) { + for (int i = 0; i < M; ++i) { + int8_t* __restrict ar = A + i * K; + int32_t* __restrict cr = C + i * N; + for (int j = 0; j < N; ++j) { + int32_t s = 0; + for (int k = 0; k < K; ++k) + s += (int32_t)ar[k] * (int32_t)B[k * N + j]; + cr[j] = s; + } + } + g_sink += C[0]; + } + auto t1 = Clock::now(); + return std::chrono::duration(t1 - t0).count(); + }; + + // 短跑估计 rep + int reps = 1; + double t = run(reps); + while (t < 0.1 && reps < 10000) { reps *= 2; t = run(reps); } + + // 按 target_sec 缩放 reps + int final_reps = (int)(reps * target_sec / t); + if (final_reps < 1) final_reps = 1; + + // 正式跑 + double t_real = run(final_reps); + if (t_real < 0.5) { + final_reps *= 10; + t_real = run(final_reps); + } + + double ops = (double)M * N * K * final_reps * 2; // 1 MAC = 2 ops + double gops = ops / t_real / 1e9; + printf(" (%4dx%4dx%4d) x%6d rep %7.1f ms → %8.3f GOPS (%8.3f GMACS)\n", + M, N, K, final_reps, t_real * 1000.0, gops, gops/2.0); + return gops; +} + +int main() +{ + printf("═══════════════════════════════════════════\n"); + printf(" INT8 算力持续压力测试 (每项 ~10s)\n"); + printf("═══════════════════════════════════════════\n\n"); + + init(); + // warmup + bench(64, 64, 64, 1.0); + + const double T = 10.0; // 每项跑 10 秒 + + struct { int m,n,k; const char* desc; } tests[] = { + { 16, 16, 16, "微型 16x16x16" }, + { 32, 32, 32, "小阵 32x32x32" }, + { 64, 64, 64, "中阵 64x64x64" }, + {128, 64, 64, "宽中 128x64x64" }, + {256, 64, 64, "宽大 256x64x64" }, + {128,128, 64, "大方 128x128x64" }, + { 64,128, 64, "长条 64x128x64" }, + { 64, 64,128, "深乘 64x64x128" }, + { 32,128,128, "扁平 32x128x128" }, + }; + + double sum = 0; int cnt = 0; + for (auto& t : tests) + { + double g = bench(t.m, t.n, t.k, T); + sum += g; cnt++; + } + + double avg = sum / cnt; + printf("\n═══════════════════════════════════════════\n"); + printf(" INT8 平均: %.3f GOPS\n", avg); + printf("═══════════════════════════════════════════\n"); + + printf("\n 模型帧率预估 (INT8):\n"); + printf(" %-30s %8s %8s\n", "模型", "MACs(M)", "FPS"); + printf(" ----------------------------------------------------\n"); + + struct { const char* n; double m; } models[] = { + {"TinyCNN 40x30x3 -> 8ch k3", 3}, + {"TinyCNN 40x30x8 -> 16ch k3", 8}, + {"TinyCNN 40x30x16 -> 32ch k3", 18}, + {"TinyCNN 56x56x3 -> 16ch k3", 12}, + {"TinyCNN 56x56x16 -> 32ch k3", 45}, + {"MicroNet 32x32x8, 3层", 5}, + {"MicroNet 32x32x16, 3层", 15}, + {"MicroNet 48x48x8, 3层", 12}, + {"MicroNet 48x48x16, 3层", 40}, + {"ShuffleNetV2 0.5x", 41}, + {"MobileNetV2 0.35x 96x96", 35}, + {"FC-128 (分类头)", 0.1}, + }; + + for (auto& m : models) + printf(" %-30s %8.0f %8.1f\n", m.n, m.m, avg * 1e3 / m.m); + + printf("\n >>> 稳 30FPS 上限: %.0fM MACs (INT8 %.3f GOPS)\n\n", avg * 1e3 / 30.0, avg); + return 0; +} diff --git a/build.sh b/build.sh new file mode 100644 index 0000000..92ab485 --- /dev/null +++ b/build.sh @@ -0,0 +1,14 @@ +#!/bin/bash +export PATH="/opt/loongson-gnu-toolchain-8.3-x86_64-loongarch64-linux-gnu-rc1.6/bin:$PATH" + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +BUILD_DIR="${SCRIPT_DIR}/build" +REMOTE_IP="192.168.31.21" +REMOTE_USER="root" +REMOTE_PATH="/home/root/" + +mkdir -p "${BUILD_DIR}" && cd "${BUILD_DIR}" || exit 1 +cmake "${SCRIPT_DIR}" || exit 1 +make -j$(nproc) || exit 1 +scp -O smartcar_framework "${REMOTE_USER}@${REMOTE_IP}:${REMOTE_PATH}" || exit 1 +echo "[OK] Build + upload done" diff --git a/control/imu.cpp b/control/imu.cpp new file mode 100644 index 0000000..443aa81 --- /dev/null +++ b/control/imu.cpp @@ -0,0 +1,81 @@ +#include "imu.hpp" +#include +#include +#include +#include +#include + +YawTracker::YawTracker() : _fd(-1), _yaw(0), _unbounded_yaw(0), _gyro_z(0), _gyro_bias(0), _ready(false) {} + +bool YawTracker::begin(const char* device, int baud) +{ + _fd = open(device, O_RDWR | O_NOCTTY); + if (_fd < 0) return false; + + termios opt; + tcgetattr(_fd, &opt); + cfsetispeed(&opt, B115200); + cfsetospeed(&opt, B115200); + opt.c_cflag |= (CLOCAL | CREAD); + opt.c_cflag &= ~PARENB; + opt.c_cflag &= ~CSTOPB; + opt.c_cflag &= ~CSIZE; + opt.c_cflag |= CS8; + tcsetattr(_fd, TCSANOW, &opt); + + calibrate(); + return true; +} + +void YawTracker::calibrate(int samples) +{ + float sum = 0; + for (int i = 0; i < samples; ++i) + { + int ret = _parseJY62(); + if (ret > 0) sum += _gyro_z; + usleep(5000); + } + _gyro_bias = sum / samples; + _yaw = 0; + _unbounded_yaw = 0; + _ready = true; +} + +void YawTracker::update(float dt) +{ + if (_parseJY62() <= 0) return; + + float gyro_filt = _gyro_z - _gyro_bias; + if (std::fabs(gyro_filt) < 0.03f) gyro_filt = 0; // 死区 + + _unbounded_yaw += gyro_filt * dt; + _yaw = _unbounded_yaw; + + while (_yaw > 180.0f) _yaw -= 360.0f; + while (_yaw < -180.0f) _yaw += 360.0f; +} + +int YawTracker::_parseJY62() +{ + uint8 buf[11]; + int total = 0; + while (total < 11) + { + int n = read(_fd, buf + total, 11 - total); + if (n <= 0) return -1; + total += n; + } + + while (buf[0] != 0x55 && total > 10) + { + memmove(buf, buf + 1, 10); + int n = read(_fd, buf + 10, 1); + if (n <= 0) return -1; + } + if (buf[0] != 0x55) return -1; + + int16 gz = (buf[5] << 8) | buf[4]; + _gyro_z = gz * (2000.0f / 32768.0f); // ±2000°/s + return 1; +} diff --git a/control/imu.hpp b/control/imu.hpp new file mode 100644 index 0000000..9a28230 --- /dev/null +++ b/control/imu.hpp @@ -0,0 +1,24 @@ +#pragma once +#include "types.hpp" +#include + +class YawTracker { +public: + YawTracker(); + bool begin(const char* device = IMU_DEVICE, int baud = IMU_BAUDRATE); + void update(float dt); + void calibrate(int samples = 200); + + float yaw() const { return _yaw; } + float unbounded_yaw() const { return _unbounded_yaw; } + float gyro_z() const { return _gyro_z; } + bool ready() const { return _ready; } + +private: + int _fd; + float _yaw, _unbounded_yaw; + float _gyro_z; + float _gyro_bias; + bool _ready; + int _parseJY62(); +}; diff --git a/control/motor.cpp b/control/motor.cpp new file mode 100644 index 0000000..c64a8bb --- /dev/null +++ b/control/motor.cpp @@ -0,0 +1,72 @@ +#include "motor.hpp" +#include "hal/gpio.hpp" +#include "hal/pwm.hpp" +#include "types.hpp" +#include +#include + +static PWM* pwm_left = nullptr; +static PWM* pwm_right = nullptr; +static GPIO* dir_left = nullptr; +static GPIO* dir_right = nullptr; +static GPIO* left_in2 = nullptr; +static GPIO* mortor_en = nullptr; +static bool initialized = false; + +void motor_init() +{ + if (initialized) return; + + mortor_en = new GPIO(MOTOR_ENABLE_GPIO); + mortor_en->setDirection("out"); + mortor_en->setValue(1); + + left_in2 = new GPIO(MOTOR_LEFT_IN2_GPIO); + left_in2->setDirection("out"); + left_in2->setValue(0); + + pwm_left = new PWM(MOTOR_PWMCHIP, MOTOR_LEFT_PWMID, MOTOR_PWM_PERIOD_NS); + pwm_right = new PWM(MOTOR_PWMCHIP, MOTOR_RIGHT_PWMID, MOTOR_PWM_PERIOD_NS); + pwm_left->enable(); + pwm_right->enable(); + + dir_left = new GPIO(MOTOR_LEFT_DIR_GPIO); + dir_right = new GPIO(MOTOR_RIGHT_DIR_GPIO); + dir_left->setDirection("out"); + dir_right->setDirection("out"); + + initialized = true; +} + +void motor_set_pwm(int idx, float duty) +{ + if (!initialized) return; + duty = std::clamp(duty, -100.0f, 100.0f); + + PWM* pwm = (idx == MOTOR_LEFT) ? pwm_left : pwm_right; + GPIO* dir = (idx == MOTOR_LEFT) ? dir_left : dir_right; + + if (duty >= 0) + { + dir->setValue(1); + unsigned int d_ns = static_cast(MOTOR_PWM_PERIOD_NS * duty / 100.0f); + pwm->setDutyCycle(d_ns); + } + else + { + dir->setValue(0); + unsigned int d_ns = static_cast(MOTOR_PWM_PERIOD_NS * (-duty) / 100.0f); + pwm->setDutyCycle(d_ns); + } +} + +void motor_stop_all() +{ + motor_set_pwm(MOTOR_LEFT, 0); + motor_set_pwm(MOTOR_RIGHT, 0); +} + +void motor_enable(bool on) +{ + if (mortor_en) mortor_en->setValue(on ? 1 : 0); +} diff --git a/control/motor.hpp b/control/motor.hpp new file mode 100644 index 0000000..4663c5e --- /dev/null +++ b/control/motor.hpp @@ -0,0 +1,6 @@ +enum MotorIndex { MOTOR_LEFT = 0, MOTOR_RIGHT = 1 }; + +void motor_init(); +void motor_set_pwm(int idx, float duty); // duty: -100 ~ +100 +void motor_stop_all(); +void motor_enable(bool on); diff --git a/control/pid.cpp b/control/pid.cpp new file mode 100644 index 0000000..c467986 --- /dev/null +++ b/control/pid.cpp @@ -0,0 +1,53 @@ +#include "pid.hpp" +#include +#include + +PID::PID(float kp, float ki, float kd, float target, + PIDMode mode, float out_limit, float int_limit) + : _kp(kp), _ki(ki), _kd(kd), _target(target), _mode(mode) + , _out_limit(out_limit), _int_limit(int_limit) + , _output(0), _integral(0), _prev_error(0), _last_measured(0), _first(true) {} + +void PID::setPID(float kp, float ki, float kd) +{ + _kp = kp; _ki = ki; _kd = kd; + reset(); +} + +void PID::reset() +{ + _integral = 0; + _prev_error = 0; + _output = 0; + _first = true; +} + +float PID::update(float measured) +{ + float error = _target - measured; + + if (_mode == PID_POSITIONAL) + { + _integral += error; + _integral = std::clamp(_integral, -_int_limit, _int_limit); + + float derivative = 0; + if (!_first) derivative = error - _prev_error; + + _output = _kp * error + _ki * _integral + _kd * derivative; + } + else // PID_INCREMENTAL + { + float inc = _kp * (error - _prev_error) + _ki * error; + if (!_first) + inc += _kd * (error - 2.0f * _prev_error + (_prev_error - (_target - _last_measured))); + _output += inc; + } + + _output = std::clamp(_output, -_out_limit, _out_limit); + _prev_error = error; + _last_measured = measured; + _first = false; + + return _output; +} diff --git a/control/pid.hpp b/control/pid.hpp new file mode 100644 index 0000000..9785666 --- /dev/null +++ b/control/pid.hpp @@ -0,0 +1,28 @@ +#pragma once +#include "types.hpp" + +class PID { +public: + PID(float kp = 0, float ki = 0, float kd = 0, float target = 0, + PIDMode mode = PID_INCREMENTAL, float out_limit = 100, float int_limit = 100); + + float update(float measured); + void setTarget(float t) { _target = t; } + void setPID(float kp, float ki, float kd); + void setMode(PIDMode m) { _mode = m; } + void reset(); + + float output() const { return _output; } + +private: + float _kp, _ki, _kd; + float _target; + PIDMode _mode; + float _out_limit, _int_limit; + + float _output; + float _integral; + float _prev_error; + float _last_measured; + bool _first; +}; diff --git a/control/servo.cpp b/control/servo.cpp new file mode 100644 index 0000000..43f156f --- /dev/null +++ b/control/servo.cpp @@ -0,0 +1,57 @@ +#include "servo.hpp" +#include "hal/pwm.hpp" +#include +#include +#include + +static PWM* servo_pwm = nullptr; +static PID* servo_pid = nullptr; +static bool serv_initialized = false; + +static float g_deadband = 0.03f; +static float g_steer_gain = 1.5f; + +void servo_init() +{ + if (serv_initialized) return; + + servo_pwm = new PWM(SERVO_PWMCHIP, SERVO_PWMID, SERVO_PERIOD_NS); + servo_pwm->setDutyCycle(SERVO_CENTER_NS); + servo_pwm->enable(); + + servo_pid = new PID(400000, 200000, 0, 0, PID_POSITIONAL, + static_cast(SERVO_MAX_DELTA_NS), + static_cast(SERVO_MAX_DELTA_NS)); + + serv_initialized = true; +} + +void servo_set_deadband(float v) { g_deadband = std::max(0.0f, v); } +void servo_set_steer_gain(float v) { g_steer_gain = std::max(0.1f, v); } +void servo_set_pid(double kp, double ki, double kd) { if (servo_pid) servo_pid->setPID(kp, ki, kd); } + +void servo_set_angle(float deviation) +{ + if (!serv_initialized) return; + deviation = std::clamp(deviation, -1.0f, 1.0f); + + if (std::abs(deviation) < g_deadband) + { + servo_pwm->setDutyCycle(SERVO_CENTER_NS); + return; + } + + float scaled = deviation * g_steer_gain; + float out = servo_pid->update(scaled); + float duty_ns = SERVO_CENTER_NS + out; + duty_ns = std::clamp(duty_ns, + static_cast(SERVO_CENTER_NS - SERVO_MAX_DELTA_NS), + static_cast(SERVO_CENTER_NS + SERVO_MAX_DELTA_NS)); + servo_pwm->setDutyCycle(static_cast(duty_ns)); +} + +void servo_center() +{ + if (serv_initialized) + servo_pwm->setDutyCycle(SERVO_CENTER_NS); +} diff --git a/control/servo.hpp b/control/servo.hpp new file mode 100644 index 0000000..41b3bc5 --- /dev/null +++ b/control/servo.hpp @@ -0,0 +1,12 @@ +#pragma once +#include "types.hpp" +#include "control/pid.hpp" + +void servo_init(); +void servo_set_angle(float deviation); +void servo_center(); +void servo_set_deadband(float v); +void servo_set_steer_gain(float v); +void servo_set_pid(double kp, double ki, double kd); + +inline float servo_get_duty_ns() { return SERVO_CENTER_NS; } diff --git a/cross.cmake b/cross.cmake new file mode 100644 index 0000000..8d7416f --- /dev/null +++ b/cross.cmake @@ -0,0 +1,14 @@ +SET(CROSS_COMPILE 1) + +IF(CROSS_COMPILE) +SET(CMAKE_SYSTEM_NAME Linux) +set(CMAKE_SYSTEM_PROCESSOR loongson) +SET(TOOLCHAIN_DIR "/opt/loongson-gnu-toolchain-8.3-x86_64-loongarch64-linux-gnu-rc1.6") +set(CMAKE_CXX_COMPILER ${TOOLCHAIN_DIR}/bin/loongarch64-linux-gnu-g++) +set(CMAKE_C_COMPILER ${TOOLCHAIN_DIR}/bin/loongarch64-linux-gnu-gcc) + +SET(CMAKE_FIND_ROOT_PATH ${TOOLCHAIN_DIR}) +SET(CMAKE_FIND_ROOT_PATH_MODE_PROGRAM NEVER) +SET(CMAKE_FIND_ROOT_PATH_MODE_LIBRARY ONLY) +SET(CMAKE_FIND_ROOT_PATH_MODE_INCLUDE ONLY) +ENDIF(CROSS_COMPILE) diff --git a/ctl.sh b/ctl.sh new file mode 100644 index 0000000..809d3b2 --- /dev/null +++ b/ctl.sh @@ -0,0 +1,87 @@ +#!/bin/sh +# SmartCar Framework 控制脚本 +# 用法: sh ctl.sh start | stop | init + +PWMCHIP=8 +PWMPERIOD=50000 +DIR="$(dirname "$0")" + +init_pins() { + echo "[framework] 初始化引脚..." + for pin in 73 12 13 66 67 75 72; do + echo $pin > /sys/class/gpio/export 2>/dev/null + done + echo out > /sys/class/gpio/gpio73/direction 2>/dev/null + echo out > /sys/class/gpio/gpio12/direction 2>/dev/null + echo out > /sys/class/gpio/gpio13/direction 2>/dev/null + echo out > /sys/class/gpio/gpio66/direction 2>/dev/null + echo out > /sys/class/gpio/gpio67/direction 2>/dev/null + echo out > /sys/class/gpio/gpio75/direction 2>/dev/null + echo in > /sys/class/gpio/gpio72/direction 2>/dev/null + echo 1 > /sys/class/gpio/gpio73/value 2>/dev/null + echo 0 > /sys/class/gpio/gpio12/value 2>/dev/null + echo 0 > /sys/class/gpio/gpio13/value 2>/dev/null + echo 0 > /sys/class/gpio/gpio66/value 2>/dev/null + echo 0 > /sys/class/gpio/gpio67/value 2>/dev/null + + for ch in 1 2; do + echo $ch > /sys/class/pwm/pwmchip${PWMCHIP}/export 2>/dev/null + echo ${PWMPERIOD} > /sys/class/pwm/pwmchip${PWMCHIP}/pwm${ch}/period 2>/dev/null + echo 1 > /sys/class/pwm/pwmchip${PWMCHIP}/pwm${ch}/enable 2>/dev/null + echo 0 > /sys/class/pwm/pwmchip${PWMCHIP}/pwm${ch}/duty_cycle 2>/dev/null + done + + echo 0 > /sys/class/pwm/pwmchip1/export 2>/dev/null + echo 3000000 > /sys/class/pwm/pwmchip1/pwm0/period 2>/dev/null + echo 1 > /sys/class/pwm/pwmchip1/pwm0/enable 2>/dev/null + echo 1520000 > /sys/class/pwm/pwmchip1/pwm0/duty_cycle 2>/dev/null + + echo 11 > "$DIR/speed" 2>/dev/null + echo 0.03 > "$DIR/deadband" 2>/dev/null + echo 1.5 > "$DIR/steer_gain" 2>/dev/null + echo 400000 > "$DIR/kp" 2>/dev/null + echo 200000 > "$DIR/ki" 2>/dev/null + echo 0 > "$DIR/kd" 2>/dev/null + echo 0.6 > "$DIR/mortor_kp" 2>/dev/null + echo 0.2 > "$DIR/mortor_ki" 2>/dev/null + echo 0 > "$DIR/mortor_kd" 2>/dev/null + echo 0 > "$DIR/start" 2>/dev/null + echo 1 > "$DIR/showImg" 2>/dev/null + echo "[framework] 引脚初始化完成" +} + +do_start() { + echo "[framework] 启动..." + echo 1 > "$DIR/start" + cd "$DIR" + nohup ./smartcar_framework > "$DIR/out.log" 2>&1 & + echo "[framework] PID=$!" +} + +do_stop() { + echo "[framework] 停止..." + echo 0 > "$DIR/start" 2>/dev/null + killall -9 smartcar_framework 2>/dev/null + sleep 0.3 + echo 0 > /sys/class/pwm/pwmchip${PWMCHIP}/pwm1/duty_cycle 2>/dev/null + echo 0 > /sys/class/pwm/pwmchip${PWMCHIP}/pwm2/duty_cycle 2>/dev/null + echo 0 > /sys/class/gpio/gpio73/value 2>/dev/null + echo "[framework] 已停止, PWM已归零" +} + +case "${1}" in + start) + init_pins + do_start + ;; + stop) + do_stop + ;; + init) + init_pins + ;; + *) + echo "用法: sh ctl.sh start | stop | init" + exit 1 + ;; +esac diff --git a/debug/config.cpp b/debug/config.cpp new file mode 100644 index 0000000..1e6985f --- /dev/null +++ b/debug/config.cpp @@ -0,0 +1,20 @@ +#include "config.hpp" +#include + +double read_config_double(const char* filename, double default_val) +{ + std::ifstream f(filename); + if (!f.is_open()) return default_val; + double v = default_val; + f >> v; + return v; +} + +bool read_config_flag(const char* filename, bool default_val) +{ + std::ifstream f(filename); + if (!f.is_open()) return default_val; + int v = default_val ? 1 : 0; + f >> v; + return v != 0; +} diff --git a/debug/config.hpp b/debug/config.hpp new file mode 100644 index 0000000..043d274 --- /dev/null +++ b/debug/config.hpp @@ -0,0 +1,4 @@ +#pragma once + +double read_config_double(const char* filename, double default_val = 0); +bool read_config_flag(const char* filename, bool default_val = false); diff --git a/debug/draw.cpp b/debug/draw.cpp new file mode 100644 index 0000000..fbec680 --- /dev/null +++ b/debug/draw.cpp @@ -0,0 +1,88 @@ +#include "draw.hpp" +#include +#include + +DebugOverlay g_dbg; + +static uint16* g_buf = nullptr; +static int g_w = 0; +static int g_h = 0; + +static inline uint16 rgb565(uint8 r, uint8 g, uint8 b) +{ + return ((r >> 3) << 11) | ((g >> 2) << 5) | (b >> 3); +} + +static inline bool in_screen(int x, int y) +{ + return x >= 0 && x < g_w && y >= 0 && y < g_h; +} + +void dbg_draw_init(uint16* fb_buffer, int w, int h) +{ + g_buf = fb_buffer; + g_w = w; g_h = h; +} + +void dbg_draw_line(int x0, int y0, int x1, int y1, uint8 r, uint8 g, uint8 b) +{ + if (!g_buf) return; + uint16 color = rgb565(r, g, b); + int dx = std::abs(x1 - x0), sx = x0 < x1 ? 1 : -1; + int dy = -std::abs(y1 - y0), sy = y0 < y1 ? 1 : -1; + int err = dx + dy; + + while (true) + { + if (in_screen(x0, y0)) g_buf[y0 * g_w + x0] = color; + if (x0 == x1 && y0 == y1) break; + int e2 = 2 * err; + if (e2 >= dy) { err += dy; x0 += sx; } + if (e2 <= dx) { err += dx; y0 += sy; } + } +} + +void dbg_draw_cross(int cx, int cy, int size, uint8 r, uint8 g, uint8 b) +{ + dbg_draw_line(cx - size, cy, cx + size, cy, r, g, b); + dbg_draw_line(cx, cy - size, cx, cy + size, r, g, b); +} + +void dbg_draw_rect(int x, int y, int w, int h, uint8 r, uint8 g, uint8 b) +{ + dbg_draw_line(x, y, x + w, y, r, g, b); + dbg_draw_line(x + w, y, x + w, y + h, r, g, b); + dbg_draw_line(x + w, y + h, x, y + h, r, g, b); + dbg_draw_line(x, y + h, x, y, r, g, b); +} + +void dbg_draw_points(const int points[][2], int count, uint8 r, uint8 g, uint8 b) +{ + if (!g_buf) return; + uint16 color = rgb565(r, g, b); + for (int i = 0; i < count; ++i) + { + int x = points[i][0], y = points[i][1]; + if (in_screen(x, y)) g_buf[y * g_w + x] = color; + } +} + +void dbg_draw_edge_zoom(const EdgeLines& lines, uint8 r, uint8 g, uint8 b) +{ + const int thick = 7; // 车道线宽度 (像素) + + for (int y = 0; y < IPM_ROW_COUNT; ++y) + { + if (lines.left[y] > 0) + for (int dx = -thick; dx <= thick; ++dx) + dbg_draw_line(lines.left[y] + dx, y, lines.left[y] + dx, y, r, g, b); + + if (lines.right[y] > 0) + for (int dx = -thick; dx <= thick; ++dx) + dbg_draw_line(lines.right[y] + dx, y, lines.right[y] + dx, y, 0, 255, 0); + + if (lines.mid[y] > 0) + for (int dx = -thick; dx <= thick; ++dx) + dbg_draw_line(lines.mid[y] + dx, y, lines.mid[y] + dx, y, 255, 255, 0); + } +} diff --git a/debug/draw.hpp b/debug/draw.hpp new file mode 100644 index 0000000..d8670a0 --- /dev/null +++ b/debug/draw.hpp @@ -0,0 +1,17 @@ +#pragma once +#include +#include "types.hpp" + +struct DebugOverlay { + bool enable = true; + int mode = DBG_OVERLAY; +}; + +extern DebugOverlay g_dbg; + +void dbg_draw_init(uint16* fb_buffer, int w, int h); +void dbg_draw_line(int x0, int y0, int x1, int y1, uint8 r, uint8 g, uint8 b); +void dbg_draw_cross(int cx, int cy, int size, uint8 r, uint8 g, uint8 b); +void dbg_draw_rect(int x, int y, int w, int h, uint8 r, uint8 g, uint8 b); +void dbg_draw_points(const int points[][2], int count, uint8 r, uint8 g, uint8 b); +void dbg_draw_edge_zoom(const EdgeLines& lines, uint8 r, uint8 g, uint8 b); diff --git a/debug/tcp.cpp b/debug/tcp.cpp new file mode 100644 index 0000000..9e24d0a --- /dev/null +++ b/debug/tcp.cpp @@ -0,0 +1,25 @@ +#include "tcp.hpp" +#include + +bool tcp_init(const char* host, int port) +{ + // TODO: 实现 TCP 客户端连接逐飞助手 + // 当前为占位实现，后续接入 seekfree_assistant 协议 + printf("[TCP] 占位: 连接 %s:%d (未实现)\n", host, port); + return false; +} + +void tcp_send_image(const EdgeLines& lines) +{ + // TODO: 发送 RGB565 调试图像 +} + +void tcp_send_osc(float ch1, float ch2, float ch3, float ch4) +{ + // TODO: 发送虚拟示波器数据 +} + +void tcp_close() +{ + // TODO: 关闭 TCP 连接 +} diff --git a/debug/tcp.hpp b/debug/tcp.hpp new file mode 100644 index 0000000..d68e008 --- /dev/null +++ b/debug/tcp.hpp @@ -0,0 +1,8 @@ +#pragma once +#include "types.hpp" +#include "vision/search.hpp" + +bool tcp_init(const char* host = "192.168.31.20", int port = 8086); +void tcp_send_image(const EdgeLines& lines); // 发送带叠加线的图传 +void tcp_send_osc(float ch1, float ch2, float ch3, float ch4); +void tcp_close(); diff --git a/hal/encoder.cpp b/hal/encoder.cpp new file mode 100644 index 0000000..f73000b --- /dev/null +++ b/hal/encoder.cpp @@ -0,0 +1,64 @@ +#include "encoder.hpp" +#include +#include +#include +#include + +Encoder::Encoder(int pwm_channel, int dir_gpio, int direction_polarity) + : _channel(pwm_channel) + , _dir_gpio(dir_gpio) + , _polarity(direction_polarity) + , _reg_base(nullptr) + , _mem_fd(-1) + , _rps(0.0f) + , _last_period(0) +{ + _dir_gpio.setDirection("in"); + _mmapRegs(); +} + +Encoder::~Encoder() { _munmapRegs(); } + +void Encoder::_mmapRegs() +{ + _mem_fd = open("/dev/mem", O_RDWR); + if (_mem_fd < 0) return; + + _reg_base = (volatile uint32*)mmap(nullptr, 0x10000, PROT_READ | PROT_WRITE, + MAP_SHARED, _mem_fd, ENCODER_BASE); +} + +void Encoder::_munmapRegs() +{ + if (_reg_base && _reg_base != MAP_FAILED) + munmap((void*)_reg_base, 0x10000); + if (_mem_fd >= 0) { close(_mem_fd); _mem_fd = -1; } +} + +uint32 Encoder::_readRegister(unsigned int offset) const +{ + if (!_reg_base || _reg_base == MAP_FAILED) return 0; + return _reg_base[offset / sizeof(uint32)]; +} + +int Encoder::getDirection() const +{ + return _dir_gpio.getValue() * _polarity; +} + +void Encoder::update() +{ + if (!_reg_base || _reg_base == MAP_FAILED) return; + + uint32 period = _readRegister(REG_OFFSET + _channel * 0x100); + if (period > 0 && period < 0xFFFFFF) + { + float freq = 50000000.0f / period; // 50MHz 基频 + _rps = (freq / 4.0f) * getDirection(); // 4倍频编码器 + _last_period = period; + } + else + { + _rps *= 0.6f; // 无脉冲时速度衰减 + } +} diff --git a/hal/encoder.hpp b/hal/encoder.hpp new file mode 100644 index 0000000..9af02a4 --- /dev/null +++ b/hal/encoder.hpp @@ -0,0 +1,26 @@ +#pragma once +#include "types.hpp" +#include "gpio.hpp" + +class Encoder { +public: + Encoder(int pwm_channel, int dir_gpio, int direction_polarity); + ~Encoder(); + + void update(); + float getRPS() const { return _rps; } + int getDirection() const; + +private: + int _channel; + GPIO _dir_gpio; + int _polarity; // 1 或 -1 + volatile uint32* _reg_base; + int _mem_fd; + float _rps; + uint32 _last_period; + static constexpr unsigned int REG_OFFSET = 0x0100; + void _mmapRegs(); + void _munmapRegs(); + uint32 _readRegister(unsigned int offset) const; +}; diff --git a/hal/framebuffer.cpp b/hal/framebuffer.cpp new file mode 100644 index 0000000..8e39459 --- /dev/null +++ b/hal/framebuffer.cpp @@ -0,0 +1,60 @@ +#include "framebuffer.hpp" +#include +#include +#include +#include +#include +#include + +FrameBuffer::FrameBuffer() : _fd(-1), _buffer(nullptr), _width(0), _height(0), _screensize(0) {} + +FrameBuffer::~FrameBuffer() { release(); } + +bool FrameBuffer::init(const char* device) +{ + _fd = open(device, O_RDWR); + if (_fd < 0) return false; + + fb_var_screeninfo vinfo; + if (ioctl(_fd, FBIOGET_VSCREENINFO, &vinfo) < 0) return false; + + _width = vinfo.xres; + _height = vinfo.yres; + _screensize = _width * _height * 2; // RGB565 = 2 bytes/pixel + + _buffer = (uint16*)mmap(nullptr, _screensize, PROT_READ | PROT_WRITE, + MAP_SHARED, _fd, 0); + return (_buffer != MAP_FAILED); +} + +void FrameBuffer::release() +{ + if (_buffer && _buffer != MAP_FAILED) + munmap(_buffer, _screensize); + if (_fd >= 0) { close(_fd); _fd = -1; } + _buffer = nullptr; +} + +void FrameBuffer::_rgb888_to_rgb565(const uint8* src, uint16* dst, int pixels) +{ + for (int i = 0; i < pixels; ++i) + { + uint8 r = src[i * 3]; + uint8 g = src[i * 3 + 1]; + uint8 b = src[i * 3 + 2]; + dst[i] = ((r >> 3) << 11) | ((g >> 2) << 5) | (b >> 3); + } +} + +bool FrameBuffer::write(const uint8* rgb888_data, int w, int h) +{ + if (!_buffer || _buffer == MAP_FAILED) return false; + + int copy_w = (w < _width) ? w : _width; + int copy_h = (h < _height) ? h : _height; + + for (int y = 0; y < copy_h; ++y) + _rgb888_to_rgb565(rgb888_data + y * w * 3, _buffer + y * _width, copy_w); + + return true; +} diff --git a/hal/framebuffer.hpp b/hal/framebuffer.hpp new file mode 100644 index 0000000..9dbb4e8 --- /dev/null +++ b/hal/framebuffer.hpp @@ -0,0 +1,23 @@ +#pragma once +#include "../types.hpp" + +class FrameBuffer { +public: + FrameBuffer(); + ~FrameBuffer(); + + bool init(const char* device = "/dev/fb0"); + void release(); + int width() const { return _width; } + int height() const { return _height; } + uint16* buffer() const { return _buffer; } + bool write(const uint8* rgb888_data, int w, int h); + +private: + int _fd; + uint16* _buffer; + int _width; + int _height; + int _screensize; + void _rgb888_to_rgb565(const uint8* src, uint16* dst, int pixels); +}; diff --git a/hal/gpio.cpp b/hal/gpio.cpp new file mode 100644 index 0000000..b328554 --- /dev/null +++ b/hal/gpio.cpp @@ -0,0 +1,43 @@ +#include "gpio.hpp" +#include +#include +#include +#include + +GPIO::GPIO(int pin) : _pin(pin), _exported(false) { _export(); } + +GPIO::~GPIO() { _unexport(); } + +void GPIO::_export() +{ + std::ofstream exp("/sys/class/gpio/export"); + if (exp.is_open()) { exp << _pin; _exported = true; } +} + +void GPIO::_unexport() +{ + std::ofstream unexp("/sys/class/gpio/unexport"); + if (unexp.is_open()) unexp << _pin; +} + +void GPIO::setDirection(const std::string& dir) +{ + std::string path = "/sys/class/gpio/gpio" + std::to_string(_pin) + "/direction"; + std::ofstream f(path); + if (f.is_open()) f << dir; +} + +void GPIO::setValue(int val) +{ + std::string path = "/sys/class/gpio/gpio" + std::to_string(_pin) + "/value"; + std::ofstream f(path); + if (f.is_open()) f << val; +} + +int GPIO::getValue() const +{ + std::string path = "/sys/class/gpio/gpio" + std::to_string(_pin) + "/value"; + std::ifstream f(path); + int v = 0; if (f.is_open()) f >> v; + return v; +} diff --git a/hal/gpio.hpp b/hal/gpio.hpp new file mode 100644 index 0000000..569aca4 --- /dev/null +++ b/hal/gpio.hpp @@ -0,0 +1,17 @@ +#pragma once +#include + +class GPIO { +public: + explicit GPIO(int pin); + ~GPIO(); + void setDirection(const std::string& dir); + void setValue(int val); + int getValue() const; + +private: + int _pin; + bool _exported; + void _export(); + void _unexport(); +}; diff --git a/hal/pwm.cpp b/hal/pwm.cpp new file mode 100644 index 0000000..029c5ae --- /dev/null +++ b/hal/pwm.cpp @@ -0,0 +1,57 @@ +#include "pwm.hpp" +#include +#include +#include + +PWM::PWM(int chip, int channel, unsigned int period_ns) + : _chip(chip), _channel(channel), _period_ns(period_ns), _exported(false) +{ + _export(); + setPeriod(period_ns); +} + +PWM::~PWM() { _unexport(); } + +std::string PWM::_basePath() const +{ + return "/sys/class/pwm/pwmchip" + std::to_string(_chip) + + "/pwm" + std::to_string(_channel); +} + +void PWM::_export() +{ + std::ofstream f("/sys/class/pwm/pwmchip" + std::to_string(_chip) + "/export"); + if (f.is_open()) { f << _channel; _exported = true; } + std::this_thread::sleep_for(std::chrono::milliseconds(10)); +} + +void PWM::_unexport() +{ + std::ofstream f("/sys/class/pwm/pwmchip" + std::to_string(_chip) + "/unexport"); + if (f.is_open()) f << _channel; +} + +void PWM::setDutyCycle(unsigned int duty_ns) +{ + std::ofstream f(_basePath() + "/duty_cycle"); + if (f.is_open()) f << duty_ns; +} + +void PWM::setPeriod(unsigned int period_ns) +{ + _period_ns = period_ns; + std::ofstream f(_basePath() + "/period"); + if (f.is_open()) f << period_ns; +} + +void PWM::enable(bool on) +{ + std::ofstream f(_basePath() + "/enable"); + if (f.is_open()) f << (on ? 1 : 0); +} + +void PWM::setPolarity(const char* pol) +{ + std::ofstream f(_basePath() + "/polarity"); + if (f.is_open()) f << pol; +} diff --git a/hal/pwm.hpp b/hal/pwm.hpp new file mode 100644 index 0000000..2f86dc5 --- /dev/null +++ b/hal/pwm.hpp @@ -0,0 +1,24 @@ +#pragma once +#include +#include "types.hpp" + +class PWM { +public: + PWM(int chip, int channel, unsigned int period_ns); + ~PWM(); + + void setDutyCycle(unsigned int duty_ns); + void setPeriod(unsigned int period_ns); + void enable(bool on = true); + void setPolarity(const char* pol = "normal"); + unsigned int readPeriod() const { return _period_ns; } + +private: + int _chip; + int _channel; + unsigned int _period_ns; + bool _exported; + std::string _basePath() const; + void _export(); + void _unexport(); +}; diff --git a/main.cpp b/main.cpp new file mode 100644 index 0000000..55c76b1 --- /dev/null +++ b/main.cpp @@ -0,0 +1,97 @@ +#include "types.hpp" +#include "hal/framebuffer.hpp" +#include "vision/camera.hpp" +#include "scheduler.hpp" +#include "control/motor.hpp" +#include "control/servo.hpp" +#include "debug/config.hpp" +#include "debug/draw.hpp" + +#include +#include +#include +#include + +static std::atomic g_running{true}; +static FrameBuffer g_fb; + +static void signal_handler(int) +{ + g_running.store(false); +} + +int main() +{ + std::signal(SIGINT, signal_handler); + std::signal(SIGTERM, signal_handler); + setvbuf(stdout, NULL, _IONBF, 0); // stdout 无缓冲, 日志实时落地 + + printf("========================================\n"); + printf(" SmartCar Framework v0.1\n"); + printf(" 龙芯 2K0300 智能车开发框架\n"); + printf("========================================\n\n"); + + // 1. 显示屏 + printf("[INIT] 初始化显示屏...\n"); + if (g_fb.init(FB_DEVICE)) + { + printf("[INIT] 显示屏: %dx%d\n", g_fb.width(), g_fb.height()); + dbg_draw_init(g_fb.buffer(), g_fb.width(), g_fb.height()); + } + else + printf("[INIT] 显示屏初始化失败, 继续无屏运行\n"); + + // 2. 摄像头 + printf("[INIT] 初始化摄像头...\n"); + if (camera_init()) + { + camera_stream_start(); + printf("[INIT] 摄像头: %dx%d MJPG\n", CAMERA_WIDTH, CAMERA_HEIGHT); + } + else + { + printf("[INIT] 摄像头初始化失败\n"); + return -1; + } + + // 3. 调度器 (内部初始化电机/舵机/编码器/IMU) + printf("[INIT] 初始化调度器...\n"); + if (!scheduler_init(&g_fb)) + { + printf("[INIT] 调度器初始化失败\n"); + return -1; + } + + // 4. 启动调度器 + scheduler_start(); + + // 5. 主循环 — 低频监控 + printf("\n[READY] 系统运行中, Ctrl+C 停止\n"); + while (g_running.load()) + { + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + + // 热调参数 (读取文件) + bool running = read_config_flag(CFG_START, true); + if (!running) + { + motor_stop_all(); + servo_center(); + } + + // 打印状态 + static int last_print = 0; + if (g_print_flag.exchange(0)) + { + printf("[STATUS] tick=%d 车辆运行中\n", last_print++); + } + } + + printf("\n[EXIT] 正在安全停止...\n"); + scheduler_stop(); + camera_deinit(); + g_fb.release(); + printf("[EXIT] 系统已安全退出\n"); + + return 0; +} diff --git a/model/best.pth.1 b/model/best.pth.1 new file mode 100644 index 0000000..32c0f81 Binary files /dev/null and b/model/best.pth.1 differ diff --git a/model/config.json b/model/config.json new file mode 100644 index 0000000..f1360e2 --- /dev/null +++ b/model/config.json @@ -0,0 +1,33 @@ +{ + "seed": 42, + "data_path": "preprocessed/dataset_al8.npz", + "save_dir": "checkpoints_heat_v4", + "val_split": 0.15, + "batch_size": 64, + "epochs": 60, + "lr": 0.003, + "weight_decay": 0.0001, + "img_w": 160, + "img_h": 120, + "num_classes": 3, + "out_h": 15, + "out_w": 20, + "stride": 8, + "size_loss_weight": 2.0, + "bg_weight": 0.05, + "resume_from": "checkpoints_heat_v4/best.pth", + "ref_sizes": { + "0": [ + 75.35844421386719, + 62.14570236206055 + ], + "1": [ + 49.79265213012695, + 38.633811950683594 + ], + "2": [ + 67.58988189697266, + 34.93575668334961 + ] + } +} \ No newline at end of file diff --git a/model/deploy_README.md b/model/deploy_README.md new file mode 100644 index 0000000..ff47092 --- /dev/null +++ b/model/deploy_README.md @@ -0,0 +1,39 @@ +# NanoDetHeat v4 部署说明 + +## 模型文件 + +| 文件 | 说明 | +|------|------| +| model_heat_v4.py | 模型架构定义 (PyTorch) | +| best.pth | 训练权重 (141KB, FP32) | +| model_heat_v4.onnx | ONNX 格式模型 | +| config.json | 配置 + 类别参考尺寸 | + +## 输入 + +- 格式: RGB, 160×120, 归一化到 [0,1] +- 预处理: BGR→RGB 翻转, /255.0 + +## 输出 + +- 形状: [1, 6, 15, 20] +- 通道 0-3: 类别 logits (红绿灯/锥桶/人行道/背景) +- 通道 4-5: 尺寸预测 (宽/高, 需乘参考尺寸) + +## 后处理 + +1. softmax 通道 0-3 → 概率 +2. 对每类找局部峰值 (3×3 邻域抑制) +3. 筛选置信度 > 阈值 (推荐 0.8) +4. 尺寸解码: 实际宽 = 预测宽 × 参考宽 +5. 中心坐标: cx=(gx+0.5)×8, cy=(gy+0.5)×8 + +## 类别参考尺寸 (像素, 160×120) + +红绿灯: 73.7 × 60.9 +锥桶: 49.4 × 38.6 +人行道: 71.2 × 36.3 + +## 推荐阈值 + +th=0.8: 精确度 62.9%, 召回 88.7% diff --git a/model/model.cpp b/model/model.cpp new file mode 100644 index 0000000..601114f --- /dev/null +++ b/model/model.cpp @@ -0,0 +1,13 @@ +#include "model.hpp" + +bool model_init() +{ + // TODO: 加载 TFLite 模型，初始化解释器 + return false; +} + +void model_infer(const uint8* image, int w, int h, float* output, int num_classes) +{ + // TODO: 运行推理，填充 output[0..num_classes-1] + for (int i = 0; i < num_classes; ++i) output[i] = 0.0f; +} diff --git a/model/model.hpp b/model/model.hpp new file mode 100644 index 0000000..827b2ec --- /dev/null +++ b/model/model.hpp @@ -0,0 +1,11 @@ +#pragma once +#include "types.hpp" + +// TFLM 模型推理接口 — 占位实现，后续接入模型 +// 用法: +// 1. 将 .tflite 模型转为 C header 数组放入 model/ 目录 +// 2. 实现 model_init() / model_infer() +// 3. 在 element.cpp 中调用 + +bool model_init(); +void model_infer(const uint8* image, int w, int h, float* output, int num_classes); diff --git a/model/model_heat_v4.onnx b/model/model_heat_v4.onnx new file mode 100644 index 0000000..8927e5a Binary files /dev/null and b/model/model_heat_v4.onnx differ diff --git a/model/model_heat_v4.py b/model/model_heat_v4.py new file mode 100644 index 0000000..8be1ace --- /dev/null +++ b/model/model_heat_v4.py @@ -0,0 +1,100 @@ +""" +NanoDetHeat v4: 残差 + SE注意力 + 扩通道 +MACs: ~8.7M, 参数: ~18K, 目标: 20 TPS @ 0.44 GOPS (79%) +""" + +import torch +import torch.nn as nn +import torch.nn.functional as F +import numpy as np + + +def conv_bn_relu(in_c, out_c, kernel, stride=1, groups=1): + pad = kernel // 2 + return nn.Sequential( + nn.Conv2d(in_c, out_c, kernel, stride, pad, groups=groups, bias=False), + nn.BatchNorm2d(out_c), + nn.ReLU(inplace=True), + ) + + +class SEResDWBlock(nn.Module): + """残差DW + SE通道注意力""" + def __init__(self, in_c, out_c, stride=1, reduction=4): + super().__init__() + self.dw = conv_bn_relu(in_c, in_c, 3, stride=stride, groups=in_c) + self.pw = conv_bn_relu(in_c, out_c, 1) + + self.skip = nn.Identity() if (stride == 1 and in_c == out_c) else nn.Sequential( + nn.Conv2d(in_c, out_c, 1, stride, bias=False), + nn.BatchNorm2d(out_c), + ) + + self.se = nn.Sequential( + nn.AdaptiveAvgPool2d(1), + nn.Conv2d(out_c, out_c // reduction, 1), + nn.ReLU(inplace=True), + nn.Conv2d(out_c // reduction, out_c, 1), + nn.Sigmoid(), + ) + self.relu = nn.ReLU(inplace=True) + + def forward(self, x): + out = self.pw(self.dw(x)) + out = out + self.skip(x) + return self.relu(out * self.se(out)) + + +class NanoDetHeatV4(nn.Module): + def __init__(self, num_classes=3): + super().__init__() + self.stem = conv_bn_relu(3, 8, 3, stride=2) + self.block1 = SEResDWBlock(8, 16, stride=1) + self.block2 = SEResDWBlock(16, 28, stride=2) + self.block3 = SEResDWBlock(28, 40, stride=1) + self.block4 = SEResDWBlock(40, 56, stride=2) + self.block5 = SEResDWBlock(56, 64, stride=1) + + self.shared = conv_bn_relu(64, 24, 1) + self.cls_head = nn.Conv2d(24, num_classes + 1, 1, bias=True) + self.size_head = nn.Conv2d(24, 2, 1, bias=True) + self._init_weights() + + def _init_weights(self): + for m in self.modules(): + if isinstance(m, nn.Conv2d): + nn.init.kaiming_normal_(m.weight, mode='fan_out', nonlinearity='relu') + if m.bias is not None: nn.init.constant_(m.bias, 0) + elif isinstance(m, nn.BatchNorm2d): + nn.init.constant_(m.weight, 1); nn.init.constant_(m.bias, 0) + self.cls_head.bias.data[-1] = 0.5 # 轻微背景偏置,焦点损失自动调整 + + def forward(self, x): + x = self.stem(x); x = self.block1(x); x = self.block2(x) + x = self.block3(x); x = self.block4(x); x = self.block5(x) + x = self.shared(x) + return torch.cat([self.cls_head(x), self.size_head(x)], dim=1) + + +OUT_H, OUT_W = 15, 20 +STRIDE = 8 + + +if __name__ == "__main__": + m = NanoDetHeatV4() + p = sum(p.numel() for p in m.parameters()) + + macs = (3*3*3*8*80*60 + # stem + 8*9*80*60 + 8*16*80*60 + 8*16*80*60 + # b1 + 16*9*40*30 + 16*28*40*30 + 16*28*40*30 + # b2 + 28*9*40*30 + 28*40*40*30 + # b3 + 40*9*20*15 + 40*56*20*15 + 40*56*20*15 + # b4 + 56*9*20*15 + 56*64*20*15 + # b5 + 64*24*20*15 + 24*4*20*15 + 24*2*20*15) # heads + + print(f"Params: {p:,} = ~{p/1000:.1f}K") + print(f"MACs: {macs:,} = ~{macs/1e6:.2f}M") + print(f"20 TPS: {macs*20*2/1e9:.2f} GOPS ({(macs*20*2/0.44e9)*100:.0f}%)") + print(f"25 TPS: {macs*25*2/1e9:.2f} GOPS ({(macs*25*2/0.44e9)*100:.0f}%)") + x = torch.randn(1,3,120,160); y = m(x) + print(f"Output: {y.shape}") diff --git a/scheduler.cpp b/scheduler.cpp new file mode 100644 index 0000000..aaa69ea --- /dev/null +++ b/scheduler.cpp @@ -0,0 +1,289 @@ +#include "scheduler.hpp" +#include "types.hpp" +#include "hal/gpio.hpp" +#include "hal/framebuffer.hpp" +#include "hal/encoder.hpp" +#include "control/pid.hpp" +#include "control/imu.hpp" +#include "control/motor.hpp" +#include "control/servo.hpp" +#include "vision/camera.hpp" +#include "vision/preprocess.hpp" +#include "vision/search.hpp" +#include "vision/element.hpp" +#include "strategy/deviation.hpp" +#include "strategy/speed.hpp" +#include "debug/draw.hpp" +#include "debug/config.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +// ========================================================= +// 全局控制对象 +// ========================================================= +static YawTracker g_imu; +static Encoder* g_enc_l = nullptr; +static Encoder* g_enc_r = nullptr; +static PID g_pid_speed_l(0.6f, 0.2f, 0.0f, 0.0f, PID_INCREMENTAL, 100.0f, 50.0f); +static PID g_pid_speed_r(0.6f, 0.2f, 0.0f, 0.0f, PID_INCREMENTAL, 100.0f, 50.0f); + +static float g_vision_yaw = 0.0f; +static float g_speed_l = 0.0f; +static float g_speed_r = 0.0f; +static float g_base_speed = 20.0f; +static float g_user_speed = 11.0f; + +static std::atomic g_sched_running{false}; +std::atomic g_print_flag{0}; +static int g_timer_fd = -1; +static int g_epoll_fd = -1; +static std::thread g_sched_thread; +static std::mutex g_vision_mutex; +static int g_tick = 0; +static int g_frame_cnt = 0; +static int g_bad_frame = 0; +static int g_empty_cnt = 0; +static FrameBuffer* g_fb_ptr = nullptr; + +// ========================================================= +// 5ms 回调 +// ========================================================= + +static void sched_5ms(float dt) +{ + if (g_enc_l) g_enc_l->update(); + if (g_enc_r) g_enc_r->update(); + g_imu.update(dt); + + g_speed_l = g_enc_l ? g_enc_l->getRPS() : 0; + g_speed_r = g_enc_r ? g_enc_r->getRPS() : 0; +} + +static void sched_10ms(float dt) +{ + // 1. 视觉处理 + cv::Mat bgr = camera_capture(); + if (!bgr.empty()) + { + std::lock_guard lock(g_vision_mutex); + preprocess_run(bgr); + + SearchResult sr; + search_run(sr); + + TrackInfo info; + element_recognize(sr, info); + + fit_midline(sr.lines); + info.deviation = calc_deviation(sr.lines); + info.base_speed = calc_base_speed(info); + info.curvature = info.deviation; + + // 十字路口锁定直行 + if (element_is_cross()) + info.deviation = 0.0f; + + g_vision_yaw = info.deviation; + g_base_speed = info.base_speed; + info.line_valid = (sr.lines.left_valid + sr.lines.right_valid) >= 1; + + bool line_ok = (sr.lines.left_valid || sr.lines.right_valid); + if (!line_ok) ++g_bad_frame; + + // LCD 预览 — 原始摄像头画面 + 车道线 + if (read_config_flag(CFG_SHOW_IMG, false) && g_fb_ptr) + { + cv::Mat rgb; + cv::cvtColor(bgr, rgb, cv::COLOR_BGR2RGB); + cv::Mat disp; + cv::resize(rgb, disp, cv::Size(g_fb_ptr->width(), g_fb_ptr->height())); + + int fw = g_fb_ptr->width(); + int fh = g_fb_ptr->height(); + float sx = (float)fw / IMAGE_WIDTH; + float sy = (float)fh / IMAGE_HEIGHT; + + for (int y = 0; y < IPM_ROW_COUNT; ++y) + { + int dy = (int)(y * sy); + if (sr.lines.left[y] > 0) + cv::line(disp, cv::Point((int)(sr.lines.left[y] * sx), dy), + cv::Point((int)(sr.lines.left[y] * sx), dy), cv::Scalar(255,0,0), 5); + if (sr.lines.right[y] > 0) + cv::line(disp, cv::Point((int)(sr.lines.right[y] * sx), dy), + cv::Point((int)(sr.lines.right[y] * sx), dy), cv::Scalar(0,255,0), 5); + if (sr.lines.mid[y] > 0) + cv::line(disp, cv::Point((int)(sr.lines.mid[y] * sx), dy), + cv::Point((int)(sr.lines.mid[y] * sx), dy), cv::Scalar(255,255,0), 5); + } + + // 帧计数闪烁点 — 验证屏幕在刷新 + static int blink = 0; + blink = (blink + 1) % 20; + cv::circle(disp, cv::Point(10, fh - 10), 5, + blink < 10 ? cv::Scalar(255,255,255) : cv::Scalar(0,0,0), -1); + + g_fb_ptr->write(disp.data, g_fb_ptr->width(), g_fb_ptr->height()); + } + + // 截帧到文件 + if (read_config_flag(CFG_SAVE_IMG, false)) + { + static int save_cnt = 0; + char fname[64]; + snprintf(fname, sizeof(fname), "./capture_%04d.jpg", save_cnt++); + cv::imwrite(fname, bgr); + printf("[SAVE] %s (%dx%d)\n", fname, bgr.cols, bgr.rows); + } + + ++g_frame_cnt; + } + else + { + ++g_empty_cnt; + } + + // 2. 舵机转向 + servo_set_angle(g_vision_yaw); + + // 3. 差速辅助 + float steer = g_vision_yaw; + float curve_factor = (std::abs(g_vision_yaw) < 0.1f) ? 1.0f : 0.8f; + float base_spd = g_user_speed * curve_factor; + float diff = steer * 0.15f * g_user_speed; + float pwm_l = base_spd + diff; + float pwm_r = base_spd - diff; + + motor_set_pwm(MOTOR_LEFT, pwm_l); + motor_set_pwm(MOTOR_RIGHT, pwm_r); +} + +static void sched_1s() +{ + g_print_flag.store(1); + printf("[FPS] 视觉: %d 丢线: %d 空帧: %d\n", + g_frame_cnt, g_bad_frame, g_empty_cnt); + g_frame_cnt = 0; + g_bad_frame = 0; + g_empty_cnt = 0; + + double speed_val = read_config_double(CFG_SPEED, 11.0); + g_user_speed = static_cast(speed_val); + + if (read_config_flag(CFG_START, false)) + { + double kp = read_config_double(CFG_SERVO_KP, 3.5); + double ki = read_config_double(CFG_SERVO_KI, 0.3); + double kd = read_config_double(CFG_SERVO_KD, 0.2); + servo_set_pid(kp, ki, kd); + + double deadband = read_config_double(CFG_DEADBAND, 0.03); + double gain = read_config_double(CFG_STEER_GAIN, 1.5); + servo_set_deadband(static_cast(deadband)); + servo_set_steer_gain(static_cast(gain)); + + double mkp = read_config_double(CFG_MOTOR_KP, 0.6); + double mki = read_config_double(CFG_MOTOR_KI, 0.2); + double mkd = read_config_double(CFG_MOTOR_KD, 0.0); + g_pid_speed_l.setPID(mkp, mki, mkd); + g_pid_speed_r.setPID(mkp, mki, mkd); + } +} + +static void scheduler_loop() +{ + epoll_event ev; + while (g_sched_running.load()) + { + int n = epoll_wait(g_epoll_fd, &ev, 1, 10); + if (n <= 0) continue; + + uint64 expirations; + read(g_timer_fd, &expirations, sizeof(expirations)); + + for (uint64 i = 0; i < expirations; ++i) + { + float dt = 0.005f; + + sched_5ms(dt); + + if (g_tick % 2 == 0) sched_10ms(dt); + + if (g_tick % 200 == 0) sched_1s(); + + ++g_tick; + } + } +} + +// ========================================================= +// 初始化 +// ========================================================= + +bool scheduler_init(FrameBuffer* fb) +{ + g_fb_ptr = fb; + + motor_init(); + servo_init(); + servo_center(); + motor_enable(true); + + g_enc_l = new Encoder(ENCODER_LEFT_CHANNEL, ENCODER_LEFT_DIR_GPIO, 1); + g_enc_r = new Encoder(ENCODER_RIGHT_CHANNEL, ENCODER_RIGHT_DIR_GPIO, -1); + + g_imu.begin(IMU_DEVICE, IMU_BAUDRATE); + + preprocess_init(); + element_init(); + speed_strategy_reset(); + + g_timer_fd = timerfd_create(CLOCK_MONOTONIC, 0); + if (g_timer_fd < 0) { printf("[SCHED] timerfd create failed\n"); return false; } + + itimerspec its; + its.it_interval.tv_sec = 0; + its.it_interval.tv_nsec = 5 * 1000 * 1000; // 5ms + its.it_value = its.it_interval; + timerfd_settime(g_timer_fd, 0, &its, nullptr); + + g_epoll_fd = epoll_create1(0); + epoll_event ev; + ev.events = EPOLLIN; + ev.data.fd = g_timer_fd; + epoll_ctl(g_epoll_fd, EPOLL_CTL_ADD, g_timer_fd, &ev); + + printf("[SCHED] 调度器初始化完成 (5ms)\n"); + return true; +} + +void scheduler_start() +{ + g_sched_running.store(true); + g_sched_thread = std::thread(scheduler_loop); + printf("[SCHED] 调度器启动\n"); +} + +void scheduler_stop() +{ + g_sched_running.store(false); + if (g_sched_thread.joinable()) + g_sched_thread.join(); + + motor_stop_all(); + servo_center(); + + if (g_enc_l) { delete g_enc_l; g_enc_l = nullptr; } + if (g_enc_r) { delete g_enc_r; g_enc_r = nullptr; } + + close(g_timer_fd); + close(g_epoll_fd); + printf("[SCHED] 调度器停止\n"); +} diff --git a/scheduler.hpp b/scheduler.hpp new file mode 100644 index 0000000..ae4c16a --- /dev/null +++ b/scheduler.hpp @@ -0,0 +1,10 @@ +#pragma once +#include + +class FrameBuffer; + +bool scheduler_init(FrameBuffer* fb = nullptr); +void scheduler_start(); +void scheduler_stop(); + +extern std::atomic g_print_flag; diff --git a/strategy/deviation.cpp b/strategy/deviation.cpp new file mode 100644 index 0000000..435a988 --- /dev/null +++ b/strategy/deviation.cpp @@ -0,0 +1,72 @@ +#include "deviation.hpp" +#include +#include +#include + +void fit_midline(EdgeLines& lines) +{ + static EdgeLines prev; + static bool has_prev = false; + + if (!has_prev) + { + std::memcpy(&prev, &lines, sizeof(EdgeLines)); + has_prev = true; + return; + } + + for (int y = 0; y < IPM_ROW_COUNT; ++y) + { + if (lines.left[y] == 0 || lines.right[y] == 0) continue; + + if (std::abs(static_cast(lines.mid[y]) - static_cast(prev.mid[y])) > 20) + lines.mid[y] = prev.mid[y]; + } + + for (int y = 0; y < IPM_ROW_COUNT; ++y) + { + if (y >= 2 && y < IPM_ROW_COUNT - 2 && lines.mid[y] > 0) + { + int sum = 0, cnt = 0; + for (int dy = -2; dy <= 2; ++dy) + if (lines.mid[y + dy] > 0) { sum += lines.mid[y + dy]; ++cnt; } + if (cnt > 0) lines.mid[y] = sum / cnt; + } + } + + std::memcpy(&prev, &lines, sizeof(EdgeLines)); +} + +float calc_deviation(const EdgeLines& lines) +{ + float dev_near = 0, dev_mid = 0, dev_far = 0; + float cnt_near = 0, cnt_mid = 0, cnt_far = 0; + int mid_ref = IMAGE_WIDTH / 2; + + int total = IPM_ROW_COUNT; + int zone_far = total / 5; // 远: 上20% + int zone_mid = total * 3 / 5; // 中: 中间40% + + for (int y = 0; y < total; ++y) + { + if (lines.mid[y] == 0) continue; + float d = static_cast(lines.mid[y]) - mid_ref; + + if (y < zone_far) + { dev_far += d; cnt_far += 1; } + else if (y < zone_mid) + { dev_mid += d; cnt_mid += 1; } + else + { dev_near += d; cnt_near += 1; } + } + + if (cnt_far < 3) cnt_far = 3; + if (cnt_mid < 3) cnt_mid = 3; + if (cnt_near < 3) cnt_near = 3; + + float dev = (dev_far / cnt_far) * 0.50f + + (dev_mid / cnt_mid) * 0.30f + + (dev_near / cnt_near) * 0.20f; + + return std::clamp(dev / mid_ref, -1.0f, 1.0f); +} diff --git a/strategy/deviation.hpp b/strategy/deviation.hpp new file mode 100644 index 0000000..8fa3b5c --- /dev/null +++ b/strategy/deviation.hpp @@ -0,0 +1,6 @@ +#pragma once +#include "types.hpp" +#include "vision/search.hpp" + +float calc_deviation(const EdgeLines& lines); +void fit_midline(EdgeLines& lines); diff --git a/strategy/speed.cpp b/strategy/speed.cpp new file mode 100644 index 0000000..70e7860 --- /dev/null +++ b/strategy/speed.cpp @@ -0,0 +1,47 @@ +#include "speed.hpp" +#include +#include + +static float current_speed = 0.0f; +static float max_speed = 40.0f; +static float accel_rate = 2.0f; +static float decel_rate = 5.0f; + +void speed_strategy_reset() +{ + current_speed = 0.0f; +} + +float calc_base_speed(const TrackInfo& info) +{ + float target = max_speed * 0.7f; // 默认中速 + + switch (info.scene) + { + case TRACK_STRAIGHT: + target = max_speed; + break; + case TRACK_GENTLE_CURVE: + target = max_speed * 0.6f; + break; + case TRACK_SHARP_CURVE: + target = max_speed * 0.35f; + break; + case TRACK_LOST_LINE: + target = max_speed * 0.2f; + break; + case TRACK_CROSS: + target = max_speed * 0.5f; + break; + } + + if (std::fabs(info.deviation) > 0.6f) + target *= 0.6f; + + float diff = target - current_speed; + float rate = (diff > 0) ? accel_rate : decel_rate; + current_speed += rate * (diff > 0 ? 1 : -1) * 0.01f; + current_speed = std::clamp(current_speed, 0.0f, max_speed); + + return current_speed; +} diff --git a/strategy/speed.hpp b/strategy/speed.hpp new file mode 100644 index 0000000..b004088 --- /dev/null +++ b/strategy/speed.hpp @@ -0,0 +1,5 @@ +#pragma once +#include "types.hpp" + +float calc_base_speed(const TrackInfo& info); +void speed_strategy_reset(); diff --git a/tools/calibrate.py b/tools/calibrate.py new file mode 100644 index 0000000..b322713 --- /dev/null +++ b/tools/calibrate.py @@ -0,0 +1,43 @@ +import cv2 +import sys + +img = cv2.imread(sys.argv[1] if len(sys.argv) > 1 else "_capture.jpg") +if img is None: + print("找不到图片") + sys.exit(1) + +H, W = img.shape[:2] +points = [] +labels = ["A 近左", "B 近右", "C 远左", "D 远右"] + +def on_click(event, x, y, flags, param): + if event == cv2.EVENT_LBUTTONDOWN and len(points) < 4: + points.append((x, y)) + cv2.circle(img, (x, y), 4, (0, 0, 255), -1) + cv2.putText(img, labels[len(points)-1], (x+8, y-8), + cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 255), 1) + print(f" {labels[len(points)-1]}: ({x:4d}, {y:4d})") + if len(points) == 4: + print("\n--- 标定数据 ---") + for i, p in enumerate(points): + print(f"{labels[i]}: ({p[0]:4d}, {p[1]:4d})") + # 换算到 80x60 + sx, sy = 80/W, 60/H + print(f"\n80x60 处理分辨率:") + for i, p in enumerate(points): + print(f"{labels[i]}: ({int(p[0]*sx):3d}, {int(p[1]*sy):3d})") + print("\n按任意键退出...") + +cv2.namedWindow("calib", cv2.WINDOW_NORMAL) +cv2.setMouseCallback("calib", on_click) + +print(f"图片 {W}x{H}，按 A B C D 顺序点击 4 个点:") +print(" A=近处左边界 B=近处右边界 C=远处左边界 D=远处右边界\n") + +while True: + cv2.imshow("calib", img) + if cv2.waitKey(20) != -1 or len(points) >= 4: + break + +cv2.waitKey(0) +cv2.destroyAllWindows() diff --git a/types.hpp b/types.hpp new file mode 100644 index 0000000..8f5d947 --- /dev/null +++ b/types.hpp @@ -0,0 +1,195 @@ +#pragma once +#include + +// ========================================================= +// 类型别名 — 统一使用短名，与 Seekfree SDK 风格一致 +// ========================================================= +using uint8 = uint8_t; +using uint16 = uint16_t; +using uint32 = uint32_t; +using int16 = int16_t; +using int32 = int32_t; + +// ========================================================= +// 图像常量 — 摄像头采集 320×240，处理降采样至 160×120 +// ========================================================= +constexpr int CAMERA_WIDTH = 320; // UVC 摄像头原始宽度 +constexpr int CAMERA_HEIGHT = 240; // UVC 摄像头原始高度 +constexpr int IMAGE_WIDTH = 160; // 处理用图像宽度 (节省算力) +constexpr int IMAGE_HEIGHT = 120; // 处理用图像高度 +constexpr int IPM_ROW_COUNT = 120; // IPM/边界线数组行数 = IMAGE_HEIGHT + +// ========================================================= +// 电机引脚 — PWM 通过 sysfs 控制 +// +// 左驱 (DRV8701 IN1/IN2 模式): +// IN1=GPIO12 (正转), IN2=GPIO13 (反转, 必须锁0) +// 左轮: pwmchip8/pwm2 +// +// 右驱 (DRV8701 方向脚硬件锁死, 已物理交换电机电源线反转方向): +// 方向脚 GPIO67 (哑脚, 不影响任何硬件) +// 右轮: pwmchip8/pwm1 +// +// 使能: GPIO73 (高有效) +// PWM 频率 = 20kHz (周期 50000ns) +// ========================================================= +constexpr int MOTOR_PWMCHIP = 8; // PWM 芯片编号 +constexpr int MOTOR_LEFT_PWMID = 2; // 左电机 PWM 通道 +constexpr int MOTOR_RIGHT_PWMID = 1; // 右电机 PWM 通道 +constexpr int MOTOR_LEFT_DIR_GPIO = 12; // 左电机 IN1 (正转) +constexpr int MOTOR_LEFT_IN2_GPIO = 13; // 左电机 IN2 (反转, 初始化为0) +constexpr int MOTOR_RIGHT_DIR_GPIO = 67; // 右电机方向 (哑脚, 无效) +constexpr int MOTOR_ENABLE_GPIO = 73; // 电机总使能 (1=启动, 0=紧急制动) +constexpr int MOTOR_PWM_PERIOD_NS = 50000; // PWM 周期 50µs = 20kHz + +// ========================================================= +// 编码器 — 通过 /dev/mem 直接 mmap PWM 硬件寄存器读取 +// +// 基址 0x1611B000 是龙芯2K0300 PWM 外设的物理地址 +// 左编码器取通道 0, 方向 GPIO75, 极性 +1 +// 右编码器取通道 3, 方向 GPIO72, 极性 -1 (安装相反) +// 需 root 权限访问 /dev/mem +// ========================================================= +constexpr uintptr_t ENCODER_BASE = 0x1611B000; // PWM 寄存器基址 +constexpr int ENCODER_LEFT_CHANNEL = 0; // 左编码器 PWM 通道 +constexpr int ENCODER_RIGHT_CHANNEL = 3; // 右编码器 PWM 通道 +constexpr int ENCODER_LEFT_DIR_GPIO = 75; // 左编码器方向检测脚 +constexpr int ENCODER_RIGHT_DIR_GPIO= 72; // 右编码器方向检测脚 + +// ========================================================= +// 舵机引脚 — 前轮转向 +// +// pwmchip1/pwm0, 3ms 周期 (333Hz) +// 中位 = 1.52ms, 最大行程 ±0.5ms → 角度范围约 ±45° +// ========================================================= +constexpr int SERVO_PWMCHIP = 1; // 舵机 PWM 芯片 +constexpr int SERVO_PWMID = 0; // 舵机 PWM 通道 +constexpr int SERVO_PERIOD_NS = 3000000; // PWM 周期 3ms (333Hz) +constexpr int SERVO_CENTER_NS = 1520000; // 舵机中位 1.52ms +constexpr int SERVO_MAX_DELTA_NS = 210000; // 舵机偏离中位的最大行程 ±0.21ms + +// ========================================================= +// IMU — JY62 六轴陀螺仪 +// +// 连接 /dev/ttyS2, 波特率 115200 +// 数据帧: 0x55 头 → 11 字节包 → 解析角速度 z 轴 +// 用于偏航角积分 (YawTracker) +// ========================================================= +constexpr const char* IMU_DEVICE = "/dev/ttyS1"; // IMU 串口设备 +constexpr int IMU_BAUDRATE = 115200; // 波特率 + +// ========================================================= +// 外设路径 — 各传感器与显示器的 Linux 设备节点 +// ========================================================= +constexpr const char* FB_DEVICE = "/dev/fb0"; // LCD 帧缓冲 (RGB565) +constexpr const char* UVC_DEVICE = "/dev/video0"; // USB 摄像头 +constexpr const char* VL53L0X_DEV = "/dev/stmvl53l0x_ranging"; // ToF 激光测距 +constexpr const char* I2C2_DEV = "/dev/i2c-2"; // I2C 总线 2 + +// ========================================================= +// 运行时热调参数 — 从当前工作目录读取文件 +// +// 用法: 在车运行过程中 echo 值 > 对应文件即可实时生效 +// 例如: echo 4.0 > ./kp # 调大舵机 P +// echo 1 > ./start # 启动车 +// echo 0 > ./start # 停止车 +// ========================================================= +constexpr const char* CFG_START = "./start"; // 1=启动, 0=停止 +constexpr const char* CFG_SPEED = "./speed"; // 目标占空比 +constexpr const char* CFG_SHOW_IMG = "./showImg"; // 1=LCD 预览, 0=关闭 +constexpr const char* CFG_SAVE_IMG = "./saveImg"; // 1=保存当前帧到文件 +constexpr const char* CFG_FORESEE = "./foresee"; // 前瞻行号 (越大看越远) +constexpr const char* CFG_SERVO_KP = "./kp"; // 舵机 PID 比例系数 +constexpr const char* CFG_SERVO_KI = "./ki"; // 舵机 PID 积分系数 +constexpr const char* CFG_SERVO_KD = "./kd"; // 舵机 PID 微分系数 +constexpr const char* CFG_MOTOR_KP = "./mortor_kp";// 电机 PID 比例系数 +constexpr const char* CFG_MOTOR_KI = "./mortor_ki";// 电机 PID 积分系数 +constexpr const char* CFG_MOTOR_KD = "./mortor_kd";// 电机 PID 微分系数 +constexpr const char* CFG_DEADBAND = "./deadband"; // 舵机死区 (归一化偏差阈值) +constexpr const char* CFG_STEER_GAIN= "./steer_gain";// 舵机增益 (放大系数) + +// ========================================================= +// 赛道场景枚举 — 按危险程度从低到高排列 +// +// 本赛道只包含直线/弯道/十字/丢线，不含圆环 +// element_recognize() 负责从搜线结果判定当前场景 +// calc_base_speed() 根据场景决定目标车速 +// ========================================================= +enum TrackScene : uint8 { + TRACK_STRAIGHT = 0, // 直道 — 全速 + TRACK_GENTLE_CURVE, // 缓弯 — 中高速 (0.6x 全速) + TRACK_SHARP_CURVE, // 急弯 — 低速 (0.35x 全速) + TRACK_LOST_LINE, // 丢线 — 最低速 (0.2x 全速), 等待恢复 + TRACK_CROSS, // 十字路口 — 锁定直行, 降速 (0.5x 全速) +}; + +// ========================================================= +// 调试显示模式 — 控制 LCD 或 TCP 图传的叠加内容 +// ========================================================= +enum DebugDisplayMode : uint8 { + DBG_ORIGINAL = 0, // 原始彩色图像 + DBG_BINARY, // 二值化结果 + DBG_OVERLAY, // 叠加边界线/中线 + DBG_HSV_H, // HSV H 通道 + DBG_HSV_S // HSV S 通道 +}; + +// ========================================================= +// PID 模式 — 位置式用于角度外环, 增量式用于速度内环 +// ========================================================= +enum PIDMode : uint8 { + PID_POSITIONAL = 0, // 位置式: u(t) = Kp*e(t) + Ki*Σe + Kd*Δe + PID_INCREMENTAL = 1 // 增量式: Δu = Kp*(e-e') + Ki*e + Kd*(e-2e'+e") +}; + +// ========================================================= +// TrackInfo — 每帧计算出的赛道信息，传递给控制层 +// +// scene: 当前场景枚举，决定速度策略 +// deviation: 归一化中线偏差 (-1~+1), 0=正中间 +// curvature: 曲率估计 (越大越急) +// base_speed: 速度策略计算出的目标车速 (0~100) +// line_valid: 是否检测到有效赛道线 +// ========================================================= +struct TrackInfo { + TrackScene scene = TRACK_STRAIGHT; + float deviation = 0.0f; // -1(偏左) ~ +1(偏右) + float curvature = 0.0f; // 0(直道) ~ 1(急弯) + float base_speed = 0.0f; // 目标车速 + bool line_valid = false; // 搜线有效标志 +}; + +// ========================================================= +// ServoState — 舵机状态 (调试用) +// ========================================================= +struct ServoState { + float duty_ns = SERVO_CENTER_NS; // 当前 PWM 脉宽 (ns) + float target_angle = 0.0f; // 目标角度 (归一化) + float current_angle = 0.0f; // 实际角度 (归一化) +}; + +// ========================================================= +// MotorState — 单侧电机状态 (调试用) +// ========================================================= +struct MotorState { + float target_speed = 0.0f; // 目标转速 (rps) + float current_speed = 0.0f; // 实际转速 (rps, 来自编码器) + float pwm_output = 0.0f; // 当前 PWM 输出 (-100~+100) +}; + +// ========================================================= +// EdgeLines — 每行边界/中线列坐标 +// +// 坐标系: row=0 是图像顶部(远处), row=119 是底部(近处) +// left[row] / right[row] 为第 row 行的左右边界 x 坐标 (0~159) +// mid[row] 为中线 = (left + right) / 2 +// 未检测到边界的行值 = 0, 调用方需判断 +// left_valid / right_valid: 该帧是否有足够多的边线点 (>10 个) +// ========================================================= +struct EdgeLines { + uint8 left[IPM_ROW_COUNT] = {0}; // 左边界每行列坐标 + uint8 right[IPM_ROW_COUNT] = {0}; // 右边界每行列坐标 + uint8 mid[IPM_ROW_COUNT] = {0}; // 中线每行列坐标 + uint8 left_valid = 0; // 左边界有效标志 + uint8 right_valid = 0; // 右边界有效标志 +}; diff --git a/vision/camera.cpp b/vision/camera.cpp new file mode 100644 index 0000000..95ca3a2 --- /dev/null +++ b/vision/camera.cpp @@ -0,0 +1,56 @@ +#include "camera.hpp" +#include +#include +#include + +static cv::VideoCapture* cap = nullptr; +static cv::Mat pubframe; +static std::mutex frame_mutex; +static std::atomic stream_running{false}; + +bool camera_init() +{ + cap = new cv::VideoCapture(0); + if (!cap->isOpened()) return false; + + cap->set(cv::CAP_PROP_FOURCC, cv::VideoWriter::fourcc('M', 'J', 'P', 'G')); + cap->set(cv::CAP_PROP_FRAME_WIDTH, CAMERA_WIDTH); + cap->set(cv::CAP_PROP_FRAME_HEIGHT, CAMERA_HEIGHT); + cap->set(cv::CAP_PROP_AUTO_EXPOSURE, -1); + + return true; +} + +void camera_deinit() +{ + if (cap) { cap->release(); delete cap; cap = nullptr; } +} + +cv::Mat camera_capture() +{ + std::lock_guard lock(frame_mutex); + return pubframe.clone(); +} + +cv::Mat camera_resize(const cv::Mat& src) +{ + cv::Mat resized; + cv::resize(src, resized, cv::Size(IMAGE_WIDTH, IMAGE_HEIGHT)); + return resized; +} + +void camera_stream_start() +{ + stream_running.store(true); + std::thread([]() { + cv::Mat tmp; + while (stream_running.load()) + { + if (cap && cap->read(tmp)) + { + std::lock_guard lock(frame_mutex); + pubframe = tmp.clone(); + } + } + }).detach(); +} diff --git a/vision/camera.hpp b/vision/camera.hpp new file mode 100644 index 0000000..c2b5a50 --- /dev/null +++ b/vision/camera.hpp @@ -0,0 +1,9 @@ +#pragma once +#include +#include "types.hpp" + +bool camera_init(); +void camera_deinit(); +cv::Mat camera_capture(); // 返回最新的 BGR 320x240 帧 +cv::Mat camera_resize(const cv::Mat& src); // 缩放到 160x120 +void camera_stream_start(); // 后台线程持续采集 diff --git a/vision/element.cpp b/vision/element.cpp new file mode 100644 index 0000000..3ed27e2 --- /dev/null +++ b/vision/element.cpp @@ -0,0 +1,188 @@ +#include "element.hpp" +#include +#include +#include + +ElementMachine g_elm; + +namespace { + static constexpr uint8 k_cross_confirm_frames = 3; + static constexpr uint8 k_cross_exit_frames = 5; + static constexpr uint8 k_lock_min_frames = 5; + static constexpr float k_width_widen_ratio = 1.6f; + static constexpr float k_width_normal_ratio = 1.3f; + + static int bottom_width(const SearchResult& sr) + { + int y = IMAGE_HEIGHT - 1; + if (sr.lines.left[y] > 0 && sr.lines.right[y] > 0) + return sr.lines.right[y] - sr.lines.left[y]; + return -1; + } + + static uint8 line_lost_state(const SearchResult& sr) + { + uint8 st = 0; + if (!sr.lines.left_valid) st |= 1; + if (!sr.lines.right_valid) st |= 2; + return st; + } + + static void calibrate_width(const SearchResult& sr) + { + int w = bottom_width(sr); + if (w > 10 && w < IMAGE_WIDTH - 10) + { + g_elm.normal_width = (g_elm.normal_width * g_elm.calib_cnt + w) / (g_elm.calib_cnt + 1); + if (g_elm.calib_cnt < 50) ++g_elm.calib_cnt; + } + } + + static bool is_cross_entry(const SearchResult& sr) + { + if (g_elm.calib_cnt < 20) return false; + + int w = bottom_width(sr); + if (w < 0) return false; + + bool too_wide = (w > g_elm.normal_width * k_width_widen_ratio); + uint8 lost = line_lost_state(sr); + bool both_lost_upper = (lost == 3); + + if (too_wide || both_lost_upper) g_elm.confirm_cnt++; + else g_elm.confirm_cnt = 0; + + return g_elm.confirm_cnt >= k_cross_confirm_frames; + } + + static bool is_cross_exit(const SearchResult& sr) + { + int w = bottom_width(sr); + if (w < 0) { g_elm.exit_cnt = 0; return false; } + + bool width_normal = (w < g_elm.normal_width * k_width_normal_ratio); + bool bounds_ok = (sr.lines.left_valid && sr.lines.right_valid); + + if (width_normal && bounds_ok) g_elm.exit_cnt++; + else g_elm.exit_cnt = 0; + + return g_elm.exit_cnt >= k_cross_exit_frames; + } + + static void cross_patch_midline(SearchResult& sr) + { + int bottom_y = IMAGE_HEIGHT - 1; + int mid_x = sr.lines.mid[bottom_y]; + if (mid_x <= 0) return; + + int lx = mid_x - static_cast(g_elm.normal_width / 2); + int rx = mid_x + static_cast(g_elm.normal_width / 2); + lx = std::max(0, lx); + rx = std::min(IMAGE_WIDTH - 1, rx); + + for (int y = 0; y < IMAGE_HEIGHT; ++y) + { + if (sr.lines.left[y] == 0 && sr.lines.right[y] == 0) + { + sr.lines.left[y] = static_cast(lx); + sr.lines.right[y] = static_cast(rx); + sr.lines.mid[y] = static_cast(mid_x); + } + } + sr.lines.left_valid = 1; + sr.lines.right_valid = 1; + } +} + +void element_init() +{ + std::memset(&g_elm, 0, sizeof(g_elm)); + g_elm.state = TRACK_STRAIGHT; + g_elm.last_state = TRACK_STRAIGHT; + g_elm.normal_width = 50.0f; +} + +bool element_is_cross() +{ + return g_elm.state == TRACK_CROSS; +} + +void element_recognize(SearchResult& sr, TrackInfo& info) +{ + calibrate_width(sr); + + g_elm.last_state = g_elm.state; + ++g_elm.in_state_frames; + + if (g_elm.lock_cnt > 0) + { + --g_elm.lock_cnt; + info.scene = g_elm.state; + return; + } + + switch (g_elm.state) + { + case TRACK_STRAIGHT: + case TRACK_GENTLE_CURVE: + if (is_cross_entry(sr)) + { + int bottom_y = IMAGE_HEIGHT - 1; + g_elm.entry_deviation = (sr.lines.mid[bottom_y] > 0) + ? (sr.lines.mid[bottom_y] - IMAGE_WIDTH / 2) / static_cast(IMAGE_WIDTH / 2) + : 0.0f; + + g_elm.state = TRACK_CROSS; + g_elm.lock_cnt = k_lock_min_frames; + g_elm.in_state_frames = 0; + g_elm.confirm_cnt = 0; + g_elm.exit_cnt = 0; + info.scene = TRACK_CROSS; + } + else + { + info.scene = sr.lines.left_valid || sr.lines.right_valid + ? TRACK_STRAIGHT : TRACK_LOST_LINE; + } + info.line_valid = (info.scene != TRACK_LOST_LINE); + break; + + case TRACK_CROSS: + cross_patch_midline(sr); + + if (is_cross_exit(sr)) + { + g_elm.state = TRACK_STRAIGHT; + g_elm.lock_cnt = 0; + g_elm.in_state_frames = 0; + g_elm.confirm_cnt = 0; + g_elm.exit_cnt = 0; + info.scene = TRACK_STRAIGHT; + } + else + { + info.scene = TRACK_CROSS; + } + info.line_valid = true; + break; + + case TRACK_LOST_LINE: + g_elm.confirm_cnt = 0; + if (sr.lines.left_valid && sr.lines.right_valid) + { + g_elm.state = TRACK_STRAIGHT; + info.scene = TRACK_STRAIGHT; + } + else + { + info.scene = TRACK_LOST_LINE; + } + info.line_valid = (info.scene != TRACK_LOST_LINE); + break; + + default: + info.scene = TRACK_STRAIGHT; + info.line_valid = true; + break; + } +} diff --git a/vision/element.hpp b/vision/element.hpp new file mode 100644 index 0000000..90473aa --- /dev/null +++ b/vision/element.hpp @@ -0,0 +1,23 @@ +#pragma once +#include "types.hpp" +#include "search.hpp" + +struct ElementMachine { + TrackScene state = TRACK_STRAIGHT; + TrackScene last_state = TRACK_STRAIGHT; + + uint8 lock_cnt = 0; + uint8 confirm_cnt = 0; + uint8 exit_cnt = 0; + uint16 in_state_frames= 0; + + float normal_width = 0.0f; + int calib_cnt = 0; + float entry_deviation= 0.0f; +}; + +extern ElementMachine g_elm; + +void element_init(); +void element_recognize(SearchResult& sr, TrackInfo& info); +bool element_is_cross(); diff --git a/vision/preprocess.cpp b/vision/preprocess.cpp new file mode 100644 index 0000000..65fb719 --- /dev/null +++ b/vision/preprocess.cpp @@ -0,0 +1,131 @@ +#include "preprocess.hpp" +#include +#include + +uint8 g_ipm_image[IMAGE_HEIGHT][IMAGE_WIDTH] = {{0}}; +uint8 g_valid_l_bound[IMAGE_HEIGHT] = {0}; +uint8 g_valid_r_bound[IMAGE_HEIGHT] = {0}; + +static constexpr int PROC_W = IMAGE_WIDTH / 2; // 80 — 与demo对齐 +static constexpr int PROC_H = IMAGE_HEIGHT / 2; // 60 + +// ============================================================ +// IPM 4 点标定 — 80×60 相机视角 ↔ 80×60 俯视输出 +// +// 地面实况: 矩形赛道 40cm 宽 × 50cm 长, 近边距摄像头 55cm +// 摄像头垂直投影 ≈ 车头后 5cm, 车头距矩形 ≈ 50cm +// ============================================================ +static constexpr float CAL_NEAR_Y_MM = 550.0f; // 矩形近边 Y (mm) +static constexpr float CAL_FAR_Y_MM = 1050.0f; // 矩形远边 Y (mm) +static constexpr float CAL_HALF_W_MM = 200.0f; // 矩形半宽 (mm) + +static bool g_ipm_enabled = true; // 启用 IPM +static cv::Mat g_ipm_matrix; // 3×3 透视变换矩阵 + +void preprocess_init() {} + +static void init_ipm_calibration() +{ + // 四点: (相机 80×60 像素) → 标注的矩形角 + std::vector src = { + {19.f, 17.f}, // A 近左 + {57.f, 15.f}, // B 近右 + {27.f, 8.f}, // C 远左 + {49.f, 7.f} // D 远右 + }; + + // 目标: 俯视图中也保持矩形 (同宽), 近边 row=55, 远边 row=20 + // mm/pixel ≈ 14.3, 半宽 200mm → 14px + const float cx = PROC_W / 2.0f; // 40 + const float hw = 14.0f; // 半宽像素 + std::vector dst = { + {cx - hw, 55.f}, // A' 近左 + {cx + hw, 55.f}, // B' 近右 + {cx - hw, 20.f}, // C' 远左 + {cx + hw, 20.f} // D' 远右 + }; + + // getPerspectiveTransform(dst, src): 输出(俯视) → 输入(相机) + g_ipm_matrix = cv::getPerspectiveTransform(dst, src); + + printf("[IPM] 4点标定完成\n"); + printf(" 近边 Y=%.0f mm 远边 Y=%.0f mm 半宽=%.0f mm\n", + CAL_NEAR_Y_MM, CAL_FAR_Y_MM, CAL_HALF_W_MM); +} + +static void update_bounds(const cv::Mat& binary); + +static cv::Mat track_binarize(const cv::Mat& bgr) +{ + cv::Mat hsv, thresh_h, thresh_s, result; + cv::cvtColor(bgr, hsv, cv::COLOR_BGR2HSV); + + std::vector ch; + cv::split(hsv, ch); + + cv::threshold(ch[0], thresh_h, 0, 255, cv::THRESH_BINARY_INV | cv::THRESH_OTSU); + cv::threshold(ch[1], thresh_s, 0, 255, cv::THRESH_BINARY_INV | cv::THRESH_OTSU); + cv::bitwise_or(thresh_h, thresh_s, result); + + return result; +} + +static cv::Mat flood_fill_track(const cv::Mat& binary) +{ + cv::Mat kernel = cv::getStructuringElement(cv::MORPH_CROSS, cv::Size(2, 2)); + cv::Mat opened; + cv::morphologyEx(binary, opened, cv::MORPH_OPEN, kernel); + + cv::Mat mask = cv::Mat::zeros(PROC_H + 2, PROC_W + 2, CV_8UC1); + cv::Point seed(PROC_W / 2, PROC_H - 10); + cv::circle(opened, seed, 10, cv::Scalar(255), -1); + + cv::floodFill(opened, mask, seed, cv::Scalar(128), + nullptr, + cv::Scalar(20), cv::Scalar(20), 8); + + cv::Mat result = cv::Mat::zeros(PROC_H, PROC_W, CV_8UC1); + mask(cv::Rect(1, 1, PROC_W, PROC_H)).copyTo(result); + return result; +} + +void preprocess_run(const cv::Mat& bgr_frame) +{ + cv::Mat small; + cv::resize(bgr_frame, small, cv::Size(PROC_W, PROC_H)); + + cv::Mat bin = track_binarize(small); + + // IPM 透视校正 (首次调用时计算矩阵) + if (g_ipm_enabled) + { + if (g_ipm_matrix.empty()) + init_ipm_calibration(); + + cv::Mat ipm_out; + cv::warpPerspective(bin, ipm_out, g_ipm_matrix, + cv::Size(PROC_W, PROC_H), + cv::INTER_LINEAR, + cv::BORDER_CONSTANT, cv::Scalar(0)); + bin = ipm_out; + } + + cv::Mat track = flood_fill_track(bin); + cv::Mat full; + cv::resize(track, full, cv::Size(IMAGE_WIDTH, IMAGE_HEIGHT), 0, 0, cv::INTER_NEAREST); + + update_bounds(full); + std::memcpy(g_ipm_image, full.data, IMAGE_WIDTH * IMAGE_HEIGHT); +} + +static void update_bounds(const cv::Mat& binary) +{ + for (int y = 0; y < IMAGE_HEIGHT; ++y) + { + int l = 0, r = IMAGE_WIDTH - 1; + while (l < IMAGE_WIDTH && binary.at(y, l) == 0) ++l; + while (r >= 0 && binary.at(y, r) == 0) --r; + g_valid_l_bound[y] = static_cast(std::max(0, l)); + g_valid_r_bound[y] = static_cast(std::min(IMAGE_WIDTH - 1, r)); + } +} diff --git a/vision/preprocess.hpp b/vision/preprocess.hpp new file mode 100644 index 0000000..1c796ec --- /dev/null +++ b/vision/preprocess.hpp @@ -0,0 +1,10 @@ +#pragma once +#include +#include "types.hpp" + +void preprocess_init(); +void preprocess_run(const cv::Mat& bgr_frame); // 全彩 HSV 二值化 + IPM + +extern uint8 g_ipm_image[IMAGE_HEIGHT][IMAGE_WIDTH]; +extern uint8 g_valid_l_bound[IMAGE_HEIGHT]; +extern uint8 g_valid_r_bound[IMAGE_HEIGHT]; diff --git a/vision/search.cpp b/vision/search.cpp new file mode 100644 index 0000000..8c3242e --- /dev/null +++ b/vision/search.cpp @@ -0,0 +1,122 @@ +#include "search.hpp" +#include "preprocess.hpp" +#include +#include + +void search_init(SearchResult& r) +{ + std::memset(&r, 0, sizeof(r)); +} + +void search_run(SearchResult& r) +{ + search_init(r); + + int left_raw[IPM_ROW_COUNT] = {0}; + int right_raw[IPM_ROW_COUNT] = {0}; + int mid_raw[IPM_ROW_COUNT] = {0}; + + // ============================================================ + // 逐行最长白段搜索 (与 demo image_main() 完全一致) + // ============================================================ + for (int y = 0; y < IMAGE_HEIGHT; ++y) + { + int best_start = -1, best_end = -1, best_len = 0; + int cur_start = -1, cur_len = 0; + + for (int x = 0; x < IMAGE_WIDTH; ++x) + { + if (g_ipm_image[y][x]) + { + if (cur_len == 0) { cur_start = x; cur_len = 1; } + else cur_len++; + if (cur_len >= best_len) + { + best_len = cur_len; + best_start = cur_start; + best_end = x; + } + } + else + { + cur_len = 0; + cur_start = -1; + } + } + + if (best_len > 0) + { + left_raw[y] = best_start; + right_raw[y] = best_end; + } + } + + // ============================================================ + // 丢线补点 + 中线计算 (从底向上, 复用 demo 逻辑) + // ============================================================ + for (int y = IMAGE_HEIGHT - 2; y >= 0; --y) + { + if (left_raw[y] == 0 && right_raw[y] == 0) + { + mid_raw[y] = mid_raw[y + 1]; + if (mid_raw[y] > IMAGE_WIDTH / 2) + { + right_raw[y] = IMAGE_WIDTH - 1; + left_raw[y] = mid_raw[y + 1]; + } + else + { + left_raw[y] = 0; + right_raw[y] = mid_raw[y + 1]; + } + } + else + { + mid_raw[y] = (left_raw[y] + right_raw[y]) / 2; + } + } + if (left_raw[IMAGE_HEIGHT - 1] > 0 || right_raw[IMAGE_HEIGHT - 1] > 0) + mid_raw[IMAGE_HEIGHT - 1] = (left_raw[IMAGE_HEIGHT - 1] + right_raw[IMAGE_HEIGHT - 1]) / 2; + + // ============================================================ + // EMA 滤波 (从底向上, a=0.4, 与 demo 一致) + // ============================================================ + float left_f[IPM_ROW_COUNT] = {0}; + float right_f[IPM_ROW_COUNT] = {0}; + float mid_f[IPM_ROW_COUNT] = {0}; + constexpr float a = 0.4f; + int bot = IMAGE_HEIGHT - 1; + + for (int y = bot; y >= 0; --y) + { + if (y == bot) + { + left_f[y] = static_cast(left_raw[y]); + right_f[y] = static_cast(right_raw[y]); + mid_f[y] = (left_f[y] + right_f[y]) * 0.5f; + } + else + { + left_f[y] = a * left_raw[y] + (1.0f - a) * left_f[y + 1]; + right_f[y] = a * right_raw[y] + (1.0f - a) * right_f[y + 1]; + mid_f[y] = (left_f[y] + right_f[y]) * 0.5f; + } + + int lv = static_cast(left_f[y] + 0.5f); + int rv = static_cast(right_f[y] + 0.5f); + int mv = static_cast(mid_f[y] + 0.5f); + + r.lines.left[y] = static_cast(std::clamp(lv, 0, 255)); + r.lines.right[y] = static_cast(std::clamp(rv, 0, 255)); + r.lines.mid[y] = static_cast(std::clamp(mv, 0, 255)); + } + + int left_cnt = 0, right_cnt = 0; + for (int y = 0; y < IPM_ROW_COUNT; ++y) + { + if (r.lines.left[y] > 0) ++left_cnt; + if (r.lines.right[y] > 0) ++right_cnt; + } + r.lines.left_valid = (left_cnt > 10) ? 1 : 0; + r.lines.right_valid = (right_cnt > 10) ? 1 : 0; +} diff --git a/vision/search.hpp b/vision/search.hpp new file mode 100644 index 0000000..0d4db83 --- /dev/null +++ b/vision/search.hpp @@ -0,0 +1,9 @@ +#pragma once +#include "types.hpp" + +struct SearchResult { + EdgeLines lines; +}; + +void search_init(SearchResult& r); +void search_run(SearchResult& r);