Adding current sensor driver + BQ

config/hoverjet/default_current_sensor.yaml

@@ -0,0 +1,3 @@
+i2c_bus_path: /dev/i2c-1
+i2c_address: 64
+sensor_id: default

embedded/current_sensor/current_sensor_bq.cc

@@ -0,0 +1,50 @@
+//%bin(current_sensor_bq_main)
+//%deps(balsa_queue)
+//%deps(message)
+
+#include "embedded/current_sensor/current_sensor_bq.hh"
+#include "embedded/current_sensor/power_reading.hh"
+#include "infrastructure/balsa_queue/bq_main_macro.hh"
+
+#include <iostream>
+
+namespace jet {
+
+void CurrentSensorBq::init(const Config& config) {
+  assert(config["i2c_bus_path"]);
+  assert(config["i2c_address"]);
+  assert(config["sensor_name"]);
+
+  const int i2c_addr = config["i2c_address"].as<int>();
+  int i2c_handle = i2c_open(config["i2c_bus_path"].as<std::string>().c_str());
+  if (i2c_handle == -1) {
+    std::cerr << "Failed to open i2c" << std::endl;
+    exit(1);
+  }
+
+  sensor_ptr_ =
+      std::make_unique<ina219::INA219Driver>(i2c_handle, i2c_addr, ina219::DriverConfiguration::make_32V_2A());
+  power_publisher_ = make_publisher(std::string("current_sensor_sensor") + config["sensor_name"].as<std::string>());
+}
+
+void CurrentSensorBq::loop() {
+  PowerReading power_reading_message;
+  if (const auto shunt_voltage_mV = sensor_ptr_->get_shunt_voltage_mV()) {
+    power_reading_message.bus_voltage_mV = shunt_voltage_mV.value();
+  }
+  if (const auto shunt_voltage_mV = sensor_ptr_->get_current_mA()) {
+    power_reading_message.current_mA = shunt_voltage_mV.value();
+  }
+  if (const auto shunt_voltage_mV = sensor_ptr_->get_power_mW()) {
+    power_reading_message.power_mW = shunt_voltage_mV.value();
+  }
+  power_publisher_->publish(power_reading_message);
+}
+
+void CurrentSensorBq::shutdown() {
+  std::cout << "Shutting down!" << std::endl;
+}
+
+}  // namespace jet
+
+BALSA_QUEUE_MAIN_FUNCTION(jet::CurrentSensorBq)

embedded/current_sensor/current_sensor_bq.hh

@@ -0,0 +1,22 @@
+#pragma once
+
+#include "embedded/current_sensor/ina219_driver.hh"
+#include "infrastructure/balsa_queue/balsa_queue.hh"
+
+#include <memory>
+
+namespace jet {
+
+class CurrentSensorBq : public BalsaQ {
+ public:
+  CurrentSensorBq() = default;
+  void init(const Config& config);
+  void loop();
+  void shutdown();
+
+ private:
+  PublisherPtr power_publisher_;
+  std::unique_ptr<ina219::INA219Driver> sensor_ptr_;
+};
+
+}  // namespace jet

embedded/current_sensor/ina219_driver.cc

