[wpilib, hal] DigitalGlitchFilter: Fix sim crash and clean up construction (#6937)

Fixes error when >3 are constructed- in java, m_filterAllocated[index] would be evaluated before the bounds check and throw IndexOutOfBounds, in c++ a vague assertion error would be thrown.

Makes DoAdd static in c++

Makes the error message when HAL_SetFilterSelect fails consistent with java

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) {

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;
 };
 

wpilibc/src/main/native/cpp/DigitalGlitchFilter.cpp

@@ -25,14 +25,11 @@ std::array<bool, 3> 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);
   }
 }
 

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<bool, 3> m_filterAllocated;
 };

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];
 }