在前面的环节，我们实现了走马观碑关键模块的驱动程序，同时在 Gazebo 仿真环境验证了巡线算法的可行性。接下来我们将编写智能车实物下位机工程，也就是 car_project。

一、项目整体设计

car_project 是龙芯 2K0300 智能车下位机工程，主要包含摄像头巡线、LCD 回显、双电机差速控制、编码器测速、MPU6050 姿态反馈、VL53L0X 测距、按键控制以及上位机遥测监视。

创建工程目录：

zhengyang@ubuntu:~$ cd /opt/2k0300/loongson_2k300_lib/
zhengyang@ubuntu:/opt/2k0300/loongson_2k300_lib$ mkdir car_project

当前 car_project 目录结构如下：

car_project/
├── CMakeLists.txt          # CMake 构建脚本
├── README.md               # 项目说明
├── config.yaml             # 全局配置文件
├── main.cpp                # 主程序入口
│
├── app/                    # 应用编排层
├── drivers/                # 硬件驱动适配层
├── tracking/               # 巡线算法模块
├── core/                   # 核心控制模块
├── telemetry/              # 下位机遥测上传
├── upper_monitor/          # Python 上位机监视工具
├── utils/                  # 日志、配置、滤波、几何工具
├── test/                   # 单元测试和硬件测试
├── scripts/                # 构建、部署、运行脚本
└── tools/                  # 预留工具目录

几个主要目录的职责如下：

目录	职责	说明
app/	应用编排	负责初始化硬件、启动线程、处理按键、注册任务和退出清理
drivers/	硬件驱动	封装摄像头、LCD、IMU、测距、电机、编码器、按键
tracking/	巡线算法	图像预处理、边界扫描、中线生成、道路识别、策略处理
core/	核心控制	串级控制、传感器融合、周期调度
telemetry/	遥测上传	通过 UDP 上传状态 JSON 和 JPEG 调试图
upper_monitor/	上位机	接收下位机遥测，显示图像和波形
utils/	公共工具	日志、配置解析、滤波、巡线几何工具
test/	测试	纯逻辑单元测试和硬件验证用例
scripts/	脚本	本机编译、交叉编译、部署和远程运行

1.1 程序运行流程

main.cpp 只保留最外层入口逻辑，主要做 4 件事：

1. 加载 config.yaml；
2. 初始化日志系统；
3. 创建 CarApplication；
4. 启动应用并等待退出信号。

真正的业务流程由 app/CarApplication 负责。这样做的好处是避免 main.cpp 越写越大，后续维护时可以清楚地区分“程序入口”和“业务编排”。

整体运行链路如下：

main.cpp
  └── CarApplication
        ├── 初始化配置、日志、控制器
        ├── 初始化摄像头、LCD、IMU、编码器、测距、电机、按键
        ├── 启动摄像头异步采集
        ├── 启动 IMU / VL53L0X 连续读取
        ├── 启动视觉、控制、编码器实时线程
        ├── 周期执行 LCD 回显和遥测上传
        └── 退出时停止电机并释放所有资源

控制闭环的大致数据流如下：

摄像头图像
  -> tracking 巡线算法
  -> 得到巡线误差 tracking_error 和道路状态 road_state
  -> controller 计算目标角速度和目标轮速
  -> data_fusion 提供 IMU yaw rate、编码器 RPS、测距状态
  -> controller 输出左右电机占空比
  -> motor_controller 控制左右电机

1.2 配置文件

config.yaml 是整个工程的集中配置入口，硬件路径、控制参数、巡线参数、调度周期、遥测参数都放在这里。

配置加载由 utils/config 完成。配置模块只负责解析 YAML 并生成配置对象，不直接访问硬件。

常用配置包括：

camera:
  device: "/dev/video0"
  width: 160
  height: 120
  fps: 60

mpu6050:
  enabled: true
  interval_ms: 20

vl53l0x:
  enabled: false

key:
  enabled: false

telemetry:
  enabled: false
  host: "192.168.1.100"
  state_port: 9000
  image_port: 9001

当前速度控制单位统一使用 RPS，也就是每秒转数。这样可以和编码器驱动、控制器、遥测字段保持一致，避免在控制链路里反复进行 RPM 和 RPS 的换算。

