SNS - Inverter Current

config/debug.toml

@@ -35,6 +35,11 @@ address = 0x38
 enabled = false
 bus = "can1"
 
+[sensors.inverter_current]
+enabled = false
+bus = 2
+adc_mux_channel = 0
+
 [sensors.keyence]
 enabled = false
 pin = 1

lib/debug/commands/inverter_current_commands.cpp

@@ -0,0 +1,54 @@
+#include "inverter_current_commands.hpp"
+
+#include <cstdint>
+
+namespace hyped::debug {
+
+core::Result InverterCurrentCommands::addCommands(core::ILogger &logger,
+                                                  std::shared_ptr<Repl> repl,
+                                                  toml::v3::node_view<toml::v3::node> config)
+{
+  // Get the I2C bus number from the config file
+  const auto optional_bus = config["bus"].value<std::uint8_t>();
+  if (!optional_bus) {
+    logger.log(core::LogLevel::kFatal, "No I2C bus specified");
+    return core::Result::kFailure;
+  };
+  const auto bus = *optional_bus;
+  // Get the ADC mux channel from the config file
+  const auto optional_adc_mux_channel = config["adc_mux_channel"].value<std::uint8_t>();
+  if (!optional_adc_mux_channel) {
+    logger.log(core::LogLevel::kFatal, "No ADC mux channel specified");
+    return core::Result::kFailure;
+  };
+  const auto adc_mux_channel = *optional_adc_mux_channel;
+  // Get the I2C instance
+  const auto optional_i2c = repl->getI2c(bus);
+  if (!optional_i2c) {
+    logger.log(core::LogLevel::kFatal, "Error getting I2C bus");
+    return core::Result::kFailure;
+  };
+  const auto i2c = std::move(*optional_i2c);
+
+  // Create the sensor instance
+  auto inverter_current_sensor = std::make_shared<sensors::InverterCurrent>(
+    logger, i2c, static_cast<sensors::AdcMuxChannel>(adc_mux_channel));
+
+  // Create the read command
+  const auto read_command_name        = "current read";
+  const auto read_command_description = "Read inverter current";
+  const auto read_command_handler     = [&logger, inverter_current_sensor]() {
+    const auto current = inverter_current_sensor->readCurrent();
+    if (!current) {
+      logger.log(core::LogLevel::kFatal, "Failed to read inverter current");
+      return;
+    }
+    logger.log(core::LogLevel::kDebug, "Inverter current: %d", *current);
+  };
+  auto read_command
+    = std::make_unique<Command>(read_command_name, read_command_description, read_command_handler);
+  repl->addCommand(std::move(read_command));
+  return core::Result::kSuccess;
+}
+
+}  // namespace hyped::debug

lib/debug/commands/inverter_current_commands.hpp

@@ -0,0 +1,20 @@
+#pragma once
+#include "command.hpp"
+
+#include <memory>
+
+#include <core/logger.hpp>
+#include <core/types.hpp>
+#include <debug/repl.hpp>
+#include <sensors/inverter_current.hpp>
+
+namespace hyped::debug {
+
+class InverterCurrentCommands {
+ public:
+  static core::Result addCommands(core::ILogger &logger,
+                                  std::shared_ptr<Repl> repl,
+                                  toml::v3::node_view<toml::v3::node> config);
+};
+
+}  // namespace hyped::debug

lib/debug/repl.cpp

@@ -6,6 +6,7 @@
 #include "commands/can_commands.hpp"
 #include "commands/gpio_commands.hpp"
 #include "commands/i2c_commands.hpp"
+#include "commands/inverter_current_commands.hpp"
 #include "commands/keyence_commands.hpp"
 #include "commands/pwm_commands.hpp"
 #include "commands/spi_commands.hpp"
@@ -81,6 +82,15 @@ std::optional<std::shared_ptr<Repl>> Repl::create(core::ILogger &logger,
       return std::nullopt;
     }
   }
