初始提交：龙芯2K0300智能车卖家Demo完整代码
This commit is contained in:
spdis
+284
@@ -0,0 +1,284 @@
|
||||
#include "camera.h"
|
||||
|
||||
cv::VideoCapture cap;
|
||||
|
||||
double kp = 0;
|
||||
double ki = 0;
|
||||
double kd = 0;
|
||||
|
||||
int screenWidth, screenHeight;
|
||||
int newWidth, newHeight;
|
||||
int fb;
|
||||
// 创建帧缓冲区
|
||||
uint16_t *fb_buffer;
|
||||
PwmController servo(1, 0);
|
||||
|
||||
#define calc_scale 2
|
||||
|
||||
int CameraInit(uint8_t camera_id, double dest_fps, int width, int height)
|
||||
{
|
||||
servo.setPeriod(3000000);
|
||||
servo.setDutyCycle(1500000);
|
||||
servo.enable();
|
||||
|
||||
// 打开帧缓冲区设备
|
||||
fb = open("/dev/fb0", O_RDWR);
|
||||
if (fb == -1)
|
||||
{
|
||||
std::cerr << "无法打开帧缓冲区设备" << std::endl;
|
||||
return -1;
|
||||
}
|
||||
|
||||
// 获取帧缓冲区设备信息
|
||||
struct fb_var_screeninfo vinfo;
|
||||
if (ioctl(fb, FBIOGET_VSCREENINFO, &vinfo) == -1)
|
||||
{
|
||||
std::cerr << "无法获取帧缓冲区信息" << std::endl;
|
||||
close(fb);
|
||||
return -1;
|
||||
}
|
||||
|
||||
// 动态设置屏幕分辨率
|
||||
screenWidth = vinfo.xres;
|
||||
screenHeight = vinfo.yres;
|
||||
|
||||
// 计算帧缓冲区大小
|
||||
size_t fb_size = vinfo.yres_virtual * vinfo.xres_virtual * vinfo.bits_per_pixel / 8;
|
||||
|
||||
// 使用 mmap 映射帧缓冲区到内存
|
||||
fb_buffer = (uint16_t *)mmap(NULL, fb_size, PROT_READ | PROT_WRITE, MAP_SHARED, fb, 0);
|
||||
if (fb_buffer == MAP_FAILED)
|
||||
{
|
||||
std::cerr << "无法映射帧缓冲区到内存" << std::endl;
|
||||
close(fb);
|
||||
return -1;
|
||||
}
|
||||
|
||||
// 打开默认摄像头（设备编号 0）
|
||||
cap.open(0, cv::CAP_V4L2);
|
||||
if (!cap.isOpened()) cap.open(0);
|
||||
|
||||
cap.set(cv::CAP_PROP_FRAME_WIDTH, 320);
|
||||
cap.set(cv::CAP_PROP_FRAME_HEIGHT, 240);
|
||||
cap.set(cv::CAP_PROP_FOURCC, cv::VideoWriter::fourcc('M', 'J', 'P', 'G'));
|
||||
|
||||
// 检查摄像头是否成功打开
|
||||
if (!cap.isOpened())
|
||||
{
|
||||
printf("无法打开摄像头\n");
|
||||
munmap(fb_buffer, fb_size);
|
||||
close(fb);
|
||||
return -1;
|
||||
}
|
||||
|
||||
cap.set(cv::CAP_PROP_FRAME_WIDTH, width); // 宽度
|
||||
cap.set(cv::CAP_PROP_FRAME_HEIGHT, height); // 高度
|
||||
cap.set(cv::CAP_PROP_FOURCC, cv::VideoWriter::fourcc('M', 'J', 'P', 'G')); // 视频流格式
|
||||
cap.set(cv::CAP_PROP_AUTO_EXPOSURE, -1); // 设置自动曝光
|
||||
|
||||
// 获取摄像头实际分辨率
|
||||
int cameraWidth = cap.get(cv::CAP_PROP_FRAME_WIDTH);
|
||||
int cameraHeight = cap.get(cv::CAP_PROP_FRAME_HEIGHT);
|
||||
printf("摄像头分辨率: %d x %d\n", cameraWidth, cameraHeight);
|
||||
|
||||
// 计算 newWidth 和 newHeight，确保图像适应屏幕
|
||||
double widthRatio = static_cast<double>(screenWidth) / cameraWidth;
|
||||
double heightRatio = static_cast<double>(screenHeight) / cameraHeight;
|
||||
double scale = std::min(widthRatio, heightRatio); // 选择较小的比例，确保图像不超出屏幕
|
||||
|
||||
newWidth = static_cast<int>(cameraWidth * scale);
|
||||
newHeight = static_cast<int>(cameraHeight * scale);
|
||||
printf("自适应分辨率: %d x %d\n", newWidth, newHeight);
|
||||
|
||||
// 计算帧率
|
||||
double fps = cap.get(cv::CAP_PROP_FPS);
|
||||
printf("Camera fps:%lf\n", fps);
|
||||
|
||||
line_tracking_width = newWidth / calc_scale;
|
||||
line_tracking_height = newHeight / calc_scale;
|
||||
|
||||
// 计算每帧的延迟时间（ms）
|
||||
return static_cast<int>(1000.0 / std::min(fps, dest_fps));
|
||||
}
|
||||
|
||||
void cameraDeInit(void)
|
||||
{
|
||||
cap.release();
|
||||
|
||||
// 获取帧缓冲区设备信息
|
||||
struct fb_var_screeninfo vinfo;
|
||||
if (ioctl(fb, FBIOGET_VSCREENINFO, &vinfo) == -1)
|
||||
{
|
||||
std::cerr << "无法获取帧缓冲区信息" << std::endl;
|
||||
}
|
||||
else
|
||||
{
|
||||
// 计算帧缓冲区大小
|
||||
size_t fb_size = vinfo.yres_virtual * vinfo.xres_virtual * vinfo.bits_per_pixel / 8;
|
||||
|
||||
// 取消映射
|
||||
munmap(fb_buffer, fb_size);
|
||||
}
|
||||
|
||||
close(fb);
|
||||
}
|
||||
|
||||
int saved_frame_count = 0;
|
||||
bool saveCameraImage(cv::Mat frame, const std::string &directory)
|
||||
{
|
||||
if (frame.empty())
|
||||
{
|
||||
std::cerr << "Save Error: Frame is empty." << std::endl;
|
||||
return 0;
|
||||
}
|
||||
|
||||
// 构建文件名
|
||||
std::ostringstream filename;
|
||||
filename << directory << "/image_" << std::setw(5) << std::setfill('0') << saved_frame_count << ".jpg";
|
||||
|
||||
saved_frame_count++;
|
||||
// 保存图像
|
||||
return cv::imwrite(filename.str(), frame);
|
||||
}
|
||||
|
||||
std::mutex frameMutex;
|
||||
cv::Mat pubframe;
|
||||
bool streamCaptureRunning;
|
||||
void streamCapture(void)
|
||||
{
|
||||
cv::Mat frame;
|
||||
while (streamCaptureRunning)
|
||||
{
|
||||
cap.read(frame);
|
||||
frameMutex.lock();
|
||||
pubframe = frame;
|
||||
frameMutex.unlock();
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
// ===================================================
|
||||
// PIDController ServoControl(P=1.0, I=0, D=2.0, target=0, 位置式, 输出限幅=1,250,000)
|
||||
// 输出单位: 百分之一脉宽周期 (÷100 × period_ns → ns)
|
||||
// 实际等效线性增益: Kp=1.0 起主导, I=0 无积分, D=2.0 微分量抑制过冲
|
||||
// ===================================================
|
||||
double g_steer_deviation = 0; // 全局偏差, 供速度控制用
|
||||
|
||||
PIDController ServoControl(1.0, 0.0, 2.0, 0.0, POSITION, 1250000);
|
||||
int CameraHandler(void)
|
||||
{
|
||||
cv::Mat resizedFrame;
|
||||
|
||||
frameMutex.lock();
|
||||
raw_frame = pubframe;
|
||||
frameMutex.unlock();
|
||||
|
||||
if (raw_frame.empty())
|
||||
{
|
||||
printf("无法捕获图像\n");
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (readFlag(saveImg_file))
|
||||
{
|
||||
if (saveCameraImage(raw_frame, "./image"))
|
||||
{
|
||||
printf("图像%d已保存\n", saved_frame_count);
|
||||
}
|
||||
else
|
||||
{
|
||||
printf("图像保存失败\n");
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
// ── 1. 视觉巡线 ──────────────────────────────────
|
||||
// image_main() 处理 raw_frame → 80×60 图
|
||||
// 产出: left_line[60], right_line[60], mid_line[60] (EMA 滤波后)
|
||||
{ // 图像计算
|
||||
image_main();
|
||||
}
|
||||
|
||||
// ── 2. 舵机转向控制 ──────────────────────────────
|
||||
// 仅在 start 文件为 1 时执行 (readFlag(start_file))
|
||||
// 否则保持上一次的脉宽 (不做任何转向)
|
||||
if (readFlag(start_file))
|
||||
{
|
||||
// 2a. 前瞻行号换算
|
||||
// foresee 是 newWidth×newHeight (160×120) 坐标系下的行号
|
||||
// calc_scale=2, 除以 2 得到 80×60 (line_tracking) 下的行号
|
||||
int foresee = readDoubleFromFile(foresee_file);
|
||||
int check_row = foresee / calc_scale;
|
||||
|
||||
// 2b. 单行偏差计算 (80×60 坐标系 → 160×120 像素偏差)
|
||||
if (check_row >= 0 && check_row < line_tracking_height && mid_line[check_row] != 255)
|
||||
{
|
||||
double deviation = mid_line[check_row] * calc_scale - newWidth / 2;
|
||||
double bias = readDoubleFromFile(center_bias_file); // 中位偏置(像素), 正=偏右
|
||||
deviation -= bias;
|
||||
double norm = deviation / (newWidth / 2.0); // 归一化到 ±1
|
||||
g_steer_deviation = norm; // 给速度控制用
|
||||
|
||||
// 2c. 死区 (像素)
|
||||
double deadband = readDoubleFromFile(deadband_file);
|
||||
if (std::abs(deviation) < deadband)
|
||||
{
|
||||
servo.setDutyCycle(1500000); // 中位 1.50ms
|
||||
}
|
||||
else
|
||||
{
|
||||
// 2d. 比例转向: 像素偏差 → 归一化 → 舵机脉宽
|
||||
double steer_gain = readDoubleFromFile(steer_gain_file);
|
||||
double norm = deviation / (newWidth / 2.0); // 归一化到 ±1
|
||||
double offset = norm * steer_gain * 300000; // 半行程 0.30ms
|
||||
double duty_ns = 1500000.0 + offset;
|
||||
duty_ns = std::clamp(duty_ns, 1200000.0, 1800000.0);
|
||||
servo.setDutyCycle(static_cast<unsigned int>(duty_ns));
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// servo.setDutyCycle(1520000);
|
||||
}
|
||||
|
||||
// 显示图片
|
||||
if (readFlag(showImg_file))
|
||||
{
|
||||
cv::Mat fbImage(screenHeight, screenWidth, CV_8UC3, cv::Scalar(0, 0, 0));
|
||||
|
||||
// 缩放视频到新尺寸
|
||||
cv::resize(track, resizedFrame, cv::Size(newWidth, newHeight));
|
||||
// 将单通道的二值化图像转换为三通道的彩色图像
|
||||
cv::Mat coloredResizedFrame;
|
||||
cv::cvtColor(resizedFrame, coloredResizedFrame, cv::COLOR_GRAY2BGR); // 转换为彩色图像
|
||||
|
||||
// 将缩放后的图像居中放置在帧缓冲区图像中，填充黑色边框
|
||||
fbImage.setTo(cv::Scalar(0, 0, 0)); // 清空缓冲区（填充黑色）
|
||||
cv::Rect roi((screenWidth - newWidth) / 2, (screenHeight - newHeight) / 2, newWidth, newHeight);
|
||||
coloredResizedFrame.copyTo(fbImage(roi));
|
||||
|
||||
// 绘制左右边界线和中线
|
||||
int scaledLeftX, scaledRightX, scaledMidX, scaledY;
|
||||
|
||||
for (int y = 0; y < line_tracking_height; y++)
|
||||
{
|
||||
// 根据缩放比例调整X坐标
|
||||
scaledLeftX = static_cast<int>(left_line[y] * calc_scale);
|
||||
scaledRightX = static_cast<int>(right_line[y] * calc_scale);
|
||||
scaledMidX = static_cast<int>(mid_line[y] * calc_scale);
|
||||
scaledY = static_cast<int>(y * calc_scale);
|
||||
|
||||
// 绘制左边界（红）
|
||||
cv::line(fbImage(roi), cv::Point(scaledLeftX, scaledY), cv::Point(scaledLeftX, scaledY), cv::Scalar(0, 0, 255), calc_scale);
|
||||
// 绘制右边界（绿）
|
||||
cv::line(fbImage(roi), cv::Point(scaledRightX, scaledY), cv::Point(scaledRightX, scaledY), cv::Scalar(0, 255, 0), calc_scale);
|
||||
// 绘制中线 （蓝）
|
||||
cv::line(fbImage(roi), cv::Point(scaledMidX, scaledY), cv::Point(scaledMidX, scaledY), cv::Scalar(255, 0, 0), calc_scale);
|
||||
}
|
||||
|
||||
// 将帧缓冲区图像转换为RGB565格式
|
||||
convertMatToRGB565(fbImage, fb_buffer, screenWidth, screenHeight);
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
Reference in New Issue
Block a user