[hal] Add autodetect for power modules (#3706)

Default autodetect to REV if no devices are found, as only the REV devices have control functionality (the switchable output).

hal/src/main/native/athena/HAL.cpp

@@ -34,6 +34,7 @@ using namespace hal;
 
 static std::unique_ptr<tGlobal> global;
 static std::unique_ptr<tSysWatchdog> 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;

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

hal/src/main/native/athena/PowerDistribution.cpp

@@ -4,11 +4,14 @@
 
 #include "hal/PowerDistribution.h"
 
+#include <thread>
+
 #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<HAL_PowerDistributionHandle>(pdpHandle);
+      }
+      HAL_CleanPDP(pdpHandle);
+    }
+    *status = 0;
+    auto pdhHandle = HAL_REV_InitializePDH(1, allocationLocation, status);
+    return static_cast<HAL_PowerDistributionHandle>(pdhHandle);
   }
 
   if (type == HAL_PowerDistributionType::HAL_PowerDistributionType_kCTRE) {
@@ -27,7 +64,7 @@ HAL_PowerDistributionHandle HAL_InitializePowerDistribution(
       moduleNumber = 0;
     }
     return static_cast<HAL_PowerDistributionHandle>(
-        HAL_InitializePDP(moduleNumber, allocationLocation, status));  // TODO
+        HAL_InitializePDP(moduleNumber, allocationLocation, status));
   } else {
     if (moduleNumber == HAL_DEFAULT_POWER_DISTRIBUTION_MODULE) {
       moduleNumber = 1;