@@ -0,0 +1,107 @@
+#include "ina219_driver.hh"
+
+#include <string.h>  // memset
+
+namespace ina219 {
+
+[[nodiscard]] bool INA219Driver::write_register(Register reg, uint16_t value) const {
+  uint16_t write_buffer = ((value & 0xFF) << 8) + (value >> 8);
+  if (i2c_write(&i2c_device_, static_cast<uint8_t>(reg), &write_buffer, 2) == -1) {
+    return false;
+  }
+  return true;
+}
+
+    [[nodiscard]] bool INA219Driver::read_register(Register reg, uint16_t& value) const {
+  uint16_t read_buffer;
+  if (i2c_read(&i2c_device_, static_cast<uint8_t>(reg), &read_buffer, 2) == -1) {
+    return false;
+  }
+  value = ((read_buffer & 0xFF) << 8) + (read_buffer >> 8);
+  return true;
+}
+
+INA219Driver::INA219Driver(int i2c_handle, int i2c_address, DriverConfiguration config) : config_(config) {
+  memset(&i2c_device_, 0, sizeof(i2c_device_));
+  i2c_device_.bus = i2c_handle;
+  i2c_device_.addr = i2c_address;
+  i2c_device_.iaddr_bytes = I2C_ADDR_BYTES;
+  i2c_device_.page_bytes = I2C_PAGE_BYTES;
+}
+
+std::optional<int16_t> INA219Driver::get_raw_bus_voltage() const {
+  uint16_t raw_bus_voltage_buffer;
+  if (!read_register(Register::BUS_VOLTAGE, raw_bus_voltage_buffer)) {
+    return {};
+  }
+
+  return (int16_t)((raw_bus_voltage_buffer >> 3) * 4);
+}
+
+std::optional<int16_t> INA219Driver::get_raw_shunt_voltage() const {
+  uint16_t raw_shunt_voltage_buffer;
+  if (!read_register(Register::SHUNT_VOLTAGE, raw_shunt_voltage_buffer)) {
+    return {};
+  }
+  return static_cast<int16_t>(raw_shunt_voltage_buffer);
+}
+
+std::optional<int16_t> INA219Driver::get_raw_current() const {
+  if (!write_register(Register::CALIBRATION, config_.calibration_value)) {
+    return {};
+  }
+
+  uint16_t raw_current_buffer;
+  if (!read_register(Register::CURRENT, raw_current_buffer)) {
+    return {};
+  }
+
+  return static_cast<int16_t>(raw_current_buffer);
+}
+
+std::optional<int16_t> INA219Driver::get_raw_power() const {
+  if (!write_register(Register::CALIBRATION, config_.calibration_value)) {
+    return {};
+  }
+
+  uint16_t raw_power_buffer;
+  if (!read_register(Register::POWER, raw_power_buffer)) {
+    return {};
+  }
+
+  return static_cast<int16_t>(raw_power_buffer);
+}
+
+std::optional<float> INA219Driver::get_shunt_voltage_mV() const {
+  std::optional<int16_t> raw_shunt_voltage_value = get_raw_shunt_voltage();
+  if (!raw_shunt_voltage_value) {
+    return {};
+  }
+  return raw_shunt_voltage_value.value() * MILLIVOLTS_PER_MICROVOLT;
+}
+
+std::optional<float> INA219Driver::get_bus_voltage_V() const {
+  std::optional<int16_t> raw_bus_voltage_value = get_raw_bus_voltage();
+  if (!raw_bus_voltage_value) {
+    return {};
+  }
+  return raw_bus_voltage_value.value() * VOLTS_PER_MILLIVOLT;
+}
+
+std::optional<float> INA219Driver::get_current_mA() const {
+  std::optional<float> raw_current_value = get_raw_current();
+  if (!raw_current_value) {
+    return {};
+  }
+  return raw_current_value.value() / config_.current_divider_mA;
+}
+
+std::optional<float> INA219Driver::get_power_mW() const {
+  std::optional<float> raw_power_value = get_raw_power();
+  if (!raw_power_value) {
+    return {};
+  }
+  return raw_power_value.value() * config_.power_multiplier_mW;
+}
+
+}  // namespace ina219

embedded/current_sensor/ina219_driver.hh

@@ -0,0 +1,88 @@
+#pragma once
+//%deps(i2c)
+
+#include "ina219_driver_configuration.hh"
+
+#include "third_party/i2c/i2c.h"
+
+#include <stdint.h>
+
+#include <optional>
+
+namespace ina219 {
+
+static constexpr float MILLIVOLTS_PER_MICROVOLT{0.01};
+static constexpr float VOLTS_PER_MILLIVOLT{0.001};
+static constexpr int32_t I2C_ADDR_BYTES{1};
+static constexpr int32_t I2C_PAGE_BYTES{16};
+
+/// @brief  Register IDs
+enum class Register : uint8_t {
+  CONFIG = 0x00,
+  SHUNT_VOLTAGE = 0x01,
+  BUS_VOLTAGE = 0x02,
+  POWER = 0x03,
+  CURRENT = 0x04,
+  CALIBRATION = 0x05
+};
+
+/// @brief Class for interacting with the INA219 I2C current sensor
+class INA219Driver {
+ public:
+  /// @brief  INA219Driver constructor
+  /// @param  i2c_handle - The I2C bus file descriptor
+  /// @param  i2c_address - The I2C address of the INA219 device
+  /// @param  config - The configuration for this INA219
+  INA219Driver(int i2c_handle, int i2c_address, DriverConfiguration config);
+
+  /// @brief  Gets the shunt voltage in volts
+  /// @return Bus voltage in volts
+  std::optional<float> get_bus_voltage_V() const;
+
+  /// @brief  Gets the shunt voltage in mV
+  /// @return Shunt voltage in millivolts
+  std::optional<float> get_shunt_voltage_mV() const;
+
+  /// @brief  Gets the current value in mA
+  /// @return Current in milliamps
+  std::optional<float> get_current_mA() const;
+
+  /// @brief  Gets the measured power in mW
+  /// @return Power in milliwatts
+  std::optional<float> get_power_mW() const;
+
+ private:
+  const DriverConfiguration config_;
+
+  /// @brief  Writes two bytes to an INA219 register
+  /// @param  reg - ID of the register to write to
+  /// @param  value - The value to write
+  /// @return Returns true on success, false on failure
+  [[nodiscard]] bool write_register(Register reg, uint16_t value) const;
+
+  /// @brief  Reads 16 bits from an INA219 register
+  /// @param  reg - ID of the register to read from
+  /// @param  value - Buffer to write the value to
+  /// @return Returns true on success, false on failure
+  [[nodiscard]] bool read_register(Register reg, uint16_t& value) const;
+
+  /// @brief  Gets the raw bus voltage value
+  /// @return The value read rom the INA219's bus voltage register
+  std::optional<int16_t> get_raw_bus_voltage() const;
+
+  /// @brief  Gets the raw shunt voltage value
+  /// @return The value read rom the INA219's shunt voltage register
+  std::optional<int16_t> get_raw_shunt_voltage() const;
+
+  /// @brief  Gets the raw current value
+  /// @return The value read rom the INA219's current register
+  std::optional<int16_t> get_raw_current() const;
+
+  /// @brief  Gets the raw power value
+  /// @return The value read rom the INA219's power register
+  std::optional<int16_t> get_raw_power() const;
+
+  i2c_device i2c_device_;
+};
+
+}  // namespace ina219

