frc971/wpilib/loop_output_handler.cc

#include "frc971/wpilib/loop_output_handler.h"

#include <sys/timerfd.h>

#include <chrono>
#include <functional>
#include <thread>

#include "aos/events/event-loop.h"
#include "aos/init.h"
#include "aos/robot_state/robot_state.q.h"

namespace frc971 {
namespace wpilib {

namespace chrono = ::std::chrono;

LoopOutputHandler::LoopOutputHandler(::aos::EventLoop *event_loop,
                                     ::std::chrono::nanoseconds timeout)
    : event_loop_(event_loop),
      joystick_state_fetcher_(event_loop_->MakeFetcher<::aos::JoystickState>(
          ".aos.joystick_state")),
      watchdog_(this, timeout) {}

void LoopOutputHandler::operator()() {
  ::std::thread watchdog_thread(::std::ref(watchdog_));
  ::aos::SetCurrentThreadName("OutputHandler");

  ::aos::SetCurrentThreadRealtimePriority(30);
  while (run_) {
    no_joystick_state_.Print();
    fake_joystick_state_.Print();
    Read();
    joystick_state_fetcher_.Fetch();
    if (!joystick_state_fetcher_.get()) {
      LOG_INTERVAL(no_joystick_state_);
      continue;
    }
    if (joystick_state_fetcher_->fake) {
      LOG_INTERVAL(fake_joystick_state_);
      continue;
    }

    watchdog_.Reset();
    Write();
  }

  Stop();

  watchdog_.Quit();
  watchdog_thread.join();
}

LoopOutputHandler::Watchdog::Watchdog(LoopOutputHandler *handler,
                                      ::std::chrono::nanoseconds timeout)
    : handler_(handler),
      timeout_(timeout),
      timerfd_(timerfd_create(CLOCK_MONOTONIC, 0)) {
  if (timerfd_.get() == -1) {
    PLOG(FATAL, "timerfd_create(CLOCK_MONOTONIC, 0)");
  }
}

void LoopOutputHandler::Watchdog::operator()() {
  ::aos::SetCurrentThreadRealtimePriority(35);
  ::aos::SetCurrentThreadName("OutputWatchdog");
  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.tv_sec = chrono::duration_cast<chrono::seconds>(timeout_).count();
  value.it_value.tv_nsec =
      chrono::duration_cast<chrono::nanoseconds>(
          timeout_ - chrono::seconds(value.it_value.tv_sec))
          .count();
  PCHECK(timerfd_settime(timerfd_.get(), 0, &value, nullptr));
}

}  // namespace wpilib
}  // namespace frc971