Initial commit: SmartCar Framework v0.1 — 龙芯2K0300智能车开发框架\n\n- HAL: GPIO/PWM/Encoder/Framebuffer 驱动\n- Control: PID/IMU/Motor/Servo 控制\n- Vision: HSV双Otsu→4点标定IPM→洪泛填充→逐行搜线\n- Strategy: 三区前瞻偏差+速度策略\n- Debug: 文件热调参+LCD预览+cv截帧\n- Scheduler: 5ms timerfd+epoll 中央调度器

control/imu.cpp

@@ -0,0 +1,81 @@
+#include "imu.hpp"
+#include <fcntl.h>
+#include <unistd.h>
+#include <cmath>
+#include <cstring>
+#include <termios.h>
+
+YawTracker::YawTracker() : _fd(-1), _yaw(0), _unbounded_yaw(0), _gyro_z(0), _gyro_bias(0), _ready(false) {}
+
+bool YawTracker::begin(const char* device, int baud)
+{
+    _fd = open(device, O_RDWR | O_NOCTTY);
+    if (_fd < 0) return false;
+
+    termios opt;
+    tcgetattr(_fd, &opt);
+    cfsetispeed(&opt, B115200);
+    cfsetospeed(&opt, B115200);
+    opt.c_cflag |= (CLOCAL | CREAD);
+    opt.c_cflag &= ~PARENB;
+    opt.c_cflag &= ~CSTOPB;
+    opt.c_cflag &= ~CSIZE;
+    opt.c_cflag |= CS8;
+    tcsetattr(_fd, TCSANOW, &opt);
+
+    calibrate();
+    return true;
+}
+
+void YawTracker::calibrate(int samples)
+{
+    float sum = 0;
+    for (int i = 0; i < samples; ++i)
+    {
+        int ret = _parseJY62();
+        if (ret > 0) sum += _gyro_z;
+        usleep(5000);
+    }
+    _gyro_bias = sum / samples;
+    _yaw = 0;
+    _unbounded_yaw = 0;
+    _ready = true;
+}
+
+void YawTracker::update(float dt)
+{
+    if (_parseJY62() <= 0) return;
+
+    float gyro_filt = _gyro_z - _gyro_bias;
+    if (std::fabs(gyro_filt) < 0.03f) gyro_filt = 0;  // 死区
+
+    _unbounded_yaw += gyro_filt * dt;
+    _yaw = _unbounded_yaw;
+
+    while (_yaw >  180.0f) _yaw -= 360.0f;
+    while (_yaw < -180.0f) _yaw += 360.0f;
+}
+
+int YawTracker::_parseJY62()
+{
+    uint8 buf[11];
+    int total = 0;
+    while (total < 11)
+    {
+        int n = read(_fd, buf + total, 11 - total);
+        if (n <= 0) return -1;
+        total += n;
+    }
+
+    while (buf[0] != 0x55 && total > 10)
+    {
+        memmove(buf, buf + 1, 10);
+        int n = read(_fd, buf + 10, 1);
+        if (n <= 0) return -1;
+    }
+    if (buf[0] != 0x55) return -1;
+
+    int16 gz  = (buf[5] << 8) | buf[4];
+    _gyro_z = gz * (2000.0f / 32768.0f);  // ±2000°/s
+    return 1;
+}