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 中央调度器

.gitignore

@@ -0,0 +1,7 @@
+build/
+*.o
+*.a
+smartcar_framework
+bench/bench
+.vscode/
+*~

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)

bench/bench.cpp

@@ -0,0 +1,128 @@
+#include <cstdio>
+#include <cstring>
+#include <chrono>
+#include <algorithm>
+
+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<double>(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;
+}

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"

control/imu.cpp

@@ -0,0 +1,81 @@
+#include "imu.hpp"
+#include <fcntl.h>
+#include <unistd.h>
+#include <cmath>
+#include <cstring>
+#include <termios.h>
+
+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;
+}

control/imu.hpp

@@ -0,0 +1,24 @@
+#pragma once
+#include "types.hpp"
+#include <cstdint>
+
+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();
+};

control/motor.cpp

@@ -0,0 +1,72 @@
+#include "motor.hpp"
+#include "hal/gpio.hpp"
+#include "hal/pwm.hpp"
+#include "types.hpp"
+#include <cstdio>
+#include <algorithm>
+
+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<unsigned int>(MOTOR_PWM_PERIOD_NS * duty / 100.0f);
+        pwm->setDutyCycle(d_ns);
+    }
+    else
+    {
+        dir->setValue(0);
+        unsigned int d_ns = static_cast<unsigned int>(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);
+}

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);

control/pid.cpp

@@ -0,0 +1,53 @@
+#include "pid.hpp"
+#include <algorithm>
+#include <cmath>
+
+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;
+}

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;
+};

control/servo.cpp

@@ -0,0 +1,57 @@
+#include "servo.hpp"
+#include "hal/pwm.hpp"
+#include <algorithm>
+#include <cstdio>
+#include <cmath>
+
+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<float>(SERVO_MAX_DELTA_NS),
+                        static_cast<float>(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<float>(SERVO_CENTER_NS - SERVO_MAX_DELTA_NS),
+                         static_cast<float>(SERVO_CENTER_NS + SERVO_MAX_DELTA_NS));
+    servo_pwm->setDutyCycle(static_cast<unsigned int>(duty_ns));
+}
+
+void servo_center()
+{
+    if (serv_initialized)
+        servo_pwm->setDutyCycle(SERVO_CENTER_NS);
+}

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; }

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)

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

debug/config.cpp

@@ -0,0 +1,20 @@
+#include "config.hpp"
+#include <fstream>
+
+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;
+}

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);

debug/draw.cpp

@@ -0,0 +1,88 @@
+#include "draw.hpp"
+#include <algorithm>
+#include <cmath>
+
+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);
+    }
+}

debug/draw.hpp

@@ -0,0 +1,17 @@
+#pragma once
+#include <cstdint>
+#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);

debug/tcp.cpp

@@ -0,0 +1,25 @@
+#include "tcp.hpp"
+#include <cstdio>
+
+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 连接
+}

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();

hal/encoder.cpp

@@ -0,0 +1,64 @@
+#include "encoder.hpp"
+#include <fcntl.h>
+#include <unistd.h>
+#include <sys/mman.h>
+#include <cmath>
+
+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;  // 无脉冲时速度衰减
+    }
+}

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;
+};

hal/framebuffer.cpp

@@ -0,0 +1,60 @@
+#include "framebuffer.hpp"
+#include <fcntl.h>
+#include <unistd.h>
+#include <sys/mman.h>
+#include <sys/ioctl.h>
+#include <linux/fb.h>
+#include <cstring>
+
+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;
+}

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);
+};

hal/gpio.cpp

@@ -0,0 +1,43 @@
+#include "gpio.hpp"
+#include <cstdio>
+#include <cstdlib>
+#include <fstream>
+#include <sstream>
+
+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;
+}

hal/gpio.hpp

@@ -0,0 +1,17 @@
+#pragma once
+#include <string>
+
+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();
+};

hal/pwm.cpp

@@ -0,0 +1,57 @@
+#include "pwm.hpp"
+#include <fstream>
+#include <thread>
+#include <chrono>
+
+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;
+}

hal/pwm.hpp

