run control loops and write their output on new sensor values
This also includes sending solenoid values from their own thread at 50Hz
(formerly I613f95a6efb5efe428029e4825ba6caeb34ea326).
Change-Id: I3d3021cdbbf2ddf895e5ceebd4db299b4743e124
diff --git a/frc971/wpilib/loop_output_handler.cc b/frc971/wpilib/loop_output_handler.cc
new file mode 100644
index 0000000..b7b112e
--- /dev/null
+++ b/frc971/wpilib/loop_output_handler.cc
@@ -0,0 +1,72 @@
+#include "frc971/wpilib/loop_output_handler.h"
+
+#include <sys/timerfd.h>
+
+#include <thread>
+#include <functional>
+
+#include "aos/linux_code/init.h"
+#include "aos/common/messages/robot_state.q.h"
+
+namespace frc971 {
+namespace wpilib {
+
+constexpr ::aos::time::Time LoopOutputHandler::kStopTimeout;
+
+LoopOutputHandler::LoopOutputHandler() : watchdog_(this) {}
+
+void LoopOutputHandler::operator()() {
+ ::aos::SetCurrentThreadName("OutputHandler");
+ ::std::thread watchdog_thread(::std::ref(watchdog_));
+
+ ::aos::SetCurrentThreadRealtimePriority(30);
+ while (run_) {
+ no_robot_state_.Print();
+ fake_robot_state_.Print();
+ Read();
+ ::aos::robot_state.FetchLatest();
+ if (!::aos::robot_state.get()) {
+ LOG_INTERVAL(no_robot_state_);
+ continue;
+ }
+ if (::aos::robot_state->fake) {
+ LOG_INTERVAL(fake_robot_state_);
+ continue;
+ }
+
+ watchdog_.Reset();
+ Write();
+ }
+
+ Stop();
+
+ watchdog_.Quit();
+ watchdog_thread.join();
+}
+
+LoopOutputHandler::Watchdog::Watchdog(LoopOutputHandler *handler)
+ : handler_(handler),
+ timerfd_(timerfd_create(::aos::time::Time::kDefaultClock, 0)) {
+ if (timerfd_.get() == -1) {
+ PLOG(FATAL, "timerfd_create(Time::kDefaultClock, 0)");
+ }
+ ::aos::SetCurrentThreadRealtimePriority(35);
+ ::aos::SetCurrentThreadName("OutputWatchdog");
+}
+
+void LoopOutputHandler::Watchdog::operator()() {
+ uint8_t buf[8];
+ while (run_) {
+ PCHECK(read(timerfd_.get(), buf, sizeof(buf)));
+ handler_->Stop();
+ }
+}
+
+void LoopOutputHandler::Watchdog::Reset() {
+ itimerspec value = itimerspec();
+ value.it_value = kStopTimeout.ToTimespec();
+ PCHECK(timerfd_settime(timerfd_.get(), 0, &value, nullptr));
+}
+
+} // namespace wpilib
+} // namespace frc971