+  if (config["sensors"]["inverter_current"]["enabled"].value_or(false)) {
+    const auto result
+      = InverterCurrentCommands::addCommands(logger, repl, config["sensors"]["inverter_current"]);
+    if (result == core::Result::kFailure) {
+      logger.log(core::LogLevel::kFatal, "Error adding inverter current commands");
+      return std::nullopt;
+    }
+  }
+  const auto aliases = config["aliases"].as_table();
   if (config["sensors"]["keyence"]["enabled"].value_or(false)) {
     const auto result
       = KeyenceCommands::addCommands(logger, repl, time, config["sensors"]["keyence"]);

lib/sensors/adc_mux.hpp

@@ -4,14 +4,17 @@
 
 namespace hyped::sensors {
 
-constexpr std::uint8_t kAdcMuxAddress  = 0x1D;
-constexpr std::uint8_t kAdcMuxChannel1 = 0x20;
-constexpr std::uint8_t kAdcMuxChannel2 = 0x21;
-constexpr std::uint8_t kAdcMuxChannel3 = 0x22;
-constexpr std::uint8_t kAdcMuxChannel4 = 0x23;
-constexpr std::uint8_t kAdcMuxChannel5 = 0x24;
-constexpr std::uint8_t kAdcMuxChannel6 = 0x25;
-constexpr std::uint8_t kAdcMuxChannel7 = 0x26;
-constexpr std::uint8_t kAdcMuxChannel0 = 0x27;
+constexpr std::uint8_t kAdcMuxAddress = 0x1D;
+
+enum class AdcMuxChannel : std::uint8_t {
+  kAdcMuxChannel1 = 0x20,
+  kAdcMuxChannel2 = 0x21,
+  kAdcMuxChannel3 = 0x22,
+  kAdcMuxChannel4 = 0x23,
+  kAdcMuxChannel5 = 0x24,
+  kAdcMuxChannel6 = 0x25,
+  kAdcMuxChannel7 = 0x26,
+  kAdcMuxChannel0 = 0x27,
+};
 
 }  // namespace hyped::sensors

lib/sensors/inverter_current.cpp

@@ -0,0 +1,54 @@
+#include "adc_mux.hpp"
+
+#include <cstdint>
+
+#include <inverter_current.hpp>
+
+namespace hyped::sensors {
+
+InverterCurrent::InverterCurrent(core::ILogger &logger,
+                                 std::shared_ptr<io::II2c> i2c,
+                                 const AdcMuxChannel adc_mux_channel)
+    : logger_(logger),
+      i2c_(i2c),
+      adc_mux_channel_(adc_mux_channel)
+{
+}
+
+InverterCurrent::~InverterCurrent()
+{
+}
+
+std::optional<core::Float> InverterCurrent::readCurrent()
+{
+  const auto inverter_current
+    = i2c_->readByte(kAdcMuxAddress, static_cast<std::uint8_t>(adc_mux_channel_));
+  if (!inverter_current) {
+    logger_.log(core::LogLevel::kFatal, "Failed to read inverter current");
+    return std::nullopt;
+  }
+
+  // The sensor maps -75A to 75A into -3V to 3V
+  // Since the ADC isn't differential, we need to differentiate the reference signal and the output
+  // signal
+  static const int MAX_CURRENT         = 75;
+  static const int MIN_CURRENT         = -75;
+  static const core::Float MAX_VOLTAGE = 3.3;
+  static const core::Float MIN_VOLTAGE = -3.3;
+
+  // These can be adjusted
+  const int reference_voltage      = 1.65;
+  const core::Float virtual_ground = MAX_VOLTAGE / 2;
+
+  // Calculate the current
+  const int inverter_current_voltage
+    = *inverter_current * (MAX_VOLTAGE - virtual_ground) + virtual_ground;
+  const core::Float current
+    = inverter_current_voltage * ((MAX_CURRENT - MIN_CURRENT) / (MAX_VOLTAGE - MIN_VOLTAGE))
+      + MIN_CURRENT;
+  const core::Float reference_voltage_biased = reference_voltage + virtual_ground;
+  const core::Float current_difference       = current - reference_voltage_biased;
+  return current_difference;
+}
+
+}  // namespace hyped::sensors

lib/sensors/inverter_current.hpp

@@ -0,0 +1,31 @@
+#pragma once
+
+#include "adc_mux.hpp"
+
+#include <cstdint>
+#include <memory>
+#include <optional>
+
+#include <core/logger.hpp>
+#include <core/types.hpp>
+#include <io/i2c.hpp>
+
+namespace hyped::sensors {
+
+class InverterCurrent {
+ public:
+  InverterCurrent(core::ILogger &logger,
+                  std::shared_ptr<io::II2c> i2c,
+                  const AdcMuxChannel adc_mux_channel);
+
+  ~InverterCurrent();
+
+  std::optional<core::Float> readCurrent();
+
+ private:
+  core::ILogger &logger_;
+  std::shared_ptr<io::II2c> i2c_;
+  const AdcMuxChannel adc_mux_channel_;
+};
+
+}  // namespace hyped::sensors