| #include "y2019/jevois/spi.h" |
| |
| #include "aos/util/bitpacking.h" |
| #include "y2019/jevois/jevois_crc.h" |
| #ifdef __linux__ |
| #include "aos/logging/logging.h" |
| #else |
| #define AOS_CHECK(...) |
| #define AOS_CHECK_GE(...) |
| #endif |
| |
| // 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. 2 bits of quantity+index |
| // The 6 bits of quantity+index (between all three targets) are: |
| // 1. 4 bits camera index + 1 (0 means this isn't a valid frame) |
| // 2. 2 bits target count |
| // 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() { return 10.0; } |
| 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 quantity_index_offset() { return height_offset() + height_bits(); } |
| void camera_index_pack(int camera_index, gsl::span<char> destination) { |
| aos::PackBits<uint32_t, 2, quantity_index_offset()>(camera_index & 3, |
| destination); |
| aos::PackBits<uint32_t, 2, quantity_index_offset() + 32>( |
| (camera_index >> 2) & 3, destination); |
| } |
| int camera_index_unpack(gsl::span<const char> source) { |
| int result = 0; |
| result |= aos::UnpackBits<uint32_t, 2, quantity_index_offset()>(source); |
| result |= aos::UnpackBits<uint32_t, 2, quantity_index_offset() + 32>(source) |
| << 2; |
| return result; |
| } |
| void target_count_pack(int target_count, gsl::span<char> destination) { |
| aos::PackBits<uint32_t, 2, quantity_index_offset() + 32 * 2>(target_count, |
| destination); |
| } |
| int target_count_unpack(gsl::span<const char> source) { |
| return aos::UnpackBits<uint32_t, 2, quantity_index_offset() + 32 * 2>(source); |
| } |
| |
| constexpr int next_offset() { return quantity_index_offset() + 2; } |
| 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) { |
| // Zero out all three targets and the age. |
| for (int i = 0; i < 3 * 4 + 1; ++i) { |
| remaining_space[i] = 0; |
| } |
| |
| if (static_cast<int>(message.frames.size()) > frame) { |
| camera_index_pack(message.frames[frame].camera_index + 1, |
| remaining_space); |
| target_count_pack(message.frames[frame].targets.size(), remaining_space); |
| |
| for (int target = 0; target < 3; ++target) { |
| 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); |
| } |
| remaining_space = remaining_space.subspan(4); |
| } |
| |
| const uint8_t age_count = message.frames[frame].age.count(); |
| memcpy(&remaining_space[0], &age_count, 1); |
| remaining_space = remaining_space.subspan(1); |
| } else { |
| remaining_space = remaining_space.subspan(4 * 3 + 1); |
| } |
| } |
| { |
| uint16_t crc = jevois_crc_init(); |
| crc = jevois_crc_update(crc, transfer.data(), |
| transfer.size() - remaining_space.size()); |
| crc = jevois_crc_finalize(crc); |
| AOS_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)); |
| } |
| AOS_CHECK(remaining_space.empty()); |
| return transfer; |
| } |
| |
| std::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) { |
| const int camera_index_plus = camera_index_unpack(remaining_input); |
| if (camera_index_plus > 0) { |
| message.frames.push_back({}); |
| message.frames.back().camera_index = camera_index_plus - 1; |
| |
| const int target_count = target_count_unpack(remaining_input); |
| for (int target = 0; target < 3; ++target) { |
| if (target < target_count) { |
| 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); |
| } |
| |
| { |
| 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); |
| } else { |
| remaining_input = remaining_input.subspan(4 * 3 + 1); |
| } |
| } |
| { |
| 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; |
| AOS_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)); |
| AOS_CHECK(remaining_input.empty()); |
| if (calculated_crc != received_crc) { |
| return std::nullopt; |
| } |
| } |
| 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)); |
| } |
| 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, transfer.data(), |
| transfer.size() - remaining_space.size()); |
| crc = jevois_crc_finalize(crc); |
| AOS_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; |
| } |
| |
| std::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)); |
| } |
| 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, transfer.data(), |
| transfer.size() - remaining_input.size()); |
| calculated_crc = jevois_crc_finalize(calculated_crc); |
| uint16_t received_crc; |
| AOS_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 std::nullopt; |
| } |
| } |
| return message; |
| } |
| |
| } // namespace jevois |
| } // namespace frc971 |