二、主要模块说明

2.1 应用编排模块 app

app 是下位机应用层，核心类是 CarApplication。

2.1.1 源码路径

• car_application.h
• car_application.cpp
• hardware.cpp
• keys.cpp
• realtime.cpp
• tasks.cpp

2.1.2 主要功能

CarApplication 的主要职责：

• 初始化所有硬件驱动；
• 初始化巡线算法、控制器、数据融合和遥测模块；
• 启动摄像头异步采集；
• 启动控制线程、视觉线程、编码器线程；
• 注册 LCD 显示和遥测上传任务；
• 处理 KEY0 / KEY1 按键业务；
• 程序退出时按顺序停止电机、线程和硬件资源。

2.1.3 按键逻辑

按键业务当前定义如下：

KEY0 短按        -> 发车/暂停
KEY1 短按        -> 速度档位切换
KEY1 长按        -> 陀螺仪静态标定

速度档位通过 CascadeController::set_speed_scale() 实现，当前 3 档为：

gear 0 -> 80%
gear 1 -> 100%
gear 2 -> 120%

启用按键后，默认从 gear 0 启动，每次短按 KEY1 切换到下一档。

2.2 工具模块 utils

utils 目录放置与具体硬件无关的公共工具。

2.2.1 log 日志模块

源码路径：

• log.h
• log.cpp

主要功能包括日志级别、控制台输出、文件输出和多线程安全写日志，用于替代程序中零散的 printf。

2.2.2 config 配置模块

源码路径：

• config.h
• config.cpp

主要功能是解析 config.yaml，生成摄像头、电机、编码器、IMU、测距、控制器、遥测等配置对象，并给缺失字段提供默认值。

2.2.3 filter 滤波模块

源码路径：

• filter.h
• filter.cpp

当前主要提供一阶低通滤波，用于抑制 IMU yaw rate 和编码器 RPS 的单点跳变。

2.2.4 巡线工具函数

源码路径：

• tracking_utils.h
• tracking_utils.cpp
• curvature_calculator.h
• curvature_calculator.cpp

这些工具主要提供巡线算法中复用的几何计算能力，例如点集处理、曲率估计、方向判断等。

2.3 驱动模块 drivers

drivers 是硬件驱动适配层。上层模块不直接操作 /dev、sysfs、pwm、gpio，而是通过这里封装好的 C++ 类访问硬件。

2.3.1 VL53L0X 激光测距

源码路径：

• vl53l0x.h
• vl53l0x.cpp

主要功能是通过 IIO/sysfs 读取距离值，支持单次读取和后台连续读取。VL53L0X 不直接控制电机，只负责提供距离信息；是否减速或停车由应用层和控制层根据配置判断。

2.3.2 ST7735 显示模块

源码路径：

• st7735.h
• st7735.cpp

主要功能是基于 Linux framebuffer 输出图像，支持 RGB565 格式转换，用于显示摄像头巡线调试画面。显示任务属于低优先级任务，不能阻塞电机控制线程。

2.3.3 MPU6050 陀螺仪模块

源码路径：

• mpu6050.h
• mpu6050.cpp

主要功能是通过 Linux IIO 读取三轴加速度和三轴角速度，计算 yaw rate、pitch、roll，并给角速度内环提供实际 yaw rate。当前 IMU 初始化阶段不强制自动标定，标定动作通过 KEY1 长按触发。

2.3.4 摄像头模块 camera

源码路径：

• camera.h
• camera.cpp

主要功能是基于 V4L2 打开 USB 摄像头，支持分辨率、帧率、像素格式配置，支持异步采集、图像翻转和灰度输出，为 tracking 模块提供最新图像帧。

2.3.5 电机模块

源码路径：

• motor.h
• motor.cpp
• motor_controller.h
• motor_controller.cpp

motor 负责单个电机，motor_controller 负责左右两个电机。主要功能包括配置 PWM 周期和占空比、控制方向 GPIO、输出左右轮差速占空比，并在退出或异常时停止电机。当前巡线控制只输出前进或停转占空比，不主动倒车，也不依赖左右轮反向来原地反打。

