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/aos/containers/sized_array.h b/aos/containers/sized_array.h
index 0c13ecc..6d4209a 100644
--- a/aos/containers/sized_array.h
+++ b/aos/containers/sized_array.h
@@ -109,6 +109,12 @@
--size_;
}
+ // These allow access to the underlying storage. The data here may be outside
+ // the current logical extents of the container.
+ const array &backing_array() const { return array_; }
+ array *mutable_backing_array() { return &array_; }
+ void set_size(size_t size) { size_ = size; }
+
private:
void check_index(size_t i) const {
if (i >= size_) {
diff --git a/aos/time/time.cc b/aos/time/time.cc
index 06f4b12..29b2956 100644
--- a/aos/time/time.cc
+++ b/aos/time/time.cc
@@ -135,6 +135,7 @@
} // namespace time
constexpr monotonic_clock::time_point monotonic_clock::min_time;
+constexpr realtime_clock::time_point realtime_clock::min_time;
monotonic_clock::time_point monotonic_clock::now() noexcept {
#ifdef __linux__
diff --git a/y2019/jevois/BUILD b/y2019/jevois/BUILD
index 7d26c1e..0e0d3bd 100644
--- a/y2019/jevois/BUILD
+++ b/y2019/jevois/BUILD
@@ -79,6 +79,7 @@
deps = [
":jevois_crc",
":structures",
+ "//aos/logging",
"//aos/util:bitpacking",
"//third_party/GSL",
"//third_party/optional",
@@ -94,13 +95,29 @@
"uart.h",
],
deps = [
+ ":cobs",
+ ":jevois_crc",
":structures",
"//aos/containers:sized_array",
+ "//aos/logging",
+ "//aos/util:bitpacking",
+ "//third_party/GSL",
"//third_party/optional",
],
)
cc_test(
+ name = "uart_test",
+ srcs = [
+ "uart_test.cc",
+ ],
+ deps = [
+ ":uart",
+ "//aos/testing:googletest",
+ ],
+)
+
+cc_test(
name = "spi_test",
srcs = [
"spi_test.cc",
diff --git a/y2019/jevois/spi.cc b/y2019/jevois/spi.cc
index fee4f24..28df8cf 100644
--- a/y2019/jevois/spi.cc
+++ b/y2019/jevois/spi.cc
@@ -1,7 +1,6 @@
#include "y2019/jevois/spi.h"
-#include <assert.h>
-
+#include "aos/logging/logging.h"
#include "aos/util/bitpacking.h"
#include "third_party/GSL/include/gsl/gsl"
#include "y2019/jevois/jevois_crc.h"
@@ -175,11 +174,11 @@
crc = jevois_crc_update(crc, transfer.data(),
transfer.size() - remaining_space.size());
crc = jevois_crc_finalize(crc);
- assert(static_cast<size_t>(remaining_space.size()) >= sizeof(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));
}
- assert(remaining_space.empty());
+ CHECK(remaining_space.empty());
return transfer;
}
@@ -222,14 +221,14 @@
transfer.size() - remaining_input.size());
calculated_crc = jevois_crc_finalize(calculated_crc);
uint16_t received_crc;
- assert(static_cast<size_t>(remaining_input.size()) >= sizeof(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;
}
}
- assert(remaining_input.empty());
return message;
}
diff --git a/y2019/jevois/structures.h b/y2019/jevois/structures.h
index 15889c4..fcc268e 100644
--- a/y2019/jevois/structures.h
+++ b/y2019/jevois/structures.h
@@ -107,7 +107,7 @@
// This is all the information sent from the Teensy to each camera.
struct CameraCalibration {
- enum class CameraCommand {
+ enum class CameraCommand : char {
// Stay in normal mode.
kNormal,
// Go to camera passthrough mode.
@@ -120,6 +120,15 @@
if (other.calibration != calibration) {
return false;
}
+ if (other.teensy_now != teensy_now) {
+ return false;
+ }
+ if (other.realtime_now != realtime_now) {
+ return false;
+ }
+ if (other.camera_command != camera_command) {
+ return false;
+ }
return true;
}
bool operator!=(const CameraCalibration &other) const {
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
diff --git a/y2019/jevois/uart.h b/y2019/jevois/uart.h
index 04a1554..32ca110 100644
--- a/y2019/jevois/uart.h
+++ b/y2019/jevois/uart.h
@@ -3,6 +3,7 @@
#include "aos/containers/sized_array.h"
#include "third_party/optional/tl/optional.hpp"
+#include "y2019/jevois/cobs.h"
#include "y2019/jevois/structures.h"
// This file manages serializing and deserializing the various structures for
@@ -11,14 +12,27 @@
namespace frc971 {
namespace jevois {
-constexpr size_t uart_max_size() {
- // TODO(Brian): Make this real.
- return 10;
+constexpr size_t uart_to_teensy_size() {
+ return 3 /* targets */ * (sizeof(float) * 4 /* fields */) + 1 /* age */ +
+ 2 /* CRC-16 */;
}
-using UartBuffer = aos::SizedArray<char, uart_max_size()>;
+using UartToTeensyBuffer =
+ aos::SizedArray<char, CobsMaxEncodedSize(uart_to_teensy_size())>;
-UartBuffer UartPackToTeensy(const Frame &message);
-tl::optional<CameraCalibration> UartUnpackToCamera(const UartBuffer &message);
+constexpr size_t uart_to_camera_size() {
+ return sizeof(float) * 3 * 4 /* calibration */ +
+ sizeof(int64_t) /* teensy_now */ + sizeof(int64_t) /* realtime_now */ +
+ 1 /* camera_command */ + 2 /* CRC-16 */;
+}
+using UartToCameraBuffer =
+ aos::SizedArray<char, CobsMaxEncodedSize(uart_to_camera_size())>;
+
+UartToTeensyBuffer UartPackToTeensy(const Frame &message);
+tl::optional<Frame> UartUnpackToTeensy(const UartToTeensyBuffer &buffer);
+
+UartToCameraBuffer UartPackToCamera(const CameraCalibration &message);
+tl::optional<CameraCalibration> UartUnpackToCamera(
+ const UartToCameraBuffer &buffer);
} // namespace jevois
} // namespace frc971
diff --git a/y2019/jevois/uart_test.cc b/y2019/jevois/uart_test.cc
new file mode 100644
index 0000000..82160c7
--- /dev/null
+++ b/y2019/jevois/uart_test.cc
@@ -0,0 +1,98 @@
+#include "y2019/jevois/uart.h"
+
+#include <stdint.h>
+
+#include "gtest/gtest.h"
+
+namespace frc971 {
+namespace jevois {
+namespace testing {
+
+// Tests packing and then unpacking a message with arbitrary values.
+TEST(UartToTeensyTest, Basic) {
+ Frame input_message;
+ for (int i = 0; i < 3; ++i) {
+ input_message.targets[i].distance = i * 7 + 1;
+ input_message.targets[i].height = i * 7 + 2;
+ input_message.targets[i].heading = i * 7 + 3;
+ input_message.targets[i].skew = i * 7 + 5;
+ }
+ input_message.age = camera_duration(123);
+ const UartToTeensyBuffer buffer = UartPackToTeensy(input_message);
+ const auto output_message = UartUnpackToTeensy(buffer);
+ ASSERT_TRUE(output_message);
+ EXPECT_EQ(input_message, output_message.value());
+}
+
+// Tests packing and then unpacking a message with arbitrary values.
+TEST(UartToCameraTest, Basic) {
+ CameraCalibration input_message;
+ for (int i = 0; i < 3; ++i) {
+ for (int j = 0; j < 3; ++j) {
+ input_message.calibration(i, j) = i * 5 + j * 971;
+ }
+ }
+ input_message.teensy_now =
+ aos::monotonic_clock::time_point(std::chrono::seconds(1678));
+ input_message.realtime_now = aos::realtime_clock::min_time;
+ input_message.camera_command =
+ CameraCalibration::CameraCommand::kCameraPassthrough;
+ const UartToCameraBuffer buffer = UartPackToCamera(input_message);
+ const auto output_message = UartUnpackToCamera(buffer);
+ ASSERT_TRUE(output_message);
+ EXPECT_EQ(input_message, output_message.value());
+}
+
+// Tests that corrupting the data in various ways is handled properly.
+TEST(UartToTeensyTest, CorruptData) {
+ Frame input_message{};
+ {
+ UartToTeensyBuffer buffer = UartPackToTeensy(input_message);
+ buffer[0]++;
+ EXPECT_FALSE(UartUnpackToTeensy(buffer));
+ }
+ {
+ UartToTeensyBuffer buffer = UartPackToTeensy(input_message);
+ buffer[buffer.size() - 1]++;
+ EXPECT_FALSE(UartUnpackToTeensy(buffer));
+ }
+ {
+ UartToTeensyBuffer buffer = UartPackToTeensy(input_message);
+ buffer.set_size(buffer.size() - 1);
+ EXPECT_FALSE(UartUnpackToTeensy(buffer));
+ }
+ {
+ UartToTeensyBuffer buffer = UartPackToTeensy(input_message);
+ buffer[0] = 255;
+ EXPECT_FALSE(UartUnpackToTeensy(buffer));
+ }
+}
+
+// Tests that corrupting the data in various ways is handled properly.
+TEST(UartToCameraTest, CorruptData) {
+ CameraCalibration input_message{};
+ {
+ UartToCameraBuffer buffer = UartPackToCamera(input_message);
+ buffer[0]++;
+ EXPECT_FALSE(UartUnpackToCamera(buffer));
+ }
+ {
+ UartToCameraBuffer buffer = UartPackToCamera(input_message);
+ buffer[buffer.size() - 1]++;
+ EXPECT_FALSE(UartUnpackToCamera(buffer));
+ }
+ {
+ UartToCameraBuffer buffer = UartPackToCamera(input_message);
+ buffer.set_size(buffer.size() - 1);
+ EXPECT_FALSE(UartUnpackToCamera(buffer));
+ }
+ {
+ UartToCameraBuffer buffer = UartPackToCamera(input_message);
+ buffer[0] = 255;
+ EXPECT_FALSE(UartUnpackToCamera(buffer));
+ }
+}
+
+} // namespace testing
+} // namespace jevois
+} // namespace frc971