blob: 5c504e2a1167e75b4c914eae03f015d3df9e08b8 [file] [log] [blame]
#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