Swap can_translator to batched frames
This makes it so that the IMU can use standard CAN rather than CANfd.
Change-Id: I23d86c8cd20c6e658b0e680e7c33264b44e2ce93
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/frc971/imu_fdcan/can_translator_lib.cc b/frc971/imu_fdcan/can_translator_lib.cc
index e4a927e..4336c82 100644
--- a/frc971/imu_fdcan/can_translator_lib.cc
+++ b/frc971/imu_fdcan/can_translator_lib.cc
@@ -3,7 +3,6 @@
using frc971::imu_fdcan::CANTranslator;
constexpr std::size_t kCanFrameSize = 64;
-constexpr int kImuCanId = 1;
CANTranslator::CANTranslator(aos::EventLoop *event_loop,
std::string_view canframe_channel)
@@ -13,21 +12,23 @@
can_translator_status_sender_(
event_loop->MakeSender<frc971::imu::CanTranslatorStatusStatic>(
"/imu")) {
+ packets_arrived_.fill(false);
// TODO(max): Update this with a proper priority
event_loop->SetRuntimeRealtimePriority(15);
event_loop->MakeWatcher(
canframe_channel, [this](const frc971::can_logger::CanFrame &can_frame) {
- if (can_frame.data()->size() / sizeof(uint8_t) != 64) {
+ if (can_frame.data()->size() / sizeof(uint8_t) != 8) {
invalid_packet_count_++;
+ return;
}
- if (can_frame.can_id() != kImuCanId) {
+ if (can_frame.can_id() == 0 || can_frame.can_id() > 8) {
invalid_can_id_count_++;
return;
}
- if (can_frame.data()->size() / sizeof(uint8_t) == 64) {
+ if (can_frame.data()->size() / sizeof(uint8_t) == 8) {
valid_packet_count_++;
HandleFrame(&can_frame);
}
@@ -41,6 +42,7 @@
status_builder->set_valid_packet_count(valid_packet_count_);
status_builder->set_invalid_packet_count(invalid_packet_count_);
status_builder->set_invalid_can_id_count(invalid_can_id_count_);
+ status_builder->set_out_of_order_count(out_of_order_count_);
status_builder.CheckOk(status_builder.Send());
},
@@ -61,10 +63,26 @@
// Values from the data field mapping table in
// https://docs.google.com/document/d/12AJUruW7DZ2pIrDzTyPC0qqFoia4QOSVlax6Jd7m4H0/edit?usp=sharing
void CANTranslator::HandleFrame(const frc971::can_logger::CanFrame *can_frame) {
+ const size_t frame_index = can_frame->can_id() - 1u;
+ packets_arrived_[frame_index] = true;
+ for (size_t index = 0; index <= frame_index; ++index) {
+ if (!packets_arrived_[index]) {
+ ++out_of_order_count_;
+ packets_arrived_.fill(false);
+ return;
+ }
+ }
+ // Should have already checked length.
+ CHECK_EQ(can_frame->data()->size(), 8u);
+ memcpy(current_frame_.data() + frame_index * 8, can_frame->data()->data(), 8);
+ if (frame_index < 7) {
+ return;
+ }
+
aos::Sender<frc971::imu::DualImuStatic>::StaticBuilder dual_imu_builder =
dual_imu_sender_.MakeStaticBuilder();
- std::span can_data(can_frame->data()->data(), kCanFrameSize);
+ std::span<const uint8_t> can_data(current_frame_.data(), kCanFrameSize);
frc971::imu::SingleImuStatic *murata = dual_imu_builder->add_murata();
diff --git a/frc971/imu_fdcan/can_translator_lib.h b/frc971/imu_fdcan/can_translator_lib.h
index 12acb0a..af05aba 100644
--- a/frc971/imu_fdcan/can_translator_lib.h
+++ b/frc971/imu_fdcan/can_translator_lib.h
@@ -21,9 +21,13 @@
aos::Sender<imu::DualImuStatic> dual_imu_sender_;
aos::Sender<imu::CanTranslatorStatusStatic> can_translator_status_sender_;
+ std::array<uint8_t, 64> current_frame_;
+ std::array<bool, 8> packets_arrived_;
+
uint64_t valid_packet_count_ = 0;
uint64_t invalid_packet_count_ = 0;
uint64_t invalid_can_id_count_ = 0;
+ uint64_t out_of_order_count_ = 0;
};
} // namespace frc971::imu_fdcan
diff --git a/frc971/imu_fdcan/can_translator_lib_test.cc b/frc971/imu_fdcan/can_translator_lib_test.cc
index f8e6d85..fcef6a4 100644
--- a/frc971/imu_fdcan/can_translator_lib_test.cc
+++ b/frc971/imu_fdcan/can_translator_lib_test.cc
@@ -49,80 +49,23 @@
aos::monotonic_clock::epoch() + std::chrono::seconds(0),
aos::realtime_clock::epoch() + std::chrono::seconds(100));
can_frame_event_loop_->OnRun([this] {
- aos::Sender<frc971::can_logger::CanFrameStatic>::StaticBuilder
- can_frame_builder = can_frame_sender_.MakeStaticBuilder();
+ std::array<uint8_t, 64> full_frame{
+ 226, 100, 108, 8, 152, 40, 202, 121, 202, 121, 202, 121, 85,
+ 85, 81, 189, 0, 0, 8, 189, 85, 213, 127, 191, 12, 189,
+ 34, 187, 255, 219, 220, 59, 147, 173, 5, 61, 88, 68, 205,
+ 188, 230, 92, 24, 189, 235, 1, 127, 191, 210, 7, 34, 54,
+ 86, 103, 133, 186, 100, 205, 101, 185, 29, 26, 26, 0};
+ for (size_t i = 0; i < 8; ++i) {
+ aos::Sender<frc971::can_logger::CanFrameStatic>::StaticBuilder
+ can_frame_builder = can_frame_sender_.MakeStaticBuilder();
- can_frame_builder->set_can_id(1);
- can_frame_builder->set_realtime_timestamp_ns(100e9 + 971);
- auto can_data = can_frame_builder->add_data();
- CHECK(can_data->reserve(sizeof(uint8_t) * 64));
+ can_frame_builder->set_can_id(i + 1);
+ can_frame_builder->set_realtime_timestamp_ns(100e9 + 971);
+ auto can_data = can_frame_builder->add_data();
+ CHECK(can_data->FromData(full_frame.data() + i * 8, 8));
- CHECK(can_data->emplace_back(226));
- CHECK(can_data->emplace_back(100));
- CHECK(can_data->emplace_back(108));
- CHECK(can_data->emplace_back(8));
- CHECK(can_data->emplace_back(152));
- CHECK(can_data->emplace_back(40));
- CHECK(can_data->emplace_back(202));
- CHECK(can_data->emplace_back(121));
- CHECK(can_data->emplace_back(202));
- CHECK(can_data->emplace_back(121));
- CHECK(can_data->emplace_back(202));
- CHECK(can_data->emplace_back(121));
- CHECK(can_data->emplace_back(85));
- CHECK(can_data->emplace_back(85));
- CHECK(can_data->emplace_back(81));
- CHECK(can_data->emplace_back(189));
- CHECK(can_data->emplace_back(0));
- CHECK(can_data->emplace_back(0));
- CHECK(can_data->emplace_back(8));
- CHECK(can_data->emplace_back(189));
- CHECK(can_data->emplace_back(85));
- CHECK(can_data->emplace_back(213));
- CHECK(can_data->emplace_back(127));
- CHECK(can_data->emplace_back(191));
- CHECK(can_data->emplace_back(12));
- CHECK(can_data->emplace_back(189));
- CHECK(can_data->emplace_back(34));
- CHECK(can_data->emplace_back(187));
- CHECK(can_data->emplace_back(255));
- CHECK(can_data->emplace_back(219));
- CHECK(can_data->emplace_back(220));
- CHECK(can_data->emplace_back(59));
- CHECK(can_data->emplace_back(147));
- CHECK(can_data->emplace_back(173));
- CHECK(can_data->emplace_back(5));
- CHECK(can_data->emplace_back(61));
- CHECK(can_data->emplace_back(88));
- CHECK(can_data->emplace_back(68));
- CHECK(can_data->emplace_back(205));
- CHECK(can_data->emplace_back(188));
- CHECK(can_data->emplace_back(230));
- CHECK(can_data->emplace_back(92));
- CHECK(can_data->emplace_back(24));
- CHECK(can_data->emplace_back(189));
- CHECK(can_data->emplace_back(235));
- CHECK(can_data->emplace_back(1));
- CHECK(can_data->emplace_back(127));
- CHECK(can_data->emplace_back(191));
- CHECK(can_data->emplace_back(210));
- CHECK(can_data->emplace_back(7));
- CHECK(can_data->emplace_back(34));
- CHECK(can_data->emplace_back(54));
- CHECK(can_data->emplace_back(86));
- CHECK(can_data->emplace_back(103));
- CHECK(can_data->emplace_back(133));
- CHECK(can_data->emplace_back(186));
- CHECK(can_data->emplace_back(100));
- CHECK(can_data->emplace_back(205));
- CHECK(can_data->emplace_back(101));
- CHECK(can_data->emplace_back(185));
- CHECK(can_data->emplace_back(29));
- CHECK(can_data->emplace_back(26));
- CHECK(can_data->emplace_back(26));
- CHECK(can_data->emplace_back(0));
-
- can_frame_builder.CheckOk(can_frame_builder.Send());
+ can_frame_builder.CheckOk(can_frame_builder.Send());
+ }
});
event_loop_factory_.RunFor(std::chrono::milliseconds(200));
@@ -132,7 +75,7 @@
ASSERT_FALSE(can_translator_status_fetcher_->invalid_packet_count() > 0);
ASSERT_FALSE(can_translator_status_fetcher_->invalid_can_id_count() > 0);
- EXPECT_EQ(can_translator_status_fetcher_->valid_packet_count(), 1);
+ EXPECT_EQ(can_translator_status_fetcher_->valid_packet_count(), 8);
EXPECT_EQ(dual_imu_fetcher_->board_timestamp_us(), 141321442);
EXPECT_EQ(dual_imu_fetcher_->packet_counter(), 10392);
@@ -177,12 +120,11 @@
aos::Sender<frc971::can_logger::CanFrameStatic>::StaticBuilder
can_frame_builder = can_frame_sender_.MakeStaticBuilder();
- can_frame_builder->set_can_id(2);
+ can_frame_builder->set_can_id(10);
can_frame_builder->set_realtime_timestamp_ns(100);
auto can_data = can_frame_builder->add_data();
- CHECK(can_data->reserve(sizeof(uint8_t) * 1));
-
- CHECK(can_data->emplace_back(0));
+ CHECK(can_data->reserve(sizeof(uint8_t) * 8));
+ can_data->resize(8);
can_frame_builder.CheckOk(can_frame_builder.Send());
});
@@ -192,6 +134,6 @@
ASSERT_TRUE(can_translator_status_fetcher_.Fetch());
ASSERT_FALSE(dual_imu_fetcher_.Fetch());
- EXPECT_EQ(can_translator_status_fetcher_->invalid_packet_count(), 1);
+ EXPECT_EQ(can_translator_status_fetcher_->invalid_packet_count(), 0);
EXPECT_EQ(can_translator_status_fetcher_->invalid_can_id_count(), 1);
}
diff --git a/frc971/imu_fdcan/can_translator_status.fbs b/frc971/imu_fdcan/can_translator_status.fbs
index 232f3ba..63273bc 100644
--- a/frc971/imu_fdcan/can_translator_status.fbs
+++ b/frc971/imu_fdcan/can_translator_status.fbs
@@ -7,6 +7,8 @@
invalid_packet_count: uint64 (id: 1);
// Number of times we've gotten an invalid can id
invalid_can_id_count: uint64 (id: 2);
+ // Number of times that we have observed an out of order can id.
+ out_of_order_count: uint64 (id: 3);
}
root_type CanTranslatorStatus;
diff --git a/y2024/y2024_imu.json b/y2024/y2024_imu.json
index 8a679fe..3da3e11 100644
--- a/y2024/y2024_imu.json
+++ b/y2024/y2024_imu.json
@@ -274,7 +274,8 @@
"name": "/can/cana",
"type": "frc971.can_logger.CanFrame",
"source_node": "imu",
- "frequency": 6000,
+ "frequency": 9000,
+ "channel_storage_duration": 7000000000,
"num_senders": 2,
"max_size": 200
},
@@ -385,7 +386,7 @@
"name": "can_translator",
"executable_name": "can_translator",
"args": [
- "--channel=/can/canb"
+ "--channel=/can/cana"
],
"nodes": [
"imu"