Add SPI serialization/deserialization for the cameras
Change-Id: Ide6b13de583a65907ff2b927a2a6fd1fc507a5b0
diff --git a/y2019/jevois/BUILD b/y2019/jevois/BUILD
index 8a8bca3..871ee15 100644
--- a/y2019/jevois/BUILD
+++ b/y2019/jevois/BUILD
@@ -1,3 +1,57 @@
+spi_crc_args = [
+ "$(location //third_party/pycrc:pycrc_main)",
+ "--width=16",
+ # This is the recommendation from
+ # http://users.ece.cmu.edu/~koopman/roses/dsn04/koopman04_crc_poly_embedded.pdf
+ # for messages of 242 - 2048 bits, which covers what we want.
+ # That's an analysis from an exhaustive search of all polynomials for
+ # various CRCs to find the best ones. This is 0xBAAD, converted from the
+ # weird format used there to the standard one used by pycrc.
+ "--poly=0x755b",
+ "--reflect-in=False",
+ "--xor-in=0xffff",
+ "--reflect-out=False",
+ "--xor-out=0xffff",
+ "--std=C99",
+ "--algorithm=table-driven",
+ "--symbol-prefix=jevois_spi_crc_",
+ "--crc-type=uint16_t",
+]
+
+genrule(
+ name = "gen_spi_crc",
+ outs = [
+ "spi_crc.h",
+ "spi_crc.c",
+ ],
+ cmd = " && ".join([
+ " ".join(spi_crc_args + [
+ "--generate=h",
+ "--output=$(location spi_crc.h)",
+ ]),
+ " ".join(spi_crc_args + [
+ "--generate=c",
+ "--output=$(location spi_crc.c)",
+ ]),
+ ]),
+ tools = [
+ "//third_party/pycrc:pycrc_main",
+ ],
+)
+
+cc_library(
+ name = "spi_crc",
+ srcs = [
+ "spi_crc.c",
+ ],
+ hdrs = [
+ "spi_crc.h",
+ ],
+ deps = [
+ "//third_party/GSL",
+ ],
+)
+
cc_library(
name = "structures",
hdrs = [
@@ -9,3 +63,31 @@
"//third_party/eigen",
],
)
+
+cc_library(
+ name = "spi",
+ srcs = [
+ "spi.cc",
+ ],
+ hdrs = [
+ "spi.h",
+ ],
+ deps = [
+ ":spi_crc",
+ ":structures",
+ "//aos/util:bitpacking",
+ "//third_party/GSL",
+ "//third_party/optional",
+ ],
+)
+
+cc_test(
+ name = "spi_test",
+ srcs = [
+ "spi_test.cc",
+ ],
+ deps = [
+ ":spi",
+ "//aos/testing:googletest",
+ ],
+)
diff --git a/y2019/jevois/spi.cc b/y2019/jevois/spi.cc
new file mode 100644
index 0000000..0500860
--- /dev/null
+++ b/y2019/jevois/spi.cc
@@ -0,0 +1,237 @@
+#include "y2019/jevois/spi.h"
+
+#include <assert.h>
+
+#include "aos/util/bitpacking.h"
+#include "third_party/GSL/include/gsl/gsl"
+#include "y2019/jevois/spi_crc.h"
+
+// SPI transfer format (6x 8 bit frames):
+// 1. 1-byte brightness for each beacon channel.
+// 2. 1-byte specifying on/off for each light ring.
+// 3. 2-byte CRC
+//
+// SPI transfer format (41x 8 bit frames):
+// 1. Camera frame 0
+// 2. Camera frame 1
+// 3. Camera frame 2
+// 4. 2-byte CRC-16
+// Each camera frame (13x 8 bit frames):
+// 1. Duration for how old the frame is. This is a value received from the
+// camera, added to the time between the first character being received
+// by the MCU to the CS line being asserted. Specifically it's an 8 bit
+// unsigned number of ms.
+// 2. Target 0
+// 3. Target 1
+// 4. Target 2
+// Each target (4x 8 bit frames):
+// 1. 10 bits heading
+// 2. 8 bits distance
+// 3. 6 bits skew
+// 4. 6 bits height
+// 5. 1 bit target valid (a present frame has all-valid targets)
+// 6. 1 bit target present (a present frame can have from 0 to 3
+// targets, depending on how many were found)
+// Note that empty frames are still sent to indicate that the camera is
+// still working even though it doesn't see any targets.
+
+namespace frc971 {
+namespace jevois {
+namespace {
+
+constexpr float heading_min() { return -3; }
+constexpr float heading_max() { return 3; }
+constexpr int heading_bits() { return 10; }
+constexpr int heading_offset() { return 0; }
+void heading_pack(float heading, gsl::span<char> destination) {
+ const auto integer = aos::FloatToIntLinear<heading_bits()>(
+ heading_min(), heading_max(), heading);
+ aos::PackBits<uint32_t, heading_bits(), heading_offset()>(integer,
+ destination);
+}
+float heading_unpack(gsl::span<const char> source) {
+ const auto integer =
+ aos::UnpackBits<uint32_t, heading_bits(), heading_offset()>(source);
+ return aos::IntToFloatLinear<heading_bits()>(heading_min(), heading_max(),
+ integer);
+}
+
+constexpr float distance_min() { return 0; }
+constexpr float distance_max() {
+ // The field is 18.4m diagonally.
+ return 18.4;
+}
+constexpr int distance_bits() { return 8; }
+constexpr int distance_offset() { return heading_offset() + heading_bits(); }
+void distance_pack(float distance, gsl::span<char> destination) {
+ const auto integer = aos::FloatToIntLinear<distance_bits()>(
+ distance_min(), distance_max(), distance);
+ aos::PackBits<uint32_t, distance_bits(), distance_offset()>(integer,
+ destination);
+}
+float distance_unpack(gsl::span<const char> source) {
+ const auto integer =
+ aos::UnpackBits<uint32_t, distance_bits(), distance_offset()>(source);
+ return aos::IntToFloatLinear<distance_bits()>(distance_min(), distance_max(),
+ integer);
+}
+
+constexpr float skew_min() { return -3; }
+constexpr float skew_max() { return 3; }
+constexpr int skew_bits() { return 6; }
+constexpr int skew_offset() { return distance_offset() + distance_bits(); }
+void skew_pack(float skew, gsl::span<char> destination) {
+ const auto integer =
+ aos::FloatToIntLinear<skew_bits()>(skew_min(), skew_max(), skew);
+ aos::PackBits<uint32_t, skew_bits(), skew_offset()>(integer, destination);
+}
+float skew_unpack(gsl::span<const char> source) {
+ const auto integer =
+ aos::UnpackBits<uint32_t, skew_bits(), skew_offset()>(source);
+ return aos::IntToFloatLinear<skew_bits()>(skew_min(), skew_max(), integer);
+}
+
+constexpr float height_min() { return 0; }
+constexpr float height_max() { return 1.5; }
+constexpr int height_bits() { return 6; }
+constexpr int height_offset() { return skew_offset() + skew_bits(); }
+void height_pack(float height, gsl::span<char> destination) {
+ const auto integer =
+ aos::FloatToIntLinear<height_bits()>(height_min(), height_max(), height);
+ aos::PackBits<uint32_t, height_bits(), height_offset()>(integer, destination);
+}
+float height_unpack(gsl::span<const char> source) {
+ const auto integer =
+ aos::UnpackBits<uint32_t, height_bits(), height_offset()>(source);
+ return aos::IntToFloatLinear<height_bits()>(height_min(), height_max(),
+ integer);
+}
+
+constexpr int valid_bits() { return 1; }
+constexpr int valid_offset() { return height_offset() + height_bits(); }
+void valid_pack(bool valid, gsl::span<char> destination) {
+ aos::PackBits<uint32_t, valid_bits(), valid_offset()>(valid, destination);
+}
+bool valid_unpack(gsl::span<const char> source) {
+ return aos::UnpackBits<uint32_t, valid_bits(), valid_offset()>(source);
+}
+
+constexpr int present_bits() { return 1; }
+constexpr int present_offset() { return valid_offset() + valid_bits(); }
+void present_pack(bool present, gsl::span<char> destination) {
+ aos::PackBits<uint32_t, present_bits(), present_offset()>(present,
+ destination);
+}
+bool present_unpack(gsl::span<const char> source) {
+ return aos::UnpackBits<uint32_t, present_bits(), present_offset()>(source);
+}
+
+constexpr int next_offset() { return present_offset() + present_bits(); }
+static_assert(next_offset() <= 32, "Target is too big");
+
+} // namespace
+
+SpiTransfer SpiPackToRoborio(const TeensyToRoborio &message) {
+ SpiTransfer transfer;
+ gsl::span<char> remaining_space = transfer;
+ for (int frame = 0; frame < 3; ++frame) {
+ for (int target = 0; target < 3; ++target) {
+ remaining_space[0] = 0;
+ remaining_space[1] = 0;
+ remaining_space[2] = 0;
+ remaining_space[3] = 0;
+
+ if (static_cast<int>(message.frames.size()) > frame) {
+ valid_pack(true, remaining_space);
+ if (static_cast<int>(message.frames[frame].targets.size()) > target) {
+ heading_pack(message.frames[frame].targets[target].heading,
+ remaining_space);
+ distance_pack(message.frames[frame].targets[target].distance,
+ remaining_space);
+ skew_pack(message.frames[frame].targets[target].skew,
+ remaining_space);
+ height_pack(message.frames[frame].targets[target].height,
+ remaining_space);
+ present_pack(true, remaining_space);
+ } else {
+ present_pack(false, remaining_space);
+ }
+ } else {
+ valid_pack(false, remaining_space);
+ }
+
+ remaining_space = remaining_space.subspan(4);
+ }
+ if (static_cast<int>(message.frames.size()) > frame) {
+ const uint8_t age_count = message.frames[frame].age.count();
+ memcpy(&remaining_space[0], &age_count, 1);
+ } else {
+ remaining_space[0] = 0;
+ }
+ remaining_space = remaining_space.subspan(1);
+ }
+ {
+ uint16_t crc = jevois_spi_crc_init();
+ crc = jevois_spi_crc_update(crc, transfer.data(),
+ transfer.size() - remaining_space.size());
+ crc = jevois_spi_crc_finalize(crc);
+ assert(static_cast<size_t>(remaining_space.size()) >= sizeof(crc));
+ memcpy(&remaining_space[0], &crc, sizeof(crc));
+ remaining_space = remaining_space.subspan(sizeof(crc));
+ }
+ assert(remaining_space.empty());
+ return transfer;
+}
+
+tl::optional<TeensyToRoborio> SpiUnpackToRoborio(const SpiTransfer &transfer) {
+ TeensyToRoborio message;
+ gsl::span<const char> remaining_input = transfer;
+ for (int frame = 0; frame < 3; ++frame) {
+ const bool have_frame = valid_unpack(remaining_input);
+ if (have_frame) {
+ message.frames.push_back({});
+ }
+ for (int target = 0; target < 3; ++target) {
+ if (present_unpack(remaining_input)) {
+ if (have_frame) {
+ message.frames.back().targets.push_back({});
+ message.frames.back().targets.back().heading =
+ heading_unpack(remaining_input);
+ message.frames.back().targets.back().distance =
+ distance_unpack(remaining_input);
+ message.frames.back().targets.back().skew =
+ skew_unpack(remaining_input);
+ message.frames.back().targets.back().height =
+ height_unpack(remaining_input);
+ }
+ }
+
+ remaining_input = remaining_input.subspan(4);
+ }
+ if (have_frame) {
+ uint8_t age_count;
+ memcpy(&age_count, &remaining_input[0], 1);
+ message.frames.back().age = camera_duration(age_count);
+ }
+ remaining_input = remaining_input.subspan(1);
+ }
+ {
+ uint16_t calculated_crc = jevois_spi_crc_init();
+ calculated_crc =
+ jevois_spi_crc_update(calculated_crc, transfer.data(),
+ transfer.size() - remaining_input.size());
+ calculated_crc = jevois_spi_crc_finalize(calculated_crc);
+ uint16_t received_crc;
+ assert(static_cast<size_t>(remaining_input.size()) >= sizeof(received_crc));
+ memcpy(&received_crc, &remaining_input[0], sizeof(received_crc));
+ remaining_input = remaining_input.subspan(sizeof(received_crc));
+ if (calculated_crc != received_crc) {
+ return tl::nullopt;
+ }
+ }
+ assert(remaining_input.empty());
+ return message;
+}
+
+} // namespace jevois
+} // namespace frc971
diff --git a/y2019/jevois/spi.h b/y2019/jevois/spi.h
new file mode 100644
index 0000000..e0c4d90
--- /dev/null
+++ b/y2019/jevois/spi.h
@@ -0,0 +1,34 @@
+#ifndef Y2019_JEVOIS_SPI_H_
+#define Y2019_JEVOIS_SPI_H_
+
+#include <stdint.h>
+
+#include <array>
+
+#include "third_party/optional/tl/optional.hpp"
+#include "y2019/jevois/structures.h"
+
+// This file manages serializing and deserializing the various structures for
+// transport via SPI.
+//
+// Our SPI transfers are fixed-size to simplify everything.
+
+namespace frc971 {
+namespace jevois {
+
+constexpr size_t spi_transfer_size() {
+ // The teensy->RoboRIO side is way bigger, so just calculate that.
+ return 3 /* 3 frames */ *
+ (1 /* age */ + 3 /* targets */ * 4 /* target size */) +
+ 2 /* CRC-16 */;
+}
+static_assert(spi_transfer_size() == 41, "hand math is wrong");
+using SpiTransfer = std::array<char, spi_transfer_size()>;
+
+SpiTransfer SpiPackToRoborio(const TeensyToRoborio &message);
+tl::optional<TeensyToRoborio> SpiUnpackToRoborio(const SpiTransfer &transfer);
+
+} // namespace jevois
+} // namespace frc971
+
+#endif // Y2019_JEVOIS_SPI_H_
diff --git a/y2019/jevois/spi_test.cc b/y2019/jevois/spi_test.cc
new file mode 100644
index 0000000..de5158f
--- /dev/null
+++ b/y2019/jevois/spi_test.cc
@@ -0,0 +1,110 @@
+#include "y2019/jevois/spi.h"
+
+#include <stdint.h>
+
+#include "gtest/gtest.h"
+
+namespace frc971 {
+namespace jevois {
+namespace testing {
+
+// Tests packing and then unpacking an empty message.
+TEST(SpiToRoborioPackTest, Empty) {
+ TeensyToRoborio input_message;
+ const SpiTransfer transfer = SpiPackToRoborio(input_message);
+ const auto output_message = SpiUnpackToRoborio(transfer);
+ ASSERT_TRUE(output_message);
+ EXPECT_EQ(input_message, output_message.value());
+}
+
+// Tests that unpacking after the message has been modified results in a
+// checksum failure.
+TEST(SpiToRoborioPackTest, CorruptChecksum) {
+ TeensyToRoborio input_message;
+ {
+ SpiTransfer transfer = SpiPackToRoborio(input_message);
+ transfer[0]++;
+ ASSERT_FALSE(SpiUnpackToRoborio(transfer));
+ }
+ {
+ SpiTransfer transfer = SpiPackToRoborio(input_message);
+ transfer[0] ^= 0xFF;
+ ASSERT_FALSE(SpiUnpackToRoborio(transfer));
+ }
+ {
+ SpiTransfer transfer = SpiPackToRoborio(input_message);
+ transfer[transfer.size() - 1]++;
+ ASSERT_FALSE(SpiUnpackToRoborio(transfer));
+ }
+ input_message.frames.push_back({});
+ {
+ SpiTransfer transfer = SpiPackToRoborio(input_message);
+ transfer[0]++;
+ ASSERT_FALSE(SpiUnpackToRoborio(transfer));
+ }
+ {
+ SpiTransfer transfer = SpiPackToRoborio(input_message);
+ transfer[3]++;
+ ASSERT_FALSE(SpiUnpackToRoborio(transfer));
+ }
+ input_message.frames.back().targets.push_back({});
+ {
+ SpiTransfer transfer = SpiPackToRoborio(input_message);
+ transfer[3]++;
+ ASSERT_FALSE(SpiUnpackToRoborio(transfer));
+ }
+}
+
+// Tests packing and then unpacking a full message.
+TEST(SpiToRoborioPackTest, Full) {
+ TeensyToRoborio input_message;
+ input_message.frames.push_back({});
+ input_message.frames.back().age = camera_duration(9);
+ input_message.frames.push_back({});
+ input_message.frames.back().age = camera_duration(7);
+ input_message.frames.push_back({});
+ input_message.frames.back().age = camera_duration(1);
+
+ const SpiTransfer transfer = SpiPackToRoborio(input_message);
+ const auto output_message = SpiUnpackToRoborio(transfer);
+ ASSERT_TRUE(output_message);
+ EXPECT_EQ(input_message, output_message.value());
+}
+
+// Tests that packing and unpacking a target results in values close to before.
+TEST(SpiToRoborioPackTest, Target) {
+ TeensyToRoborio input_message;
+ input_message.frames.push_back({});
+ input_message.frames.back().targets.push_back({});
+ input_message.frames.back().targets.back().distance = 9;
+ input_message.frames.back().targets.back().height = 1;
+ input_message.frames.back().targets.back().heading = 0.5;
+ input_message.frames.back().targets.back().skew = -0.5;
+ input_message.frames.push_back({});
+ input_message.frames.back().targets.push_back({});
+ input_message.frames.back().targets.push_back({});
+ input_message.frames.push_back({});
+ input_message.frames.back().targets.push_back({});
+ input_message.frames.back().targets.push_back({});
+ input_message.frames.back().targets.push_back({});
+
+ const SpiTransfer transfer = SpiPackToRoborio(input_message);
+ const auto output_message = SpiUnpackToRoborio(transfer);
+ ASSERT_TRUE(output_message);
+ ASSERT_EQ(3u, output_message->frames.size());
+ ASSERT_EQ(1u, output_message->frames[0].targets.size());
+ ASSERT_EQ(2u, output_message->frames[1].targets.size());
+ ASSERT_EQ(3u, output_message->frames[2].targets.size());
+ EXPECT_NEAR(input_message.frames.back().targets.back().distance,
+ output_message->frames.back().targets.back().distance, 0.1);
+ EXPECT_NEAR(input_message.frames.back().targets.back().height,
+ output_message->frames.back().targets.back().height, 0.1);
+ EXPECT_NEAR(input_message.frames.back().targets.back().heading,
+ output_message->frames.back().targets.back().heading, 0.1);
+ EXPECT_NEAR(input_message.frames.back().targets.back().skew,
+ output_message->frames.back().targets.back().skew, 0.1);
+}
+
+} // namespace testing
+} // namespace jevois
+} // namespace frc971
diff --git a/y2019/jevois/structures.h b/y2019/jevois/structures.h
index d2ab3e5..c82a089 100644
--- a/y2019/jevois/structures.h
+++ b/y2019/jevois/structures.h
@@ -41,6 +41,25 @@
// just use floats and not worry about it.
struct Target {
+ bool operator==(const Target &other) const {
+ if (other.distance != distance) {
+ return false;
+ }
+ if (other.height != height) {
+ return false;
+ }
+ if (other.heading != heading) {
+ return false;
+ }
+ if (other.skew != skew) {
+ return false;
+ }
+ return true;
+ }
+ bool operator!=(const Target &other) const {
+ return !(*this == other);
+ }
+
// Distance to the target in meters. Specifically, the distance from the
// center of the camera's image plane to the center of the target.
float distance;
@@ -65,6 +84,19 @@
//
// This is all the information sent from each camera to the Teensy.
struct Frame {
+ bool operator==(const Frame &other) const {
+ if (other.targets != targets) {
+ return false;
+ }
+ if (other.age != age) {
+ return false;
+ }
+ return true;
+ }
+ bool operator!=(const Frame &other) const {
+ return !(*this == other);
+ }
+
// The top most interesting targets found in this frame.
aos::SizedArray<Target, 3> targets;
@@ -74,14 +106,34 @@
// This is all the information sent from the Teensy to each camera.
struct CameraCalibration {
+ bool operator==(const CameraCalibration &other) const {
+ if (other.calibration != calibration) {
+ return false;
+ }
+ return true;
+ }
+ bool operator!=(const CameraCalibration &other) const {
+ return !(*this == other);
+ }
+
// The calibration matrix. This defines where the camera is pointing.
//
- // TODO(Parker): What are the details on how this is defined.
+ // TODO(Parker): What are the details on how this is defined?
Eigen::Matrix<float, 3, 4> calibration;
};
// This is all the information the Teensy sends to the RoboRIO.
struct TeensyToRoborio {
+ bool operator==(const TeensyToRoborio &other) const {
+ if (other.frames != frames) {
+ return false;
+ }
+ return true;
+ }
+ bool operator!=(const TeensyToRoborio &other) const {
+ return !(*this == other);
+ }
+
// The newest frames received from up to three cameras. These will be the
// three earliest-received of all buffered frames.
aos::SizedArray<Frame, 3> frames;
@@ -89,6 +141,19 @@
// This is all the information the RoboRIO sends to the Teensy.
struct RoborioToTeensy {
+ bool operator==(const RoborioToTeensy &other) const {
+ if (other.beacon_brightness != beacon_brightness) {
+ return false;
+ }
+ if (other.light_rings != light_rings) {
+ return false;
+ }
+ return true;
+ }
+ bool operator!=(const RoborioToTeensy &other) const {
+ return !(*this == other);
+ }
+
// Brightnesses for each of the beacon light channels. 0 is off, 255 is fully
// on.
std::array<uint8_t, 3> beacon_brightness;