[wpilibc] DriverStation: Add DataLog support for modes and joystick data

wpilibc/src/main/native/cpp/DriverStation.cpp

@@ -22,8 +22,10 @@
 #include <networktables/NetworkTable.h>
 #include <networktables/NetworkTableEntry.h>
 #include <networktables/NetworkTableInstance.h>
+#include <wpi/DataLog.h>
 #include <wpi/condition_variable.h>
 #include <wpi/mutex.h>
+#include <wpi/timestamp.h>
 
 #include "frc/Errors.h"
 #include "frc/MotorSafety.h"
@@ -86,11 +88,48 @@ struct MatchDataSender {
   MatchDataSenderEntry<double> controlWord{table, "FMSControlData", 0.0};
 };
 
+class JoystickLogSender {
+ public:
+  void Init(wpi::log::DataLog& log, unsigned int stick, int64_t timestamp);
+  void Send(uint64_t timestamp);
+
+ private:
+  void AppendButtons(HAL_JoystickButtons buttons, uint64_t timestamp);
+  void AppendPOVs(const HAL_JoystickPOVs& povs, uint64_t timestamp);
+
+  unsigned int m_stick;
+  HAL_JoystickButtons m_prevButtons;
+  HAL_JoystickAxes m_prevAxes;
+  HAL_JoystickPOVs m_prevPOVs;
+  wpi::log::BooleanArrayLogEntry m_logButtons;
+  wpi::log::FloatArrayLogEntry m_logAxes;
+  wpi::log::IntegerArrayLogEntry m_logPOVs;
+};
+
+class DataLogSender {
+ public:
+  void Init(wpi::log::DataLog& log, bool logJoysticks, int64_t timestamp);
+  void Send(uint64_t timestamp);
+
+ private:
+  std::atomic_bool m_initialized{false};
+
+  HAL_ControlWord m_prevControlWord;
+  wpi::log::BooleanLogEntry m_logEnabled;
+  wpi::log::BooleanLogEntry m_logAutonomous;
+  wpi::log::BooleanLogEntry m_logTest;
+  wpi::log::BooleanLogEntry m_logEstop;
+
+  bool m_logJoysticks;
+  std::array<JoystickLogSender, DriverStation::kJoystickPorts> m_joysticks;
+};
+
 struct Instance {
   Instance();
   ~Instance();
 
   MatchDataSender matchDataSender;
+  std::atomic<DataLogSender*> dataLogSender{nullptr};
 
   // Joystick button rising/falling edge flags
   wpi::mutex buttonEdgeMutex;
@@ -188,6 +227,10 @@ Instance::~Instance() {
   // Trigger a DS mutex release in case there is no driver station running.
   HAL_ReleaseDSMutex();
   dsThread.join();
+
+  if (dataLogSender) {
+    delete dataLogSender.load();
+  }
 }
 
 DriverStation& DriverStation::GetInstance() {
@@ -678,6 +721,9 @@ void GetData() {
 
   DriverStation::WakeupWaitForData();
   SendMatchData();
+  if (auto sender = inst.dataLogSender.load()) {
+    sender->Send(wpi::Now());
+  }
 }
 
 void DriverStation::SilenceJoystickConnectionWarning(bool silence) {
@@ -688,6 +734,24 @@ bool DriverStation::IsJoystickConnectionWarningSilenced() {
   return !IsFMSAttached() && ::GetInstance().silenceJoystickWarning;
 }
 
+void DriverStation::StartDataLog(wpi::log::DataLog& log, bool logJoysticks) {
+  auto& inst = ::GetInstance();
+  // Note: cannot safely replace, because we wouldn't know when to delete the
+  // "old" one. Instead do a compare and exchange with nullptr. We check first
+  // with a simple load to avoid the new in the common case.
+  if (inst.dataLogSender.load()) {
+    return;
+  }
+  DataLogSender* oldSender = nullptr;
+  DataLogSender* newSender = new DataLogSender;
+  inst.dataLogSender.compare_exchange_strong(oldSender, newSender);
+  if (oldSender) {
+    delete newSender;  // already had a sender
+  } else {
+    newSender->Init(log, logJoysticks, wpi::Now());
+  }
+}
+
 void ReportJoystickUnpluggedErrorV(fmt::string_view format,
                                    fmt::format_args args) {
   auto& inst = GetInstance();
@@ -793,3 +857,130 @@ void SendMatchData() {
   std::memcpy(&wordInt, &ctlWord, sizeof(wordInt));
   inst.matchDataSender.controlWord.Set(wordInt);
 }
+
+void JoystickLogSender::Init(wpi::log::DataLog& log, unsigned int stick,
+                             int64_t timestamp) {
+  m_stick = stick;
+
+  m_logButtons = wpi::log::BooleanArrayLogEntry{
+      log, fmt::format("DS:joystick{}/buttons", stick), timestamp};
+  m_logAxes = wpi::log::FloatArrayLogEntry{
+      log, fmt::format("DS:joystick{}/axes", stick), timestamp};
+  m_logPOVs = wpi::log::IntegerArrayLogEntry{
+      log, fmt::format("DS:joystick{}/povs", stick), timestamp};
+
+  HAL_GetJoystickButtons(m_stick, &m_prevButtons);
+  HAL_GetJoystickAxes(m_stick, &m_prevAxes);
+  HAL_GetJoystickPOVs(m_stick, &m_prevPOVs);
+  AppendButtons(m_prevButtons, timestamp);
+  m_logAxes.Append(
+      wpi::span<const float>{m_prevAxes.axes,
+                             static_cast<size_t>(m_prevAxes.count)},
+      timestamp);
+  AppendPOVs(m_prevPOVs, timestamp);
+}
+
+void JoystickLogSender::Send(uint64_t timestamp) {
+  HAL_JoystickButtons buttons;
+  HAL_GetJoystickButtons(m_stick, &buttons);
+  if (buttons.count != m_prevButtons.count ||
+      buttons.buttons != m_prevButtons.buttons) {
+    AppendButtons(buttons, timestamp);
+  }
+  m_prevButtons = buttons;
+
+  HAL_JoystickAxes axes;
+  HAL_GetJoystickAxes(m_stick, &axes);
+  if (axes.count != m_prevAxes.count ||
+      std::memcmp(axes.axes, m_prevAxes.axes,
+                  sizeof(axes.axes[0]) * axes.count) != 0) {
+    m_logAxes.Append(
+        wpi::span<const float>{axes.axes, static_cast<size_t>(axes.count)},
+        timestamp);
+  }
+  m_prevAxes = axes;
+
+  HAL_JoystickPOVs povs;
+  HAL_GetJoystickPOVs(m_stick, &povs);
+  if (povs.count != m_prevPOVs.count ||
+      std::memcmp(povs.povs, m_prevPOVs.povs,
+                  sizeof(povs.povs[0]) * povs.count) != 0) {
+    AppendPOVs(povs, timestamp);
+  }
+  m_prevPOVs = povs;
+}
+
+void JoystickLogSender::AppendButtons(HAL_JoystickButtons buttons,
+                                      uint64_t timestamp) {
+  uint8_t buttonsArr[32];
+  for (unsigned int i = 0; i < buttons.count; ++i) {
+    buttonsArr[i] = (buttons.buttons & (1u << i)) != 0;
+  }
+  m_logButtons.Append(wpi::span<const uint8_t>{buttonsArr, buttons.count},
+                      timestamp);
+}
+
+void JoystickLogSender::AppendPOVs(const HAL_JoystickPOVs& povs,
+                                   uint64_t timestamp) {
+  int64_t povsArr[HAL_kMaxJoystickPOVs];
+  for (int i = 0; i < povs.count; ++i) {
+    povsArr[i] = povs.povs[i];
+  }
+  m_logPOVs.Append(
+      wpi::span<const int64_t>{povsArr, static_cast<size_t>(povs.count)},
+      timestamp);
+}
+
+void DataLogSender::Init(wpi::log::DataLog& log, bool logJoysticks, int64_t timestamp) {
+  m_logEnabled = wpi::log::BooleanLogEntry{log, "DS:enabled", timestamp};
+  m_logAutonomous = wpi::log::BooleanLogEntry{log, "DS:autonomous", timestamp};
+  m_logTest = wpi::log::BooleanLogEntry{log, "DS:test", timestamp};
+  m_logEstop = wpi::log::BooleanLogEntry{log, "DS:estop", timestamp};
+
+  // append initial control word values
+  HAL_GetControlWord(&m_prevControlWord);
+  m_logEnabled.Append(m_prevControlWord.enabled, timestamp);
+  m_logAutonomous.Append(m_prevControlWord.autonomous, timestamp);
+  m_logTest.Append(m_prevControlWord.test, timestamp);
+  m_logEstop.Append(m_prevControlWord.eStop, timestamp);
+
+  m_logJoysticks = logJoysticks;
+  if (logJoysticks) {
+    unsigned int i = 0;
+    for (auto&& joystick : m_joysticks) {
+      joystick.Init(log, i++, timestamp);
+    }
+  }
+
+  m_initialized = true;
+}
+
+void DataLogSender::Send(uint64_t timestamp) {
+  if (!m_initialized) {
+    return;
+  }
+
+  // append control word value changes
+  HAL_ControlWord ctlWord;
+  HAL_GetControlWord(&ctlWord);
+  if (ctlWord.enabled != m_prevControlWord.enabled) {
+    m_logEnabled.Append(ctlWord.enabled, timestamp);
+  }
+  if (ctlWord.autonomous != m_prevControlWord.autonomous) {
+    m_logAutonomous.Append(ctlWord.autonomous, timestamp);
+  }
+  if (ctlWord.test != m_prevControlWord.test) {
+    m_logTest.Append(ctlWord.test, timestamp);
+  }
+  if (ctlWord.eStop != m_prevControlWord.eStop) {
+    m_logEstop.Append(ctlWord.eStop, timestamp);
+  }
+  m_prevControlWord = ctlWord;
+
+  if (m_logJoysticks) {
+    // append joystick value changes
+    for (auto&& joystick : m_joysticks) {
+      joystick.Send(timestamp);
+    }
+  }
+}

wpilibc/src/main/native/include/frc/DriverStation.h

@@ -9,6 +9,10 @@
 #include <units/time.h>
 #include <wpi/deprecated.h>
 
+namespace wpi::log {
+class DataLog;
+}  // namespace
+
 namespace frc {
 
 /**
@@ -441,6 +445,14 @@ class DriverStation {
    */
   static bool IsJoystickConnectionWarningSilenced();
 
+  /**
+   * Starts logging DriverStation data to data log. Repeated calls are ignored.
+   *
+   * @param log data log
+   * @param logJoysticks if true, log joystick data
+   */
+  static void StartDataLog(wpi::log::DataLog& log, bool logJoysticks = true);
+
  private:
   DriverStation() = default;
 };