[hal] [wpilib] Add initial support for the REV PDH (#3503)

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

@@ -7,6 +7,7 @@
 #include "CTREPDP.h"
 #include "HALInternal.h"
 #include "PortsInternal.h"
+#include "REVPDH.h"
 #include "hal/Errors.h"
 #include "hal/handles/HandlesInternal.h"
 
@@ -15,18 +16,24 @@ using namespace hal;
 extern "C" {
 
 HAL_PowerDistributionHandle HAL_InitializePowerDistribution(
-    int32_t moduleNumber, HAL_PowerDistributionType type, int32_t* status) {
+    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 (type == HAL_PowerDistributionType::HAL_PowerDistributionType_kCTRE) {
+    if (moduleNumber == HAL_DEFAULT_POWER_DISTRIBUTION_MODULE) {
+      moduleNumber = 0;
+    }
     return static_cast<HAL_PowerDistributionHandle>(
-        HAL_InitializePDP(moduleNumber, nullptr, status));  // TODO
+        HAL_InitializePDP(moduleNumber, allocationLocation, status));  // TODO
   } else {
-    *status = PARAMETER_OUT_OF_RANGE;
-    SetLastError(status, "Rev Power not currently supported");
-    return HAL_kInvalidHandle;
+    if (moduleNumber == HAL_DEFAULT_POWER_DISTRIBUTION_MODULE) {
+      moduleNumber = 1;
+    }
+    return static_cast<HAL_PowerDistributionHandle>(
+        HAL_REV_InitializePDH(moduleNumber, allocationLocation, status));
   }
 }
 
@@ -36,7 +43,16 @@ void HAL_CleanPowerDistribution(HAL_PowerDistributionHandle handle) {
   if (IsCtre(handle)) {
     HAL_CleanPDP(handle);
   } else {
-    // TODO
+    HAL_REV_FreePDH(handle);
+  }
+}
+
+int32_t HAL_GetPowerDistributionModuleNumber(HAL_PowerDistributionHandle handle,
+                                             int32_t* status) {
+  if (IsCtre(handle)) {
+    return HAL_GetPDPModuleNumber(handle, status);
+  } else {
+    return HAL_REV_GetPDHModuleNumber(handle, status);
   }
 }
 
@@ -45,8 +61,7 @@ HAL_Bool HAL_CheckPowerDistributionChannel(HAL_PowerDistributionHandle handle,
   if (IsCtre(handle)) {
     return HAL_CheckPDPChannel(channel);
   } else {
-    return false;
-    // TODO
+    return HAL_REV_CheckPDHChannelNumber(channel);
   }
 }
 
@@ -55,8 +70,7 @@ HAL_Bool HAL_CheckPowerDistributionModule(int32_t module,
   if (type == HAL_PowerDistributionType::HAL_PowerDistributionType_kCTRE) {
     return HAL_CheckPDPModule(module);
   } else {
-    return false;
-    // TODO
+    return HAL_REV_CheckPDHModuleNumber(module);
   }
 }
 
@@ -67,14 +81,22 @@ HAL_PowerDistributionType HAL_GetPowerDistributionType(
              : HAL_PowerDistributionType::HAL_PowerDistributionType_kRev;
 }
 
+int32_t HAL_GetPowerDistributionNumChannels(HAL_PowerDistributionHandle handle,
+                                            int32_t* status) {
+  if (IsCtre(handle)) {
+    return kNumCTREPDPChannels;
+  } else {
+    return kNumREVPDHChannels;
+  }
+}
+
 double HAL_GetPowerDistributionTemperature(HAL_PowerDistributionHandle handle,
                                            int32_t* status) {
   if (IsCtre(handle)) {
     return HAL_GetPDPTemperature(handle, status);
   } else {
-    *status = PARAMETER_OUT_OF_RANGE;
-    SetLastError(status, "Rev Power not currently supported");
-    return false;
+    // Not supported
+    return 0;
   }
 }
 
@@ -83,9 +105,7 @@ double HAL_GetPowerDistributionVoltage(HAL_PowerDistributionHandle handle,
   if (IsCtre(handle)) {
     return HAL_GetPDPVoltage(handle, status);
   } else {
-    *status = PARAMETER_OUT_OF_RANGE;
-    SetLastError(status, "Rev Power not currently supported");
-    return false;
+    return HAL_REV_GetPDHSupplyVoltage(handle, status);
   }
 }
 
@@ -94,9 +114,7 @@ double HAL_GetPowerDistributionChannelCurrent(
   if (IsCtre(handle)) {
     return HAL_GetPDPChannelCurrent(handle, channel, status);
   } else {
-    *status = PARAMETER_OUT_OF_RANGE;
-    SetLastError(status, "Rev Power not currently supported");
-    return 0;
+    return HAL_REV_GetPDHChannelCurrent(handle, channel, status);
   }
 }
 
@@ -104,15 +122,19 @@ void HAL_GetPowerDistributionAllChannelCurrents(
     HAL_PowerDistributionHandle handle, double* currents,
     int32_t currentsLength, int32_t* status) {
   if (IsCtre(handle)) {
-    if (currentsLength < kNumPDPChannels) {
+    if (currentsLength < kNumCTREPDPChannels) {
       *status = PARAMETER_OUT_OF_RANGE;
       SetLastError(status, "Output array not large enough");
       return;
     }
     return HAL_GetPDPAllChannelCurrents(handle, currents, status);
   } else {
-    *status = PARAMETER_OUT_OF_RANGE;
-    SetLastError(status, "Rev Power not currently supported");
+    if (currentsLength < kNumREVPDHChannels) {
+      *status = PARAMETER_OUT_OF_RANGE;
+      SetLastError(status, "Output array not large enough");
+      return;
+    }
+    return HAL_REV_GetPDHAllChannelCurrents(handle, currents, status);
   }
 }
 
@@ -121,9 +143,7 @@ double HAL_GetPowerDistributionTotalCurrent(HAL_PowerDistributionHandle handle,
   if (IsCtre(handle)) {
     return HAL_GetPDPTotalCurrent(handle, status);
   } else {
-    *status = PARAMETER_OUT_OF_RANGE;
-    SetLastError(status, "Rev Power not currently supported");
-    return 0;
+    return HAL_REV_GetPDHTotalCurrent(handle, status);
   }
 }
 
@@ -132,8 +152,7 @@ double HAL_GetPowerDistributionTotalPower(HAL_PowerDistributionHandle handle,
   if (IsCtre(handle)) {
     return HAL_GetPDPTotalPower(handle, status);
   } else {
-    *status = PARAMETER_OUT_OF_RANGE;
-    SetLastError(status, "Rev Power not currently supported");
+    // Not currently supported
     return 0;
   }
 }
@@ -143,8 +162,7 @@ double HAL_GetPowerDistributionTotalEnergy(HAL_PowerDistributionHandle handle,
   if (IsCtre(handle)) {
     return HAL_GetPDPTotalEnergy(handle, status);
   } else {
-    *status = PARAMETER_OUT_OF_RANGE;
-    SetLastError(status, "Rev Power not currently supported");
+    // Not currently supported
     return 0;
   }
 }
@@ -154,8 +172,7 @@ void HAL_ResetPowerDistributionTotalEnergy(HAL_PowerDistributionHandle handle,
   if (IsCtre(handle)) {
     HAL_ResetPDPTotalEnergy(handle, status);
   } else {
-    *status = PARAMETER_OUT_OF_RANGE;
-    SetLastError(status, "Rev Power not currently supported");
+    // Not supported
   }
 }
 
@@ -164,8 +181,28 @@ void HAL_ClearPowerDistributionStickyFaults(HAL_PowerDistributionHandle handle,
   if (IsCtre(handle)) {
     HAL_ClearPDPStickyFaults(handle, status);
   } else {
-    *status = PARAMETER_OUT_OF_RANGE;
-    SetLastError(status, "Rev Power not currently supported");
+    HAL_REV_ClearPDHFaults(handle, status);
   }
 }
+
+void HAL_SetPowerDistributionSwitchableChannel(
+    HAL_PowerDistributionHandle handle, HAL_Bool enabled, int32_t* status) {
+  if (IsCtre(handle)) {
+    // No-op on CTRE
+    return;
+  } else {
+    HAL_REV_SetPDHSwitchableChannel(handle, enabled, status);
+  }
+}
+
+HAL_Bool HAL_GetPowerDistributionSwitchableChannel(
+    HAL_PowerDistributionHandle handle, int32_t* status) {
+  if (IsCtre(handle)) {
+    // No-op on CTRE
+    return false;
+  } else {
+    return HAL_REV_GetPDHSwitchableChannelState(handle, status);
+  }
+}
+
 }  // extern "C"