diff --git a/hal/src/main/native/systemcore/AnalogInput.cpp b/hal/src/main/native/systemcore/AnalogInput.cpp index 920cf3fd439..2eebd68ef05 100644 --- 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 +#include #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 index b8fd11c37de..890096b95b8 100644 --- 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 index 80cdac6a99b..b1128314d86 100644 --- 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*