Implement UART decoder
Ultraworked with [Sisyphus](https://github.com/code-yeongyu/oh-my-openagent) Co-authored-by: Sisyphus <clio-agent@sisyphuslabs.ai>
This commit is contained in:
parent
05098bbdac
commit
a7f07c67b8
2 changed files with 385 additions and 0 deletions
124
switch_uart_protocol.cpp
Normal file
124
switch_uart_protocol.cpp
Normal file
|
|
@ -0,0 +1,124 @@
|
|||
#include "switch_uart_protocol.h"
|
||||
|
||||
namespace {
|
||||
|
||||
SwitchInputState make_neutral_state() {
|
||||
SwitchInputState state{};
|
||||
state.lx = SWITCH_PRO_JOYSTICK_MID;
|
||||
state.ly = SWITCH_PRO_JOYSTICK_MID;
|
||||
state.rx = SWITCH_PRO_JOYSTICK_MID;
|
||||
state.ry = SWITCH_PRO_JOYSTICK_MID;
|
||||
state.imu_sample_count = 0;
|
||||
return state;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
bool switch_uart_decode_input_frame(
|
||||
const uint8_t* packet,
|
||||
uint8_t length,
|
||||
SwitchInputState* out_state) {
|
||||
if (length < 12) {
|
||||
return false;
|
||||
}
|
||||
if (packet[0] != 0xAA) {
|
||||
return false;
|
||||
}
|
||||
if (packet[1] != 0x02) {
|
||||
return false;
|
||||
}
|
||||
|
||||
const uint8_t payload_len = packet[2];
|
||||
if (static_cast<uint16_t>(payload_len) + 4u != length) {
|
||||
return false;
|
||||
}
|
||||
|
||||
uint16_t sum = 0;
|
||||
for (uint16_t index = 0; index < static_cast<uint16_t>(3u + payload_len); ++index) {
|
||||
sum += packet[index];
|
||||
}
|
||||
if ((sum & 0xFF) != packet[length - 1]) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (payload_len < 8) {
|
||||
return false;
|
||||
}
|
||||
|
||||
const uint16_t buttons = static_cast<uint16_t>(packet[3]) |
|
||||
(static_cast<uint16_t>(packet[4]) << 8);
|
||||
const uint8_t hat = packet[5];
|
||||
const uint8_t lx = packet[6];
|
||||
const uint8_t ly = packet[7];
|
||||
const uint8_t rx = packet[8];
|
||||
const uint8_t ry = packet[9];
|
||||
uint8_t imu_count = packet[10];
|
||||
if (imu_count > 3) {
|
||||
imu_count = 3;
|
||||
}
|
||||
|
||||
const uint16_t required_payload_len =
|
||||
static_cast<uint16_t>(8u + static_cast<uint16_t>(imu_count) * 12u);
|
||||
if (payload_len < required_payload_len) {
|
||||
return false;
|
||||
}
|
||||
|
||||
const auto expand_axis = [](uint8_t value) -> uint16_t {
|
||||
return static_cast<uint16_t>(value) << 8 | value;
|
||||
};
|
||||
const auto read_int16 = [](const uint8_t* source) -> int16_t {
|
||||
return static_cast<int16_t>(
|
||||
static_cast<uint16_t>(source[0]) |
|
||||
(static_cast<uint16_t>(source[1]) << 8));
|
||||
};
|
||||
|
||||
SwitchInputState state = make_neutral_state();
|
||||
state.imu_sample_count = imu_count;
|
||||
for (uint8_t index = 0; index < imu_count; ++index) {
|
||||
const uint8_t* base = &packet[11 + index * 12];
|
||||
state.imu_samples[index].accel_x = read_int16(base);
|
||||
state.imu_samples[index].accel_y = read_int16(base + 2);
|
||||
state.imu_samples[index].accel_z = read_int16(base + 4);
|
||||
state.imu_samples[index].gyro_x = read_int16(base + 6);
|
||||
state.imu_samples[index].gyro_y = read_int16(base + 8);
|
||||
state.imu_samples[index].gyro_z = read_int16(base + 10);
|
||||
}
|
||||
|
||||
switch (hat) {
|
||||
case SWITCH_PRO_HAT_UP: state.dpad_up = true; break;
|
||||
case SWITCH_PRO_HAT_UPRIGHT: state.dpad_up = true; state.dpad_right = true; break;
|
||||
case SWITCH_PRO_HAT_RIGHT: state.dpad_right = true; break;
|
||||
case SWITCH_PRO_HAT_DOWNRIGHT: state.dpad_down = true; state.dpad_right = true; break;
|
||||
case SWITCH_PRO_HAT_DOWN: state.dpad_down = true; break;
|
||||
case SWITCH_PRO_HAT_DOWNLEFT: state.dpad_down = true; state.dpad_left = true; break;
|
||||
case SWITCH_PRO_HAT_LEFT: state.dpad_left = true; break;
|
||||
case SWITCH_PRO_HAT_UPLEFT: state.dpad_up = true; state.dpad_left = true; break;
|
||||
default: break;
|
||||
}
|
||||
|
||||
state.button_y = buttons & SWITCH_PRO_MASK_Y;
|
||||
state.button_x = buttons & SWITCH_PRO_MASK_X;
|
||||
state.button_b = buttons & SWITCH_PRO_MASK_B;
|
||||
state.button_a = buttons & SWITCH_PRO_MASK_A;
|
||||
state.button_r = buttons & SWITCH_PRO_MASK_R;
|
||||
state.button_zr = buttons & SWITCH_PRO_MASK_ZR;
|
||||
state.button_plus = buttons & SWITCH_PRO_MASK_PLUS;
|
||||
state.button_minus = buttons & SWITCH_PRO_MASK_MINUS;
|
||||
state.button_r3 = buttons & SWITCH_PRO_MASK_R3;
|
||||
state.button_l3 = buttons & SWITCH_PRO_MASK_L3;
|
||||
state.button_home = buttons & SWITCH_PRO_MASK_HOME;
|
||||
state.button_capture = buttons & SWITCH_PRO_MASK_CAPTURE;
|
||||
state.button_zl = buttons & SWITCH_PRO_MASK_ZL;
|
||||
state.button_l = buttons & SWITCH_PRO_MASK_L;
|
||||
|
||||
state.lx = expand_axis(lx);
|
||||
state.ly = expand_axis(ly);
|
||||
state.rx = expand_axis(rx);
|
||||
state.ry = expand_axis(ry);
|
||||
|
||||
if (!out_state) {
|
||||
return false;
|
||||
}
|
||||
*out_state = state;
|
||||
return true;
|
||||
}
|
||||
261
tests/firmware/test_switch_uart_protocol.cpp
Normal file
261
tests/firmware/test_switch_uart_protocol.cpp
Normal file
|
|
@ -0,0 +1,261 @@
|
|||
#include "test_support.h"
|
||||
|
||||
#include <array>
|
||||
#include <cstddef>
|
||||
#include <cstdint>
|
||||
#include <vector>
|
||||
|
||||
#include "../../switch_input.h"
|
||||
#include "../../switch_uart_protocol.h"
|
||||
|
||||
namespace {
|
||||
|
||||
using ImuSamples = std::vector<SwitchImuSample>;
|
||||
|
||||
void append_int16(std::vector<uint8_t>& bytes, int16_t value) {
|
||||
const uint16_t encoded = static_cast<uint16_t>(value);
|
||||
bytes.push_back(static_cast<uint8_t>(encoded & 0xFF));
|
||||
bytes.push_back(static_cast<uint8_t>(encoded >> 8));
|
||||
}
|
||||
|
||||
std::vector<uint8_t> make_frame(
|
||||
uint16_t buttons = 0,
|
||||
uint8_t hat = SWITCH_PRO_HAT_NOTHING,
|
||||
uint8_t imu_count = 0,
|
||||
const ImuSamples& samples = {}) {
|
||||
std::vector<uint8_t> frame = {
|
||||
0xAA, 0x02, 0x00,
|
||||
static_cast<uint8_t>(buttons & 0xFF),
|
||||
static_cast<uint8_t>(buttons >> 8),
|
||||
hat, 0x80, 0x80, 0x80, 0x80, imu_count,
|
||||
};
|
||||
for (const SwitchImuSample& sample : samples) {
|
||||
append_int16(frame, sample.accel_x);
|
||||
append_int16(frame, sample.accel_y);
|
||||
append_int16(frame, sample.accel_z);
|
||||
append_int16(frame, sample.gyro_x);
|
||||
append_int16(frame, sample.gyro_y);
|
||||
append_int16(frame, sample.gyro_z);
|
||||
}
|
||||
frame[2] = static_cast<uint8_t>(frame.size() - 3);
|
||||
uint8_t checksum = 0;
|
||||
for (uint8_t byte : frame) {
|
||||
checksum = static_cast<uint8_t>(checksum + byte);
|
||||
}
|
||||
frame.push_back(checksum);
|
||||
return frame;
|
||||
}
|
||||
|
||||
bool decode(const std::vector<uint8_t>& frame, SwitchInputState& state) {
|
||||
return switch_uart_decode_input_frame(
|
||||
frame.data(), static_cast<uint8_t>(frame.size()), &state);
|
||||
}
|
||||
|
||||
bool uart_decoder_rejects_short_frame() {
|
||||
// Given: a frame shorter than the legacy 12-byte minimum.
|
||||
const std::array<uint8_t, 11> frame{};
|
||||
SwitchInputState state{};
|
||||
|
||||
// When: the frame is decoded. Then: it is rejected.
|
||||
CHECK(!switch_uart_decode_input_frame(frame.data(), frame.size(), &state));
|
||||
return true;
|
||||
}
|
||||
|
||||
bool uart_decoder_rejects_wrong_header_and_version() {
|
||||
// Given: otherwise-valid frames with invalid framing bytes.
|
||||
std::vector<uint8_t> wrong_header = make_frame();
|
||||
std::vector<uint8_t> wrong_version = make_frame();
|
||||
wrong_header[0] = 0xAB;
|
||||
wrong_version[1] = 0x01;
|
||||
SwitchInputState state{};
|
||||
|
||||
// When: either frame is decoded. Then: both are rejected before payload use.
|
||||
CHECK(!decode(wrong_header, state));
|
||||
CHECK(!decode(wrong_version, state));
|
||||
return true;
|
||||
}
|
||||
|
||||
bool uart_decoder_rejects_declared_length_and_checksum_mismatch() {
|
||||
// Given: valid frames corrupted independently at length and checksum.
|
||||
std::vector<uint8_t> wrong_length = make_frame();
|
||||
std::vector<uint8_t> wrong_checksum = make_frame();
|
||||
++wrong_length[2];
|
||||
++wrong_checksum.back();
|
||||
SwitchInputState state{};
|
||||
state.lx = 0x1234;
|
||||
|
||||
// When: either frame is decoded. Then: both validation failures are rejected.
|
||||
CHECK(!decode(wrong_length, state));
|
||||
CHECK(!decode(wrong_checksum, state));
|
||||
CHECK(state.lx == 0x1234);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool uart_decoder_decodes_neutral_frame() {
|
||||
// Given: the canonical 12-byte neutral frame.
|
||||
const std::vector<uint8_t> frame = make_frame();
|
||||
SwitchInputState state{};
|
||||
|
||||
// When: the frame is decoded. Then: buttons/hat/IMU are clear and sticks expand exactly.
|
||||
CHECK(decode(frame, state));
|
||||
CHECK(!state.dpad_up && !state.dpad_down && !state.dpad_left && !state.dpad_right);
|
||||
CHECK(!state.button_a && !state.button_b && !state.button_x && !state.button_y);
|
||||
CHECK(!state.button_l && !state.button_r && !state.button_zl && !state.button_zr);
|
||||
CHECK(!state.button_plus && !state.button_minus && !state.button_home && !state.button_capture);
|
||||
CHECK(!state.button_l3 && !state.button_r3);
|
||||
CHECK(state.lx == 0x8080 && state.ly == 0x8080);
|
||||
CHECK(state.rx == 0x8080 && state.ry == 0x8080);
|
||||
CHECK(state.imu_sample_count == 0);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool uart_decoder_maps_every_button_bit() {
|
||||
struct ButtonCase {
|
||||
uint16_t mask;
|
||||
bool SwitchInputState::*field;
|
||||
};
|
||||
static constexpr ButtonCase cases[] = {
|
||||
{SWITCH_PRO_MASK_Y, &SwitchInputState::button_y},
|
||||
{SWITCH_PRO_MASK_B, &SwitchInputState::button_b},
|
||||
{SWITCH_PRO_MASK_A, &SwitchInputState::button_a},
|
||||
{SWITCH_PRO_MASK_X, &SwitchInputState::button_x},
|
||||
{SWITCH_PRO_MASK_L, &SwitchInputState::button_l},
|
||||
{SWITCH_PRO_MASK_R, &SwitchInputState::button_r},
|
||||
{SWITCH_PRO_MASK_ZL, &SwitchInputState::button_zl},
|
||||
{SWITCH_PRO_MASK_ZR, &SwitchInputState::button_zr},
|
||||
{SWITCH_PRO_MASK_MINUS, &SwitchInputState::button_minus},
|
||||
{SWITCH_PRO_MASK_PLUS, &SwitchInputState::button_plus},
|
||||
{SWITCH_PRO_MASK_L3, &SwitchInputState::button_l3},
|
||||
{SWITCH_PRO_MASK_R3, &SwitchInputState::button_r3},
|
||||
{SWITCH_PRO_MASK_HOME, &SwitchInputState::button_home},
|
||||
{SWITCH_PRO_MASK_CAPTURE, &SwitchInputState::button_capture},
|
||||
};
|
||||
|
||||
// Given/When: each legacy button bit is decoded independently.
|
||||
for (const ButtonCase& button : cases) {
|
||||
SwitchInputState state{};
|
||||
CHECK(decode(make_frame(button.mask), state));
|
||||
|
||||
// Then: the corresponding shared input field is set.
|
||||
CHECK(state.*(button.field));
|
||||
const int pressed_count =
|
||||
state.button_y + state.button_b + state.button_a + state.button_x +
|
||||
state.button_l + state.button_r + state.button_zl + state.button_zr +
|
||||
state.button_minus + state.button_plus + state.button_l3 + state.button_r3 +
|
||||
state.button_home + state.button_capture;
|
||||
CHECK(pressed_count == 1);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool uart_decoder_maps_every_hat_value() {
|
||||
struct HatCase {
|
||||
uint8_t hat;
|
||||
bool up;
|
||||
bool down;
|
||||
bool left;
|
||||
bool right;
|
||||
};
|
||||
static constexpr HatCase cases[] = {
|
||||
{SWITCH_PRO_HAT_UP, true, false, false, false},
|
||||
{SWITCH_PRO_HAT_UPRIGHT, true, false, false, true},
|
||||
{SWITCH_PRO_HAT_RIGHT, false, false, false, true},
|
||||
{SWITCH_PRO_HAT_DOWNRIGHT, false, true, false, true},
|
||||
{SWITCH_PRO_HAT_DOWN, false, true, false, false},
|
||||
{SWITCH_PRO_HAT_DOWNLEFT, false, true, true, false},
|
||||
{SWITCH_PRO_HAT_LEFT, false, false, true, false},
|
||||
{SWITCH_PRO_HAT_UPLEFT, true, false, true, false},
|
||||
{SWITCH_PRO_HAT_NOTHING, false, false, false, false},
|
||||
{0xFF, false, false, false, false},
|
||||
};
|
||||
|
||||
// Given/When: every legacy hat value is decoded.
|
||||
for (const HatCase& hat : cases) {
|
||||
SwitchInputState state{};
|
||||
CHECK(decode(make_frame(0, hat.hat), state));
|
||||
|
||||
// Then: its exact cardinal/diagonal field combination is produced.
|
||||
CHECK(state.dpad_up == hat.up && state.dpad_down == hat.down);
|
||||
CHECK(state.dpad_left == hat.left && state.dpad_right == hat.right);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool uart_decoder_expands_stick_bytes() {
|
||||
// Given: a valid frame with distinct byte values on every axis.
|
||||
std::vector<uint8_t> frame = make_frame();
|
||||
frame[6] = 0x00;
|
||||
frame[7] = 0x7F;
|
||||
frame[8] = 0x80;
|
||||
frame[9] = 0xFF;
|
||||
frame.back() = 0;
|
||||
for (std::size_t index = 0; index + 1 < frame.size(); ++index) {
|
||||
frame.back() = static_cast<uint8_t>(frame.back() + frame[index]);
|
||||
}
|
||||
SwitchInputState state{};
|
||||
|
||||
// When: the frame is decoded. Then: each byte is duplicated into 16 bits.
|
||||
CHECK(decode(frame, state));
|
||||
CHECK(state.lx == 0x0000 && state.ly == 0x7F7F);
|
||||
CHECK(state.rx == 0x8080 && state.ry == 0xFFFF);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool uart_decoder_decodes_one_and_three_imu_samples() {
|
||||
// Given: one-sample and three-sample frames with signed extrema and distinct values.
|
||||
const SwitchImuSample first{-32768, -2, -1, 0, 1, 32767};
|
||||
const SwitchImuSample second{10, 20, 30, 40, 50, 60};
|
||||
const SwitchImuSample third{-10, -20, -30, -40, -50, -60};
|
||||
SwitchInputState one{};
|
||||
SwitchInputState three{};
|
||||
|
||||
// When: both frames are decoded.
|
||||
CHECK(decode(make_frame(0, SWITCH_PRO_HAT_NOTHING, 1, {first}), one));
|
||||
CHECK(decode(make_frame(0, SWITCH_PRO_HAT_NOTHING, 3, {first, second, third}), three));
|
||||
|
||||
// Then: counts and little-endian signed sample fields remain exact.
|
||||
CHECK(one.imu_sample_count == 1 && one.imu_samples[0].accel_x == -32768);
|
||||
CHECK(one.imu_samples[0].gyro_z == 32767);
|
||||
CHECK(three.imu_sample_count == 3);
|
||||
CHECK(three.imu_samples[1].accel_z == 30 && three.imu_samples[1].gyro_y == 50);
|
||||
CHECK(three.imu_samples[2].accel_y == -20 && three.imu_samples[2].gyro_z == -60);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool uart_decoder_caps_imu_count_and_rejects_truncation() {
|
||||
// Given: a count of four backed by three samples, and a count of one with none.
|
||||
const SwitchImuSample sample{1, 2, 3, 4, 5, 6};
|
||||
const std::vector<uint8_t> capped = make_frame(0, SWITCH_PRO_HAT_NOTHING, 4, {sample, sample, sample});
|
||||
const std::vector<uint8_t> truncated = make_frame(0, SWITCH_PRO_HAT_NOTHING, 1);
|
||||
SwitchInputState state{};
|
||||
|
||||
// When: both frames are decoded. Then: three samples are accepted and truncation is rejected.
|
||||
CHECK(decode(capped, state));
|
||||
CHECK(state.imu_sample_count == 3);
|
||||
CHECK(!decode(truncated, state));
|
||||
return true;
|
||||
}
|
||||
|
||||
bool uart_decoder_rejects_null_output() {
|
||||
// Given: an otherwise-valid frame. When: no output state is supplied.
|
||||
const std::vector<uint8_t> frame = make_frame();
|
||||
|
||||
// Then: the legacy parser returns false rather than mutating driver state.
|
||||
CHECK(!switch_uart_decode_input_frame(frame.data(), frame.size(), nullptr));
|
||||
return true;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
void run_switch_uart_protocol_tests(TestRunner& runner) {
|
||||
runner.run("UART rejects short frame", uart_decoder_rejects_short_frame);
|
||||
runner.run("UART rejects header and version", uart_decoder_rejects_wrong_header_and_version);
|
||||
runner.run("UART rejects length and checksum", uart_decoder_rejects_declared_length_and_checksum_mismatch);
|
||||
runner.run("UART decodes neutral frame", uart_decoder_decodes_neutral_frame);
|
||||
runner.run("UART maps every button", uart_decoder_maps_every_button_bit);
|
||||
runner.run("UART maps every hat", uart_decoder_maps_every_hat_value);
|
||||
runner.run("UART expands stick bytes", uart_decoder_expands_stick_bytes);
|
||||
runner.run("UART decodes IMU samples", uart_decoder_decodes_one_and_three_imu_samples);
|
||||
runner.run("UART caps and validates IMU count", uart_decoder_caps_imu_count_and_rejects_truncation);
|
||||
runner.run("UART rejects null output", uart_decoder_rejects_null_output);
|
||||
}
|
||||
Loading…
Add table
Add a link
Reference in a new issue