Update WPILib, roborio compilers, and CTRE Phoenix libraries
This borrows heavily from work that Ravago did to initially get this
stuff working.
Tested rudimentary functionality on a test bench, ensured that we could:
* Enable the robot.
* Read joystick and button values.
* Switch between auto and teleop modes.
* Read sensor values (encoder, absolute encoder, potentiometer).
* Read PDP values.
* Drive PWM motors.
* Drive CANivore motors.
Non-WPILib changes are made to accommodate the upgrade roborio
compiler's improved pickiness.
Merge commit '125aac16d9bf03c833ffa18de2f113a33758a4b8' into HEAD
Change-Id: I8648956fb7517b2d784bf58e0a236742af7a306a
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/frc971/constants/constants_sender_lib.h b/frc971/constants/constants_sender_lib.h
index 6c849c3..92c14fa 100644
--- a/frc971/constants/constants_sender_lib.h
+++ b/frc971/constants/constants_sender_lib.h
@@ -14,17 +14,14 @@
template <typename ConstantsData, typename ConstantsList>
class ConstantSender {
public:
- ConstantSender<ConstantsData, ConstantsList>(
- aos::EventLoop *event_loop, std::string constants_path,
- std::string_view channel_name = "/constants")
+ ConstantSender(aos::EventLoop *event_loop, std::string constants_path,
+ std::string_view channel_name = "/constants")
: ConstantSender<ConstantsData, ConstantsList>(
event_loop, constants_path, aos::network::GetTeamNumber(),
channel_name) {}
- ConstantSender<ConstantsData, ConstantsList>(aos::EventLoop *event_loop,
- std::string constants_path,
- const uint16_t team_number,
- std::string_view channel_name)
+ ConstantSender(aos::EventLoop *event_loop, std::string constants_path,
+ const uint16_t team_number, std::string_view channel_name)
: team_number_(team_number),
channel_name_(channel_name),
constants_path_(constants_path),
diff --git a/frc971/control_loops/dlqr.h b/frc971/control_loops/dlqr.h
index ec70593..d7af426 100644
--- a/frc971/control_loops/dlqr.h
+++ b/frc971/control_loops/dlqr.h
@@ -38,9 +38,9 @@
// Computes the optimal LQR controller K for the provided system and costs.
template <int kN, int kM>
int dlqr(::Eigen::Matrix<double, kN, kN> A, ::Eigen::Matrix<double, kN, kM> B,
- ::Eigen::Matrix<double, kN, kN> Q, ::Eigen::Matrix<double, kM, kM> R,
- ::Eigen::Matrix<double, kM, kN> *K,
- ::Eigen::Matrix<double, kN, kN> *S) {
+ ::Eigen::Matrix<double, kN, kN> Q, ::Eigen::Matrix<double, kM, kM> R,
+ ::Eigen::Matrix<double, kM, kN> *K,
+ ::Eigen::Matrix<double, kN, kN> *S) {
*K = ::Eigen::Matrix<double, kM, kN>::Zero();
*S = ::Eigen::Matrix<double, kN, kN>::Zero();
// Discrete (not continuous)
@@ -69,9 +69,9 @@
double ALFAR[kN * 2];
double ALFAI[kN * 2];
double BETA[kN * 2];
- memset(ALFAR, 0, kN * 2);
- memset(ALFAI, 0, kN * 2);
- memset(BETA, 0, kN * 2);
+ memset(ALFAR, 0, sizeof(ALFAR));
+ memset(ALFAI, 0, sizeof(ALFAI));
+ memset(BETA, 0, sizeof(BETA));
long LDS = 2 * kN + kM;
::Eigen::Matrix<double, 2 * kN + kM, 2 *kN + kM> S_schur =
@@ -88,17 +88,17 @@
double TOL = 0.0;
long IWORK[2 * kN > kM ? 2 * kN : kM];
- memset(IWORK, 0, 2 * kN > kM ? 2 * kN : kM);
+ memset(IWORK, 0, sizeof(IWORK));
long LDWORK = 16 * kN + 3 * kM + 16;
double DWORK[LDWORK];
- memset(DWORK, 0, LDWORK);
+ memset(DWORK, 0, sizeof(DWORK));
long INFO = 0;
- long BWORK[2 * kN];
- memset(BWORK, 0, 2 * kN);
+ long BWORK[kN * 2];
+ memset(BWORK, 0, sizeof(BWORK));
// TODO(austin): I can't tell if anything here is transposed...
sb02od_(&DICO, &JOBB, &FACT, &UPLO, &JOBL, &SORT, &N, &M, &P, A.data(), &N,
diff --git a/frc971/control_loops/drivetrain/hybrid_ekf_test.cc b/frc971/control_loops/drivetrain/hybrid_ekf_test.cc
index b491719..4bdf4de 100644
--- a/frc971/control_loops/drivetrain/hybrid_ekf_test.cc
+++ b/frc971/control_loops/drivetrain/hybrid_ekf_test.cc
@@ -338,8 +338,8 @@
// Ensure that we check kSaveSamples - 1, for potential corner cases.
INSTANTIATE_TEST_SUITE_P(OldCorrectionTest, HybridEkfOldCorrectionsTest,
- ::testing::Values(0, 1, 10,
- HybridEkf<>::kSaveSamples - 1));
+ ::testing::Values(0, 1, 10,
+ HybridEkf<>::kSaveSamples - 1));
// Tests that creating a correction that is too old results in the correction
// being dropped and ignored.
@@ -492,9 +492,10 @@
TEST_F(HybridEkfDeathTest, DieOnNoU) {
// Expect death if the user does not provide U when creating a fresh
// measurement.
- EXPECT_DEATH(ekf_.Correct({1, 2, 3}, nullptr, {}, {}, {}, {},
- t0_ + std::chrono::seconds(1)),
- "U != nullptr");
+ EXPECT_DEATH(
+ ekf_.Correct({1, 2, 3}, nullptr, {}, {}, {}, Eigen::Matrix3d::Zero(),
+ t0_ + std::chrono::seconds(1)),
+ "U != nullptr");
}
// Because the user can choose to provide only one of make_h or (h, dhdx), check
@@ -503,21 +504,22 @@
// Check that we die when no h-related functions are provided:
Input U;
U << 1.0, 2.0, 0.0, 0.0;
- EXPECT_DEATH(ekf_.Correct({1, 2, 3}, &U, {}, {}, {}, {},
+ EXPECT_DEATH(ekf_.Correct({1, 2, 3}, &U, {}, {}, {}, Eigen::Matrix3d::Zero(),
t0_ + std::chrono::seconds(1)),
"make_h");
// Check that we die when only one of h and dhdx are provided:
- EXPECT_DEATH(ekf_.Correct({1, 2, 3}, &U, {}, {},
- [](const State &) {
- return Eigen::Matrix<double, 3, 12>::Zero();
- },
- {}, t0_ + std::chrono::seconds(1)),
- "make_h");
- EXPECT_DEATH(ekf_.Correct({1, 2, 3}, &U, {},
- [](const State &, const Input &) {
- return Eigen::Matrix<double, 3, 1>::Zero();
- },
- {}, {}, t0_ + std::chrono::seconds(1)),
+ EXPECT_DEATH(
+ ekf_.Correct(
+ {1, 2, 3}, &U, {}, {},
+ [](const State &) { return Eigen::Matrix<double, 3, 12>::Zero(); },
+ Eigen::Matrix3d::Zero(), t0_ + std::chrono::seconds(1)),
+ "make_h");
+ EXPECT_DEATH(ekf_.Correct(
+ {1, 2, 3}, &U, {},
+ [](const State &, const Input &) {
+ return Eigen::Matrix<double, 3, 1>::Zero();
+ },
+ {}, Eigen::Matrix3d::Zero(), t0_ + std::chrono::seconds(1)),
"make_h");
}
diff --git a/frc971/wpilib/ADIS16448.cc b/frc971/wpilib/ADIS16448.cc
index 4be346c..597c5e8 100644
--- a/frc971/wpilib/ADIS16448.cc
+++ b/frc971/wpilib/ADIS16448.cc
@@ -128,9 +128,7 @@
// 0.781MHz, so that's what this actually does.
spi_->SetClockRate(1e5);
spi_->SetChipSelectActiveLow();
- spi_->SetClockActiveLow();
- spi_->SetSampleDataOnFalling();
- spi_->SetMSBFirst();
+ spi_->SetMode(frc::SPI::Mode::kMode3);
dio1_->RequestInterrupts();
dio1_->SetUpSourceEdge(true, false);
@@ -154,9 +152,7 @@
if (dummy_spi_) {
dummy_spi_->SetClockRate(1e5);
dummy_spi_->SetChipSelectActiveLow();
- dummy_spi_->SetClockActiveLow();
- dummy_spi_->SetSampleDataOnFalling();
- dummy_spi_->SetMSBFirst();
+ dummy_spi_->SetMode(frc::SPI::Mode::kMode3);
}
}
diff --git a/frc971/wpilib/ADIS16470.cc b/frc971/wpilib/ADIS16470.cc
index b7b315b..d43d2fb 100644
--- a/frc971/wpilib/ADIS16470.cc
+++ b/frc971/wpilib/ADIS16470.cc
@@ -204,9 +204,7 @@
// We're not doing burst mode, so this is the IMU's rated speed.
spi_->SetClockRate(2'000'000);
spi_->SetChipSelectActiveLow();
- spi_->SetClockActiveLow();
- spi_->SetSampleDataOnTrailingEdge();
- spi_->SetMSBFirst();
+ spi_->SetMode(frc::SPI::Mode::kMode3);
// NI's SPI driver defaults to SCHED_OTHER. Find it's PID with ps, and change
// it to a RT priority of 33.
diff --git a/frc971/wpilib/ahal/DriverStation.cc b/frc971/wpilib/ahal/DriverStation.cc
index 8b31af7..8762203 100644
--- a/frc971/wpilib/ahal/DriverStation.cc
+++ b/frc971/wpilib/ahal/DriverStation.cc
@@ -8,9 +8,9 @@
#include "frc971/wpilib/ahal/DriverStation.h"
#include <chrono>
+#include <functional>
#include <memory>
#include <string_view>
-#include <functional>
#include "FRC_NetworkCommunication/FRCComm.h"
#include "frc971/wpilib/ahal/AnalogInput.h"
@@ -272,10 +272,10 @@
}
/**
- * Returns the game specific message provided by the FMS.
- *
- * @return A string containing the game specific message.
- */
+ * Returns the game specific message provided by the FMS.
+ *
+ * @return A string containing the game specific message.
+ */
std::string_view DriverStation::GetGameSpecificMessage() const {
return std::string_view(
reinterpret_cast<const char *>(info_.gameSpecificMessage),
@@ -283,41 +283,35 @@
}
/**
- * Returns the name of the competition event provided by the FMS.
- *
- * @return A string containing the event name
- */
-std::string_view DriverStation::GetEventName() const {
- return info_.eventName;
-}
+ * Returns the name of the competition event provided by the FMS.
+ *
+ * @return A string containing the event name
+ */
+std::string_view DriverStation::GetEventName() const { return info_.eventName; }
/**
- * Returns the match number provided by the FMS.
- *
- * @return The number of the match
- */
+ * Returns the match number provided by the FMS.
+ *
+ * @return The number of the match
+ */
DriverStation::MatchType DriverStation::GetMatchType() const {
return static_cast<DriverStation::MatchType>(info_.matchType);
}
/**
- * Returns the match number provided by the FMS.
- *
- * @return The number of the match
- */
-int DriverStation::GetMatchNumber() const {
- return info_.matchNumber;
-}
+ * Returns the match number provided by the FMS.
+ *
+ * @return The number of the match
+ */
+int DriverStation::GetMatchNumber() const { return info_.matchNumber; }
/**
- * Returns the number of times the current match has been replayed from the
- * FMS.
- *
- * @return The number of replays
- */
-int DriverStation::GetReplayNumber() const {
- return info_.replayNumber;
-}
+ * Returns the number of times the current match has been replayed from the
+ * FMS.
+ *
+ * @return The number of replays
+ */
+int DriverStation::GetReplayNumber() const { return info_.replayNumber; }
/**
* Return the alliance that the driver station says it is on.
@@ -408,6 +402,8 @@
* the data will be copied from the DS polling loop.
*/
void DriverStation::GetData() {
+ HAL_RefreshDSData();
+
// Get the status of all of the joysticks, and save to the cache
for (uint8_t stick = 0; stick < kJoystickPorts; stick++) {
HAL_GetJoystickAxes(stick, &m_joystickAxesCache[stick]);
@@ -475,7 +471,6 @@
}
void DriverStation::RunIteration(std::function<void()> on_data) {
- HAL_WaitForDSData();
GetData();
// We have to feed some sort of watchdog so that the driver's station knows
diff --git a/frc971/wpilib/ahal/DutyCycle.cc b/frc971/wpilib/ahal/DutyCycle.cc
index 13ccaa0..b400cf3 100644
--- a/frc971/wpilib/ahal/DutyCycle.cc
+++ b/frc971/wpilib/ahal/DutyCycle.cc
@@ -75,13 +75,6 @@
return retVal;
}
-unsigned int DutyCycle::GetOutputRaw() const {
- int32_t status = 0;
- auto retVal = HAL_GetDutyCycleOutputRaw(m_handle, &status);
- wpi_setHALError(status);
- return retVal;
-}
-
unsigned int DutyCycle::GetOutputScaleFactor() const {
int32_t status = 0;
auto retVal = HAL_GetDutyCycleOutputScaleFactor(m_handle, &status);
diff --git a/frc971/wpilib/ahal/DutyCycle.h b/frc971/wpilib/ahal/DutyCycle.h
index cfc9256..88d6a54 100644
--- a/frc971/wpilib/ahal/DutyCycle.h
+++ b/frc971/wpilib/ahal/DutyCycle.h
@@ -84,16 +84,6 @@
double GetOutput() const;
/**
- * Get the raw output ratio of the duty cycle signal.
- *
- * <p> 0 means always low, an output equal to
- * GetOutputScaleFactor() means always high.
- *
- * @return output ratio in raw units
- */
- unsigned int GetOutputRaw() const;
-
- /**
* Get the scale factor of the output.
*
* <p> An output equal to this value is always high, and then linearly scales
diff --git a/frc971/wpilib/ahal/SPI.cc b/frc971/wpilib/ahal/SPI.cc
index d51b20f..8568210 100644
--- a/frc971/wpilib/ahal/SPI.cc
+++ b/frc971/wpilib/ahal/SPI.cc
@@ -7,13 +7,13 @@
#include "frc971/wpilib/ahal/SPI.h"
-#include <cstring>
-#include <utility>
-
#include <hal/SPI.h>
#include <wpi/SmallVector.h>
#include <wpi/mutex.h>
+#include <cstring>
+#include <utility>
+
#include "absl/types/span.h"
#include "frc971/wpilib/ahal/DigitalSource.h"
#include "frc971/wpilib/ahal/WPIErrors.h"
@@ -30,44 +30,37 @@
void SPI::SetClockRate(int hz) { HAL_SetSPISpeed(m_port, hz); }
-void SPI::SetMSBFirst() {
- m_msbFirst = true;
- HAL_SetSPIOpts(m_port, m_msbFirst, m_sampleOnTrailing, m_clockIdleHigh);
-}
-
-void SPI::SetLSBFirst() {
- m_msbFirst = false;
- HAL_SetSPIOpts(m_port, m_msbFirst, m_sampleOnTrailing, m_clockIdleHigh);
+void SPI::SetMode(Mode mode) {
+ m_mode = static_cast<HAL_SPIMode>(mode & 0x3);
+ HAL_SetSPIMode(m_port, m_mode);
}
void SPI::SetSampleDataOnLeadingEdge() {
- m_sampleOnTrailing = false;
- HAL_SetSPIOpts(m_port, m_msbFirst, m_sampleOnTrailing, m_clockIdleHigh);
+ int mode = m_mode;
+ mode &= 2;
+ m_mode = static_cast<HAL_SPIMode>(mode);
+ HAL_SetSPIMode(m_port, m_mode);
}
void SPI::SetSampleDataOnTrailingEdge() {
- m_sampleOnTrailing = true;
- HAL_SetSPIOpts(m_port, m_msbFirst, m_sampleOnTrailing, m_clockIdleHigh);
-}
-
-void SPI::SetSampleDataOnFalling() {
- m_sampleOnTrailing = true;
- HAL_SetSPIOpts(m_port, m_msbFirst, m_sampleOnTrailing, m_clockIdleHigh);
-}
-
-void SPI::SetSampleDataOnRising() {
- m_sampleOnTrailing = false;
- HAL_SetSPIOpts(m_port, m_msbFirst, m_sampleOnTrailing, m_clockIdleHigh);
+ int mode = m_mode;
+ mode |= 2;
+ m_mode = static_cast<HAL_SPIMode>(mode);
+ HAL_SetSPIMode(m_port, m_mode);
}
void SPI::SetClockActiveLow() {
- m_clockIdleHigh = true;
- HAL_SetSPIOpts(m_port, m_msbFirst, m_sampleOnTrailing, m_clockIdleHigh);
+ int mode = m_mode;
+ mode |= 1;
+ m_mode = static_cast<HAL_SPIMode>(mode);
+ HAL_SetSPIMode(m_port, m_mode);
}
void SPI::SetClockActiveHigh() {
- m_clockIdleHigh = false;
- HAL_SetSPIOpts(m_port, m_msbFirst, m_sampleOnTrailing, m_clockIdleHigh);
+ int mode = m_mode;
+ mode &= 1;
+ m_mode = static_cast<HAL_SPIMode>(mode);
+ HAL_SetSPIMode(m_port, m_mode);
}
void SPI::SetChipSelectActiveHigh() {
@@ -82,13 +75,13 @@
wpi_setHALError(status);
}
-int SPI::Write(uint8_t* data, int size) {
+int SPI::Write(uint8_t *data, int size) {
int retVal = 0;
retVal = HAL_WriteSPI(m_port, data, size);
return retVal;
}
-int SPI::Read(bool initiate, uint8_t* dataReceived, int size) {
+int SPI::Read(bool initiate, uint8_t *dataReceived, int size) {
int retVal = 0;
if (initiate) {
wpi::SmallVector<uint8_t, 32> dataToSend;
@@ -100,7 +93,7 @@
return retVal;
}
-int SPI::Transaction(uint8_t* dataToSend, uint8_t* dataReceived, int size) {
+int SPI::Transaction(uint8_t *dataToSend, uint8_t *dataReceived, int size) {
int retVal = 0;
retVal = HAL_TransactionSPI(m_port, dataToSend, dataReceived, size);
return retVal;
@@ -132,7 +125,7 @@
wpi_setHALError(status);
}
-void SPI::StartAutoTrigger(DigitalSource& source, bool rising, bool falling) {
+void SPI::StartAutoTrigger(DigitalSource &source, bool rising, bool falling) {
int32_t status = 0;
HAL_StartSPIAutoTrigger(
m_port, source.GetPortHandleForRouting(),
@@ -155,8 +148,8 @@
int SPI::ReadAutoReceivedData(uint32_t *buffer, int numToRead, double timeout) {
int32_t status = 0;
- int32_t val = HAL_ReadSPIAutoReceivedData(m_port, buffer, numToRead,
- timeout, &status);
+ int32_t val =
+ HAL_ReadSPIAutoReceivedData(m_port, buffer, numToRead, timeout, &status);
wpi_setHALError(status);
return val;
}
diff --git a/frc971/wpilib/ahal/SPI.h b/frc971/wpilib/ahal/SPI.h
index 6421535..542db80 100644
--- a/frc971/wpilib/ahal/SPI.h
+++ b/frc971/wpilib/ahal/SPI.h
@@ -9,11 +9,12 @@
#include <hal/SPITypes.h>
#include <wpi/deprecated.h>
-#include <wpi/span.h>
-#include "absl/types/span.h"
#include <cstdint>
#include <memory>
+#include <span>
+
+#include "absl/types/span.h"
namespace frc {
@@ -29,6 +30,12 @@
class SPI final {
public:
enum Port { kOnboardCS0 = 0, kOnboardCS1, kOnboardCS2, kOnboardCS3, kMXP };
+ enum Mode {
+ kMode0 = HAL_SPI_kMode0,
+ kMode1 = HAL_SPI_kMode1,
+ kMode2 = HAL_SPI_kMode2,
+ kMode3 = HAL_SPI_kMode3
+ };
/**
* Constructor
@@ -55,54 +62,73 @@
/**
* Configure the order that bits are sent and received on the wire
* to be most significant bit first.
+ *
+ * @deprecated Does not work, will be removed.
*/
+ WPI_DEPRECATED("Not supported by roboRIO.")
void SetMSBFirst();
/**
* Configure the order that bits are sent and received on the wire
* to be least significant bit first.
+ *
+ * @deprecated Does not work, will be removed.
*/
+ WPI_DEPRECATED("Not supported by roboRIO.")
void SetLSBFirst();
/**
* Configure that the data is stable on the leading edge and the data
* changes on the trailing edge.
+ *
+ * @deprecated Use SetMode() instead.
*/
+ WPI_DEPRECATED("Use SetMode() instead")
void SetSampleDataOnLeadingEdge();
/**
* Configure that the data is stable on the trailing edge and the data
* changes on the leading edge.
+ *
+ * @deprecated Use SetMode() instead.
*/
+ WPI_DEPRECATED("Use SetMode() instead")
void SetSampleDataOnTrailingEdge();
/**
- * Configure that the data is stable on the falling edge and the data
- * changes on the rising edge.
- */
- WPI_DEPRECATED("Use SetSampleDataOnTrailingEdge in most cases.")
- void SetSampleDataOnFalling();
-
- /**
- * Configure that the data is stable on the rising edge and the data
- * changes on the falling edge.
- */
- WPI_DEPRECATED("Use SetSampleDataOnLeadingEdge in most cases")
- void SetSampleDataOnRising();
-
- /**
* Configure the clock output line to be active low.
* This is sometimes called clock polarity high or clock idle high.
+ *
+ * @deprecated Use SetMode() instead.
*/
+ WPI_DEPRECATED("Use SetMode() instead")
void SetClockActiveLow();
/**
* Configure the clock output line to be active high.
* This is sometimes called clock polarity low or clock idle low.
+ *
+ * @deprecated Use SetMode() instead.
*/
+ WPI_DEPRECATED("Use SetMode() instead")
void SetClockActiveHigh();
/**
+ * Sets the mode for the SPI device.
+ *
+ * <p>Mode 0 is Clock idle low, data sampled on rising edge
+ *
+ * <p>Mode 1 is Clock idle low, data sampled on falling edge
+ *
+ * <p>Mode 2 is Clock idle high, data sampled on falling edge
+ *
+ * <p>Mode 3 is Clock idle high, data sampled on rising edge
+ *
+ * @param mode The mode to set.
+ */
+ void SetMode(Mode mode);
+
+ /**
* Configure the chip select line to be active high.
*/
void SetChipSelectActiveHigh();
@@ -247,9 +273,7 @@
protected:
hal::SPIPort m_port;
- bool m_msbFirst = false; // Default little-endian
- bool m_sampleOnTrailing = false; // Default data updated on falling edge
- bool m_clockIdleHigh = false; // Default clock active high
+ HAL_SPIMode m_mode = HAL_SPIMode::HAL_SPI_kMode0;
private:
void Init();
diff --git a/frc971/wpilib/dma.cc b/frc971/wpilib/dma.cc
index 6cc8bf0..a2b2d20 100644
--- a/frc971/wpilib/dma.cc
+++ b/frc971/wpilib/dma.cc
@@ -317,7 +317,9 @@
}
}
-static_assert(::std::is_pod<DMASample>::value, "DMASample needs to be POD");
+static_assert(::std::is_trivial<DMASample>::value &&
+ ::std::is_standard_layout<DMASample>::value,
+ "DMASample needs to be POD");
ssize_t DMASample::offset(int index) const {
return dma_->channel_offsets_[index];
diff --git a/frc971/wpilib/gyro_interface.cc b/frc971/wpilib/gyro_interface.cc
index 7ed24b7..a90e95a 100644
--- a/frc971/wpilib/gyro_interface.cc
+++ b/frc971/wpilib/gyro_interface.cc
@@ -18,9 +18,7 @@
// The myRIO goes up to 4MHz, so the roboRIO probably does too.
gyro_->SetClockRate(4e6);
gyro_->SetChipSelectActiveLow();
- gyro_->SetClockActiveHigh();
- gyro_->SetSampleDataOnRising();
- gyro_->SetMSBFirst();
+ gyro_->SetMode(frc::SPI::Mode::kMode3);
}
bool GyroInterface::InitializeGyro() {
diff --git a/frc971/wpilib/joystick_sender.cc b/frc971/wpilib/joystick_sender.cc
index 9137a99..26f7545 100644
--- a/frc971/wpilib/joystick_sender.cc
+++ b/frc971/wpilib/joystick_sender.cc
@@ -24,9 +24,13 @@
event_loop_->OnRun([this]() {
frc::DriverStation *const ds = &frc::DriverStation::GetInstance();
+ wpi::Event event{false, false};
+ HAL_ProvideNewDataEventHandle(event.GetHandle());
+
// TODO(Brian): Fix the potential deadlock when stopping here (condition
// variable / mutex needs to get exposed all the way out or something).
while (event_loop_->is_running()) {
+ wpi::WaitForObject(event.GetHandle());
ds->RunIteration([&]() {
auto builder = joystick_state_sender_.MakeBuilder();
@@ -129,6 +133,8 @@
}
});
}
+
+ HAL_RemoveNewDataEventHandle(event.GetHandle());
});
}