[wpilib] Add class to calculate robot resistance

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

@@ -4,17 +4,21 @@
 
 #include "frc/PowerDistribution.h"
 
+#include <utility>
 #include <vector>
 
 #include <fmt/format.h>
 #include <hal/FRCUsageReporting.h>
 #include <hal/Ports.h>
 #include <hal/PowerDistribution.h>
+#include <units/current.h>
+#include <units/impedance.h>
 #include <wpi/StackTrace.h>
 #include <wpi/sendable/SendableBuilder.h>
 #include <wpi/sendable/SendableRegistry.h>
 
 #include "frc/Errors.h"
+#include "frc/ResistanceCalculator.h"
 
 static_assert(static_cast<HAL_PowerDistributionType>(
                   frc::PowerDistribution::ModuleType::kCTRE) ==
@@ -27,7 +31,8 @@ static_assert(frc::PowerDistribution::kDefaultModule ==
 
 using namespace frc;
 
-PowerDistribution::PowerDistribution() {
+PowerDistribution::PowerDistribution()
+    : m_totalResistanceNotifier([this] { UpdateResistance(); }) {
   auto stack = wpi::GetStackTrace(1);
 
   int32_t status = 0;
@@ -41,9 +46,12 @@ PowerDistribution::PowerDistribution() {
 
   HAL_Report(HALUsageReporting::kResourceType_PDP, m_module + 1);
   wpi::SendableRegistry::AddLW(this, "PowerDistribution", m_module);
+
+  m_totalResistanceNotifier.StartPeriodic(PowerDistribution::kUpdatePeriod);
 }
 
-PowerDistribution::PowerDistribution(int module, ModuleType moduleType) {
+PowerDistribution::PowerDistribution(int module, ModuleType moduleType)
+    : m_totalResistanceNotifier([this] { UpdateResistance(); }) {
   auto stack = wpi::GetStackTrace(1);
 
   int32_t status = 0;
@@ -56,6 +64,25 @@ PowerDistribution::PowerDistribution(int module, ModuleType moduleType) {
 
   HAL_Report(HALUsageReporting::kResourceType_PDP, m_module + 1);
   wpi::SendableRegistry::AddLW(this, "PowerDistribution", m_module);
+
+  m_totalResistanceNotifier.StartPeriodic(PowerDistribution::kUpdatePeriod);
+}
+
+PowerDistribution::PowerDistribution(PowerDistribution&& other)
+    : m_handle(std::move(other.m_handle)),
+      m_module(other.m_module),
+      m_totalResistanceCalculator(std::move(other.m_totalResistanceCalculator)),
+      m_totalResistanceNotifier(std::move(other.m_totalResistanceNotifier)) {
+  m_totalResistance.store(other.m_totalResistance.load());
+}
+
+PowerDistribution& PowerDistribution::operator=(PowerDistribution&& other) {
+  m_handle = std::move(other.m_handle);
+  m_module = other.m_module;
+  m_totalResistanceCalculator = std::move(other.m_totalResistanceCalculator);
+  m_totalResistanceNotifier = std::move(other.m_totalResistanceNotifier);
+  m_totalResistance.store(other.m_totalResistance.load());
+  return *this;
 }
 
 int PowerDistribution::GetNumChannels() const {
@@ -310,6 +337,10 @@ PowerDistribution::StickyFaults PowerDistribution::GetStickyFaults() const {
   return stickyFaults;
 }
 
+units::ohm_t PowerDistribution::GetTotalResistance() const {
+  return m_totalResistance.load();
+}
+
 void PowerDistribution::InitSendable(wpi::SendableBuilder& builder) {
   builder.SetSmartDashboardType("PowerDistribution");
   int numChannels = GetNumChannels();
@@ -347,4 +378,12 @@ void PowerDistribution::InitSendable(wpi::SendableBuilder& builder) {
         int32_t lStatus = 0;
         HAL_SetPowerDistributionSwitchableChannel(m_handle, value, &lStatus);
       });
+  builder.AddDoubleProperty(
+      "TotalResistance", [this] { return GetTotalResistance().value(); },
+      nullptr);
+}
+
+void PowerDistribution::UpdateResistance() {
+  m_totalResistance.store(m_totalResistanceCalculator.Calculate(
+      units::ampere_t(GetTotalCurrent()), units::volt_t(GetVoltage())));
 }

wpilibc/src/main/native/cpp/ResistanceCalculator.cpp

@@ -0,0 +1,55 @@
+// 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.
+
+#include "frc/ResistanceCalculator.h"
+
+#include <cmath>
+
+using namespace frc;
+
+ResistanceCalculator::ResistanceCalculator(int bufferSize,
+                                           double rSquaredThreshold)
+    : m_currentBuffer(bufferSize),
+      m_voltageBuffer(bufferSize),
+      m_bufferSize{bufferSize},
+      m_rSquaredThreshold{rSquaredThreshold} {}
+
+units::ohm_t ResistanceCalculator::Calculate(units::ampere_t current,
+                                             units::volt_t voltage) {
+  // Update buffers only if drawing current
+  if (current != 0_A) {
+    if (m_currentBuffer.size() >= static_cast<size_t>(m_bufferSize)) {
+      // Pop the last point and remove it from the sums
+      units::ampere_t lastCurrent = m_currentBuffer.pop_back();
+      units::volt_t lastVoltage = m_voltageBuffer.pop_back();
+      m_currentVariance.Calculate(lastCurrent, lastCurrent, Operator::kRemove);
+      m_voltageVariance.Calculate(lastVoltage, lastVoltage, Operator::kRemove);
+      m_covariance.Calculate(lastCurrent, lastVoltage, Operator::kRemove);
+    }
+
+    m_currentBuffer.push_front(current);
+    m_voltageBuffer.push_front(voltage);
+    m_currentVariance.Calculate(current, current, Operator::kAdd);
+    m_voltageVariance.Calculate(voltage, voltage, Operator::kAdd);
+    m_covariance.Calculate(current, voltage, Operator::kAdd);
+  }
+
+  // Recalculate resistance
+  if (m_currentBuffer.size() < 2) {
+    return units::ohm_t{std::nan("")};
+  }
+
+  auto currentVariance = m_currentVariance.GetCovariance();
+  auto voltageVariance = m_voltageVariance.GetCovariance();
+  auto covariance = m_covariance.GetCovariance();
+  double rSquared =
+      covariance * covariance / (currentVariance * voltageVariance);
+
+  if (rSquared > m_rSquaredThreshold) {
+    // Resistance is slope of current vs voltage
+    return covariance / currentVariance;
+  } else {
+    return units::ohm_t{std::nan("")};
+  }
+}

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

@@ -4,13 +4,19 @@
 
 #pragma once
 
+#include <atomic>
 #include <vector>
 
 #include <hal/PowerDistribution.h>
 #include <hal/Types.h>
+#include <units/impedance.h>
+#include <units/time.h>
 #include <wpi/sendable/Sendable.h>
 #include <wpi/sendable/SendableHelper.h>
 
+#include "frc/Notifier.h"
+#include "frc/ResistanceCalculator.h"
+
 namespace frc {
 
 /**
@@ -23,6 +29,9 @@ class PowerDistribution : public wpi::Sendable,
   /// Default module number.
   static constexpr int kDefaultModule = -1;
 
+  /// Seconds to wait before updating resistance.
+  static constexpr units::second_t kUpdatePeriod = 25_ms;
+
   /**
    * Power distribution module type.
    */
@@ -49,8 +58,8 @@ class PowerDistribution : public wpi::Sendable,
    */
   PowerDistribution(int module, ModuleType moduleType);
 
-  PowerDistribution(PowerDistribution&&) = default;
-  PowerDistribution& operator=(PowerDistribution&&) = default;
+  PowerDistribution(PowerDistribution&&);
+  PowerDistribution& operator=(PowerDistribution&&);
 
   ~PowerDistribution() override = default;
 
@@ -157,6 +166,13 @@ class PowerDistribution : public wpi::Sendable,
    */
   void SetSwitchableChannel(bool enabled);
 
+  /**
+   * Get the robot's total resistance.
+   *
+   * @return The robot total resistance, in Ohms.
+   */
+  units::ohm_t GetTotalResistance() const;
+
   /** Version and device data received from a PowerDistribution device */
   struct Version {
     /** Firmware major version number. */
@@ -345,6 +361,14 @@ class PowerDistribution : public wpi::Sendable,
  private:
   hal::Handle<HAL_PowerDistributionHandle, HAL_CleanPowerDistribution> m_handle;
   int m_module;
+  ResistanceCalculator m_totalResistanceCalculator;
+  std::atomic<units::ohm_t> m_totalResistance;
+  frc::Notifier m_totalResistanceNotifier;
+
+  /**
+   * Calculate new total resistance.
+   */
+  void UpdateResistance();
 };
 
 }  // namespace frc

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

@@ -0,0 +1,160 @@
+// 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.
+
+#pragma once
+
+#include <utility>
+
+#include <units/current.h>
+#include <units/impedance.h>
+#include <units/voltage.h>
+#include <wpi/circular_buffer.h>
+
+namespace frc {
+
+/**
+ * Finds the resistance of a channel or the entire robot using a running linear
+ * regression over a window.
+ *
+ * Must be updated with current and voltage periodically using the Calculate()
+ * method.
+ *
+ * To use this for finding the resistance of a channel, use the calculate method
+ * with the battery voltage minus the voltage at the motor controller or
+ * whatever is plugged in to the PDP at that channel.
+ */
+class ResistanceCalculator {
+ public:
+  /// Default buffer size.
+  static constexpr int kDefaultBufferSize = 250;
+
+  /// Default R² threshold.
+  static constexpr double kDefaultRSquaredThreshold = 0.75;
+
+  /**
+   * Creates a ResistanceCalculator with a default buffer size of 250 and R²
+   * threshold of 0.5.
+   */
+  ResistanceCalculator() = default;
+
+  /**
+   * Creates a ResistanceCalculator.
+   *
+   * @param bufferSize The maximum number of points to take the linear
+   *     regression over.
+   * @param rSquaredThreshold The minimum R² value (0 to 1) considered
+   *     significant enough to return the regression slope instead of NaN. A
+   *     lower threshold allows resistance to be returned even with noisier
+   *     data.
+   */
+  ResistanceCalculator(int bufferSize, double rSquaredThreshold);
+
+  ResistanceCalculator(ResistanceCalculator&&) = default;
+  ResistanceCalculator& operator=(ResistanceCalculator&&) = default;
+
+  /**
+   * Update the buffers with new (current, voltage) points, and remove old
+   * points if necessary.
+   *
+   * @param current The current current
+   * @param voltage The current voltage
+   * @return The current resistance in ohms
+   */
+  units::ohm_t Calculate(units::ampere_t current, units::volt_t voltage);
+
+ private:
+  /**
+   * Operator to apply to OnlineCovariance<>::Calculate() inputs.
+   */
+  enum class Operator {
+    /// Add point.
+    kAdd,
+    /// Remove point.
+    kRemove
+  };
+
+  template <typename X, typename Y>
+  class OnlineCovariance {
+   public:
+    /**
+     * The previously calculated covariance.
+     */
+    decltype(auto) GetCovariance() const { return m_cov / (m_n - 1); }
+
+    /**
+     * Calculate the covariance based on a new point that may be removed or
+     * added.
+     *
+     * @param x The x value of the point.
+     * @param y The y value of the point.
+     * @param op Operator to apply with the point.
+     * @return The new sample covariance.
+     */
+    decltype(auto) Calculate(X x, Y y, Operator op) {
+      // From
+      // https://en.wikipedia.org/wiki/Algorithms_for_calculating_variance#Covariance
+
+      X dx = x - m_xMean;
+      Y dy = y - m_yMean;
+
+      if (op == Operator::kAdd) {
+        ++m_n;
+        m_xMean += dx / m_n;
+        m_yMean += dy / m_n;
+
+        // This is supposed to be (y - yMean) and not dy
+        m_cov += dx * (y - m_yMean);
+      } else if (op == Operator::kRemove) {
+        --m_n;
+        m_xMean -= dx / m_n;
+        m_yMean -= dy / m_n;
+
+        // This is supposed to be (y - yMean) and not dy
+        m_cov -= dx * (y - m_yMean);
+      }
+
+      // Correction for sample variance
+      return m_cov / (m_n - 1);
+    }
+
+   private:
+    /// Number of points covariance is calculated over.
+    int m_n = 0;
+
+    /// Current mean of x values.
+    X m_xMean{0.0};
+
+    /// Current mean of y values.
+    Y m_yMean{0.0};
+
+    /// Current approximated population covariance.
+    decltype(std::declval<X>() * std::declval<Y>()) m_cov{0.0};
+  };
+
+  /// Buffers holding the current values that will eventually need to be
+  /// subtracted from the sum when they leave the window.
+  wpi::circular_buffer<units::ampere_t> m_currentBuffer{kDefaultBufferSize};
+
+  /// Buffer holding the voltage values that will eventually need to be
+  /// subtracted from the sum when they leave the window.
+  wpi::circular_buffer<units::volt_t> m_voltageBuffer{kDefaultBufferSize};
+
+  /// The maximum number of points to take the linear regression over.
+  int m_bufferSize = kDefaultBufferSize;
+
+  /// The minimum R² value considered significant enough to return the
+  /// regression slope instead of NaN.
+  double m_rSquaredThreshold = kDefaultRSquaredThreshold;
+
+  /// Used for approximating current variance.
+  OnlineCovariance<units::ampere_t, units::ampere_t> m_currentVariance;
+
+  /// Used for approximating voltage variance.
+  OnlineCovariance<units::volt_t, units::volt_t> m_voltageVariance;
+
+  /// Used for approximating covariance of current and voltage.
+  OnlineCovariance<units::ampere_t, units::volt_t> m_covariance;
+};
+
+}  // namespace frc