345 lines
13 KiB
C++
345 lines
13 KiB
C++
// vision-anti-pinch - V4L2 MMAP 取图 + USBCANFD CAN 收发 (OpenCV 版本)
|
|
//
|
|
// 使用 OpenCV 进行 UYVY/YUYV -> BGR 转换，避免手动转换公式问题
|
|
// CAN 收发默认开启, --no-can 关闭
|
|
//
|
|
// 用法:
|
|
// ./vision-anti-pinch -d /dev/video5 -n 10 -s 2 -t jpg -o ./out
|
|
|
|
#include "v4l2_capture.h"
|
|
#include "can_comm.h"
|
|
#include "send.hpp"
|
|
|
|
#include <opencv2/core.hpp>
|
|
#include <opencv2/imgproc.hpp>
|
|
#include <opencv2/imgcodecs.hpp>
|
|
|
|
#include <cstdio>
|
|
#include <cstdlib>
|
|
#include <cstring>
|
|
#include <string>
|
|
#include <vector>
|
|
#include <bitset>
|
|
#include <atomic>
|
|
#include <chrono>
|
|
#include <csignal>
|
|
#include <linux/videodev2.h>
|
|
|
|
std::atomic<uint64_t> g_Feedback_Of_The_Actual_Position_Of_The_Left_Front_Door_391_S;
|
|
std::atomic<uint64_t> g_Feedback_Of_The_Actual_Position_Of_The_Right_Front_Door_391_S;
|
|
std::atomic<uint64_t> g_Feedback_Of_The_Actual_Position_Of_The_Left_Rear_Door_391_S;
|
|
std::atomic<uint64_t> g_Feedback_Of_The_Actual_Position_Of_The_Right_Rear_Door_391_S;
|
|
|
|
// Sensing GMSL2 驱动自定义控制 ID
|
|
#define CID_SENSOR_MODE 0x009a2008
|
|
#define CID_TRIG_PIN 0x009a2084
|
|
#define CID_TRIG_MODE 0x009a2085
|
|
#define CID_GAIN 0x009a2009
|
|
#define CID_EXPOSURE 0x009a200a
|
|
|
|
static volatile sig_atomic_t g_stop = 0;
|
|
static void onSignal(int) { g_stop = 1; }
|
|
|
|
static void printUsage(const char *prog) {
|
|
printf(
|
|
"Usage: %s [options]\n"
|
|
"V4L2 MMAP capture demo for Sensing GMSL2 camera on Jetson Orin.\n"
|
|
"\n"
|
|
"Capture options:\n"
|
|
" -d <device> V4L2 device node (default: /dev/video5)\n"
|
|
" -W <width> frame width (default: 1920)\n"
|
|
" -H <height> frame height (default: 1536)\n"
|
|
" -f <fps> target frame rate (default: 30)\n"
|
|
" -F <format> pixel format: UYVY | YUYV (default: UYVY)\n"
|
|
" -m <mode> sensor_mode control value\n"
|
|
" -p <trig_pin> trig_pin control value (hex/dec)\n"
|
|
" --gain <val> sensor gain\n"
|
|
" --exposure <val> sensor exposure\n"
|
|
"\n"
|
|
"Output options:\n"
|
|
" -n <count> frames to capture, 0 = run forever\n"
|
|
" -s <count> frames to save\n"
|
|
" -o <dir> output directory (default: ./frames)\n"
|
|
" -t <type> file type: jpg | png | bmp | raw (default: jpg)\n"
|
|
" --quality <1..100> JPEG quality (default: 95)\n"
|
|
" -v verbose output\n"
|
|
" -h print this help\n"
|
|
"\n"
|
|
"CAN options:\n"
|
|
" --no-can disable USBCANFD CAN/CANFD receive (enabled by default)\n"
|
|
"\n"
|
|
"Example:\n"
|
|
" %s -d /dev/video5 -n 100 -s 5 -t jpg -o ./out\n"
|
|
" %s -d /dev/video5 -n 0 -v\n",
|
|
prog, prog, prog);
|
|
}
|
|
|
|
struct Args {
|
|
std::string device = "/dev/video5";
|
|
uint32_t width = 1920;
|
|
uint32_t height = 1536; // SG3S-ISX031C-GMSL2F 默认分辨率
|
|
uint32_t fps = 30;
|
|
std::string format = "UYVY";
|
|
long sensor_mode = -1;
|
|
long trig_pin = -1;
|
|
long trig_mode = -1;
|
|
long gain = -1;
|
|
long exposure = -1;
|
|
long count = 0;
|
|
long save = 0;
|
|
std::string out_dir = "./frames";
|
|
std::string save_type = "jpg";
|
|
int quality = 95;
|
|
bool verbose = false;
|
|
bool can_enabled = true; // 默认启用 CAN 收发, --no-can 关闭
|
|
};
|
|
|
|
// 手动解析参数
|
|
static bool parseArgs(int argc, char **argv, Args &a) {
|
|
auto need_val = [&](int &i) -> std::string {
|
|
if (i + 1 >= argc) {
|
|
printf("missing value for %s\n", argv[i]);
|
|
std::exit(1);
|
|
}
|
|
return argv[++i];
|
|
};
|
|
|
|
for (int i = 1; i < argc; ++i) {
|
|
std::string arg = argv[i];
|
|
if (arg == "-d") a.device = need_val(i);
|
|
else if (arg == "-W") a.width = (uint32_t)std::atoi(need_val(i).c_str());
|
|
else if (arg == "-H") a.height = (uint32_t)std::atoi(need_val(i).c_str());
|
|
else if (arg == "-f") a.fps = (uint32_t)std::atoi(need_val(i).c_str());
|
|
else if (arg == "-F") a.format = need_val(i);
|
|
else if (arg == "-m") a.sensor_mode = std::strtol(need_val(i).c_str(), nullptr, 0);
|
|
else if (arg == "-p") a.trig_pin = std::strtol(need_val(i).c_str(), nullptr, 0);
|
|
else if (arg == "-M") a.trig_mode = std::strtol(need_val(i).c_str(), nullptr, 0);
|
|
else if (arg == "-n") a.count = std::strtol(need_val(i).c_str(), nullptr, 0);
|
|
else if (arg == "-s") a.save = std::strtol(need_val(i).c_str(), nullptr, 0);
|
|
else if (arg == "-o") a.out_dir = need_val(i);
|
|
else if (arg == "-t") a.save_type = need_val(i);
|
|
else if (arg == "-v") a.verbose = true;
|
|
else if (arg == "-h") { printUsage(argv[0]); std::exit(0); }
|
|
else if (arg == "--gain") a.gain = std::strtol(need_val(i).c_str(), nullptr, 0);
|
|
else if (arg == "--exposure") a.exposure = std::strtol(need_val(i).c_str(), nullptr, 0);
|
|
else if (arg == "--quality") a.quality = std::atoi(need_val(i).c_str());
|
|
else if (arg == "--no-can") a.can_enabled = false;
|
|
else {
|
|
printf("unknown option: %s\n", arg.c_str());
|
|
printUsage(argv[0]);
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
// 使用 OpenCV 将 V4L2 帧 (UYVY/YUYV) 转为 BGR Mat
|
|
static cv::Mat frameToBgr(const V4l2Capture::Frame &frame) {
|
|
// OpenCV 原生支持 UYVY/YUYV -> BGR 转换
|
|
// UYVY = COLOR_YUV2BGR_UYVY, YUYV = COLOR_YUV2BGR_YUYV
|
|
int code = (frame.pixelformat == V4L2_PIX_FMT_UYVY)
|
|
? cv::COLOR_YUV2BGR_UYVY
|
|
: cv::COLOR_YUV2BGR_YUYV;
|
|
cv::Mat yuv((int)frame.height, (int)frame.width, CV_8UC2, (void *)frame.data);
|
|
cv::Mat bgr;
|
|
cv::cvtColor(yuv, bgr, code);
|
|
return bgr;
|
|
}
|
|
|
|
/* =====================================================================
|
|
* CAN 报文接收解析 (由 can_comm 接收线程回调)
|
|
* ===================================================================== */
|
|
|
|
// 512 位报文位域, deal_msg 中按 msg_id 拷贝后可按位解析信号
|
|
static std::bitset<512> g_bitMsg;
|
|
|
|
static uint64_t get_bit_value(int star_bit, int bit_len)
|
|
{
|
|
uint64_t signal_value = 0;
|
|
for (int i = 0; i < bit_len; ++i)
|
|
{
|
|
signal_value |= g_bitMsg[star_bit + i] << i;
|
|
}
|
|
return signal_value;
|
|
}
|
|
|
|
// 收到报文的回调点: 目前仅解析 0x391, 后续信号解析在此扩展
|
|
static void deal_msg(int64_t time, uint32_t msg_id, const uint8_t *data, uint32_t len)
|
|
{
|
|
if (msg_id == 0x391 && len >= 4 && len <= 64)
|
|
{
|
|
memcpy(&g_bitMsg, data, len);
|
|
g_Feedback_Of_The_Actual_Position_Of_The_Left_Front_Door_391_S = data[0];
|
|
g_Feedback_Of_The_Actual_Position_Of_The_Right_Front_Door_391_S = data[1];
|
|
g_Feedback_Of_The_Actual_Position_Of_The_Left_Rear_Door_391_S = data[2];
|
|
g_Feedback_Of_The_Actual_Position_Of_The_Right_Rear_Door_391_S = data[3];
|
|
// Rear_BCM_0x391,0x391,64,1.0-1.7,8,Feedback_Of_The_Actual_Position_Of_The_Left_Front_Door_391_S
|
|
// Rear_BCM_0x391,0x391,64,2.0-2.7,8,Feedback_Of_The_Actual_Position_Of_The_Right_Front_Door_391_S
|
|
// Rear_BCM_0x391,0x391,64,3.0-3.7,8,Feedback_Of_The_Actual_Position_Of_The_Left_Rear_Door_391_S
|
|
// Rear_BCM_0x391,0x391,64,4.0-4.7,8,Feedback_Of_The_Actual_Position_Of_The_Right_Rear_Door_391_S
|
|
}
|
|
}
|
|
|
|
int main(int argc, char **argv) {
|
|
Args args;
|
|
if (!parseArgs(argc, argv, args)) {
|
|
return 1;
|
|
}
|
|
|
|
std::signal(SIGINT, onSignal);
|
|
std::signal(SIGTERM, onSignal);
|
|
|
|
uint32_t pixfmt = (args.format == "UYVY") ? V4L2_PIX_FMT_UYVY : V4L2_PIX_FMT_YUYV;
|
|
|
|
// 启动 CAN 收发(失败不致命, 可继续仅取图)
|
|
if (args.can_enabled) {
|
|
CanComm::set_msg_callback(deal_msg); // 注册报文解析回调
|
|
if (!CanComm::can_start()) {
|
|
printf("CAN start failed, continue with camera only.\n");
|
|
args.can_enabled = false;
|
|
}
|
|
}
|
|
|
|
V4l2Capture cap;
|
|
|
|
if (!cap.open(args.device)) {
|
|
return 1;
|
|
}
|
|
|
|
// 设置相机控制
|
|
if (args.sensor_mode >= 0) cap.setControl(CID_SENSOR_MODE, (int32_t)args.sensor_mode);
|
|
if (args.trig_pin >= 0) cap.setControl(CID_TRIG_PIN, (int32_t)args.trig_pin);
|
|
if (args.trig_mode >= 0) cap.setControl(CID_TRIG_MODE, (int32_t)args.trig_mode);
|
|
if (args.gain >= 0) cap.setControl(CID_GAIN, (int32_t)args.gain);
|
|
if (args.exposure >= 0) cap.setControl(CID_EXPOSURE, (int32_t)args.exposure);
|
|
|
|
if (!cap.setFormat(args.width, args.height, pixfmt)) {
|
|
return 1;
|
|
}
|
|
cap.setFps(args.fps);
|
|
|
|
if (!cap.initMmap(4)) {
|
|
return 1;
|
|
}
|
|
if (!cap.startStreaming()) {
|
|
return 1;
|
|
}
|
|
|
|
printf("=========================================================\n");
|
|
printf("Capturing %s %ux%u @ %u fps from %s\n",
|
|
args.format.c_str(), cap.width(), cap.height(), args.fps, args.device.c_str());
|
|
printf("Press Ctrl+C to stop.\n");
|
|
printf("=========================================================\n");
|
|
|
|
// 创建输出目录
|
|
if (args.save > 0) {
|
|
std::string mkcmd = "mkdir -p " + args.out_dir;
|
|
std::system(mkcmd.c_str());
|
|
}
|
|
|
|
using clock = std::chrono::steady_clock;
|
|
auto t_start = clock::now();
|
|
auto t_last_report = t_start;
|
|
uint64_t frame_count = 0;
|
|
uint64_t save_count = 0;
|
|
uint32_t timeout_consecutive = 0;
|
|
|
|
while (!g_stop) {
|
|
if (args.count > 0 && frame_count >= (uint64_t)args.count) {
|
|
break;
|
|
}
|
|
|
|
V4l2Capture::Frame frame;
|
|
if (!cap.getFrame(frame, 2000)) {
|
|
timeout_consecutive++;
|
|
if (timeout_consecutive > 5) {
|
|
printf("getFrame: timeout/empty for %u consecutive tries, aborting\n",
|
|
timeout_consecutive);
|
|
break;
|
|
}
|
|
continue;
|
|
}
|
|
timeout_consecutive = 0;
|
|
frame_count++;
|
|
|
|
// 保存帧
|
|
if (save_count < (uint64_t)args.save) {
|
|
bool ok = false;
|
|
char fname[512];
|
|
|
|
// 使用 OpenCV 转换并保存
|
|
cv::Mat bgr = frameToBgr(frame);
|
|
if (!bgr.empty())
|
|
{
|
|
std::snprintf(fname, sizeof(fname), "%s/frame_%06lu.%s",
|
|
args.out_dir.c_str(), save_count, args.save_type.c_str());
|
|
|
|
std::vector<int> params;
|
|
if (args.save_type == "jpg" || args.save_type == "jpeg")
|
|
{
|
|
params = {cv::IMWRITE_JPEG_QUALITY, args.quality};
|
|
}
|
|
else if (args.save_type == "png")
|
|
{
|
|
params = {cv::IMWRITE_PNG_COMPRESSION, 3};
|
|
}
|
|
ok = cv::imwrite(fname, bgr, params);
|
|
}
|
|
|
|
if (ok) {
|
|
printf("[saved] %s\n", fname);
|
|
save_count++;
|
|
} else {
|
|
printf("[save failed] frame %lu\n", save_count);
|
|
}
|
|
}
|
|
|
|
DebugInfo *pData = new DebugInfo();
|
|
// pData->CameraId = (ctx->cam_no == 1 ? 0 : 1);
|
|
// pData->TimeStampMs = epochTimeStamp_ms;
|
|
memcpy(pData->Img, frame.data, frame.size);
|
|
bool res = send_debuginfo(pData);
|
|
//printf("send %ld %d %d\n", pData->TimeStampMs, pData->CameraId, res);
|
|
delete pData;
|
|
pData = nullptr;
|
|
|
|
cap.releaseFrame(frame);
|
|
|
|
// if (args.verbose) {
|
|
// printf("frame #%lu seq=%u size=%zu\n",
|
|
// frame_count, frame.sequence, frame.size);
|
|
// }
|
|
|
|
auto now = clock::now();
|
|
auto since_report = std::chrono::duration_cast<std::chrono::milliseconds>(now - t_last_report);
|
|
if (since_report.count() >= 1000) {
|
|
double total_sec = std::chrono::duration_cast<std::chrono::milliseconds>(now - t_start).count() / 1000.0;
|
|
double avg_fps = total_sec > 0 ? frame_count / total_sec : 0;
|
|
printf("[stats] frames=%lu avg_fps=%.2f door_position[%ld %ld %ld %ld]\n", frame_count, avg_fps, g_Feedback_Of_The_Actual_Position_Of_The_Left_Front_Door_391_S.load(), g_Feedback_Of_The_Actual_Position_Of_The_Right_Front_Door_391_S.load(), g_Feedback_Of_The_Actual_Position_Of_The_Left_Rear_Door_391_S.load(), g_Feedback_Of_The_Actual_Position_Of_The_Right_Rear_Door_391_S.load());
|
|
t_last_report = now;
|
|
}
|
|
}
|
|
|
|
auto t_end = clock::now();
|
|
double total_sec = std::chrono::duration_cast<std::chrono::milliseconds>(t_end - t_start).count() / 1000.0;
|
|
|
|
printf("=========================================================\n");
|
|
printf("Capture finished. frames=%lu duration=%.2fs avg_fps=%.2f\n",
|
|
frame_count, total_sec, total_sec > 0 ? frame_count / total_sec : 0);
|
|
if (args.save > 0) {
|
|
printf("Saved %lu frames to %s/\n", save_count, args.out_dir.c_str());
|
|
}
|
|
if (args.can_enabled && CanComm::is_open()) {
|
|
printf("CAN received: %d frames\n", CanComm::receive_count());
|
|
}
|
|
printf("=========================================================\n");
|
|
|
|
cap.stopStreaming();
|
|
cap.close();
|
|
|
|
if (args.can_enabled) {
|
|
CanComm::can_stop();
|
|
}
|
|
return 0;
|
|
}
|