[wpilib] 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,131 @@ 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 wpi::log
+
 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;
 };

wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java

@@ -11,6 +11,12 @@ import edu.wpi.first.hal.MatchInfoData;
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableEntry;
 import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.util.WPIUtilJNI;
+import edu.wpi.first.util.datalog.BooleanArrayLogEntry;
+import edu.wpi.first.util.datalog.BooleanLogEntry;
+import edu.wpi.first.util.datalog.DataLog;
+import edu.wpi.first.util.datalog.FloatArrayLogEntry;
+import edu.wpi.first.util.datalog.IntegerArrayLogEntry;
 import java.nio.ByteBuffer;
 import java.util.concurrent.TimeUnit;
 import java.util.concurrent.locks.Condition;
@@ -235,6 +241,163 @@ public class DriverStation {
     }
   }
 
+  private static class JoystickLogSender {
+    JoystickLogSender(DataLog log, int stick, long timestamp) {
+      m_stick = stick;
+
+      m_logButtons = new BooleanArrayLogEntry(log, "DS:joystick" + stick + "/buttons", timestamp);
+      m_logAxes = new FloatArrayLogEntry(log, "DS:joystick" + stick + "/axes", timestamp);
+      m_logPOVs = new IntegerArrayLogEntry(log, "DS:joystick" + stick + "/povs", timestamp);
+
+      appendButtons(timestamp);
+      appendAxes(timestamp);
+      appendPOVs(timestamp);
+    }
+
+    public void send(long timestamp) {
+      if (m_joystickButtonsCache[m_stick].m_count != m_joystickButtons[m_stick].m_count
+          || m_joystickButtonsCache[m_stick].m_buttons != m_joystickButtons[m_stick].m_buttons) {
+        appendButtons(timestamp);
+      }
+
+      if (m_joystickAxesCache[m_stick].m_count != m_joystickAxes[m_stick].m_count) {
+        appendAxes(timestamp);
+      } else {
+        int count = m_joystickAxesCache[m_stick].m_count;
+        for (int i = 0; i < count; i++) {
+          if (m_joystickAxesCache[m_stick].m_axes[i] != m_joystickAxes[m_stick].m_axes[i]) {
+            appendAxes(timestamp);
+            break;
+          }
+        }
+      }
+
+      if (m_joystickPOVsCache[m_stick].m_count != m_joystickPOVs[m_stick].m_count) {
+        appendPOVs(timestamp);
+      } else {
+        int count = m_joystickPOVsCache[m_stick].m_count;
+        for (int i = 0; i < count; i++) {
+          if (m_joystickPOVsCache[m_stick].m_povs[i] != m_joystickPOVs[m_stick].m_povs[i]) {
+            appendPOVs(timestamp);
+            break;
+          }
+        }
+      }
+    }
+
+    void appendButtons(long timestamp) {
+      int count = m_joystickButtonsCache[m_stick].m_count;
+      if (m_sizedButtons == null || m_sizedButtons.length != count) {
+        m_sizedButtons = new boolean[count];
+      }
+      int buttons = m_joystickButtonsCache[m_stick].m_buttons;
+      for (int i = 0; i < count; i++) {
+        m_sizedButtons[i] = (buttons & (1 << i)) != 0;
+      }
+      m_logButtons.append(m_sizedButtons, timestamp);
+    }
+
+    void appendAxes(long timestamp) {
+      int count = m_joystickAxesCache[m_stick].m_count;
+      if (m_sizedAxes == null || m_sizedAxes.length != count) {
+        m_sizedAxes = new float[count];
+      }
+      System.arraycopy(m_joystickAxesCache[m_stick].m_axes, 0, m_sizedAxes, 0, count);
+      m_logAxes.append(m_sizedAxes, timestamp);
+    }
+
+    void appendPOVs(long timestamp) {
+      int count = m_joystickPOVsCache[m_stick].m_count;
+      if (m_sizedPOVs == null || m_sizedPOVs.length != count) {
+        m_sizedPOVs = new long[count];
+      }
+      for (int i = 0; i < count; i++) {
+        m_sizedPOVs[i] = m_joystickPOVsCache[m_stick].m_povs[i];
+      }
+      m_logPOVs.append(m_sizedPOVs, timestamp);
+    }
+
+    final int m_stick;
+    boolean[] m_sizedButtons;
+    float[] m_sizedAxes;
+    long[] m_sizedPOVs;
+    final BooleanArrayLogEntry m_logButtons;
+    final FloatArrayLogEntry m_logAxes;
+    final IntegerArrayLogEntry m_logPOVs;
+  }
+
+  private static class DataLogSender {
+    DataLogSender(DataLog log, boolean logJoysticks, long timestamp) {
+      m_logEnabled = new BooleanLogEntry(log, "DS:enabled", timestamp);
+      m_logAutonomous = new BooleanLogEntry(log, "DS:autonomous", timestamp);
+      m_logTest = new BooleanLogEntry(log, "DS:test", timestamp);
+      m_logEstop = new BooleanLogEntry(log, "DS:estop", timestamp);
+
+      // append initial control word values
+      m_wasEnabled = m_controlWordCache.getEnabled();
+      m_wasAutonomous = m_controlWordCache.getAutonomous();
+      m_wasTest = m_controlWordCache.getTest();
+      m_wasEstop = m_controlWordCache.getEStop();
+
+      m_logEnabled.append(m_wasEnabled, timestamp);
+      m_logAutonomous.append(m_wasAutonomous, timestamp);
+      m_logTest.append(m_wasTest, timestamp);
+      m_logEstop.append(m_wasEstop, timestamp);
+
+      if (logJoysticks) {
+        m_joysticks = new JoystickLogSender[kJoystickPorts];
+        for (int i = 0; i < kJoystickPorts; i++) {
+          m_joysticks[i] = new JoystickLogSender(log, i, timestamp);
+        }
+      } else {
+        m_joysticks = new JoystickLogSender[0];
+      }
+    }
+
+    public void send(long timestamp) {
+      // append control word value changes
+      boolean enabled = m_controlWordCache.getEnabled();
+      if (enabled != m_wasEnabled) {
+        m_logEnabled.append(enabled, timestamp);
+      }
+      m_wasEnabled = enabled;
+
+      boolean autonomous = m_controlWordCache.getAutonomous();
+      if (autonomous != m_wasAutonomous) {
+        m_logAutonomous.append(autonomous, timestamp);
+      }
+      m_wasAutonomous = autonomous;
+
+      boolean test = m_controlWordCache.getTest();
+      if (test != m_wasTest) {
+        m_logTest.append(test, timestamp);
+      }
+      m_wasTest = test;
+
+      boolean estop = m_controlWordCache.getEStop();
+      if (estop != m_wasEstop) {
+        m_logEstop.append(estop, timestamp);
+      }
+      m_wasEstop = estop;
+
+      // append joystick value changes
+      for (JoystickLogSender joystick : m_joysticks) {
+        joystick.send(timestamp);
+      }
+    }
+
+    boolean m_wasEnabled;
+    boolean m_wasAutonomous;
+    boolean m_wasTest;
+    boolean m_wasEstop;
+    final BooleanLogEntry m_logEnabled;
+    final BooleanLogEntry m_logAutonomous;
+    final BooleanLogEntry m_logTest;
+    final BooleanLogEntry m_logEstop;
+
+    final JoystickLogSender[] m_joysticks;
+  }
+
   private static DriverStation instance = new DriverStation();
 
   // Joystick User Data