2.3.6 编码器模块

源码路径：

• encoder.h
• encoder.cpp
• encoder_controller.h
• encoder_controller.cpp

主要功能是打开左右编码器设备，通过 ioctl 读取编码器速度。当前速度单位为 RPS，encoder_controller 会汇总左右轮速度并提供平均轮速。编码器数据主要用于速度环闭环控制，也会通过遥测上传到上位机。

2.3.7 按键模块 key

源码路径：

• key.h
• key.cpp

主要功能是读取 Linux input 事件，默认设备名为 LS2K300 99Pi Keys，识别 KEY0 / KEY1，封装短按和长按事件，并在双键同时按下时屏蔽单键误触。驱动层只负责事件识别，不直接决定业务行为。

2.4 巡线模块 tracking

tracking 是 Python 仿真巡线逻辑的 C++ 移植版本，负责从摄像头图像中提取赛道信息。

2.4.1 源码路径

主要源码路径：

• tracking_pipeline.h
• tracking_pipeline.cpp
• tracking_display.h
• tracking_display.cpp

2.4.2 基础算法层 algorithm

包括：

• boundary_scanner：边界扫描；
• line_follower：根据边界生成中线和误差；
• line_fitter：补线；
• corner_detector：拐点检测。

2.4.3 道路处理层 handler

包括：

• crossroad_handler：十字道路处理；
• left_ring_handler：左圆环处理；
• right_ring_handler：右圆环处理。

复杂道路通常不是 1 帧图像就能判断完成，因此这里会维护阶段状态，例如进入圆环、圆环中、出环等。

2.4.4 策略层 strategy

包括：

• normal_road_strategy：普通道路；
• cross_road_strategy：十字道路；
• left_ring_rad_strategy：左圆环；
• right_ring_rad_strategy：右圆环；
• barrier_road_strategy：障碍道路；
• bridge_road_strategy：坡道/桥。

2.4.5 巡线主流程

一次巡线处理的大致流程如下：

摄像头图像
  -> 灰度化/预处理
  -> 边界扫描
  -> 拐点检测
  -> 道路状态判断
  -> 按道路策略补线/生成目标线
  -> 计算 tracking_error
  -> 绘制调试图

输出结果主要包括 tracking_error、road_state、左右边界点、中线点、拐点和调试图像。

2.5 核心控制模块 core

core 不访问具体硬件，只处理控制逻辑和状态数据。

2.5.1 数据融合 data_fusion

源码路径：

• data_fusion.h
• data_fusion.cpp

主要功能是接收 IMU yaw rate、编码器左右轮 RPS、VL53L0X 测距，对 yaw rate 和 RPS 做低通滤波，并生成统一的 VehicleState，供控制器使用。

2.5.2 串级控制器 controller

源码路径：

• controller.h
• controller.cpp

控制器采用“视觉外环 + 角速度内环 + 速度环”的结构。

外环根据巡线误差计算目标角速度：

tracking_error -> target_yaw_rate

角速度内环根据目标角速度和 IMU 实际角速度计算差速修正：

target_yaw_rate - actual_yaw_rate -> turn_percent

速度环根据目标 RPS 和编码器实际 RPS 计算基础前进占空比：

target_rps - actual_rps -> base_percent

最后进行运动合成：

left_percent  = base_percent - turn_percent
right_percent = base_percent + turn_percent

正 turn_percent 表示目标 yaw rate 为正，也就是车体需要左转。差速车左转时右轮应更快，因此右轮占空比增加、左轮占空比降低。

当前控制器中，差速修正只受 max_turn_percent 和最终电机占空比限幅约束，已经移除了 max_turn_base_ratio 这层限制。

2.5.3 周期调度器 scheduler

源码路径：

• scheduler.h
• scheduler.cpp

主要功能是注册周期任务，并按固定间隔执行任务，用于 LCD 回显、遥测上传等低优先级任务。电机控制、视觉处理、编码器采样属于实时性更高的任务，不依赖这个普通调度器，而是由应用层单独启动实时工作线程。

2.6 遥测和上位机

2.6.1 下位机遥测 telemetry

