Fix vision pipeline: search-first IPM, IMU non-blocking, LCD inverse IPM, servo direction, model WIP

control/imu.cpp

@@ -3,14 +3,19 @@
 #include <unistd.h>
 #include <cmath>
 #include <cstring>
+#include <cstdio>
 #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;
+    _fd = open(device, O_RDWR | O_NOCTTY | O_NONBLOCK);
+    if (_fd < 0) {
+        printf("[IMU] 无法打开 %s, 继续无IMU运行\n", device);
+        _ready = false;
+        return false;
+    }
 
     termios opt;
     tcgetattr(_fd, &opt);
@@ -30,16 +35,18 @@ bool YawTracker::begin(const char* device, int baud)
 void YawTracker::calibrate(int samples)
 {
     float sum = 0;
-    for (int i = 0; i < samples; ++i)
+    int valid = 0, fail = 0;
+    for (int i = 0; i < samples && fail < 10; ++i)
     {
         int ret = _parseJY62();
-        if (ret > 0) sum += _gyro_z;
-        usleep(5000);
+        if (ret > 0) { sum += _gyro_z; ++valid; fail = 0; }
+        else { ++fail; }
+        usleep(2000);
     }
-    _gyro_bias = sum / samples;
-    _yaw = 0;
-    _unbounded_yaw = 0;
-    _ready = true;
+    _gyro_bias = valid > 0 ? sum / valid : 0;
+    _yaw = 0; _unbounded_yaw = 0;
+    _ready = (valid > 10);
+    if (!_ready) printf("[IMU] 校准失败 (仅 %d/%d 样本), 继续无IMU运行\n", valid, samples);
 }
 
 void YawTracker::update(float dt)
@@ -60,18 +67,20 @@ int YawTracker::_parseJY62()
 {
     uint8 buf[11];
     int total = 0;
-    while (total < 11)
+    for (int tries = 0; total < 11 && tries < 20; ++tries)
     {
         int n = read(_fd, buf + total, 11 - total);
-        if (n <= 0) return -1;
+        if (n < 0) { usleep(2000); continue; }  // EAGAIN, 等 2ms
+        if (n == 0) break;
         total += n;
     }
+    if (total < 11) return -1;
 
-    while (buf[0] != 0x55 && total > 10)
+    for (int tries = 0; buf[0] != 0x55 && total > 10 && tries < 10; ++tries)
     {
         memmove(buf, buf + 1, 10);
         int n = read(_fd, buf + 10, 1);
-        if (n <= 0) return -1;
+        if (n <= 0) { usleep(1000); continue; }
     }
     if (buf[0] != 0x55) return -1;
 

control/servo.cpp

@@ -4,13 +4,31 @@
 #include <cstdio>
 #include <cmath>
 
-static PWM* servo_pwm = nullptr;
-static PID* servo_pid = nullptr;
+// ── 全局对象 ──────────────────────────────────────────
+static PWM* servo_pwm = nullptr;      // pwmchip1/pwm0, 3ms 周期
+static PID* servo_pid = nullptr;      // 位置式 PID, 偏差→脉宽偏移量 (ns)
 static bool serv_initialized = false;
 
-static float g_deadband   = 0.03f;
-static float g_steer_gain = 1.5f;
+// ── 运行时热调参数 ──────────────────────────────────
+// 这些值会被 scheduler.cpp 每 1 秒从文件热读覆盖
+static float g_deadband   = 0.03f;    // 死区 (归一化偏差, 默认 ±0.03)
+static float g_steer_gain = 1.5f;     // 转向增益 (偏差放大系数)
 
+// ============================================================
+// servo_init — 初始化舵机 PWM + PID
+//
+// 调用时机: scheduler_init() 启动时调用一次
+//
+// 硬件: pwmchip1/pwm0
+//   周期 = 3ms (333Hz)    — 舵机标准周期
+//   中位 = 1.52ms         — 前轮摆正
+//   行程 = 1.52 ± 0.21ms  — 约 ±45° 物理摆角
+//
+// PID: 位置式
+//   Kp=400000, Ki=200000, Kd=0
+//   输出限幅 = ±SERVO_MAX_DELTA_NS (±210000ns = ±0.21ms)
+//   这组参数等效于线性增益 (因为 Ki 很大, 积分瞬间到位)
+// ============================================================
 void servo_init()
 {
     if (serv_initialized) return;
@@ -26,10 +44,33 @@ void servo_init()
     serv_initialized = true;
 }
 
-void servo_set_deadband(float v) { g_deadband = std::max(0.0f, v); }
+// ── 热调接口 ─────────────────────────────────────────
+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); }
 
+// ============================================================
+// servo_set_angle — 偏差 → 舵机脉宽 (核心控制回路)
+//
+// 调用频率: scheduler.cpp sched_control() 每 5ms 一次
+//
+// 输入: deviation  — 归一化中线偏差 (-1=偏左极限, 0=正中, +1=偏右极限)
+//        由 strategy/deviation.cpp 的 calc_deviation() 计算
+//
+// 处理流程:
+//   1. 钳位偏差至 [-1, +1]
+//   2. 死区: |deviation| < g_deadband → 直接回中, 跳过 PID
+//      典型值 g_deadband=0.03, 即偏差 < 3% 时不转向
+//   3. 增益缩放: deviation *= g_steer_gain
+//      典型值 g_steer_gain=1.5, 把微小偏差放大供 PID 处理
+//   4. PID 计算: out = PID(deviation)
+//      位置式 PID, 输出为脉宽偏移量 (ns)
+//   5. 叠加中位: duty_ns = 1520000 + out
+//      正偏差(偏右) → out>0 → 脉宽>中位 → 舵机右转
+//      负偏差(偏左) → out<0 → 脉宽<中位 → 舵机左转
+//   6. 限幅: 1.52 ± 0.21ms (1310000ns ~ 1730000ns)
+//   7. 写入 PWM sysfs
+// ============================================================
 void servo_set_angle(float deviation)
 {
     if (!serv_initialized) return;
@@ -42,7 +83,7 @@ void servo_set_angle(float deviation)
     }
 
     float scaled = deviation * g_steer_gain;
-    float out = servo_pid->update(scaled);
+    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),
@@ -50,6 +91,7 @@ void servo_set_angle(float deviation)
     servo_pwm->setDutyCycle(static_cast<unsigned int>(duty_ns));
 }
 
+// servo_center — 紧急回中 (停止时调用)
 void servo_center()
 {
     if (serv_initialized)