[hal] Add systemcore duty cycle (#7682)

hal/src/main/native/cpp/jni/DutyCycleJNI.cpp

@@ -4,6 +4,8 @@
 
 #include <jni.h>
 
+#include <wpi/jni_util.h>
+
 #include "HALUtil.h"
 #include "edu_wpi_first_hal_DutyCycleJNI.h"
 #include "hal/DutyCycle.h"
@@ -14,16 +16,16 @@ extern "C" {
 /*
  * Class:     edu_wpi_first_hal_DutyCycleJNI
  * Method:    initialize
- * Signature: (II)I
+ * Signature: (I)I
  */
 JNIEXPORT jint JNICALL
 Java_edu_wpi_first_hal_DutyCycleJNI_initialize
-  (JNIEnv* env, jclass, jint digitalSourceHandle, jint analogTriggerType)
+  (JNIEnv* env, jclass, jint portHandle)
 {
   int32_t status = 0;
-  auto handle = HAL_InitializeDutyCycle(
-      static_cast<HAL_Handle>(digitalSourceHandle),
-      static_cast<HAL_AnalogTriggerType>(analogTriggerType), &status);
+  auto stack = wpi::java::GetJavaStackTrace(env, "edu.wpi.first");
+  auto handle = HAL_InitializeDutyCycle(static_cast<HAL_Handle>(portHandle),
+                                        stack.c_str(), &status);
   CheckStatus(env, status);
   return handle;
 }

hal/src/main/native/include/hal/DutyCycle.h

@@ -20,16 +20,14 @@ extern "C" {
 /**
  * Initialize a DutyCycle input.
  *
- * @param[in] digitalSourceHandle the digital source to use (either a
- *                                 HAL_DigitalHandle or a
- *                                 HAL_AnalogTriggerHandle)
- * @param[in] triggerType the analog trigger type of the source if it is
- *                         an analog trigger
+ * @param[in] portHandle         the port handle to create from
+ * @param[in] allocationLocation the location where the allocation is occurring
+ *                               (can be null)
  * @param[out] status Error status variable. 0 on success.
  * @return the created duty cycle handle
  */
-HAL_DutyCycleHandle HAL_InitializeDutyCycle(HAL_Handle digitalSourceHandle,
-                                            HAL_AnalogTriggerType triggerType,
+HAL_DutyCycleHandle HAL_InitializeDutyCycle(HAL_PortHandle portHandle,
+                                            const char* allocationLocation,
                                             int32_t* status);
 
 /**

hal/src/main/native/sim/DutyCycle.cpp

@@ -33,8 +33,8 @@ void InitializeDutyCycle() {
 }  // namespace hal::init
 
 extern "C" {
-HAL_DutyCycleHandle HAL_InitializeDutyCycle(HAL_Handle digitalSourceHandle,
-                                            HAL_AnalogTriggerType triggerType,
+HAL_DutyCycleHandle HAL_InitializeDutyCycle(HAL_PortHandle portHandle,
+                                            const char* allocationLocation,
                                             int32_t* status) {
   hal::init::CheckInit();
 
@@ -51,7 +51,7 @@ HAL_DutyCycleHandle HAL_InitializeDutyCycle(HAL_Handle digitalSourceHandle,
   }
 
   int16_t index = getHandleIndex(handle);
-  SimDutyCycleData[index].digitalChannel = getHandleIndex(digitalSourceHandle);
+  SimDutyCycleData[index].digitalChannel = getPortHandleChannel(portHandle);
   SimDutyCycleData[index].initialized = true;
   SimDutyCycleData[index].simDevice = 0;
   dutyCycle->index = index;

hal/src/main/native/systemcore/DutyCycle.cpp

@@ -4,11 +4,16 @@
 
 #include "hal/DutyCycle.h"
 
+#include <cstdio>
 #include <memory>
+#include <thread>
 
 #include "HALInitializer.h"
+#include "HALInternal.h"
 #include "PortsInternal.h"
+#include "SmartIo.h"
 #include "hal/Errors.h"
+#include "hal/cpp/fpga_clock.h"
 #include "hal/handles/HandlesInternal.h"
 #include "hal/handles/LimitedHandleResource.h"
 
@@ -19,15 +24,67 @@ void InitializeDutyCycle() {}
 }  // namespace hal::init
 
 extern "C" {
-HAL_DutyCycleHandle HAL_InitializeDutyCycle(HAL_Handle digitalSourceHandle,
-                                            HAL_AnalogTriggerType triggerType,
+HAL_DutyCycleHandle HAL_InitializeDutyCycle(HAL_PortHandle portHandle,
+                                            const char* allocationLocation,
                                             int32_t* status) {
   hal::init::CheckInit();
 
-  *status = HAL_HANDLE_ERROR;
-  return HAL_kInvalidHandle;
+  int16_t channel = getPortHandleChannel(portHandle);
+  if (channel == InvalidHandleIndex || channel >= kNumSmartIo) {
+    *status = RESOURCE_OUT_OF_RANGE;
+    hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for DutyCycle", 0,
+                                     kNumSmartIo, channel);
+    return HAL_kInvalidHandle;
+  }
+
+  HAL_DigitalHandle handle;
+
+  auto port = smartIoHandles->Allocate(channel, HAL_HandleEnum::DutyCycle,
+                                       &handle, status);
+
+  if (*status != 0) {
+    if (port) {
+      hal::SetLastErrorPreviouslyAllocated(status, "SmartIo", channel,
+                                           port->previousAllocation);
+    } else {
+      hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for DutyCycle", 0,
+                                       kNumSmartIo, channel);
+    }
+    return HAL_kInvalidHandle;  // failed to allocate. Pass error back.
+  }
+
+  port->channel = channel;
+
+  *status = port->InitializeMode(SmartIoMode::PwmInput);
+  if (*status != 0) {
+    smartIoHandles->Free(handle, HAL_HandleEnum::DutyCycle);
+    return HAL_kInvalidHandle;
+  }
+
+  port->previousAllocation = allocationLocation ? allocationLocation : "";
+
+  return handle;
+}
+void HAL_FreeDutyCycle(HAL_DutyCycleHandle dutyCycleHandle) {
+  auto port = smartIoHandles->Get(dutyCycleHandle, HAL_HandleEnum::DutyCycle);
+  if (port == nullptr) {
+    return;
+  }
+
+  smartIoHandles->Free(dutyCycleHandle, HAL_HandleEnum::DutyCycle);
+
+  // Wait for no other object to hold this handle.
+  auto start = hal::fpga_clock::now();
+  while (port.use_count() != 1) {
+    auto current = hal::fpga_clock::now();
+    if (start + std::chrono::seconds(1) < current) {
+      std::puts("DIO handle free timeout");
+      std::fflush(stdout);
+      break;
+    }
+    std::this_thread::yield();
+  }
 }
-void HAL_FreeDutyCycle(HAL_DutyCycleHandle dutyCycleHandle) {}
 
 void HAL_SetDutyCycleSimDevice(HAL_EncoderHandle handle,
                                HAL_SimDeviceHandle device) {}
@@ -46,8 +103,15 @@ double HAL_GetDutyCycleOutput(HAL_DutyCycleHandle dutyCycleHandle,
 
 int32_t HAL_GetDutyCycleHighTime(HAL_DutyCycleHandle dutyCycleHandle,
                                  int32_t* status) {
-  *status = HAL_HANDLE_ERROR;
-  return 0;
+  auto port = smartIoHandles->Get(dutyCycleHandle, HAL_HandleEnum::DutyCycle);
+  if (port == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return false;
+  }
+
+  uint16_t ret = false;
+  *status = port->GetPwmInputMicroseconds(&ret);
+  return ret;
 }
 
 int32_t HAL_GetDutyCycleOutputScaleFactor(HAL_DutyCycleHandle dutyCycleHandle,

hal/src/main/native/systemcore/SmartIo.cpp

@@ -96,6 +96,17 @@ int32_t SmartIo::GetDigitalInput(bool* value) {
   return 0;
 }
 
+int32_t SmartIo::GetPwmInputMicroseconds(uint16_t* microseconds) {
+  if (currentMode != SmartIoMode::PwmInput) {
+    return INCOMPATIBLE_STATE;
+  }
+
+  int val = getSubscriber.Get();
+  *microseconds = val;
+
+  return 0;
+}
+
 int32_t SmartIo::SetPwmOutputPeriod(PwmOutputPeriod period) {
   if (currentMode != SmartIoMode::PwmOutput) {
     return INCOMPATIBLE_STATE;

hal/src/main/native/systemcore/SmartIo.h

@@ -58,6 +58,8 @@ struct SmartIo {
   int32_t SetDigitalOutput(bool value);
   int32_t GetDigitalInput(bool* value);
 
+  int32_t GetPwmInputMicroseconds(uint16_t* microseconds);
+
   int32_t SetPwmOutputPeriod(PwmOutputPeriod period);
 
   int32_t SetPwmMicroseconds(uint16_t microseconds);