feat: CV module — frame diff + threshold (blob tracking TODO)

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-04-13 14:26:34 -07:00
parent 7662fc4c25
commit e6843584cf
3 changed files with 168 additions and 0 deletions
--- a/firmware/lib/cv/cv.cpp
+++ b/firmware/lib/cv/cv.cpp
@@ -0,0 +1,58 @@
+// firmware/lib/cv/cv.cpp
+#include "cv.h"
+#include <string.h>
+#include <math.h>
+#include <algorithm>
+
+void cv_init(CVState& state) {
+    memset(&state, 0, sizeof(CVState));
+    state.next_id = 1;
+}
+
+void cv_reset_counts(CVState& state) {
+    state.entries = 0;
+    state.exits   = 0;
+}
+
+static void frame_diff(const uint8_t* frame, const uint8_t* bg,
+                       uint8_t* fg, int pixels) {
+    for (int i = 0; i < pixels; i++) {
+        int diff = (int)frame[i] - (int)bg[i];
+        if (diff < 0) diff = -diff;
+        fg[i] = (diff > CV_DIFF_THRESH) ? 1 : 0;
+    }
+}
+
+CVResult cv_process(CVState& state, const uint8_t* frame, uint8_t line_pct) {
+    CVResult result = {0, 0};
+    state.frame_index++;
+
+    if (!state.bg_valid) {
+        memcpy(state.background, frame, CV_PIXELS);
+        state.bg_valid = true;
+        return result;
+    }
+
+    uint8_t fg[CV_PIXELS];
+    frame_diff(frame, state.background, fg, CV_PIXELS);
+
+    int fg_count = 0;
+    for (int i = 0; i < CV_PIXELS; i++) fg_count += fg[i];
+
+    bool motion = fg_count > CV_MIN_BLOB_PX;
+    if (!motion) {
+        if (state.frame_index - state.last_motion_frame > 10) {
+            memcpy(state.background, frame, CV_PIXELS);
+        }
+        for (auto& t : state.tracks) t.missed++;
+        state.tracks.erase(
+            std::remove_if(state.tracks.begin(), state.tracks.end(),
+                [](const CVTrack& t){ return t.missed > CV_MAX_MISSED; }),
+            state.tracks.end());
+        return result;
+    }
+
+    state.last_motion_frame = state.frame_index;
+    // Blob detection and tracking added in Tasks 5-6
+    return result;
+}