陀螺识别方式改变,参数更新。

others/include/additions/additions.h

@@ -8,8 +8,9 @@
 #include <stdint.h>
 #include <sys/time.h>
 #include <serial/serial.h>
+#include <opencv2/core.hpp>
 
-struct mcu_data{
+struct mcu_data {
     float curr_yaw;
     float curr_pitch;
     uint8_t state;
@@ -23,14 +24,61 @@ struct mcu_data{
 extern mcu_data mcuData;
 
 void uartReceive(Serial *pSerial);
+
 bool checkReconnect(bool is_camera_0_connect, bool is_camera_1_connect);
+
 bool checkReconnect(bool is_camera_connect);
+
 void saveVideos(const cv::Mat &gimbal_src, const cv::Mat &chassis_src);
+
 void saveVideos(const cv::Mat &gimbal_src);
+
 void showOrigin(const cv::Mat &gimbal_src, const cv::Mat &chassis_src);
+
 void showOrigin(const cv::Mat &gimbal_src);
+
 void extract(cv::Mat &gimbal_src, cv::Mat &chassis_src);
+
 void extract(cv::Mat &gimbal_src);
-double getTimeIntervalms(const timeval& now, const timeval &last);
+
+double getTimeIntervalms(const timeval &now, const timeval &last);
+
+template<class type, int length>
+class RoundQueue {
+private:
+    type data[length];
+    int head;
+    int tail;
+public:
+    RoundQueue<type, length>() : head(0), tail(0) {};
+
+    constexpr int size() const {
+        return length;
+    };
+
+    bool empty() const {
+        return head == tail;
+    };
+
+    void push(const type &obj) {
+        data[head] = obj;
+        head = (head + 1) % length;
+        if (head == tail) {
+            tail = (tail + 1) % length;
+        }
+    };
+
+    bool pop(type &obj) {
+        if (empty()) return false;
+        obj = data[tail];
+        tail = (tail + 1) % length;
+        return true;
+    };
+
+    type &operator[](int idx) {
+        while (tail + idx < 0) idx += length;
+        return data[(tail + idx) % length];
+    };
+};
 
 #endif /* _ADDITIONS_H_ */

others/include/camera/camera_wrapper.h

@@ -7,18 +7,14 @@
 #ifndef VIDEO_TEST1_CAMERA_WRAPPER_H
 #define VIDEO_TEST1_CAMERA_WRAPPER_H
 
-#include <stdio.h>
-#include <iostream>
-#include <thread>
-#include "opencv2/core/core.hpp"
-#include "opencv2/highgui/highgui.hpp"
-#include <opencv2/imgproc/imgproc.hpp>
-#include "camera/wrapper_head.h"
+#include <additions/additions.h>
+#include <opencv2/core/core.hpp>
+#include <camera/wrapper_head.h>
 
 #ifdef Windows
     #include "camera/CameraApi.h"
 #elif defined(Linux) || defined(Darwin)
-    #include "camera/camera_api.h"
+    #include <camera/camera_api.h>
 #endif
 
 class CameraWrapper: public WrapperHead {
@@ -42,11 +38,7 @@ private:
     IplImage* iplImage;
     int channel;
 
-    cv::Mat src_queue[2];
-    volatile int qhead;
-    volatile int qtail;
-
-    std::thread *readThread;
+    RoundQueue<cv::Mat, 2> src_queue;
 public:
     int gain;
 
@@ -58,9 +50,6 @@ public:
     bool readRaw(cv::Mat& src);
     bool readProcessed(cv::Mat& src);
     bool readCallback(cv::Mat& src);
-    bool readWithThread(cv::Mat &src);
-    bool changeBrightness(int brightness);
-//    bool once
 };
 
 

others/src/camera/camera_wrapper.cpp

@@ -1,7 +1,7 @@
 //
 // Created by zhikun on 18-11-7.
 //
-
+#include <iostream>
 #include <camera/camera_wrapper.h>
 #include <log.h>
 #include <additions/additions.h>
@@ -9,9 +9,6 @@
 #include <config/setconfig.h>
 
 using namespace std;
-
-using std::cout;
-using std::endl;
 using namespace cv;
 
 CameraWrapper::CameraWrapper(int gain, int camera_mode, const std::string &n) :
@@ -23,10 +20,7 @@ CameraWrapper::CameraWrapper(int gain, int camera_mode, const std::string &n) :
         iplImage(nullptr),
         rgb_buffer(nullptr),
         channel(3),
-        gain(gain),
-        qhead(0),
-        qtail(0),
-        readThread(nullptr){
+        gain(gain){
 }
 
 void cameraCallback(CameraHandle hCamera, BYTE *pFrameBuffer, tSdkFrameHead* pFrameHead,PVOID pContext){
@@ -34,14 +28,7 @@ void cameraCallback(CameraHandle hCamera, BYTE *pFrameBuffer, tSdkFrameHead* pFr
     CameraImageProcess(hCamera, pFrameBuffer, c->rgb_buffer, pFrameHead);
     auto iplImage = cvCreateImageHeader(cvSize(pFrameHead->iWidth, pFrameHead->iHeight), IPL_DEPTH_8U, c->channel);
     cvSetData(iplImage, c->rgb_buffer, pFrameHead->iWidth * c->channel);  //此处只是设置指针，无图像块数据拷贝，不需担心转换效率
-    c->src_queue[c->qhead] = cv::cvarrToMat(iplImage).clone();
-    if((c->qhead+1)%2 == c->qtail){
-        c->qhead = (c->qhead+1)%2;
-        c->qtail = (c->qtail+1)%2;
-    } else {
-        c->qhead = (c->qhead+1)%2;
-    }
-//    LOGM("Get image, [%d %d]", c->qhead, c->qtail);
+    c->src_queue.push(cv::cvarrToMat(iplImage).clone());
 }
 
 bool CameraWrapper::init() {
@@ -162,17 +149,11 @@ bool CameraWrapper::init() {
     return true;
 }
 
-bool CameraWrapper::changeBrightness(int brightness) {
-    CameraUnInit(h_camera);
-    CameraSetAnalogGain(h_camera, brightness);
-}
-
 bool CameraWrapper::read(cv::Mat &src) {
     if(init_done) {
         if (mode == 0)return readProcessed(src);
         if (mode == 1)return readRaw(src);
         if (mode == 2)return readCallback(src);
-        if (mode == 3)return readWithThread(src);
     } else {
         return false;
     }
@@ -225,44 +206,13 @@ bool CameraWrapper::readProcessed(cv::Mat &src) {
 bool CameraWrapper::readCallback(cv::Mat &src) {
     timeval ts, te;
     gettimeofday(&ts, NULL);
-    while(qtail==qhead){
+    while(src_queue.empty()){
         gettimeofday(&te, NULL);
         if(getTimeIntervalms(te, ts) > 500){
             return false;
         }
     }
-
-    src = src_queue[qtail];
-//    cout << src.size << endl;
-    qtail = (qtail+1)%2;
-    return true;
-}
-
-bool CameraWrapper::readWithThread(cv::Mat &src){
-    if(readThread != nullptr){
-        readThread->join();
-        src = src_queue[qtail];
-        qtail = (qtail+1)%2;
-        delete readThread;
-        readThread = new std::thread([&](){
-            readProcessed(src_queue[qhead]);
-            qhead = (qhead+1)%2;
-        });
-    }else{
-        readThread = new std::thread([&](){
-            readProcessed(src_queue[qhead]);
-            qhead = (qhead+1)%2;
-        });
-        readThread->join();
-        src = src_queue[qtail];
-        qtail = (qtail+1)%2;
-        delete readThread;
-        readThread = new std::thread([&](){
-            readProcessed(src_queue[qhead]);
-            qhead = (qhead+1)%2;
-        });
-    }
-    return true;
+    return src_queue.pop(src);
 }
 
 CameraWrapper::~CameraWrapper() {
@@ -270,8 +220,4 @@ CameraWrapper::~CameraWrapper() {
     //注意，先反初始化后再free
     if (rgb_buffer != nullptr)
         free(rgb_buffer);
-    if(readThread != nullptr){
-        readThread->detach();
-        delete readThread;
-    }
 }