@@ -0,0 +1,24 @@
+#pragma once
+#include <string>
+#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();
+};

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 <csignal>
+#include <atomic>
+#include <cstdio>
+#include <thread>
+
+static std::atomic<bool> 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;
+}

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
+    ]
+  }
+}

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%

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;
+}

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);

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}")

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 <atomic>
+#include <chrono>
+#include <thread>
+#include <mutex>
+#include <cstdio>
+#include <sys/timerfd.h>
+#include <sys/epoll.h>
+#include <unistd.h>
+
+// =========================================================
+// 全局控制对象
+// =========================================================
+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<bool> g_sched_running{false};
+std::atomic<int>         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<std::mutex> 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<float>(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<float>(deadband));
+        servo_set_steer_gain(static_cast<float>(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");
+}

scheduler.hpp

@@ -0,0 +1,10 @@
+#pragma once
+#include <atomic>
+
+class FrameBuffer;
+
+bool scheduler_init(FrameBuffer* fb = nullptr);
+void scheduler_start();
+void scheduler_stop();
+
+extern std::atomic<int> g_print_flag;

strategy/deviation.cpp

@@ -0,0 +1,72 @@
+#include "deviation.hpp"
+#include <algorithm>
+#include <cmath>
+#include <cstring>
+
+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<int>(lines.mid[y]) - static_cast<int>(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<float>(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);
+}

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);

strategy/speed.cpp

@@ -0,0 +1,47 @@
+#include "speed.hpp"
+#include <algorithm>
+#include <cmath>
+
+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;
+}

strategy/speed.hpp

@@ -0,0 +1,5 @@
+#pragma once
+#include "types.hpp"
+
+float calc_base_speed(const TrackInfo& info);
+void  speed_strategy_reset();

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()

types.hpp

@@ -0,0 +1,195 @@
+#pragma once
+#include <cstdint>
+
+// =========================================================
+// 类型别名 — 统一使用短名，与 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;     // 右边界有效标志
+};

vision/camera.cpp

@@ -0,0 +1,56 @@
+#include "camera.hpp"
+#include <atomic>
+#include <mutex>
+#include <thread>
+
+static cv::VideoCapture* cap       = nullptr;
+static cv::Mat           pubframe;
+static std::mutex        frame_mutex;
+static std::atomic<bool> 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<std::mutex> 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<std::mutex> lock(frame_mutex);
+                pubframe = tmp.clone();
+            }
+        }
+    }).detach();
+}

vision/camera.hpp

@@ -0,0 +1,9 @@
+#pragma once
+#include <opencv2/opencv.hpp>
+#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();                     // 后台线程持续采集

vision/element.cpp

@@ -0,0 +1,188 @@
+#include "element.hpp"
+#include <algorithm>
+#include <cmath>
+#include <cstring>
+
+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<int>(g_elm.normal_width / 2);
+        int rx = mid_x + static_cast<int>(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<uint8>(lx);
+                sr.lines.right[y] = static_cast<uint8>(rx);
+                sr.lines.mid[y]   = static_cast<uint8>(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<float>(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;
+    }
+}

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();

vision/preprocess.cpp

@@ -0,0 +1,131 @@
+#include "preprocess.hpp"
+#include <algorithm>
+#include <cmath>
+
+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<cv::Point2f> 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<cv::Point2f> 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<cv::Mat> 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<uint8>(y, l) == 0) ++l;
+        while (r >= 0            && binary.at<uint8>(y, r) == 0) --r;
+        g_valid_l_bound[y] = static_cast<uint8>(std::max(0, l));
+        g_valid_r_bound[y] = static_cast<uint8>(std::min(IMAGE_WIDTH - 1, r));
+    }
+}

vision/preprocess.hpp

@@ -0,0 +1,10 @@
+#pragma once
+#include <opencv2/opencv.hpp>
+#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];

vision/search.cpp

@@ -0,0 +1,122 @@
+#include "search.hpp"
+#include "preprocess.hpp"
+#include <algorithm>
+#include <cstring>
+
+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<float>(left_raw[y]);
+            right_f[y] = static_cast<float>(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<int>(left_f[y]  + 0.5f);
+        int rv = static_cast<int>(right_f[y] + 0.5f);
+        int mv = static_cast<int>(mid_f[y]   + 0.5f);
+
+        r.lines.left[y]  = static_cast<uint8>(std::clamp(lv, 0, 255));
+        r.lines.right[y] = static_cast<uint8>(std::clamp(rv, 0, 255));
+        r.lines.mid[y]   = static_cast<uint8>(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;
+}

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);