spdis
149 lines
4.7 KiB
C++
149 lines
4.7 KiB
C++
#include <iostream>
|
|
#include <fstream>
|
|
#include <opencv2/opencv.hpp>
|
|
#include <string>
|
|
#include <csignal>
|
|
#include <atomic>
|
|
|
|
#include "global.h"
|
|
#include "camera.h"
|
|
#include "control.h"
|
|
#include "Timer.h"
|
|
#include "serial.h"
|
|
#include "encoder.h"
|
|
#include "video.h"
|
|
|
|
std::atomic<bool> running(true);
|
|
void signalHandler(int signal)
|
|
{
|
|
running.store(false);
|
|
}
|
|
|
|
int main(void)
|
|
{
|
|
// Video video("test.mp4", 30);
|
|
|
|
// // 获取帧缓冲区设备信息
|
|
// int fb = open("/dev/fb0", O_RDWR);
|
|
// if (fb == -1)
|
|
// {
|
|
// std::cerr << "无法打开帧缓冲区设备" << std::endl;
|
|
// return -1;
|
|
// }
|
|
// struct fb_var_screeninfo vinfo;
|
|
// ioctl(fb, FBIOGET_VSCREENINFO, &vinfo);
|
|
|
|
// // 设置屏幕参数
|
|
// int screenWidth = 160;
|
|
// int screenHeight = 128;
|
|
|
|
// int newWidth, newHeight;
|
|
|
|
// // 创建帧缓冲区
|
|
// uint16_t *fb_buffer = new uint16_t[screenWidth * screenHeight];
|
|
|
|
// int frameCount = 0;
|
|
|
|
// cv::Mat frame, resizedFrame, fbImage(screenHeight, screenWidth, CV_8UC3, cv::Scalar(0, 0, 0)); // 帧缓冲区图像
|
|
|
|
// int frame_count = 0;
|
|
|
|
// video.timer.start();
|
|
|
|
// while(1) {
|
|
// video.frameMutex.lock();
|
|
// if(newWidth == 0)
|
|
// {
|
|
// // 保持视频长宽比
|
|
// int videoWidth = video.frame.cols;
|
|
// int videoHeight = video.frame.rows;
|
|
// double aspectRatio = static_cast<double>(videoWidth) / videoHeight;
|
|
// // 根据屏幕大小计算缩放后的宽度和高度
|
|
// if (screenWidth / static_cast<double>(screenHeight) > aspectRatio)
|
|
// {
|
|
// // 屏幕更宽，以高度为基准
|
|
// newHeight = screenHeight;
|
|
// newWidth = static_cast<int>(newHeight * aspectRatio);
|
|
// }
|
|
// else
|
|
// {
|
|
// // 屏幕更高，以宽度为基准
|
|
// newWidth = screenWidth;
|
|
// newHeight = static_cast<int>(newWidth / aspectRatio);
|
|
// }
|
|
// }
|
|
|
|
// // 缩放视频到新尺寸
|
|
// cv::resize(video.frame, resizedFrame, cv::Size(newWidth, newHeight));
|
|
// video.frameMutex.unlock();
|
|
|
|
// // 将缩放后的图像居中放置在帧缓冲区图像中，填充黑色边框
|
|
// fbImage.setTo(cv::Scalar(0, 0, 0)); // 清空缓冲区（填充黑色）
|
|
// cv::Rect roi((screenWidth - newWidth) / 2, (screenHeight - newHeight) / 2, newWidth, newHeight);
|
|
// resizedFrame.copyTo(fbImage(roi));
|
|
|
|
// // 将帧缓冲区图像转换为RGB565格式
|
|
// convertMatToRGB565(fbImage, fb_buffer, screenWidth, screenHeight);
|
|
|
|
// // 写入帧缓冲区
|
|
// lseek(fb, 0, SEEK_SET);
|
|
// write(fb, fb_buffer, screenWidth * screenHeight * 2);
|
|
// std::this_thread::sleep_for(std::chrono::milliseconds(30));
|
|
// }
|
|
std::signal(SIGINT, signalHandler);
|
|
|
|
double dest_fps = readDoubleFromFile(destfps_file);
|
|
int dest_frame_duration = CameraInit(0, dest_fps, 320, 240);
|
|
printf("%d\n", dest_frame_duration);
|
|
if (dest_frame_duration != -1)
|
|
{
|
|
streamCaptureRunning = true;
|
|
std::thread camworker = std::thread(streamCapture);
|
|
std::cout << "Stream Capture Service started!\n";
|
|
ControlInit();
|
|
std::cout << "Control Initialized!\n";
|
|
|
|
Timer CameraTimer(dest_frame_duration, std::bind(CameraHandler));
|
|
Timer MortorTimer(8, std::bind(ControlMain));
|
|
CameraTimer.start();
|
|
std::cout << "Camera Service started!\n";
|
|
MortorTimer.start();
|
|
std::cout << "Control Service started!\n";
|
|
|
|
// 主循环，直到用户输入 Ctrl+C
|
|
while (running.load())
|
|
{
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(500));
|
|
target_speed = readDoubleFromFile(speed_file);
|
|
|
|
mortor_kp = readDoubleFromFile(mortor_kp_file);
|
|
mortor_ki = readDoubleFromFile(mortor_ki_file);
|
|
mortor_kd = readDoubleFromFile(mortor_kd_file);
|
|
|
|
kp = readDoubleFromFile(kp_file);
|
|
ki = readDoubleFromFile(ki_file);
|
|
kd = readDoubleFromFile(kd_file);
|
|
// vofa_image(1, 160*120, 160, 120, Format_Grayscale8, (char*)IMG);
|
|
}
|
|
std::cout << "Stopping!\n";
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(500));
|
|
|
|
CameraTimer.stop();
|
|
std::cout << "Camera Timer stopped!\n";
|
|
MortorTimer.stop();
|
|
std::cout << "Control Timer stopped!\n";
|
|
|
|
ControlExit();
|
|
std::cout << "Control Service stopped!\n";
|
|
|
|
streamCaptureRunning = false;
|
|
if (camworker.joinable())
|
|
{
|
|
camworker.join();
|
|
}
|
|
|
|
cameraDeInit();
|
|
std::cout << "Camera Service stopped!\n";
|
|
}
|
|
return 0;
|
|
} |