115 lines
3.2 KiB
C++
115 lines
3.2 KiB
C++
#include "hoverboard.h"
|
|
|
|
namespace {
|
|
|
|
// XOR of every 16-bit field except the checksum itself, matching
|
|
// usart_process_command() and the feedback packer in the firmware.
|
|
uint16_t checksumOf(const HoverCommand& c) {
|
|
return (uint16_t)(c.start ^ (uint16_t)c.left ^ (uint16_t)c.right ^ c.buzzer);
|
|
}
|
|
|
|
uint16_t checksumOf(const HoverFeedback& f) {
|
|
return (uint16_t)(f.start ^ (uint16_t)f.cmd1 ^ (uint16_t)f.cmd2 ^
|
|
(uint16_t)f.speedR_meas ^ (uint16_t)f.speedL_meas ^
|
|
(uint16_t)f.wheelR_cnt ^ (uint16_t)f.wheelL_cnt ^
|
|
(uint16_t)f.batVoltage ^ (uint16_t)f.boardTemp ^ f.cmdLed);
|
|
}
|
|
|
|
int16_t clampCmd(int32_t v) {
|
|
if (v > CMD_MAX) return CMD_MAX;
|
|
if (v < -CMD_MAX) return -CMD_MAX;
|
|
return (int16_t)v;
|
|
}
|
|
|
|
} // namespace
|
|
|
|
void HoverboardLink::begin(HardwareSerial& port) {
|
|
port_ = &port;
|
|
port_->begin(HOVER_BAUD, SERIAL_8N1, PIN_HOVER_RX, PIN_HOVER_TX);
|
|
}
|
|
|
|
void HoverboardLink::send(int16_t left, int16_t right) {
|
|
if (!port_) return;
|
|
|
|
// Expire a finished beep here rather than on a timer, so the field is always
|
|
// consistent with the frame actually going out.
|
|
if (buzzer_ != 0 && (int32_t)(millis() - beepUntilMs_) >= 0) buzzer_ = 0;
|
|
|
|
HoverCommand c;
|
|
c.start = HOVER_START_FRAME;
|
|
c.left = clampCmd(INVERT_LEFT ? -left : left);
|
|
c.right = clampCmd(INVERT_RIGHT ? -right : right);
|
|
c.buzzer = buzzer_;
|
|
c.checksum = checksumOf(c);
|
|
|
|
port_->write((const uint8_t*)&c, sizeof(c));
|
|
}
|
|
|
|
void HoverboardLink::dropByte() {
|
|
// Resync: discard the oldest byte and let the next poll re-test the window.
|
|
if (have_ > 0) {
|
|
memmove(buf_, buf_ + 1, have_ - 1);
|
|
have_--;
|
|
}
|
|
}
|
|
|
|
bool HoverboardLink::poll() {
|
|
if (!port_) return false;
|
|
bool got = false;
|
|
|
|
while (port_->available() > 0) {
|
|
if (have_ < sizeof(buf_)) {
|
|
buf_[have_++] = (uint8_t)port_->read();
|
|
rxBytes_++;
|
|
}
|
|
|
|
// Keep the window anchored on a start frame before judging the checksum.
|
|
while (have_ >= 2) {
|
|
uint16_t start;
|
|
memcpy(&start, buf_, sizeof(start));
|
|
if (start == HOVER_START_FRAME) break;
|
|
dropByte();
|
|
}
|
|
|
|
if (have_ == sizeof(buf_)) {
|
|
HoverFeedback candidate;
|
|
memcpy(&candidate, buf_, sizeof(candidate));
|
|
|
|
if (candidate.checksum == checksumOf(candidate)) {
|
|
fb_ = candidate;
|
|
lastFrameMs_ = millis();
|
|
framesOk_++;
|
|
have_ = 0;
|
|
got = true;
|
|
} else {
|
|
// A corrupt or misaligned frame: step one byte and hunt again rather
|
|
// than throwing away bytes that may hold the real start marker.
|
|
framesBad_++;
|
|
dropByte();
|
|
}
|
|
}
|
|
}
|
|
|
|
return got;
|
|
}
|
|
|
|
void HoverboardLink::beep(uint8_t pitch, uint8_t pattern, uint16_t ms) {
|
|
buzzer_ = (uint16_t)pitch | ((uint16_t)pattern << 8);
|
|
beepUntilMs_ = millis() + ms;
|
|
}
|
|
|
|
void HoverboardLink::beepOff() {
|
|
buzzer_ = 0;
|
|
}
|
|
|
|
bool HoverboardLink::linkUp() const {
|
|
return framesOk_ > 0 && (millis() - lastFrameMs_) < FEEDBACK_TIMEOUT_MS;
|
|
}
|
|
|
|
int16_t hoverTickDelta(int16_t prev, int16_t curr) {
|
|
int32_t d = (int32_t)curr - (int32_t)prev;
|
|
d = ((d % ODOM_MODULO) + ODOM_MODULO) % ODOM_MODULO; // -> [0, ODOM_MODULO)
|
|
if (d >= ODOM_MODULO / 2) d -= ODOM_MODULO; // -> shortest signed path
|
|
return (int16_t)d;
|
|
}
|