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

control/imu.hpp

@@ -0,0 +1,24 @@
+#pragma once
+#include "types.hpp"
+#include <cstdint>
+
+class YawTracker {
+public:
+    YawTracker();
+    bool  begin(const char* device = IMU_DEVICE, int baud = IMU_BAUDRATE);
+    void  update(float dt);
+    void  calibrate(int samples = 200);
+
+    float yaw()          const { return _yaw; }
+    float unbounded_yaw() const { return _unbounded_yaw; }
+    float gyro_z()       const { return _gyro_z; }
+    bool  ready()        const { return _ready; }
+
+private:
+    int   _fd;
+    float _yaw, _unbounded_yaw;
+    float _gyro_z;
+    float _gyro_bias;
+    bool  _ready;
+    int   _parseJY62();
+};

control/motor.cpp

@@ -0,0 +1,72 @@
+#include "motor.hpp"
+#include "hal/gpio.hpp"
+#include "hal/pwm.hpp"
+#include "types.hpp"
+#include <cstdio>
+#include <algorithm>
+
+static PWM*  pwm_left  = nullptr;
+static PWM*  pwm_right = nullptr;
+static GPIO* dir_left  = nullptr;
+static GPIO* dir_right = nullptr;
+static GPIO* left_in2  = nullptr;
+static GPIO* mortor_en = nullptr;
+static bool  initialized = false;
+
+void motor_init()
+{
+    if (initialized) return;
+
+    mortor_en = new GPIO(MOTOR_ENABLE_GPIO);
+    mortor_en->setDirection("out");
+    mortor_en->setValue(1);
+
+    left_in2 = new GPIO(MOTOR_LEFT_IN2_GPIO);
+    left_in2->setDirection("out");
+    left_in2->setValue(0);
+
+    pwm_left  = new PWM(MOTOR_PWMCHIP, MOTOR_LEFT_PWMID,  MOTOR_PWM_PERIOD_NS);
+    pwm_right = new PWM(MOTOR_PWMCHIP, MOTOR_RIGHT_PWMID, MOTOR_PWM_PERIOD_NS);
+    pwm_left->enable();
+    pwm_right->enable();
+
+    dir_left  = new GPIO(MOTOR_LEFT_DIR_GPIO);
+    dir_right = new GPIO(MOTOR_RIGHT_DIR_GPIO);
+    dir_left->setDirection("out");
+    dir_right->setDirection("out");
+
+    initialized = true;
+}
+
+void motor_set_pwm(int idx, float duty)
+{
+    if (!initialized) return;
+    duty = std::clamp(duty, -100.0f, 100.0f);
+
+    PWM*  pwm  = (idx == MOTOR_LEFT) ? pwm_left  : pwm_right;
+    GPIO* dir  = (idx == MOTOR_LEFT) ? dir_left  : dir_right;
+
+    if (duty >= 0)
+    {
+        dir->setValue(1);
+        unsigned int d_ns = static_cast<unsigned int>(MOTOR_PWM_PERIOD_NS * duty / 100.0f);
+        pwm->setDutyCycle(d_ns);
+    }
+    else
+    {
+        dir->setValue(0);
+        unsigned int d_ns = static_cast<unsigned int>(MOTOR_PWM_PERIOD_NS * (-duty) / 100.0f);
+        pwm->setDutyCycle(d_ns);
+    }
+}
+
+void motor_stop_all()
+{
+    motor_set_pwm(MOTOR_LEFT, 0);
+    motor_set_pwm(MOTOR_RIGHT, 0);
+}
+
+void motor_enable(bool on)
+{
+    if (mortor_en) mortor_en->setValue(on ? 1 : 0);
+}

control/motor.hpp

@@ -0,0 +1,6 @@
+enum MotorIndex { MOTOR_LEFT = 0, MOTOR_RIGHT = 1 };
+
+void motor_init();
+void motor_set_pwm(int idx, float duty);       // duty: -100 ~ +100
+void motor_stop_all();
+void motor_enable(bool on);

control/pid.cpp

