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

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
 };