diff --git a/hal/src/main/native/athena/HAL.cpp b/hal/src/main/native/athena/HAL.cpp index 0b6d05efae..93e5c15d14 100644 --- a/hal/src/main/native/athena/HAL.cpp +++ b/hal/src/main/native/athena/HAL.cpp @@ -34,6 +34,7 @@ using namespace hal; static std::unique_ptr global; static std::unique_ptr watchdog; +static uint64_t dsStartTime; using namespace hal; @@ -86,6 +87,11 @@ void ReleaseFPGAInterrupt(int32_t interruptNumber) { &status); global->strobeInterruptForceOnce(&status); } + +uint64_t GetDSInitializeTime() { + return dsStartTime; +} + } // namespace hal extern "C" { @@ -420,6 +426,11 @@ HAL_Bool HAL_Initialize(int32_t timeout, int32_t mode) { HAL_InitializeDriverStation(); + dsStartTime = HAL_GetFPGATime(&status); + if (status != 0) { + return false; + } + // Set WPI_Now to use FPGA timestamp wpi::SetNowImpl([]() -> uint64_t { int32_t status = 0; diff --git a/hal/src/main/native/athena/HALInternal.h b/hal/src/main/native/athena/HALInternal.h index 64a0dca368..52a818bf4f 100644 --- a/hal/src/main/native/athena/HALInternal.h +++ b/hal/src/main/native/athena/HALInternal.h @@ -17,4 +17,5 @@ void SetLastErrorIndexOutOfRange(int32_t* status, std::string_view message, void SetLastErrorPreviouslyAllocated(int32_t* status, std::string_view message, int32_t channel, std::string_view previousAllocation); +uint64_t GetDSInitializeTime(); } // namespace hal diff --git a/hal/src/main/native/athena/PowerDistribution.cpp b/hal/src/main/native/athena/PowerDistribution.cpp index f7fe88faaa..cc6644382d 100644 --- a/hal/src/main/native/athena/PowerDistribution.cpp +++ b/hal/src/main/native/athena/PowerDistribution.cpp @@ -4,11 +4,14 @@ #include "hal/PowerDistribution.h" +#include + #include "CTREPDP.h" #include "HALInternal.h" #include "PortsInternal.h" #include "REVPDH.h" #include "hal/Errors.h" +#include "hal/HALBase.h" #include "hal/handles/HandlesInternal.h" using namespace hal; @@ -19,7 +22,41 @@ HAL_PowerDistributionHandle HAL_InitializePowerDistribution( int32_t moduleNumber, HAL_PowerDistributionType type, const char* allocationLocation, int32_t* status) { if (type == HAL_PowerDistributionType::HAL_PowerDistributionType_kAutomatic) { - type = HAL_PowerDistributionType::HAL_PowerDistributionType_kCTRE; + if (moduleNumber != HAL_DEFAULT_POWER_DISTRIBUTION_MODULE) { + *status = PARAMETER_OUT_OF_RANGE; + hal::SetLastError( + status, "Automatic PowerDistributionType must have default module"); + return HAL_kInvalidHandle; + } + + uint64_t waitTime = hal::GetDSInitializeTime() + 400000; + + // Ensure we have been alive for long enough to receive a few Power packets. + do { + uint64_t currentTime = HAL_GetFPGATime(status); + if (*status != 0) { + return HAL_kInvalidHandle; + } + if (currentTime >= waitTime) { + break; + } + std::this_thread::sleep_for( + std::chrono::microseconds(waitTime - currentTime)); + } while (true); + + // Try PDP first + auto pdpHandle = HAL_InitializePDP(0, allocationLocation, status); + if (pdpHandle != HAL_kInvalidHandle) { + *status = 0; + HAL_GetPDPVoltage(pdpHandle, status); + if (*status == 0 || *status == HAL_CAN_TIMEOUT) { + return static_cast(pdpHandle); + } + HAL_CleanPDP(pdpHandle); + } + *status = 0; + auto pdhHandle = HAL_REV_InitializePDH(1, allocationLocation, status); + return static_cast(pdhHandle); } if (type == HAL_PowerDistributionType::HAL_PowerDistributionType_kCTRE) { @@ -27,7 +64,7 @@ HAL_PowerDistributionHandle HAL_InitializePowerDistribution( moduleNumber = 0; } return static_cast( - HAL_InitializePDP(moduleNumber, allocationLocation, status)); // TODO + HAL_InitializePDP(moduleNumber, allocationLocation, status)); } else { if (moduleNumber == HAL_DEFAULT_POWER_DISTRIBUTION_MODULE) { moduleNumber = 1;