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

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_ */