130 lines
4.1 KiB
C++
130 lines
4.1 KiB
C++
#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
|