@@ -258,6 +421,7 @@ public class DriverStation {
   private static final ByteBuffer m_buttonCountBuffer = ByteBuffer.allocateDirect(1);
 
   private static final MatchDataSender m_matchDataSender;
+  private static DataLogSender m_dataLogSender;
 
   // Internal Driver Station thread
   private static Thread m_thread;
@@ -1259,6 +1423,7 @@ public class DriverStation {
       m_controlWordMutex.unlock();
     }
 
+    DataLogSender dataLogSender;
     // lock joystick mutex to swap cache data
     m_cacheDataMutex.lock();
     try {
@@ -1288,12 +1453,17 @@ public class DriverStation {
       MatchInfoData currentInfo = m_matchInfo;
       m_matchInfo = m_matchInfoCache;
       m_matchInfoCache = currentInfo;
+
+      dataLogSender = m_dataLogSender;
     } finally {
       m_cacheDataMutex.unlock();
     }
 
     wakeupWaitForData();
     m_matchDataSender.sendMatchData();
+    if (dataLogSender != null) {
+      dataLogSender.send(WPIUtilJNI.now());
+    }
   }
 
   /**
@@ -1383,4 +1553,32 @@ public class DriverStation {
       m_lastControlWordUpdate = now;
     }
   }
+
+  /**
+   * Starts logging DriverStation data to data log. Repeated calls are ignored.
+   *
+   * @param log data log
+   * @param logJoysticks if true, log joystick data
+   */
+  @SuppressWarnings("PMD.NonThreadSafeSingleton")
+  public static void startDataLog(DataLog log, boolean logJoysticks) {
+    m_cacheDataMutex.lock();
+    try {
+      if (m_dataLogSender == null) {
+        m_dataLogSender = new DataLogSender(log, logJoysticks, WPIUtilJNI.now());
+      }
+    } finally {
+      m_cacheDataMutex.unlock();
+    }
+  }
+
+  /**
+   * Starts logging DriverStation data to data log, including joystick data. Repeated calls are
+   * ignored.
+   *
+   * @param log data log
+   */
+  public static void startDataLog(DataLog log) {
+    startDataLog(log, true);
+  }
 }