Implement UART serialization code to/from the cameras
Also fix a few things in the SPI serialization code I noticed while
reusing the pattern.
Change-Id: I645775b4c9ea1265025957e50b4d03f4932582f8
diff --git a/y2019/jevois/uart.cc b/y2019/jevois/uart.cc
index f4f66e3..a776cdd 100644
--- a/y2019/jevois/uart.cc
+++ b/y2019/jevois/uart.cc
@@ -1,13 +1,174 @@
#include "y2019/jevois/uart.h"
+#include <array>
+
+#include "aos/logging/logging.h"
+#include "aos/util/bitpacking.h"
+#include "third_party/GSL/include/gsl/gsl"
+#include "y2019/jevois/jevois_crc.h"
+
namespace frc971 {
namespace jevois {
-UartBuffer UartPackToTeensy(const Frame & /*message*/) { return UartBuffer(); }
+UartToTeensyBuffer UartPackToTeensy(const Frame &message) {
+ std::array<char, uart_to_teensy_size()> buffer;
+ gsl::span<char> remaining_space = buffer;
+ for (int i = 0; i < 3; ++i) {
+ memcpy(remaining_space.data(), &message.targets[i].distance, sizeof(float));
+ remaining_space = remaining_space.subspan(sizeof(float));
+ memcpy(remaining_space.data(), &message.targets[i].height, sizeof(float));
+ remaining_space = remaining_space.subspan(sizeof(float));
+ memcpy(remaining_space.data(), &message.targets[i].heading, sizeof(float));
+ remaining_space = remaining_space.subspan(sizeof(float));
+ memcpy(remaining_space.data(), &message.targets[i].skew, sizeof(float));
+ remaining_space = remaining_space.subspan(sizeof(float));
+ }
+ remaining_space[0] = message.age.count();
+ remaining_space = remaining_space.subspan(1);
+ {
+ uint16_t crc = jevois_crc_init();
+ crc = jevois_crc_update(crc, buffer.data(),
+ buffer.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));
+ }
+ CHECK(remaining_space.empty());
+ UartToTeensyBuffer result;
+ result.set_size(
+ CobsEncode<uart_to_teensy_size()>(buffer, result.mutable_backing_array())
+ .size());
+ return result;
+}
+
+tl::optional<Frame> UartUnpackToTeensy(
+ const UartToTeensyBuffer &encoded_buffer) {
+ std::array<char, uart_to_teensy_size()> buffer;
+ if (static_cast<size_t>(
+ CobsDecode<uart_to_teensy_size()>(encoded_buffer, &buffer).size()) !=
+ buffer.size()) {
+ return tl::nullopt;
+ }
+
+ Frame message;
+ gsl::span<const char> remaining_input = buffer;
+ for (int i = 0; i < 3; ++i) {
+ memcpy(&message.targets[i].distance, remaining_input.data(), sizeof(float));
+ remaining_input = remaining_input.subspan(sizeof(float));
+ memcpy(&message.targets[i].height, remaining_input.data(), sizeof(float));
+ remaining_input = remaining_input.subspan(sizeof(float));
+ memcpy(&message.targets[i].heading, remaining_input.data(), sizeof(float));
+ remaining_input = remaining_input.subspan(sizeof(float));
+ memcpy(&message.targets[i].skew, remaining_input.data(), sizeof(float));
+ remaining_input = remaining_input.subspan(sizeof(float));
+ }
+ message.age = camera_duration(remaining_input[0]);
+ remaining_input = remaining_input.subspan(1);
+ {
+ uint16_t calculated_crc = jevois_crc_init();
+ calculated_crc = jevois_crc_update(calculated_crc, buffer.data(),
+ buffer.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));
+ CHECK(remaining_input.empty());
+ if (calculated_crc != received_crc) {
+ return tl::nullopt;
+ }
+ }
+ return message;
+}
+
+UartToCameraBuffer UartPackToCamera(const CameraCalibration &message) {
+ std::array<char, uart_to_camera_size()> buffer;
+ gsl::span<char> remaining_space = buffer;
+ for (int i = 0; i < 3; ++i) {
+ for (int j = 0; j < 4; ++j) {
+ memcpy(remaining_space.data(), &message.calibration(i, j), sizeof(float));
+ remaining_space = remaining_space.subspan(sizeof(float));
+ }
+ }
+ {
+ const int64_t teensy_now = message.teensy_now.time_since_epoch().count();
+ memcpy(remaining_space.data(), &teensy_now, sizeof(teensy_now));
+ remaining_space = remaining_space.subspan(sizeof(teensy_now));
+ }
+ {
+ 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));
+ }
+ memcpy(remaining_space.data(), &message.camera_command, 1);
+ remaining_space = remaining_space.subspan(1);
+ {
+ uint16_t crc = jevois_crc_init();
+ crc = jevois_crc_update(crc, buffer.data(),
+ buffer.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));
+ }
+ CHECK(remaining_space.empty());
+ UartToCameraBuffer result;
+ result.set_size(
+ CobsEncode<uart_to_camera_size()>(buffer, result.mutable_backing_array())
+ .size());
+ return result;
+}
tl::optional<CameraCalibration> UartUnpackToCamera(
- const UartBuffer & /*message*/) {
- return tl::nullopt;
+ const UartToCameraBuffer &encoded_buffer) {
+ std::array<char, uart_to_camera_size()> buffer;
+ if (static_cast<size_t>(
+ CobsDecode<uart_to_camera_size()>(encoded_buffer, &buffer).size()) !=
+ buffer.size()) {
+ return tl::nullopt;
+ }
+
+ CameraCalibration message;
+ gsl::span<const char> remaining_input = buffer;
+ for (int i = 0; i < 3; ++i) {
+ for (int j = 0; j < 4; ++j) {
+ memcpy(&message.calibration(i, j), remaining_input.data(), sizeof(float));
+ remaining_input = remaining_input.subspan(sizeof(float));
+ }
+ }
+ {
+ int64_t teensy_now;
+ memcpy(&teensy_now, remaining_input.data(), sizeof(teensy_now));
+ message.teensy_now = aos::monotonic_clock::time_point(
+ aos::monotonic_clock::duration(teensy_now));
+ remaining_input = remaining_input.subspan(sizeof(teensy_now));
+ }
+ {
+ 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));
+ }
+ memcpy(&message.camera_command, remaining_input.data(), 1);
+ remaining_input = remaining_input.subspan(1);
+ {
+ uint16_t calculated_crc = jevois_crc_init();
+ calculated_crc = jevois_crc_update(calculated_crc, buffer.data(),
+ buffer.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));
+ CHECK(remaining_input.empty());
+ if (calculated_crc != received_crc) {
+ return tl::nullopt;
+ }
+ }
+ return message;
}
} // namespace jevois