源码路径：

• telemetry_publisher.h
• telemetry_publisher.cpp

遥测模块通过 UDP 向上位机发送两类数据：

数据	内容
状态 JSON	速度、占空比、误差、角速度、测距、道路状态
JPEG 图像	巡线叠加后的调试画面

典型配置如下：

telemetry:
  enabled: true
  host: "上位机IP"
  state_port: 9000
  image_port: 9001
  state_hz: 20
  image_hz: 10
  jpeg_quality: 60

UDP 不保证可靠送达，但延迟低，适合调车时实时观察最新状态。控制闭环不依赖遥测发送结果，即使上位机没有启动，小车控制也可以继续运行。

2.6.2 上位机 upper_monitor

源码路径：

• monitor.py

安装依赖：

cd upper_monitor
pip install -r requirements.txt

运行上位机：

python3 monitor.py --state-port 9000 --image-port 9001

上位机采用左右分栏显示，左侧显示下位机上传的巡线图像，图像下方显示关键遥测信息，右侧显示控制曲线。主要曲线包括巡线误差、目标 yaw rate 和实际 yaw rate、目标 RPS 和实际 RPS、左右电机占空比、基础占空比和差速修正。

如果只想看图像，不显示右侧曲线，可以运行：

python3 monitor.py --no-plot

三、构建、部署与测试

3.1 构建工程

本机编译：

cd /opt/2k0300/loongson_2k300_lib/car_project
cmake -S . -B build -DCROSS_COMPILE=OFF
cmake --build build -j$(nproc)

交叉编译：

cd /opt/2k0300/loongson_2k300_lib/car_project
cmake -S . -B build -DCROSS_COMPILE=ON
cmake --build build -j$(nproc)

工程默认使用同级目录下的交叉编译依赖：

../cross_lib/loongarch64-linux-gnu-gcc13.3
../cross_lib/opencv-4.10.0
../cross_lib/googletest-1.14.0

也可以使用脚本：

./scripts/build.sh
./scripts/run.sh

scripts/run.sh 支持上传程序、配置、测试图片和测试视频到开发板，也可以远程运行主程序或测试程序。

常用环境变量包括：

REMOTE_HOST     开发板 IP
REMOTE_DIR      开发板部署目录
CROSS_COMPILE   是否交叉编译
UPLOAD_IMAGES   是否上传测试图片
UPLOAD_VIDEOS   是否上传测试视频

3.2 单元测试

安装本机测试依赖：

sudo apt install libgtest-dev

如果要在久久派开发板运行单元测试，需要交叉编译 GoogleTest：

wget https://github.com/google/googletest/archive/refs/tags/v1.14.0.tar.gz
tar -xzf v1.14.0.tar.gz
cd googletest-1.14.0
mkdir build && cd build

CC=loongarch64-linux-gnu-gcc \
CXX=loongarch64-linux-gnu-g++ \
cmake .. -D CMAKE_INSTALL_PREFIX=../install

make -j$(nproc)
make install

当前测试目录按模块划分：

test/
├── core/       # 控制器测试
├── drivers/    # 摄像头、编码器、电机、IMU、LCD、测距测试
├── tracking/   # 巡线算法和策略测试
├── utils/      # 配置、日志、滤波、几何工具测试
└── videos/     # 测试视频

部分 drivers 测试会访问真实硬件，运行前需要确认设备节点、GPIO/PWM 权限以及传感器连接状态。没有硬件时，建议只运行 core、tracking、utils 这类纯逻辑测试。

四、维护原则与代码下载

这个工程后续还会继续调参和扩展，因此代码结构上尽量遵守几个原则：

1. drivers 只封装硬件访问，不写业务逻辑；
2. tracking 只负责图像和道路识别，不直接控制电机；
3. core 只处理控制算法和状态融合，不访问设备文件；
4. app 负责把硬件、算法、控制器和任务调度串起来；
5. utils 保持低耦合，避免反向依赖业务模块；
6. 高频控制线程不能被显示、日志、遥测阻塞；
7. 所有可调参数尽量放到 config.yaml，避免写死在代码里。

五、代码仓库

loongson_2k300_lib