Add systemcore analog input

wpilibsuite · Jan 14, 2025 · 7bfca9c · 7bfca9c
1 parent afbaa43
commit 7bfca9c
Show file tree

Hide file tree

Showing 3 changed files with 92 additions and 8 deletions.
diff --git a/hal/src/main/native/systemcore/AnalogInput.cpp b/hal/src/main/native/systemcore/AnalogInput.cpp
@@ -5,12 +5,15 @@
 #include "hal/AnalogInput.h"
 
 #include <wpi/mutex.h>
+#include <thread>
 
 #include "HALInitializer.h"
 #include "HALInternal.h"
 #include "PortsInternal.h"
+#include "SmartIo.h"
 #include "hal/AnalogAccumulator.h"
 #include "hal/Errors.h"
+#include "hal/cpp/fpga_clock.h"
 #include "hal/handles/HandlesInternal.h"
 
 namespace hal::init {
@@ -25,18 +28,71 @@ HAL_AnalogInputHandle HAL_InitializeAnalogInputPort(
     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 Analog", 0,
+                                     kNumSmartIo, channel);
+    return HAL_kInvalidHandle;
+  }
+
+  HAL_DigitalHandle handle;
+
+  auto port =
+      smartIoHandles->Allocate(channel, HAL_HandleEnum::AnalogInput, &handle, status);
+
+  if (*status != 0) {
+    if (port) {
+      hal::SetLastErrorPreviouslyAllocated(status, "SmartIo", channel,
+                                           port->previousAllocation);
+    } else {
+      hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for Analog", 0,
+                                       kNumSmartIo, channel);
+    }
+    return HAL_kInvalidHandle;  // failed to allocate. Pass error back.
+  }
+
+  port->channel = channel;
+
+  *status = port->InitializeMode(SmartIoMode::AnalogInput);
+  if (*status != 0) {
+    smartIoHandles->Free(handle, HAL_HandleEnum::AnalogInput);
+    return HAL_kInvalidHandle;
+  }
+
+  port->previousAllocation = allocationLocation ? allocationLocation : "";
+
+  return handle;
 }
 
-void HAL_FreeAnalogInputPort(HAL_AnalogInputHandle analogPortHandle) {}
+void HAL_FreeAnalogInputPort(HAL_AnalogInputHandle analogPortHandle) {
+  auto port = smartIoHandles->Get(analogPortHandle, HAL_HandleEnum::AnalogInput);
+  if (port == nullptr) {
+    return;
+  }
+
+  smartIoHandles->Free(analogPortHandle, HAL_HandleEnum::AnalogInput);
+
+  // 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();
+  }
+}
 
 HAL_Bool HAL_CheckAnalogModule(int32_t module) {
   return module == 1;
 }
 
 HAL_Bool HAL_CheckAnalogInputChannel(int32_t channel) {
-  return channel < kNumAnalogInputs && channel >= 0;
+  return channel < kNumSmartIo && channel >= 0;
 }
 
 void HAL_SetAnalogInputSimDevice(HAL_AnalogInputHandle handle,
@@ -78,8 +134,15 @@ int32_t HAL_GetAnalogOversampleBits(HAL_AnalogInputHandle analogPortHandle,
 
 int32_t HAL_GetAnalogValue(HAL_AnalogInputHandle analogPortHandle,
                            int32_t* status) {
-  *status = HAL_HANDLE_ERROR;
-  return 0;
+  auto port = smartIoHandles->Get(analogPortHandle, HAL_HandleEnum::AnalogInput);
+  if (port == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return 0;
+  }
+
+  uint16_t ret = 0;
+  *status = port->GetAnalogInput(&ret);
+  return ret;
 }
 
 int32_t HAL_GetAnalogAverageValue(HAL_AnalogInputHandle analogPortHandle,
@@ -96,8 +159,15 @@ int32_t HAL_GetAnalogVoltsToValue(HAL_AnalogInputHandle analogPortHandle,
 
 double HAL_GetAnalogVoltage(HAL_AnalogInputHandle analogPortHandle,
                             int32_t* status) {
-  *status = HAL_HANDLE_ERROR;
-  return 0;
+  auto port = smartIoHandles->Get(analogPortHandle, HAL_HandleEnum::AnalogInput);
+  if (port == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return 0;
+  }
+
+  uint16_t ret = 0;
+  *status = port->GetAnalogInput(&ret);
+  return ret / 1000.0;
 }
 
 double HAL_GetAnalogValueToVolts(HAL_AnalogInputHandle analogPortHandle,

diff --git a/hal/src/main/native/systemcore/SmartIo.cpp b/hal/src/main/native/systemcore/SmartIo.cpp
@@ -141,4 +141,16 @@ int32_t SmartIo::GetPwmMicroseconds(uint16_t* microseconds) {
   return 0;
 }
 
+int32_t SmartIo::GetAnalogInput(uint16_t* value) {
+  if (currentMode != SmartIoMode::AnalogInput) {
+    return INCOMPATIBLE_STATE;
+  }
+
+  int val = getSubscriber.Get();
+
+  *value = val;
+
+  return 0;
+}
+
 }  // namespace hal
diff --git a/hal/src/main/native/systemcore/SmartIo.h b/hal/src/main/native/systemcore/SmartIo.h
@@ -62,6 +62,8 @@ struct SmartIo {
 
   int32_t SetPwmMicroseconds(uint16_t microseconds);
   int32_t GetPwmMicroseconds(uint16_t* microseconds);
+
+  int32_t GetAnalogInput(uint16_t* value);
 };
 
 extern DigitalHandleResource<HAL_DigitalHandle, SmartIo, kNumSmartIo>*