[wpilib] Fix PowerDistribution.GetAllCurrents() (#6025)

hal/src/main/native/cpp/jni/PowerDistributionJNI.cpp

@@ -179,15 +179,18 @@ JNIEXPORT void JNICALL
 Java_edu_wpi_first_hal_PowerDistributionJNI_getAllCurrents
   (JNIEnv* env, jclass, jint handle, jdoubleArray jarr)
 {
-  double storage[16];
   int32_t status = 0;
-  // TODO fix me
-  HAL_GetPowerDistributionAllChannelCurrents(handle, storage, 16, &status);
+  int32_t size = HAL_GetPowerDistributionNumChannels(handle, &status);
+  wpi::SmallVector<double, 24> storage;
+  storage.resize_for_overwrite(size);
+
+  HAL_GetPowerDistributionAllChannelCurrents(handle, storage.data(), size,
+                                             &status);
   if (!CheckStatus(env, status, false)) {
     return;
   }
 
-  env->SetDoubleArrayRegion(jarr, 0, 16, storage);
+  env->SetDoubleArrayRegion(jarr, 0, size, storage.data());
 }
 
 /*

wpilibc/src/main/native/cpp/PowerDistribution.cpp

@@ -64,6 +64,13 @@ PowerDistribution::~PowerDistribution() {
   }
 }
 
+int PowerDistribution::GetNumChannels() const {
+  int32_t status = 0;
+  int32_t size = HAL_GetPowerDistributionNumChannels(m_handle, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  return size;
+}
+
 double PowerDistribution::GetVoltage() const {
   int32_t status = 0;
   double voltage = HAL_GetPowerDistributionVoltage(m_handle, &status);
@@ -87,6 +94,17 @@ double PowerDistribution::GetCurrent(int channel) const {
   return current;
 }
 
+std::vector<double> PowerDistribution::GetAllCurrents() const {
+  int32_t status = 0;
+  int32_t size = GetNumChannels();
+  std::vector<double> currents(size);
+
+  HAL_GetPowerDistributionAllChannelCurrents(m_handle, currents.data(), size,
+                                             &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  return currents;
+}
+
 double PowerDistribution::GetTotalCurrent() const {
   int32_t status = 0;
   double current = HAL_GetPowerDistributionTotalCurrent(m_handle, &status);
@@ -300,9 +318,7 @@ PowerDistribution::StickyFaults PowerDistribution::GetStickyFaults() const {
 
 void PowerDistribution::InitSendable(wpi::SendableBuilder& builder) {
   builder.SetSmartDashboardType("PowerDistribution");
-  int32_t status = 0;
-  int numChannels = HAL_GetPowerDistributionNumChannels(m_handle, &status);
-  FRC_ReportError(status, "Module {}", m_module);
+  int numChannels = GetNumChannels();
   // Use manual reads to avoid printing errors
   for (int i = 0; i < numChannels; ++i) {
     builder.AddDoubleProperty(

wpilibc/src/main/native/include/frc/PowerDistribution.h

@@ -4,6 +4,8 @@
 
 #pragma once
 
+#include <vector>
+
 #include <hal/Types.h>
 #include <wpi/sendable/Sendable.h>
 #include <wpi/sendable/SendableHelper.h>
@@ -50,6 +52,13 @@ class PowerDistribution : public wpi::Sendable,
   PowerDistribution(PowerDistribution&&) = default;
   PowerDistribution& operator=(PowerDistribution&&) = default;
 
+  /**
+   * Gets the number of channels for this power distribution object.
+   *
+   * @return Number of output channels (16 for PDP, 24 for PDH).
+   */
+  int GetNumChannels() const;
+
   /**
    * Query the input voltage of the PDP/PDH.
    *
@@ -75,6 +84,13 @@ class PowerDistribution : public wpi::Sendable,
    */
   double GetCurrent(int channel) const;
 
+  /**
+   * Query all currents of the PDP.
+   *
+   * @return The current of each channel in Amperes
+   */
+  std::vector<double> GetAllCurrents() const;
+
   /**
    * Query the total current of all monitored PDP/PDH channels.
    *

wpilibc/src/test/native/cpp/simulation/PDPSimTest.cpp

@@ -79,4 +79,26 @@ TEST(PowerDistributionSimTest, SetCurrent) {
     EXPECT_TRUE(callback.GetLastValue());
   }
 }
+
+TEST(PowerDistributionSimTest, GetAllCurrents) {
+  HAL_Initialize(500, 0);
+  PowerDistribution pdp{2, frc::PowerDistribution::ModuleType::kRev};
+  PowerDistributionSim sim(pdp);
+
+  // setup
+  for (int channel = 0; channel < pdp.GetNumChannels(); ++channel) {
+    const double kTestCurrent = 24 - channel;
+    sim.SetCurrent(channel, kTestCurrent);
+  }
+
+  // run it
+  std::vector<double> currents = pdp.GetAllCurrents();
+
+  // verify
+  for (int channel = 0; channel < pdp.GetNumChannels(); ++channel) {
+    const double kTestCurrent = 24 - channel;
+    EXPECT_EQ(kTestCurrent, currents[channel]);
+  }
+}
+
 }  // namespace frc::sim

wpilibj/src/main/java/edu/wpi/first/wpilibj/PowerDistribution.java

@@ -113,6 +113,17 @@ public class PowerDistribution implements Sendable, AutoCloseable {
     return PowerDistributionJNI.getChannelCurrent(m_handle, channel);
   }
 
+  /**
+   * Query all currents of the PDP.
+   *
+   * @return The current of each channel in Amperes
+   */
+  public double[] getAllCurrents() {
+    double[] currents = new double[getNumChannels()];
+    PowerDistributionJNI.getAllCurrents(m_handle, currents);
+    return currents;
+  }
+
   /**
    * Query the current of all monitored channels.
    *

wpilibj/src/test/java/edu/wpi/first/wpilibj/PowerDistributionTest.java

@@ -0,0 +1,29 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
+import edu.wpi.first.wpilibj.simulation.PDPSim;
+import org.junit.jupiter.api.Test;
+
+class PowerDistributionTest {
+  @Test
+  void testGetAllCurrents() {
+    HAL.initialize(500, 0);
+    PowerDistribution pdp = new PowerDistribution(1, ModuleType.kRev);
+    PDPSim sim = new PDPSim(pdp);
+
+    for (int i = 0; i < pdp.getNumChannels(); i++) {
+      sim.setCurrent(i, 24 - i);
+    }
+    double[] currents = pdp.getAllCurrents();
+    for (int i = 0; i < pdp.getNumChannels(); i++) {
+      assertEquals(24 - i, currents[i]);
+    }
+  }
+}