[wpilib] Remove InterruptableSensorBase and replace with interrupt classes (#2410)

wpilibc/src/main/native/cpp/SynchronousInterrupt.cpp

@@ -0,0 +1,105 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/SynchronousInterrupt.h"
+
+#include <type_traits>
+
+#include <hal/Interrupts.h>
+#include <wpi/NullDeleter.h>
+
+#include "frc/DigitalSource.h"
+#include "frc/Errors.h"
+
+using namespace frc;
+
+SynchronousInterrupt::SynchronousInterrupt(DigitalSource& source)
+    : m_source{&source, wpi::NullDeleter<DigitalSource>()} {
+  InitSynchronousInterrupt();
+}
+SynchronousInterrupt::SynchronousInterrupt(DigitalSource* source)
+    : m_source{source, wpi::NullDeleter<DigitalSource>()} {
+  if (m_source == nullptr) {
+    FRC_CheckErrorStatus(frc::err::NullParameter, "{}", "Source is null");
+  } else {
+    InitSynchronousInterrupt();
+  }
+}
+SynchronousInterrupt::SynchronousInterrupt(
+    std::shared_ptr<DigitalSource> source)
+    : m_source{std::move(source)} {
+  if (m_source == nullptr) {
+    FRC_CheckErrorStatus(frc::err::NullParameter, "{}", "Source is null");
+  } else {
+    InitSynchronousInterrupt();
+  }
+}
+
+void SynchronousInterrupt::InitSynchronousInterrupt() {
+  int32_t status = 0;
+  m_handle = HAL_InitializeInterrupts(&status);
+  FRC_CheckErrorStatus(status, "{}", "Interrupt failed to initialize");
+  HAL_RequestInterrupts(m_handle, m_source->GetPortHandleForRouting(),
+                        static_cast<HAL_AnalogTriggerType>(
+                            m_source->GetAnalogTriggerTypeForRouting()),
+                        &status);
+  FRC_CheckErrorStatus(status, "{}", "Interrupt request failed");
+  HAL_SetInterruptUpSourceEdge(m_handle, true, false, &status);
+  FRC_CheckErrorStatus(status, "{}", "Interrupt setting up source edge failed");
+}
+
+SynchronousInterrupt::~SynchronousInterrupt() {
+  HAL_CleanInterrupts(m_handle);
+}
+
+inline SynchronousInterrupt::WaitResult operator|(
+    SynchronousInterrupt::WaitResult lhs,
+    SynchronousInterrupt::WaitResult rhs) {
+  using T = std::underlying_type_t<SynchronousInterrupt::WaitResult>;
+  return static_cast<SynchronousInterrupt::WaitResult>(static_cast<T>(lhs) |
+                                                       static_cast<T>(rhs));
+}
+
+SynchronousInterrupt::WaitResult SynchronousInterrupt::WaitForInterrupt(
+    units::second_t timeout, bool ignorePrevious) {
+  int32_t status = 0;
+  auto result = HAL_WaitForInterrupt(m_handle, timeout.to<double>(),
+                                     ignorePrevious, &status);
+
+  auto rising =
+      ((result & 0xFF) != 0) ? WaitResult::kRisingEdge : WaitResult::kTimeout;
+  auto falling = ((result & 0xFF00) != 0) ? WaitResult::kFallingEdge
+                                          : WaitResult::kTimeout;
+
+  return rising | falling;
+}
+
+void SynchronousInterrupt::SetInterruptEdges(bool risingEdge,
+                                             bool fallingEdge) {
+  int32_t status = 0;
+  HAL_SetInterruptUpSourceEdge(m_handle, risingEdge, fallingEdge, &status);
+  FRC_CheckErrorStatus(status, "{}", "Interrupt setting edges failed");
+}
+
+void SynchronousInterrupt::WakeupWaitingInterrupt() {
+  int32_t status = 0;
+  HAL_ReleaseWaitingInterrupt(m_handle, &status);
+  FRC_CheckErrorStatus(status, "{}", "Interrupt wakeup failed");
+}
+
+units::second_t SynchronousInterrupt::GetRisingTimestamp() {
+  int32_t status = 0;
+  auto ts = HAL_ReadInterruptRisingTimestamp(m_handle, &status);
+  FRC_CheckErrorStatus(status, "{}", "Interrupt rising timestamp failed");
+  units::microsecond_t ms{static_cast<double>(ts)};
+  return ms;
+}
+
+units::second_t SynchronousInterrupt::GetFallingTimestamp() {
+  int32_t status = 0;
+  auto ts = HAL_ReadInterruptFallingTimestamp(m_handle, &status);
+  FRC_CheckErrorStatus(status, "{}", "Interrupt falling timestamp failed");
+  units::microsecond_t ms{static_cast<double>(ts)};
+  return ms;
+}