updates

2026-10-02 12:23:58 -04:00
parent f0af1dbbcb
commit ad1e7e6d01
219 changed files with 488314 additions and 0 deletions
@@ -0,0 +1,129 @@
+#pragma once
+
+#include <stddef.h>
+#include <stdint.h>
+
+// A fixed-size, little-endian UART frame shared by the camera and both eyes.
+namespace gaze {
+constexpr uint8_t kMagic0 = 0xA5;
+constexpr uint8_t kMagic1 = 0x5A;
+constexpr uint8_t kVersion = 1;
+constexpr uint8_t kValid = 0x01;
+constexpr size_t kFrameSize = 12;
+constexpr uint32_t kTimeoutMs = 750;
+
+struct Frame {
+  uint16_t sequence;
+  uint16_t x;  // 0..1000, left to right in the camera image
+  uint16_t y;  // 0..1000, top to bottom in the camera image
+  bool valid;
+};
+
+inline uint16_t crc16(const uint8_t *bytes, size_t length) {
+  uint16_t crc = 0xFFFF;
+  for (size_t i = 0; i < length; ++i) {
+    crc ^= static_cast<uint16_t>(bytes[i]) << 8;
+    for (int bit = 0; bit < 8; ++bit) {
+      crc = (crc & 0x8000) ? static_cast<uint16_t>((crc << 1) ^ 0x1021)
+                           : static_cast<uint16_t>(crc << 1);
+    }
+  }
+  return crc;
+}
+
+inline void encode(const Frame &frame, uint8_t out[kFrameSize]) {
+  out[0] = kMagic0;
+  out[1] = kMagic1;
+  out[2] = kVersion;
+  out[3] = frame.valid ? kValid : 0;
+  out[4] = static_cast<uint8_t>(frame.sequence);
+  out[5] = static_cast<uint8_t>(frame.sequence >> 8);
+  out[6] = static_cast<uint8_t>(frame.x);
+  out[7] = static_cast<uint8_t>(frame.x >> 8);
+  out[8] = static_cast<uint8_t>(frame.y);
+  out[9] = static_cast<uint8_t>(frame.y >> 8);
+  const uint16_t crc = crc16(out, kFrameSize - 2);
+  out[10] = static_cast<uint8_t>(crc);
+  out[11] = static_cast<uint8_t>(crc >> 8);
+}
+
+inline bool decode(const uint8_t bytes[kFrameSize], Frame *out) {
+  if (bytes[0] != kMagic0 || bytes[1] != kMagic1 ||
+      bytes[2] != kVersion || (bytes[3] & ~kValid) != 0) return false;
+  const uint16_t actual_crc = static_cast<uint16_t>(bytes[10]) |
+                              (static_cast<uint16_t>(bytes[11]) << 8);
+  if (crc16(bytes, kFrameSize - 2) != actual_crc) return false;
+  const uint16_t x = static_cast<uint16_t>(bytes[6]) |
+                     (static_cast<uint16_t>(bytes[7]) << 8);
+  const uint16_t y = static_cast<uint16_t>(bytes[8]) |
+                     (static_cast<uint16_t>(bytes[9]) << 8);
+  if (x > 1000 || y > 1000) return false;
+  out->sequence = static_cast<uint16_t>(bytes[4]) |
+                  (static_cast<uint16_t>(bytes[5]) << 8);
+  out->x = x;
+  out->y = y;
+  out->valid = (bytes[3] & kValid) != 0;
+  return true;
+}
+
+class Receiver {
+ public:
+  // Returns true only for a complete, valid, fresh frame.
+  bool feed(uint8_t byte, uint32_t now_ms, Frame *out) {
+    if (length_ == 0 && byte != kMagic0) return false;
+    if (length_ == 1 && byte != kMagic1) {
+      length_ = byte == kMagic0 ? 1 : 0;
+      return false;
+    }
+    bytes_[length_++] = byte;
+    if (length_ < kFrameSize) return false;
+
+    Frame decoded = {};
+    const bool well_formed = decode(bytes_, &decoded);
+    if (well_formed) {
+      length_ = 0;
+    } else {
+      ++rejected_format_;
+      // Keep an embedded preamble, or a trailing first magic byte.
+      size_t start = 1;
+      while (start < kFrameSize &&
+             (bytes_[start] != kMagic0 ||
+              (start + 1 < kFrameSize && bytes_[start + 1] != kMagic1))) {
+        ++start;
+      }
+      length_ = kFrameSize - start;
+      for (size_t i = 0; i < length_; ++i) bytes_[i] = bytes_[start + i];
+    }
+    if (!well_formed) return false;
+
+    if (has_sequence_ && static_cast<uint32_t>(now_ms - last_ms_) <= kTimeoutMs) {
+      const uint16_t advance = static_cast<uint16_t>(decoded.sequence - last_sequence_);
+      if (advance == 0 || advance >= 0x8000) {
+        ++rejected_sequence_;
+        return false;
+      }
+    }
+    has_sequence_ = true;
+    last_sequence_ = decoded.sequence;
+    last_ms_ = now_ms;
+    *out = decoded;
+    return true;
+  }
+
+  bool recent(uint32_t now_ms) const {
+    return has_sequence_ && static_cast<uint32_t>(now_ms - last_ms_) <= kTimeoutMs;
+  }
+
+  uint32_t rejected_format() const { return rejected_format_; }
+  uint32_t rejected_sequence() const { return rejected_sequence_; }
+
+ private:
+  uint8_t bytes_[kFrameSize] = {};
+  size_t length_ = 0;
+  bool has_sequence_ = false;
+  uint16_t last_sequence_ = 0;
+  uint32_t last_ms_ = 0;
+  uint32_t rejected_format_ = 0;
+  uint32_t rejected_sequence_ = 0;
+};
+}  // namespace gaze