Add SPI serialization/deserialization for the cameras
Change-Id: Ide6b13de583a65907ff2b927a2a6fd1fc507a5b0
diff --git a/aos/containers/sized_array.h b/aos/containers/sized_array.h
index 0b8b447..0c13ecc 100644
--- a/aos/containers/sized_array.h
+++ b/aos/containers/sized_array.h
@@ -35,6 +35,21 @@
SizedArray &operator=(const SizedArray &) = default;
SizedArray &operator=(SizedArray &&) = default;
+ bool operator==(const SizedArray &other) const {
+ if (other.size() != size()) {
+ return false;
+ }
+ for (size_t i = 0; i < size(); ++i) {
+ if (other[i] != (*this)[i]) {
+ return false;
+ }
+ }
+ return true;
+ }
+ bool operator!=(const SizedArray &other) const {
+ return !(*this == other);
+ }
+
reference at(size_t i) {
check_index(i);
return array_.at(i);
diff --git a/aos/util/BUILD b/aos/util/BUILD
index 99ae87d..e712d54 100644
--- a/aos/util/BUILD
+++ b/aos/util/BUILD
@@ -1,5 +1,27 @@
package(default_visibility = ["//visibility:public"])
+cc_library(
+ name = "bitpacking",
+ hdrs = [
+ "bitpacking.h",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//third_party/GSL",
+ ],
+)
+
+cc_test(
+ name = "bitpacking_test",
+ srcs = [
+ "bitpacking_test.cc",
+ ],
+ deps = [
+ ":bitpacking",
+ "//aos/testing:googletest",
+ ],
+)
+
py_library(
name = "py_trapezoid_profile",
srcs = [
diff --git a/aos/util/bitpacking.h b/aos/util/bitpacking.h
new file mode 100644
index 0000000..df6ad16
--- /dev/null
+++ b/aos/util/bitpacking.h
@@ -0,0 +1,140 @@
+#ifndef AOS_UTIL_BITPACKING_H_
+#define AOS_UTIL_BITPACKING_H_
+
+#include <assert.h>
+
+#include <type_traits>
+
+#include "third_party/GSL/include/gsl/gsl"
+
+namespace aos {
+
+template <typename Integer>
+typename std::enable_if<std::is_unsigned<Integer>::value, Integer>::type
+MaskOnes(size_t bits) {
+ // Get these edge cases out of the way first, so we can subtract 1 from bits
+ // safely later without getting a negative number.
+ if (bits == 0) {
+ return 0;
+ }
+ if (bits == 1) {
+ return 1;
+ }
+ static constexpr Integer kOne = 1;
+ // Note that we shift at most by bits - 1. bits == sizeof(Integer) * 8 is
+ // valid, and shifting by the width of a type is undefined behavior, so we
+ // need to get a bit fancy to make it all work. Just ORing high_bit in
+ // explicitly at the end makes it work.
+ const Integer high_bit = kOne << (bits - 1);
+ return (high_bit - kOne) | high_bit;
+}
+
+template <typename Integer, size_t bits, size_t offset>
+typename std::enable_if<std::is_unsigned<Integer>::value &&
+ sizeof(Integer) * 8 >= bits>::type
+PackBits(const Integer value, const gsl::span<char> destination) {
+ assert(static_cast<size_t>(destination.size()) * 8u >= bits + offset);
+ size_t bits_completed = 0;
+ while (bits_completed < bits) {
+ // Which logical bit (through all the bytes) we're writing at.
+ const size_t output_bit = offset + bits_completed;
+ // The lowest-numbered bit in the current byte we're writing to.
+ const size_t output_min_bit = output_bit % 8;
+ // The number of bits we're writing in this byte.
+ const size_t new_bits = std::min(8 - output_min_bit, bits - bits_completed);
+ // The highest-numbered bit in the current byte we're writing to.
+ const size_t output_max_bit = output_min_bit + new_bits;
+ // The new bits to set in the this byte.
+ const uint8_t new_byte_part =
+ (value >> bits_completed) & MaskOnes<Integer>(new_bits);
+ // A mask of bits to keep from the current value of this byte. Start with
+ // just the low ones.
+ uint8_t existing_mask = MaskOnes<uint8_t>(output_min_bit);
+ // And then add in the high bits to keep.
+ existing_mask |= MaskOnes<uint8_t>(std::max<int>(8 - output_max_bit, 0))
+ << output_max_bit;
+ // The index of the byte we're writing to.
+ const size_t byte_index = output_bit / 8;
+ // The full new value of the current byte. Start with just the existing bits
+ // we're not touching.
+ uint8_t new_byte = destination[byte_index] & existing_mask;
+ // Add in the new part.
+ new_byte |= new_byte_part << output_min_bit;
+ destination[byte_index] = new_byte;
+ bits_completed += new_bits;
+ }
+ assert(bits_completed == bits);
+}
+
+template <typename Integer, size_t bits, size_t offset>
+typename std::enable_if<std::is_unsigned<Integer>::value &&
+ sizeof(Integer) * 8 >= bits, Integer>::type
+UnpackBits(const gsl::span<const char> source) {
+ Integer result = 0;
+ assert(static_cast<size_t>(source.size()) * 8u >= bits + offset);
+ size_t bits_completed = 0;
+ while (bits_completed < bits) {
+ // Which logical bit (through all the bytes) we're reading at.
+ const size_t input_bit = offset + bits_completed;
+ // The lowest-numbered bit in the current byte we're reading from.
+ const size_t input_min_bit = input_bit % 8;
+ // The number of bits we're reading in this byte.
+ const size_t new_bits = std::min(8 - input_min_bit, bits - bits_completed);
+ // The index of the byte we're reading from.
+ const size_t byte_index = input_bit / 8;
+ // The part of the current byte we're actually reading.
+ const uint8_t new_byte_part =
+ (source[byte_index] >> input_min_bit) & MaskOnes<Integer>(new_bits);
+ result |= static_cast<Integer>(new_byte_part) << bits_completed;
+ bits_completed += new_bits;
+ }
+ assert(bits_completed == bits);
+ return result;
+}
+
+template <int bits>
+uint32_t FloatToIntLinear(float min, float max, float value) {
+ static_assert(bits <= 31, "Only support 32-bit outputs for now");
+ static_assert(bits >= 1, "Bits must be positive");
+ // Start such that value in [0, 1) maps to [0, 2**bits) in the final
+ // result.
+ float result = (value - min) / (max - min);
+ // Multiply so that value is in [0, 2**bits).
+ // Make sure we do the shifting in a 32-bit integer, despite C++'s weird
+ // integer promotions, which is safe because bits is at most 31.
+ result *= static_cast<uint32_t>(UINT32_C(1) << bits);
+ if (result <= 0.0f) {
+ return 0;
+ }
+ const float max_result = MaskOnes<uint32_t>(bits);
+ if (result >= max_result) {
+ return max_result;
+ }
+ return static_cast<uint32_t>(result);
+}
+
+template <int bits>
+float IntToFloatLinear(float min, float max, uint32_t value) {
+ static_assert(bits <= 31, "Only support 32-bit outputs for now");
+ static_assert(bits >= 1, "Bits must be positive");
+ const float max_value = MaskOnes<uint32_t>(bits);
+ if (value > max_value) {
+ value = max_value;
+ }
+ // Start such that result in [0, 2**bits) maps to [min, max) in the final
+ // result.
+ float result = value;
+ // Offset by half a bit so we return a value in the middle of each one.
+ // This causes us to return the middle floating point value which could be
+ // represented by a given integer value.
+ result += 0.5f;
+ // Multiply so that result is in [0, 1).
+ // Make sure we do the shifting in a 32-bit integer, despite C++'s weird
+ // integer promotions, which is safe because bits is at most 31.
+ result *= 1.0f / static_cast<uint32_t>(UINT32_C(1) << bits);
+ return min + result * (max - min);
+}
+
+} // namespace aos
+
+#endif // AOS_UTIL_BITPACKING_H_
diff --git a/aos/util/bitpacking_test.cc b/aos/util/bitpacking_test.cc
new file mode 100644
index 0000000..013e911
--- /dev/null
+++ b/aos/util/bitpacking_test.cc
@@ -0,0 +1,387 @@
+#include "aos/util/bitpacking.h"
+
+#include <stdint.h>
+
+#include "gtest/gtest.h"
+
+namespace aos {
+namespace testing {
+
+// Tests MaskOnes with small arguments.
+TEST(MaskOnesTest, Small) {
+ EXPECT_EQ(0u, MaskOnes<uint8_t>(0));
+ EXPECT_EQ(0u, MaskOnes<uint64_t>(0));
+ EXPECT_EQ(1u, MaskOnes<uint8_t>(1));
+ EXPECT_EQ(1u, MaskOnes<uint64_t>(1));
+}
+
+// Tests MaskOnes with large arguments.
+TEST(MaskOnesTest, Large) {
+ EXPECT_EQ(0xFFu, MaskOnes<uint8_t>(8));
+ EXPECT_EQ(0x7Fu, MaskOnes<uint8_t>(7));
+
+ EXPECT_EQ(0xFFu, MaskOnes<uint64_t>(8));
+ EXPECT_EQ(UINT64_C(0xFFFFFFFFFFFFFFFF), MaskOnes<uint64_t>(64));
+ EXPECT_EQ(UINT64_C(0x7FFFFFFFFFFFFFFF), MaskOnes<uint64_t>(63));
+}
+
+// Tests some simple non-edge-case use cases for PackBits.
+TEST(PackBitsTest, Basic) {
+ {
+ std::array<char, 3> buffer{};
+ PackBits<uint8_t, 8, 0>(0, buffer);
+ EXPECT_EQ((std::array<char, 3>{}), buffer);
+ PackBits<uint8_t, 8, 0>(9, buffer);
+ EXPECT_EQ((std::array<char, 3>{{9, 0, 0}}), buffer);
+ PackBits<uint8_t, 8, 8>(7, buffer);
+ EXPECT_EQ((std::array<char, 3>{{9, 7, 0}}), buffer);
+ PackBits<uint8_t, 8, 16>(1, buffer);
+ EXPECT_EQ((std::array<char, 3>{{9, 7, 1}}), buffer);
+ }
+ {
+ std::array<char, 3> buffer{};
+ PackBits<uint16_t, 16, 0>(0xdead, buffer);
+ EXPECT_EQ((std::array<char, 3>{
+ {static_cast<char>(0xad), static_cast<char>(0xde), 0}}),
+ buffer);
+ }
+ {
+ std::array<char, 3> buffer{};
+ PackBits<uint8_t, 4, 0>(0xd7, buffer);
+ EXPECT_EQ((std::array<char, 3>{{0x7, 0, 0}}), buffer);
+ }
+ {
+ std::array<char, 3> buffer{};
+ PackBits<uint8_t, 4, 4>(0xd7, buffer);
+ EXPECT_EQ((std::array<char, 3>{{0x70, 0, 0}}), buffer);
+ }
+}
+
+// Verifies that PackBits puts bits in an order consistent with increasing
+// offsets.
+TEST(PackBitsTest, Consistency) {
+ {
+ std::array<char, 3> buffer1{};
+ PackBits<uint8_t, 8, 0>(0x80, buffer1);
+ std::array<char, 3> buffer2{};
+ PackBits<uint8_t, 1, 7>(0x1, buffer2);
+ EXPECT_EQ(buffer1, buffer2);
+ }
+ {
+ std::array<char, 1> buffer1{{static_cast<char>(0xFF)}};
+ PackBits<uint8_t, 8, 0>(0x7F, buffer1);
+ std::array<char, 1> buffer2{{static_cast<char>(0xFF)}};
+ PackBits<uint8_t, 1, 7>(0x0, buffer2);
+ EXPECT_EQ(buffer1, buffer2);
+ }
+ {
+ std::array<char, 1> buffer1{};
+ PackBits<uint8_t, 3, 5>(0x7, buffer1);
+ std::array<char, 1> buffer2{};
+ PackBits<uint8_t, 5, 3>(0x3C, buffer2);
+ EXPECT_EQ(buffer1, buffer2);
+ }
+ {
+ std::array<char, 1> buffer1{{static_cast<char>(0xFF)}};
+ PackBits<uint8_t, 3, 5>(0x0, buffer1);
+ std::array<char, 1> buffer2{{static_cast<char>(0xFF)}};
+ PackBits<uint8_t, 5, 3>(0x03, buffer2);
+ EXPECT_EQ(buffer1, buffer2);
+ }
+}
+
+// Tests some simple non-edge-case use cases for UnpackBits.
+TEST(UnpackBitsTest, Basic) {
+ {
+ std::array<char, 3> buffer{};
+ EXPECT_EQ(0u, (UnpackBits<uint8_t, 8, 0>(buffer)));
+ buffer = {{9, 0, 0}};
+ EXPECT_EQ(9u, (UnpackBits<uint8_t, 8, 0>(buffer)));
+ buffer = {{9, 7, 0}};
+ EXPECT_EQ(9u, (UnpackBits<uint8_t, 8, 0>(buffer)));
+ EXPECT_EQ(7u, (UnpackBits<uint8_t, 8, 8>(buffer)));
+ buffer = {{9, 7, 1}};
+ EXPECT_EQ(9u, (UnpackBits<uint8_t, 8, 0>(buffer)));
+ EXPECT_EQ(7u, (UnpackBits<uint8_t, 8, 8>(buffer)));
+ EXPECT_EQ(1u, (UnpackBits<uint8_t, 8, 16>(buffer)));
+ }
+ {
+ const std::array<char, 3> buffer = {
+ {static_cast<char>(0xad), static_cast<char>(0xde), 0}};
+ EXPECT_EQ(0xdead, (UnpackBits<uint16_t, 16, 0>(buffer)));
+ }
+ {
+ const std::array<char, 3> buffer = {{static_cast<char>(0xF7), 0, 0}};
+ EXPECT_EQ(7u, (UnpackBits<uint8_t, 4, 0>(buffer)));
+ }
+ {
+ const std::array<char, 3> buffer = {{static_cast<char>(0x7F), 0, 0}};
+ EXPECT_EQ(7u, (UnpackBits<uint8_t, 4, 4>(buffer)));
+ }
+}
+
+// Tests PackBits split across multiple bytes.
+TEST(PackBitsTest, AcrossBytes) {
+ {
+ std::array<char, 2> buffer{};
+ PackBits<uint8_t, 8, 7>(0xFF, buffer);
+ EXPECT_EQ((std::array<char, 2>{
+ {static_cast<char>(0x80), static_cast<char>(0x7F)}}),
+ buffer);
+ }
+ {
+ std::array<char, 2> buffer{};
+ PackBits<uint8_t, 8, 6>(0xFF, buffer);
+ EXPECT_EQ((std::array<char, 2>{
+ {static_cast<char>(0xC0), static_cast<char>(0x3F)}}),
+ buffer);
+ }
+ {
+ std::array<char, 2> buffer{};
+ PackBits<uint8_t, 8, 5>(0xFF, buffer);
+ EXPECT_EQ((std::array<char, 2>{
+ {static_cast<char>(0xE0), static_cast<char>(0x1F)}}),
+ buffer);
+ }
+ {
+ std::array<char, 2> buffer{};
+ PackBits<uint8_t, 8, 4>(0xFF, buffer);
+ EXPECT_EQ((std::array<char, 2>{
+ {static_cast<char>(0xF0), static_cast<char>(0x0F)}}),
+ buffer);
+ }
+ {
+ std::array<char, 2> buffer{};
+ PackBits<uint8_t, 8, 3>(0xFF, buffer);
+ EXPECT_EQ((std::array<char, 2>{
+ {static_cast<char>(0xF8), static_cast<char>(0x07)}}),
+ buffer);
+ }
+ {
+ std::array<char, 2> buffer{};
+ PackBits<uint8_t, 8, 2>(0xFF, buffer);
+ EXPECT_EQ((std::array<char, 2>{
+ {static_cast<char>(0xFC), static_cast<char>(0x03)}}),
+ buffer);
+ }
+ {
+ std::array<char, 2> buffer{};
+ PackBits<uint8_t, 8, 1>(0xFF, buffer);
+ EXPECT_EQ((std::array<char, 2>{
+ {static_cast<char>(0xFE), static_cast<char>(0x01)}}),
+ buffer);
+ }
+}
+
+// Tests UnpackBits split across multiple bytes.
+TEST(UnpackBitsTest, AcrossBytes) {
+ {
+ const std::array<char, 2> buffer = {
+ {static_cast<char>(0x80), static_cast<char>(0x7F)}};
+ EXPECT_EQ(0xFFu, (UnpackBits<uint8_t, 8, 7>(buffer)));
+ }
+ {
+ const std::array<char, 2> buffer = {
+ {static_cast<char>(0xC0), static_cast<char>(0x3F)}};
+ EXPECT_EQ(0xFFu, (UnpackBits<uint8_t, 8, 6>(buffer)));
+ }
+ {
+ const std::array<char, 2> buffer = {
+ {static_cast<char>(0xE0), static_cast<char>(0x1F)}};
+ EXPECT_EQ(0xFFu, (UnpackBits<uint8_t, 8, 5>(buffer)));
+ }
+ {
+ const std::array<char, 2> buffer = {
+ {static_cast<char>(0xF0), static_cast<char>(0x0F)}};
+ EXPECT_EQ(0xFFu, (UnpackBits<uint8_t, 8, 4>(buffer)));
+ }
+ {
+ const std::array<char, 2> buffer = {
+ {static_cast<char>(0xF8), static_cast<char>(0x07)}};
+ EXPECT_EQ(0xFFu, (UnpackBits<uint8_t, 8, 3>(buffer)));
+ }
+ {
+ const std::array<char, 2> buffer = {
+ {static_cast<char>(0xFC), static_cast<char>(0x03)}};
+ EXPECT_EQ(0xFFu, (UnpackBits<uint8_t, 8, 2>(buffer)));
+ }
+ {
+ const std::array<char, 2> buffer = {
+ {static_cast<char>(0xFE), static_cast<char>(0x01)}};
+ EXPECT_EQ(0xFFu, (UnpackBits<uint8_t, 8, 1>(buffer)));
+ }
+}
+
+// Verifies that PackBits avoids touching adjacent bits.
+TEST(PackBitsTest, AdjacentBits) {
+ {
+ std::array<char, 2> buffer{
+ {static_cast<char>(0xFF), static_cast<char>(0xFF)}};
+ PackBits<uint8_t, 1, 0>(0, buffer);
+ EXPECT_EQ((std::array<char, 2>{
+ {static_cast<char>(0xFE), static_cast<char>(0xFF)}}),
+ buffer);
+ }
+ {
+ std::array<char, 2> buffer{
+ {static_cast<char>(0xFF), static_cast<char>(0xFF)}};
+ PackBits<uint8_t, 7, 0>(0, buffer);
+ EXPECT_EQ((std::array<char, 2>{
+ {static_cast<char>(0x80), static_cast<char>(0xFF)}}),
+ buffer);
+ }
+ {
+ std::array<char, 2> buffer{
+ {static_cast<char>(0xFF), static_cast<char>(0xFF)}};
+ PackBits<uint8_t, 8, 0>(0, buffer);
+ EXPECT_EQ((std::array<char, 2>{
+ {static_cast<char>(0x00), static_cast<char>(0xFF)}}),
+ buffer);
+ }
+ {
+ std::array<char, 2> buffer{
+ {static_cast<char>(0xFF), static_cast<char>(0xFF)}};
+ PackBits<uint16_t, 9, 0>(0, buffer);
+ EXPECT_EQ((std::array<char, 2>{
+ {static_cast<char>(0x00), static_cast<char>(0xFE)}}),
+ buffer);
+ }
+ {
+ std::array<char, 2> buffer{
+ {static_cast<char>(0xFF), static_cast<char>(0xFF)}};
+ PackBits<uint16_t, 14, 0>(0, buffer);
+ EXPECT_EQ((std::array<char, 2>{
+ {static_cast<char>(0x00), static_cast<char>(0xC0)}}),
+ buffer);
+ }
+ {
+ std::array<char, 2> buffer{
+ {static_cast<char>(0xFF), static_cast<char>(0xFF)}};
+ PackBits<uint16_t, 15, 0>(0, buffer);
+ EXPECT_EQ((std::array<char, 2>{
+ {static_cast<char>(0x00), static_cast<char>(0x80)}}),
+ buffer);
+ }
+ {
+ std::array<char, 2> buffer{
+ {static_cast<char>(0xFF), static_cast<char>(0xFF)}};
+ PackBits<uint16_t, 15, 1>(0, buffer);
+ EXPECT_EQ((std::array<char, 2>{
+ {static_cast<char>(0x01), static_cast<char>(0x00)}}),
+ buffer);
+ }
+ {
+ std::array<char, 2> buffer{
+ {static_cast<char>(0xFF), static_cast<char>(0xFF)}};
+ PackBits<uint16_t, 6, 8>(0, buffer);
+ EXPECT_EQ((std::array<char, 2>{
+ {static_cast<char>(0xFF), static_cast<char>(0xC0)}}),
+ buffer);
+ }
+ {
+ std::array<char, 4> buffer{
+ {static_cast<char>(0xFF), static_cast<char>(0xFF),
+ static_cast<char>(0xFF), static_cast<char>(0xFF)}};
+ PackBits<uint16_t, 6, 24>(0, buffer);
+ EXPECT_EQ((std::array<char, 4>{
+ {static_cast<char>(0xFF), static_cast<char>(0xFF),
+ static_cast<char>(0xFF), static_cast<char>(0xC0)}}),
+ buffer);
+ }
+}
+
+// Tests FloatToIntLinear with values near or outside of its boundaries.
+TEST(FloatToIntLinearTest, OutOfBounds) {
+ EXPECT_EQ(0u, (FloatToIntLinear<1>(0.0f, 1.0f, 0.0f)));
+ EXPECT_EQ(0u, (FloatToIntLinear<1>(0.0f, 1.0f, -0.1f)));
+ EXPECT_EQ(0u, (FloatToIntLinear<1>(0.0f, 1.0f, -1.0f)));
+ EXPECT_EQ(1u, (FloatToIntLinear<1>(0.0f, 1.0f, 1.0f)));
+ EXPECT_EQ(1u, (FloatToIntLinear<1>(0.0f, 1.0f, 1.1f)));
+ EXPECT_EQ(1u, (FloatToIntLinear<1>(0.0f, 1.0f, 2.0f)));
+
+ EXPECT_EQ(0u, (FloatToIntLinear<1>(0.0f, 4.0f, 0.0f)));
+ EXPECT_EQ(1u, (FloatToIntLinear<1>(0.0f, 4.0f, 4.0f)));
+ EXPECT_EQ(1u, (FloatToIntLinear<1>(0.0f, 4.0f, 10.0f)));
+
+ EXPECT_EQ(0u, (FloatToIntLinear<3>(0.0f, 4.0f, 0.0f)));
+ EXPECT_EQ(0u, (FloatToIntLinear<3>(0.0f, 4.0f, -100.0f)));
+ EXPECT_EQ(7u, (FloatToIntLinear<3>(0.0f, 4.0f, 4.0f)));
+ EXPECT_EQ(7u, (FloatToIntLinear<3>(0.0f, 4.0f, 4.01f)));
+
+ EXPECT_EQ(0u, (FloatToIntLinear<3>(-3.0f, 5.0f, -3.0f)));
+ EXPECT_EQ(0u, (FloatToIntLinear<3>(-3.0f, 5.0f, -3.1f)));
+ EXPECT_EQ(7u, (FloatToIntLinear<3>(-3.0f, 5.0f, 5.0f)));
+ EXPECT_EQ(7u, (FloatToIntLinear<3>(-3.0f, 5.0f, 5.1f)));
+}
+
+// Tests that FloatToIntLinear rounds correctly at the boundaries between output
+// values.
+TEST(FloatToIntLinearTest, Rounding) {
+ EXPECT_EQ(0u, (FloatToIntLinear<1>(0.0f, 1.0f, 0.49f)));
+ EXPECT_EQ(1u, (FloatToIntLinear<1>(0.0f, 1.0f, 0.51f)));
+ EXPECT_EQ(1u, (FloatToIntLinear<1>(-1.0f, 0.0f, -0.49f)));
+ EXPECT_EQ(0u, (FloatToIntLinear<1>(-1.0f, 0.0f, -0.51f)));
+ EXPECT_EQ(1u, (FloatToIntLinear<1>(-1.0f, 1.0f, 0.01f)));
+ EXPECT_EQ(0u, (FloatToIntLinear<1>(-1.0f, 1.0f, -0.01f)));
+
+ EXPECT_EQ(0u, (FloatToIntLinear<3>(0.0f, 1.0f, 0.124f)));
+ EXPECT_EQ(1u, (FloatToIntLinear<3>(0.0f, 1.0f, 0.126f)));
+ EXPECT_EQ(1u, (FloatToIntLinear<3>(0.0f, 1.0f, 0.249f)));
+ EXPECT_EQ(2u, (FloatToIntLinear<3>(0.0f, 1.0f, 0.251f)));
+ EXPECT_EQ(2u, (FloatToIntLinear<3>(0.0f, 1.0f, 0.374f)));
+ EXPECT_EQ(3u, (FloatToIntLinear<3>(0.0f, 1.0f, 0.376f)));
+ EXPECT_EQ(3u, (FloatToIntLinear<3>(0.0f, 1.0f, 0.499f)));
+ EXPECT_EQ(4u, (FloatToIntLinear<3>(0.0f, 1.0f, 0.501f)));
+ EXPECT_EQ(4u, (FloatToIntLinear<3>(0.0f, 1.0f, 0.624f)));
+ EXPECT_EQ(5u, (FloatToIntLinear<3>(0.0f, 1.0f, 0.626f)));
+ EXPECT_EQ(5u, (FloatToIntLinear<3>(0.0f, 1.0f, 0.749f)));
+ EXPECT_EQ(6u, (FloatToIntLinear<3>(0.0f, 1.0f, 0.751f)));
+ EXPECT_EQ(6u, (FloatToIntLinear<3>(0.0f, 1.0f, 0.874f)));
+ EXPECT_EQ(7u, (FloatToIntLinear<3>(0.0f, 1.0f, 0.876f)));
+}
+
+// Tests IntToFloatLinear with values near or outside of its boundaries.
+TEST(IntToFloatLinearTest, OutOfBounds) {
+ EXPECT_EQ(0.25f, (IntToFloatLinear<1>(0.0f, 1.0f, 0)));
+ EXPECT_EQ(0.75f, (IntToFloatLinear<1>(0.0f, 1.0f, 1)));
+ EXPECT_EQ(0.75f, (IntToFloatLinear<1>(0.0f, 1.0f, 2)));
+ EXPECT_EQ(0.75f, (IntToFloatLinear<1>(0.0f, 1.0f, 3)));
+
+ EXPECT_EQ(1.0f, (IntToFloatLinear<1>(0.0f, 4.0f, 0)));
+ EXPECT_EQ(3.0f, (IntToFloatLinear<1>(0.0f, 4.0f, 1)));
+
+ EXPECT_EQ(0.0625f, (IntToFloatLinear<3>(0.0f, 1.0f, 0)));
+ EXPECT_EQ(0.9375f, (IntToFloatLinear<3>(0.0f, 1.0f, 7)));
+ EXPECT_EQ(0.9375f, (IntToFloatLinear<3>(0.0f, 1.0f, 8)));
+}
+
+// Tests IntToFloatLinear with some specific values which are easy to calculate
+// by hand.
+TEST(IntToFloatLinearTest, Values) {
+ EXPECT_EQ(0.125f, (IntToFloatLinear<2>(0.0f, 1.0f, 0)));
+ EXPECT_EQ(0.375f, (IntToFloatLinear<2>(0.0f, 1.0f, 1)));
+ EXPECT_EQ(0.625f, (IntToFloatLinear<2>(0.0f, 1.0f, 2)));
+ EXPECT_EQ(0.875f, (IntToFloatLinear<2>(0.0f, 1.0f, 3)));
+
+ EXPECT_EQ(0.0625f, (IntToFloatLinear<3>(0.0f, 1.0f, 0)));
+ EXPECT_EQ(0.1875f, (IntToFloatLinear<3>(0.0f, 1.0f, 1)));
+ EXPECT_EQ(0.3125f, (IntToFloatLinear<3>(0.0f, 1.0f, 2)));
+ EXPECT_EQ(0.4375f, (IntToFloatLinear<3>(0.0f, 1.0f, 3)));
+ EXPECT_EQ(0.5625f, (IntToFloatLinear<3>(0.0f, 1.0f, 4)));
+ EXPECT_EQ(0.6875f, (IntToFloatLinear<3>(0.0f, 1.0f, 5)));
+ EXPECT_EQ(0.8125f, (IntToFloatLinear<3>(0.0f, 1.0f, 6)));
+ EXPECT_EQ(0.9375f, (IntToFloatLinear<3>(0.0f, 1.0f, 7)));
+
+ EXPECT_EQ(-0.875f, (IntToFloatLinear<2>(-1.0f, 0.0f, 0)));
+ EXPECT_EQ(-0.625f, (IntToFloatLinear<2>(-1.0f, 0.0f, 1)));
+ EXPECT_EQ(-0.375f, (IntToFloatLinear<2>(-1.0f, 0.0f, 2)));
+ EXPECT_EQ(-0.125f, (IntToFloatLinear<2>(-1.0f, 0.0f, 3)));
+
+ EXPECT_EQ(-0.75f, (IntToFloatLinear<2>(-1.0f, 1.0f, 0)));
+ EXPECT_EQ(-0.25f, (IntToFloatLinear<2>(-1.0f, 1.0f, 1)));
+ EXPECT_EQ(0.25f, (IntToFloatLinear<2>(-1.0f, 1.0f, 2)));
+ EXPECT_EQ(0.75f, (IntToFloatLinear<2>(-1.0f, 1.0f, 3)));
+}
+
+} // namespace testing
+} // namespace aos
diff --git a/third_party/pycrc/BUILD b/third_party/pycrc/BUILD
new file mode 100644
index 0000000..50a21e1
--- /dev/null
+++ b/third_party/pycrc/BUILD
@@ -0,0 +1,25 @@
+licenses(["notice"])
+
+genrule(
+ name = "copy_main",
+ srcs = [
+ "pycrc.py",
+ ],
+ outs = [
+ "pycrc_main.py",
+ ],
+ cmd = "cp $< $@",
+)
+
+py_binary(
+ name = "pycrc_main",
+ srcs = glob([
+ "pycrc/*.py",
+ ]) + [
+ "pycrc_main.py",
+ ],
+ imports = ["."],
+ legacy_create_init = False,
+ main = "pycrc_main.py",
+ visibility = ["//visibility:public"],
+)
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;