diff --git a/hal/src/main/native/sim/DIO.cpp b/hal/src/main/native/sim/DIO.cpp index e89c107e47..5a21122767 100644 --- a/hal/src/main/native/sim/DIO.cpp +++ b/hal/src/main/native/sim/DIO.cpp @@ -277,8 +277,8 @@ void HAL_SetFilterSelect(HAL_DigitalHandle dioPortHandle, int32_t filterIndex, *status = HAL_HANDLE_ERROR; return; } - - // TODO(Thad) Figure this out + // mimics athena HAL + port->filterIndex = filterIndex % 4; } int32_t HAL_GetFilterSelect(HAL_DigitalHandle dioPortHandle, int32_t* status) { @@ -287,8 +287,7 @@ int32_t HAL_GetFilterSelect(HAL_DigitalHandle dioPortHandle, int32_t* status) { *status = HAL_HANDLE_ERROR; return 0; } - return 0; - // TODO(Thad) Figure this out + return port->filterIndex; } void HAL_SetFilterPeriod(int32_t filterIndex, int64_t value, int32_t* status) { diff --git a/hal/src/main/native/sim/DigitalInternal.h b/hal/src/main/native/sim/DigitalInternal.h index 8fa679a1c4..4bca51f11f 100644 --- a/hal/src/main/native/sim/DigitalInternal.h +++ b/hal/src/main/native/sim/DigitalInternal.h @@ -50,6 +50,7 @@ struct DigitalPort { int32_t centerPwm = 0; int32_t deadbandMinPwm = 0; int32_t minPwm = 0; + int32_t filterIndex = 0; std::string previousAllocation; }; diff --git a/wpilibc/src/main/native/cpp/DigitalGlitchFilter.cpp b/wpilibc/src/main/native/cpp/DigitalGlitchFilter.cpp index 4abb7cfb83..44bcabe463 100644 --- a/wpilibc/src/main/native/cpp/DigitalGlitchFilter.cpp +++ b/wpilibc/src/main/native/cpp/DigitalGlitchFilter.cpp @@ -25,14 +25,11 @@ std::array DigitalGlitchFilter::m_filterAllocated = { wpi::mutex DigitalGlitchFilter::m_mutex; DigitalGlitchFilter::DigitalGlitchFilter() { - std::scoped_lock lock(m_mutex); - auto index = - std::find(m_filterAllocated.begin(), m_filterAllocated.end(), false); - FRC_Assert(index != m_filterAllocated.end()); - - m_channelIndex = std::distance(m_filterAllocated.begin(), index); - *index = true; - + m_channelIndex = AllocateFilterIndex(); + if (m_channelIndex < 0) { + throw FRC_MakeError(err::NoAvailableResources, + "No filters available to allocate."); + } HAL_Report(HALUsageReporting::kResourceType_DigitalGlitchFilter, m_channelIndex + 1); wpi::SendableRegistry::AddLW(this, "DigitalGlitchFilter", m_channelIndex); @@ -45,6 +42,18 @@ DigitalGlitchFilter::~DigitalGlitchFilter() { } } +int DigitalGlitchFilter::AllocateFilterIndex() { + std::scoped_lock lock{m_mutex}; + auto filter = + std::find(m_filterAllocated.begin(), m_filterAllocated.end(), false); + + if (filter == m_filterAllocated.end()) { + return -1; + } + *filter = true; + return std::distance(m_filterAllocated.begin(), filter); +} + void DigitalGlitchFilter::Add(DigitalSource* input) { DoAdd(input, m_channelIndex + 1); } @@ -67,7 +76,9 @@ void DigitalGlitchFilter::DoAdd(DigitalSource* input, int requestedIndex) { int actualIndex = HAL_GetFilterSelect(input->GetPortHandleForRouting(), &status); FRC_CheckErrorStatus(status, "requested index {}", requestedIndex); - FRC_Assert(actualIndex == requestedIndex); + FRC_AssertMessage(actualIndex == requestedIndex, + "HAL_SetFilterSelect({}) failed -> {}", requestedIndex, + actualIndex); } } diff --git a/wpilibc/src/main/native/include/frc/DigitalGlitchFilter.h b/wpilibc/src/main/native/include/frc/DigitalGlitchFilter.h index 7172f21db3..05ddded313 100644 --- a/wpilibc/src/main/native/include/frc/DigitalGlitchFilter.h +++ b/wpilibc/src/main/native/include/frc/DigitalGlitchFilter.h @@ -117,12 +117,20 @@ class DigitalGlitchFilter : public wpi::Sendable, void InitSendable(wpi::SendableBuilder& builder) override; private: + int m_channelIndex; + // Sets the filter for the input to be the requested index. A value of 0 // disables the filter, and the filter value must be between 1 and 3, // inclusive. - void DoAdd(DigitalSource* input, int requested_index); + static void DoAdd(DigitalSource* input, int requested_index); + + /** + * Allocates the next available filter index, or -1 if there are no filters + * available. + * @return the filter index + */ + static int AllocateFilterIndex(); - int m_channelIndex = -1; static wpi::mutex m_mutex; static std::array m_filterAllocated; }; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalGlitchFilter.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalGlitchFilter.java index 7bc705deb2..9d3086e952 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalGlitchFilter.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalGlitchFilter.java @@ -7,6 +7,7 @@ package edu.wpi.first.wpilibj; import edu.wpi.first.hal.DigitalGlitchFilterJNI; import edu.wpi.first.hal.FRCNetComm.tResourceType; import edu.wpi.first.hal.HAL; +import edu.wpi.first.hal.util.AllocationException; import edu.wpi.first.util.sendable.Sendable; import edu.wpi.first.util.sendable.SendableBuilder; import edu.wpi.first.util.sendable.SendableRegistry; @@ -22,21 +23,12 @@ public class DigitalGlitchFilter implements Sendable, AutoCloseable { /** Configures the Digital Glitch Filter to its default settings. */ @SuppressWarnings("this-escape") public DigitalGlitchFilter() { - m_mutex.lock(); - try { - int index = 0; - while (m_filterAllocated[index] && index < m_filterAllocated.length) { - index++; - } - if (index != m_filterAllocated.length) { - m_channelIndex = index; - m_filterAllocated[index] = true; - HAL.report(tResourceType.kResourceType_DigitalGlitchFilter, m_channelIndex + 1, 0); - SendableRegistry.addLW(this, "DigitalGlitchFilter", index); - } - } finally { - m_mutex.unlock(); + m_channelIndex = allocateFilterIndex(); + if (m_channelIndex < 0) { + throw new AllocationException("No filters available to allocate."); } + HAL.report(tResourceType.kResourceType_DigitalGlitchFilter, m_channelIndex + 1, 0); + SendableRegistry.addLW(this, "DigitalGlitchFilter", m_channelIndex); } @Override @@ -54,6 +46,26 @@ public class DigitalGlitchFilter implements Sendable, AutoCloseable { } } + /** + * Allocates the next available filter index, or -1 if there are no filters available. + * + * @return the filter index + */ + private static int allocateFilterIndex() { + m_mutex.lock(); + try { + for (int index = 0; index < m_filterAllocated.length; index++) { + if (!m_filterAllocated[index]) { + m_filterAllocated[index] = true; + return index; + } + } + } finally { + m_mutex.unlock(); + } + return -1; + } + private static void setFilter(DigitalSource input, int channelIndex) { if (input != null) { // Counter might have just one input // analog triggers are not supported for DigitalGlitchFilters @@ -174,7 +186,7 @@ public class DigitalGlitchFilter implements Sendable, AutoCloseable { @Override public void initSendable(SendableBuilder builder) {} - private int m_channelIndex = -1; + private int m_channelIndex; private static final Lock m_mutex = new ReentrantLock(true); private static final boolean[] m_filterAllocated = new boolean[3]; }