Merge "Add StaticZeroingSingleDOFProfiledSubsystem"
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/frc971/control_loops/drivetrain/trajectory.cc b/frc971/control_loops/drivetrain/trajectory.cc
index 64b4612..c9582e6 100644
--- a/frc971/control_loops/drivetrain/trajectory.cc
+++ b/frc971/control_loops/drivetrain/trajectory.cc
@@ -362,31 +362,32 @@
return result;
}
-::std::vector<::Eigen::Matrix<double, 3, 1>> Trajectory::PlanXVA(
- ::std::chrono::nanoseconds dt) {
+::Eigen::Matrix<double, 3, 1> Trajectory::GetNextXVA(
+ ::std::chrono::nanoseconds dt, ::Eigen::Matrix<double, 2, 1> *state) {
double dt_float =
::std::chrono::duration_cast<::std::chrono::duration<double>>(dt).count();
- double t = 0.0;
- ::Eigen::Matrix<double, 2, 1> state = ::Eigen::Matrix<double, 2, 1>::Zero();
+ // TODO(austin): This feels like something that should be pulled out into
+ // a library for re-use.
+ *state = RungeKutta([this](const ::Eigen::Matrix<double, 2, 1> x) {
+ ::Eigen::Matrix<double, 3, 1> xva = FFAcceleration(x(0));
+ return (::Eigen::Matrix<double, 2, 1>() << x(1), xva(2)).finished();
+ }, *state, dt_float);
+
+ ::Eigen::Matrix<double, 3, 1> result = FFAcceleration((*state)(0));
+ (*state)(1) = result(1);
+ return result;
+}
+
+::std::vector<::Eigen::Matrix<double, 3, 1>> Trajectory::PlanXVA(
+ ::std::chrono::nanoseconds dt) {
+ ::Eigen::Matrix<double, 2, 1> state = ::Eigen::Matrix<double, 2, 1>::Zero();
::std::vector<::Eigen::Matrix<double, 3, 1>> result;
result.emplace_back(FFAcceleration(0));
result.back()(1) = 0.0;
- while (state(0) < length() - 1e-4) {
- t += dt_float;
-
- // TODO(austin): This feels like something that should be pulled out into
- // a library for re-use.
- state = RungeKutta(
- [this](const ::Eigen::Matrix<double, 2, 1> x) {
- ::Eigen::Matrix<double, 3, 1> xva = FFAcceleration(x(0));
- return (::Eigen::Matrix<double, 2, 1>() << x(1), xva(2)).finished();
- },
- state, dt_float);
-
- result.emplace_back(FFAcceleration(state(0)));
- state(1) = result.back()(1);
+ while (!is_at_end(state)) {
+ result.emplace_back(GetNextXVA(dt, &state));
}
return result;
}
diff --git a/frc971/control_loops/drivetrain/trajectory.h b/frc971/control_loops/drivetrain/trajectory.h
index d3468b3..66dcba1 100644
--- a/frc971/control_loops/drivetrain/trajectory.h
+++ b/frc971/control_loops/drivetrain/trajectory.h
@@ -98,6 +98,11 @@
// Returns the feed forwards voltage for an explicit distance.
::Eigen::Matrix<double, 2, 1> FFVoltage(double distance);
+ // Returns whether a state represents a state at the end of the spline.
+ bool is_at_end(::Eigen::Matrix<double, 2, 1> state) const {
+ return state(0) > length() - 1e-4;
+ }
+
// Returns the length of the path in meters.
double length() const { return spline_->length(); }
@@ -151,6 +156,10 @@
const ::Eigen::DiagonalMatrix<double, 5> &Q,
const ::Eigen::DiagonalMatrix<double, 2> &R) const;
+ // Return the next position, velocity, acceleration based on the current
+ // state. Updates the passed in state for the next iteration.
+ ::Eigen::Matrix<double, 3, 1> GetNextXVA(
+ ::std::chrono::nanoseconds dt, ::Eigen::Matrix<double, 2, 1> *state);
::std::vector<::Eigen::Matrix<double, 3, 1>> PlanXVA(
::std::chrono::nanoseconds dt);
diff --git a/frc971/control_loops/drivetrain/trajectory_test.cc b/frc971/control_loops/drivetrain/trajectory_test.cc
index eb1664a..f65adf1 100644
--- a/frc971/control_loops/drivetrain/trajectory_test.cc
+++ b/frc971/control_loops/drivetrain/trajectory_test.cc
@@ -265,6 +265,15 @@
1.2e-2);
}
+// Tests that Iteratively computing the XVA plan is the same as precomputing it.
+TEST_P(ParameterizedSplineTest, IterativeXVA) {
+ ::Eigen::Matrix<double, 2, 1> state = ::Eigen::Matrix<double, 2, 1>::Zero();
+ for (size_t i = 1; i < length_plan_xva_.size(); ++i) {
+ ::Eigen::Matrix<double, 3, 1> xva = trajectory_->GetNextXVA(kDt, &state);
+ EXPECT_LT((length_plan_xva_[i] - xva).norm(), 1e-2);
+ }
+}
+
SplineTestParams MakeSplineTestParams(struct SplineTestParams params) {
return params;
}
diff --git a/frc971/wpilib/sensor_reader.h b/frc971/wpilib/sensor_reader.h
index c843735..be96eea 100644
--- a/frc971/wpilib/sensor_reader.h
+++ b/frc971/wpilib/sensor_reader.h
@@ -141,6 +141,27 @@
encoder_ratio * (2.0 * M_PI);
}
+ void CopyPosition(
+ const ::frc971::wpilib::DMAEncoderAndPotentiometer &encoder,
+ ::frc971::PotAndIndexPosition *position,
+ ::std::function<double(int32_t)> encoder_translate,
+ ::std::function<double(double)> potentiometer_translate, bool reverse,
+ double pot_offset) {
+ const double multiplier = reverse ? -1.0 : 1.0;
+ position->encoder =
+ multiplier * encoder_translate(encoder.polled_encoder_value());
+ position->pot = multiplier * potentiometer_translate(
+ encoder.polled_potentiometer_voltage()) +
+ pot_offset;
+ position->latched_encoder =
+ multiplier * encoder_translate(encoder.last_encoder_value());
+ position->latched_pot =
+ multiplier *
+ potentiometer_translate(encoder.last_potentiometer_voltage()) +
+ pot_offset;
+ position->index_pulses = encoder.index_posedge_count();
+ }
+
double encoder_translate(int32_t value, double counts_per_revolution,
double ratio) {
return static_cast<double>(value) / counts_per_revolution * ratio *
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/y2016/wpilib_interface.cc b/y2016/wpilib_interface.cc
index ee4b897..787d9a5 100644
--- a/y2016/wpilib_interface.cc
+++ b/y2016/wpilib_interface.cc
@@ -1,13 +1,13 @@
+#include <inttypes.h>
#include <stdio.h>
#include <string.h>
#include <unistd.h>
-#include <inttypes.h>
-#include <chrono>
-#include <thread>
-#include <mutex>
-#include <functional>
#include <array>
+#include <chrono>
+#include <functional>
+#include <mutex>
+#include <thread>
#include "frc971/wpilib/ahal/AnalogInput.h"
#include "frc971/wpilib/ahal/Compressor.h"
@@ -234,7 +234,6 @@
autonomous_modes_.at(i) = ::std::move(sensor);
}
-
// All of the DMA-related set_* calls must be made before this, and it doesn't
// hurt to do all of them.
@@ -278,16 +277,15 @@
{
auto superstructure_message = superstructure_queue.position.MakeMessage();
- CopyPotAndIndexPosition(intake_encoder_, &superstructure_message->intake,
- intake_translate, intake_pot_translate, false,
- values.intake.pot_offset);
- CopyPotAndIndexPosition(shoulder_encoder_,
- &superstructure_message->shoulder,
- shoulder_translate, shoulder_pot_translate, false,
- values.shoulder.pot_offset);
- CopyPotAndIndexPosition(wrist_encoder_, &superstructure_message->wrist,
- wrist_translate, wrist_pot_translate, true,
- values.wrist.pot_offset);
+ CopyPosition(intake_encoder_, &superstructure_message->intake,
+ intake_translate, intake_pot_translate, false,
+ values.intake.pot_offset);
+ CopyPosition(shoulder_encoder_, &superstructure_message->shoulder,
+ shoulder_translate, shoulder_pot_translate, false,
+ values.shoulder.pot_offset);
+ CopyPosition(wrist_encoder_, &superstructure_message->wrist,
+ wrist_translate, wrist_pot_translate, true,
+ values.wrist.pot_offset);
superstructure_message.Send();
}
@@ -314,27 +312,6 @@
}
private:
- void CopyPotAndIndexPosition(
- const ::frc971::wpilib::DMAEncoderAndPotentiometer &encoder,
- ::frc971::PotAndIndexPosition *position,
- ::std::function<double(int32_t)> encoder_translate,
- ::std::function<double(double)> potentiometer_translate, bool reverse,
- double pot_offset) {
- const double multiplier = reverse ? -1.0 : 1.0;
- position->encoder =
- multiplier * encoder_translate(encoder.polled_encoder_value());
- position->pot = multiplier * potentiometer_translate(
- encoder.polled_potentiometer_voltage()) +
- pot_offset;
- position->latched_encoder =
- multiplier * encoder_translate(encoder.last_encoder_value());
- position->latched_pot =
- multiplier *
- potentiometer_translate(encoder.last_potentiometer_voltage()) +
- pot_offset;
- position->index_pulses = encoder.index_posedge_count();
- }
-
::std::unique_ptr<AnalogInput> drivetrain_left_hall_, drivetrain_right_hall_;
::std::unique_ptr<Encoder> shooter_left_encoder_, shooter_right_encoder_;
@@ -353,9 +330,7 @@
: pcm_(pcm),
drivetrain_(".frc971.control_loops.drivetrain_queue.output"),
shooter_(".y2016.control_loops.shooter.shooter_queue.output"),
- superstructure_(
- ".y2016.control_loops.superstructure_queue.output") {
- }
+ superstructure_(".y2016.control_loops.superstructure_queue.output") {}
void set_compressor(::std::unique_ptr<Compressor> compressor) {
compressor_ = ::std::move(compressor);
@@ -371,8 +346,7 @@
climber_trigger_ = ::std::move(s);
}
- void set_traverse(
- ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> s) {
+ void set_traverse(::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> s) {
traverse_ = ::std::move(s);
}
@@ -391,8 +365,7 @@
shooter_pusher_ = ::std::move(s);
}
- void set_lights(
- ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> s) {
+ void set_lights(::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> s) {
lights_ = ::std::move(s);
}
@@ -462,9 +435,7 @@
{
::frc971::wpilib::PneumaticsToLog to_log;
- {
- to_log.compressor_on = compressor_->Enabled();
- }
+ { to_log.compressor_on = compressor_->Enabled(); }
pcm_->Flush();
to_log.read_solenoids = pcm_->GetAll();
@@ -486,8 +457,7 @@
::aos::Queue<::frc971::control_loops::DrivetrainQueue::Output> drivetrain_;
::aos::Queue<::y2016::control_loops::shooter::ShooterQueue::Output> shooter_;
- ::aos::Queue<
- ::y2016::control_loops::SuperstructureQueue::Output>
+ ::aos::Queue<::y2016::control_loops::SuperstructureQueue::Output>
superstructure_;
::std::atomic<bool> run_{true};
diff --git a/y2019/BUILD b/y2019/BUILD
index ad35ead..2eb5e24 100644
--- a/y2019/BUILD
+++ b/y2019/BUILD
@@ -1,3 +1,13 @@
+load("//frc971:downloader.bzl", "robot_downloader")
+
+robot_downloader(
+ start_binaries = [
+ ":joystick_reader",
+ ":wpilib_interface",
+ "//y2019/control_loops/drivetrain:drivetrain",
+ ],
+)
+
cc_library(
name = "constants",
srcs = [
@@ -55,7 +65,8 @@
"//third_party/Phoenix-frc-lib:phoenix",
],
)
-cc_library(
+
+cc_binary(
name = "joystick_reader",
srcs = [
":joystick_reader.cc",
@@ -63,9 +74,9 @@
deps = [
"//aos:init",
"//aos/actions:action_lib",
+ "//aos/input:action_joystick_input",
"//aos/input:drivetrain_input",
"//aos/input:joystick_input",
- "//aos/input:action_joystick_input",
"//aos/logging",
"//aos/network:team_number",
"//aos/stl_mutex",
diff --git a/y2019/constants.h b/y2019/constants.h
index e7c178b..e68143c 100644
--- a/y2019/constants.h
+++ b/y2019/constants.h
@@ -1,8 +1,8 @@
#ifndef Y2019_CONSTANTS_H_
#define Y2019_CONSTANTS_H_
-#include <stdint.h>
#include <math.h>
+#include <stdint.h>
#include "frc971/constants.h"
#include "y2019/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
@@ -28,7 +28,7 @@
return kDrivetrainCyclesPerRevolution() * 4;
}
static constexpr double kDrivetrainEncoderRatio() {
- return (52.0 / 24.0);
+ return (24.0 / 52.0);
}
static constexpr double kMaxDrivetrainEncoderPulsesPerSecond() {
return control_loops::drivetrain::kFreeSpeed / (2.0 * M_PI) *
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;
diff --git a/y2019/wpilib_interface.cc b/y2019/wpilib_interface.cc
index c696a37..bc6c25f 100644
--- a/y2019/wpilib_interface.cc
+++ b/y2019/wpilib_interface.cc
@@ -169,9 +169,9 @@
::frc971::wpilib::DrivetrainWriter drivetrain_writer;
drivetrain_writer.set_left_controller0(
- ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(2)), false);
+ ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(0)), true);
drivetrain_writer.set_right_controller0(
- ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(3)), false);
+ ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(1)), false);
::std::thread drivetrain_writer_thread(::std::ref(drivetrain_writer));
// Wait forever. Not much else to do...