Add support for serializing and deserializing the other SPI messages
Change-Id: I7d39e286f72aff0d773c948d34d1dc2354dd2c34
diff --git a/y2019/jevois/spi.cc b/y2019/jevois/spi.cc
index 28df8cf..3223742 100644
--- a/y2019/jevois/spi.cc
+++ b/y2019/jevois/spi.cc
@@ -2,7 +2,6 @@
#include "aos/logging/logging.h"
#include "aos/util/bitpacking.h"
-#include "third_party/GSL/include/gsl/gsl"
#include "y2019/jevois/jevois_crc.h"
// SPI transfer format (6x 8 bit frames):
@@ -182,7 +181,8 @@
return transfer;
}
-tl::optional<TeensyToRoborio> SpiUnpackToRoborio(const SpiTransfer &transfer) {
+tl::optional<TeensyToRoborio> SpiUnpackToRoborio(
+ gsl::span<const char, spi_transfer_size()> transfer) {
TeensyToRoborio message;
gsl::span<const char> remaining_input = transfer;
for (int frame = 0; frame < 3; ++frame) {
@@ -232,5 +232,66 @@
return message;
}
+SpiTransfer SpiPackToTeensy(const RoborioToTeensy &message) {
+ SpiTransfer transfer;
+ gsl::span<char> remaining_space = transfer;
+ for (size_t i = 0; i < message.beacon_brightness.size(); ++i) {
+ remaining_space[0] = message.beacon_brightness[i];
+ remaining_space = remaining_space.subspan(1);
+ }
+ remaining_space[0] = message.light_rings.to_ulong() & 0xFF;
+ remaining_space = remaining_space.subspan(1);
+ {
+ const int64_t realtime_now =
+ message.realtime_now.time_since_epoch().count();
+ memcpy(remaining_space.data(), &realtime_now, sizeof(realtime_now));
+ remaining_space = remaining_space.subspan(sizeof(realtime_now));
+ }
+ {
+ uint16_t crc = jevois_crc_init();
+ crc = jevois_crc_update(crc, transfer.data(),
+ transfer.size() - remaining_space.size());
+ crc = jevois_crc_finalize(crc);
+ CHECK_GE(static_cast<size_t>(remaining_space.size()), sizeof(crc));
+ memcpy(&remaining_space[0], &crc, sizeof(crc));
+ remaining_space = remaining_space.subspan(sizeof(crc));
+ }
+ return transfer;
+}
+
+tl::optional<RoborioToTeensy> SpiUnpackToTeensy(
+ gsl::span<const char, spi_transfer_size()> transfer) {
+ RoborioToTeensy message;
+ gsl::span<const char> remaining_input = transfer;
+ for (size_t i = 0; i < message.beacon_brightness.size(); ++i) {
+ message.beacon_brightness[i] = remaining_input[0];
+ remaining_input = remaining_input.subspan(1);
+ }
+ message.light_rings = remaining_input[0];
+ remaining_input = remaining_input.subspan(1);
+ {
+ int64_t realtime_now;
+ memcpy(&realtime_now, remaining_input.data(), sizeof(realtime_now));
+ message.realtime_now = aos::realtime_clock::time_point(
+ aos::realtime_clock::duration(realtime_now));
+ remaining_input = remaining_input.subspan(sizeof(realtime_now));
+ }
+ {
+ uint16_t calculated_crc = jevois_crc_init();
+ calculated_crc =
+ jevois_crc_update(calculated_crc, transfer.data(),
+ transfer.size() - remaining_input.size());
+ calculated_crc = jevois_crc_finalize(calculated_crc);
+ uint16_t received_crc;
+ CHECK_GE(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;
+ }
+ }
+ return message;
+}
+
} // namespace jevois
} // namespace frc971
diff --git a/y2019/jevois/spi.h b/y2019/jevois/spi.h
index e0c4d90..7c14a57 100644
--- a/y2019/jevois/spi.h
+++ b/y2019/jevois/spi.h
@@ -5,6 +5,7 @@
#include <array>
+#include "third_party/GSL/include/gsl/gsl"
#include "third_party/optional/tl/optional.hpp"
#include "y2019/jevois/structures.h"
@@ -26,7 +27,11 @@
using SpiTransfer = std::array<char, spi_transfer_size()>;
SpiTransfer SpiPackToRoborio(const TeensyToRoborio &message);
-tl::optional<TeensyToRoborio> SpiUnpackToRoborio(const SpiTransfer &transfer);
+tl::optional<TeensyToRoborio> SpiUnpackToRoborio(
+ gsl::span<const char, spi_transfer_size()> transfer);
+SpiTransfer SpiPackToTeensy(const RoborioToTeensy &message);
+tl::optional<RoborioToTeensy> SpiUnpackToTeensy(
+ gsl::span<const char, spi_transfer_size()> transfer);
} // namespace jevois
} // namespace frc971
diff --git a/y2019/jevois/spi_test.cc b/y2019/jevois/spi_test.cc
index de5158f..8f08fcb 100644
--- a/y2019/jevois/spi_test.cc
+++ b/y2019/jevois/spi_test.cc
@@ -105,6 +105,15 @@
output_message->frames.back().targets.back().skew, 0.1);
}
+// Tests packing and then unpacking an empty message.
+TEST(SpiToTeensyPackTest, Empty) {
+ RoborioToTeensy input_message;
+ const SpiTransfer transfer = SpiPackToTeensy(input_message);
+ const auto output_message = SpiUnpackToTeensy(transfer);
+ ASSERT_TRUE(output_message);
+ EXPECT_EQ(input_message, output_message.value());
+}
+
} // namespace testing
} // namespace jevois
} // namespace frc971