diff --git a/wpilibc/src/main/native/cpp/DriverStation.cpp b/wpilibc/src/main/native/cpp/DriverStation.cpp index 37fc65fc7f..876b985caf 100644 --- a/wpilibc/src/main/native/cpp/DriverStation.cpp +++ b/wpilibc/src/main/native/cpp/DriverStation.cpp @@ -22,8 +22,10 @@ #include #include #include +#include #include #include +#include #include "frc/Errors.h" #include "frc/MotorSafety.h" @@ -86,11 +88,48 @@ struct MatchDataSender { MatchDataSenderEntry 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 m_joysticks; +}; + struct Instance { Instance(); ~Instance(); MatchDataSender matchDataSender; + std::atomic 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{m_prevAxes.axes, + static_cast(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{axes.axes, static_cast(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{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{povsArr, static_cast(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); + } + } +} diff --git a/wpilibc/src/main/native/include/frc/DriverStation.h b/wpilibc/src/main/native/include/frc/DriverStation.h index 30bf4fc372..532c93a611 100644 --- a/wpilibc/src/main/native/include/frc/DriverStation.h +++ b/wpilibc/src/main/native/include/frc/DriverStation.h @@ -9,6 +9,10 @@ #include #include +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; }; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java index 8eeb28456c..2c10187143 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java +++ b/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); + } }