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 中央调度器

2026-05-25 10:31:55 +08:00
commit 28d9c6da58
52 changed files with 2599 additions and 0 deletions
--- a/strategy/deviation.cpp
+++ b/strategy/deviation.cpp
@@ -0,0 +1,72 @@
+#include "deviation.hpp"
+#include <algorithm>
+#include <cmath>
+#include <cstring>
+
+void fit_midline(EdgeLines& lines)
+{
+    static EdgeLines prev;
+    static bool has_prev = false;
+
+    if (!has_prev)
+    {
+        std::memcpy(&prev, &lines, sizeof(EdgeLines));
+        has_prev = true;
+        return;
+    }
+
+    for (int y = 0; y < IPM_ROW_COUNT; ++y)
+    {
+        if (lines.left[y] == 0 || lines.right[y] == 0) continue;
+
+        if (std::abs(static_cast<int>(lines.mid[y]) - static_cast<int>(prev.mid[y])) > 20)
+            lines.mid[y] = prev.mid[y];
+    }
+
+    for (int y = 0; y < IPM_ROW_COUNT; ++y)
+    {
+        if (y >= 2 && y < IPM_ROW_COUNT - 2 && lines.mid[y] > 0)
+        {
+            int sum = 0, cnt = 0;
+            for (int dy = -2; dy <= 2; ++dy)
+                if (lines.mid[y + dy] > 0) { sum += lines.mid[y + dy]; ++cnt; }
+            if (cnt > 0) lines.mid[y] = sum / cnt;
+        }
+    }
+
+    std::memcpy(&prev, &lines, sizeof(EdgeLines));
+}
+
+float calc_deviation(const EdgeLines& lines)
+{
+    float dev_near = 0, dev_mid = 0, dev_far = 0;
+    float cnt_near = 0, cnt_mid = 0, cnt_far = 0;
+    int mid_ref = IMAGE_WIDTH / 2;
+
+    int total = IPM_ROW_COUNT;
+    int zone_far  = total / 5;        // 远: 上20%
+    int zone_mid  = total * 3 / 5;    // 中: 中间40%
+
+    for (int y = 0; y < total; ++y)
+    {
+        if (lines.mid[y] == 0) continue;
+        float d = static_cast<float>(lines.mid[y]) - mid_ref;
+
+        if (y < zone_far)
+            { dev_far  += d; cnt_far  += 1; }
+        else if (y < zone_mid)
+            { dev_mid  += d; cnt_mid  += 1; }
+        else
+            { dev_near += d; cnt_near += 1; }
+    }
+
+    if (cnt_far  < 3) cnt_far  = 3;
+    if (cnt_mid  < 3) cnt_mid  = 3;
+    if (cnt_near < 3) cnt_near = 3;
+
+    float dev = (dev_far  / cnt_far)  * 0.50f +
+                (dev_mid  / cnt_mid)  * 0.30f +
+                (dev_near / cnt_near) * 0.20f;
+
+    return std::clamp(dev / mid_ref, -1.0f, 1.0f);
+}