embedded/current_sensor/ina219_driver_configuration.cc

@@ -0,0 +1,63 @@
+#include "ina219_driver_configuration.hh"
+
+namespace ina219 {
+
+DriverConfiguration DriverConfiguration::make_32V_2A() {
+  return DriverConfiguration(10,
+                             2,
+                             4096,
+                             BusVoltageRange::RANGE_32V,
+                             PGAGain::GAIN_8_320MV,
+                             BusADCResolution::RES_12BIT,
+                             ShuntADCResolution::RES_12BIT_1S_532US,
+                             OperatingMode::SANDBVOLT_CONTINUOUS);
+}
+
+DriverConfiguration DriverConfiguration::make_32V_1A() {
+  return DriverConfiguration(25,
+                             0.8,
+                             10240,
+                             BusVoltageRange::RANGE_32V,
+                             PGAGain::GAIN_8_320MV,
+                             BusADCResolution::RES_12BIT,
+                             ShuntADCResolution::RES_12BIT_1S_532US,
+                             OperatingMode::SANDBVOLT_CONTINUOUS);
+}
+
+DriverConfiguration DriverConfiguration::make_16V_400mA() {
+  return DriverConfiguration(20,
+                             1.0,
+                             8192,
+                             BusVoltageRange::RANGE_16V,
+                             PGAGain::GAIN_1_40MV,
+                             BusADCResolution::RES_12BIT,
+                             ShuntADCResolution::RES_12BIT_1S_532US,
+                             OperatingMode::SANDBVOLT_CONTINUOUS);
+}
+
+DriverConfiguration::DriverConfiguration(uint32_t current_divider_mA,
+                                         float power_multiplier_mW,
+                                         uint32_t calibration_value,
+                                         BusVoltageRange bus_voltage_range,
+                                         PGAGain pga_gain,
+                                         BusADCResolution bus_adc_resolution,
+                                         ShuntADCResolution shunt_adc_resolution,
+                                         OperatingMode operating_mode)
+    : current_divider_mA(current_divider_mA),
+      power_multiplier_mW(power_multiplier_mW),
+      calibration_value(calibration_value),
+      ina219_configuration(calculate_ina219_configuration_value(
+          bus_voltage_range, pga_gain, bus_adc_resolution, shunt_adc_resolution, operating_mode)) {
+}
+
+uint16_t DriverConfiguration::calculate_ina219_configuration_value(BusVoltageRange bus_voltage_range,
+                                                                   PGAGain pga_gain,
+                                                                   BusADCResolution bus_adc_resolution,
+                                                                   ShuntADCResolution shunt_adc_resolution,
+                                                                   OperatingMode operating_mode) const {
+  return static_cast<uint16_t>(bus_voltage_range) | static_cast<uint16_t>(pga_gain) |
+         static_cast<uint16_t>(bus_adc_resolution) | static_cast<uint16_t>(shunt_adc_resolution) |
+         static_cast<uint16_t>(operating_mode);
+}
+
+}  // namespace ina219