@@ -0,0 +1,53 @@
+#include "pid.hpp"
+#include <algorithm>
+#include <cmath>
+
+PID::PID(float kp, float ki, float kd, float target,
+         PIDMode mode, float out_limit, float int_limit)
+    : _kp(kp), _ki(ki), _kd(kd), _target(target), _mode(mode)
+    , _out_limit(out_limit), _int_limit(int_limit)
+    , _output(0), _integral(0), _prev_error(0), _last_measured(0), _first(true) {}
+
+void PID::setPID(float kp, float ki, float kd)
+{
+    _kp = kp; _ki = ki; _kd = kd;
+    reset();
+}
+
+void PID::reset()
+{
+    _integral = 0;
+    _prev_error = 0;
+    _output = 0;
+    _first = true;
+}
+
+float PID::update(float measured)
+{
+    float error = _target - measured;
+
+    if (_mode == PID_POSITIONAL)
+    {
+        _integral += error;
+        _integral = std::clamp(_integral, -_int_limit, _int_limit);
+
+        float derivative = 0;
+        if (!_first) derivative = error - _prev_error;
+
+        _output = _kp * error + _ki * _integral + _kd * derivative;
+    }
+    else // PID_INCREMENTAL
+    {
+        float inc = _kp * (error - _prev_error) + _ki * error;
+        if (!_first)
+            inc += _kd * (error - 2.0f * _prev_error + (_prev_error - (_target - _last_measured)));
+        _output += inc;
+    }
+
+    _output = std::clamp(_output, -_out_limit, _out_limit);
+    _prev_error = error;
+    _last_measured = measured;
+    _first = false;
+
+    return _output;
+}

control/pid.hpp

@@ -0,0 +1,28 @@
+#pragma once
+#include "types.hpp"
+
+class PID {
+public:
+    PID(float kp = 0, float ki = 0, float kd = 0, float target = 0,
+        PIDMode mode = PID_INCREMENTAL, float out_limit = 100, float int_limit = 100);
+
+    float update(float measured);
+    void  setTarget(float t)    { _target = t; }
+    void  setPID(float kp, float ki, float kd);
+    void  setMode(PIDMode m)    { _mode = m; }
+    void  reset();
+
+    float output() const { return _output; }
+
+private:
+    float   _kp, _ki, _kd;
+    float   _target;
+    PIDMode _mode;
+    float   _out_limit, _int_limit;
+
+    float   _output;
+    float   _integral;
+    float   _prev_error;
+    float   _last_measured;
+    bool    _first;
+};

control/servo.cpp

@@ -0,0 +1,57 @@
+#include "servo.hpp"
+#include "hal/pwm.hpp"
+#include <algorithm>
+#include <cstdio>
+#include <cmath>
+
+static PWM* servo_pwm = nullptr;
+static PID* servo_pid = nullptr;
+static bool serv_initialized = false;
+
+static float g_deadband   = 0.03f;
+static float g_steer_gain = 1.5f;
+
+void servo_init()
+{
+    if (serv_initialized) return;
+
+    servo_pwm = new PWM(SERVO_PWMCHIP, SERVO_PWMID, SERVO_PERIOD_NS);
+    servo_pwm->setDutyCycle(SERVO_CENTER_NS);
+    servo_pwm->enable();
+
+    servo_pid = new PID(400000, 200000, 0, 0, PID_POSITIONAL,
+                        static_cast<float>(SERVO_MAX_DELTA_NS),
+                        static_cast<float>(SERVO_MAX_DELTA_NS));
+
+    serv_initialized = true;
+}
+
+void servo_set_deadband(float v) { g_deadband = std::max(0.0f, v); }
+void servo_set_steer_gain(float v) { g_steer_gain = std::max(0.1f, v); }
+void servo_set_pid(double kp, double ki, double kd) { if (servo_pid) servo_pid->setPID(kp, ki, kd); }
+
+void servo_set_angle(float deviation)
+{
+    if (!serv_initialized) return;
+    deviation = std::clamp(deviation, -1.0f, 1.0f);
+
+    if (std::abs(deviation) < g_deadband)
+    {
+        servo_pwm->setDutyCycle(SERVO_CENTER_NS);
+        return;
+    }
+
+    float scaled = deviation * g_steer_gain;
+    float out = servo_pid->update(scaled);
+    float duty_ns = SERVO_CENTER_NS + out;
+    duty_ns = std::clamp(duty_ns,
+                         static_cast<float>(SERVO_CENTER_NS - SERVO_MAX_DELTA_NS),
+                         static_cast<float>(SERVO_CENTER_NS + SERVO_MAX_DELTA_NS));
+    servo_pwm->setDutyCycle(static_cast<unsigned int>(duty_ns));
+}
+
+void servo_center()
+{
+    if (serv_initialized)
+        servo_pwm->setDutyCycle(SERVO_CENTER_NS);
+}

control/servo.hpp

@@ -0,0 +1,12 @@
+#pragma once
+#include "types.hpp"
+#include "control/pid.hpp"
+
+void servo_init();
+void servo_set_angle(float deviation);
+void servo_center();
+void servo_set_deadband(float v);
+void servo_set_steer_gain(float v);
+void servo_set_pid(double kp, double ki, double kd);
+
+inline float servo_get_duty_ns() { return SERVO_CENTER_NS; }