Add a can sensor reader
Signed-off-by: Maxwell Henderson <maxwell.henderson@mailbox.org>
Change-Id: I573052e68adeab0223d10db8be72b161a1c2edae
diff --git a/frc971/wpilib/can_sensor_reader.cc b/frc971/wpilib/can_sensor_reader.cc
new file mode 100644
index 0000000..5a19e8f
--- /dev/null
+++ b/frc971/wpilib/can_sensor_reader.cc
@@ -0,0 +1,64 @@
+#include "frc971/wpilib/can_sensor_reader.h"
+
+using frc971::wpilib::CANSensorReader;
+using frc971::wpilib::kCANUpdateFreqHz;
+
+CANSensorReader::CANSensorReader(
+ aos::EventLoop *event_loop,
+ std::vector<ctre::phoenixpro::BaseStatusSignalValue *> signals_registry,
+ std::vector<std::shared_ptr<Falcon>> falcons)
+ : event_loop_(event_loop),
+ signals_(signals_registry.begin(), signals_registry.end()),
+ can_position_sender_(
+ event_loop->MakeSender<control_loops::drivetrain::CANPosition>(
+ "/drivetrain")),
+ falcons_(falcons) {
+ event_loop->SetRuntimeRealtimePriority(40);
+
+ // TODO(max): Decide if we want to keep this on this core.
+ event_loop->SetRuntimeAffinity(aos::MakeCpusetFromCpus({1}));
+ timer_handler_ = event_loop->AddTimer([this]() { Loop(); });
+ timer_handler_->set_name("CANSensorReader Loop");
+
+ event_loop->OnRun([this]() {
+ timer_handler_->Schedule(event_loop_->monotonic_now(),
+ 1 / kCANUpdateFreqHz);
+ });
+}
+
+void CANSensorReader::Loop() {
+ ctre::phoenix::StatusCode status =
+ ctre::phoenixpro::BaseStatusSignalValue::WaitForAll(20_ms, signals_);
+
+ if (!status.IsOK()) {
+ AOS_LOG(ERROR, "Failed to read signals from falcons: %s: %s",
+ status.GetName(), status.GetDescription());
+ }
+
+ auto builder = can_position_sender_.MakeBuilder();
+
+ for (auto falcon : falcons_) {
+ falcon->RefreshNontimesyncedSignals();
+ falcon->SerializePosition(builder.fbb());
+ }
+
+ auto falcon_offsets =
+ builder.fbb()
+ ->CreateVector<flatbuffers::Offset<control_loops::CANFalcon>>(
+ falcons_.size(), [this](size_t index) {
+ auto offset = falcons_.at(index)->TakeOffset();
+ CHECK(offset.has_value());
+ return offset.value();
+ });
+
+ control_loops::drivetrain::CANPosition::Builder can_position_builder =
+ builder.MakeBuilder<control_loops::drivetrain::CANPosition>();
+
+ can_position_builder.add_falcons(falcon_offsets);
+ if (!falcons_.empty()) {
+ can_position_builder.add_timestamp(falcons_.at(0)->GetTimestamp());
+ }
+ can_position_builder.add_status(static_cast<int>(status));
+
+ builder.CheckOk(builder.Send(can_position_builder.Finish()));
+}