diff --git a/config/hoverjet/default_current_sensor.yaml b/config/hoverjet/default_current_sensor.yaml new file mode 100644 index 0000000..9e6b2ba --- /dev/null +++ b/config/hoverjet/default_current_sensor.yaml @@ -0,0 +1,3 @@ +i2c_bus_path: /dev/i2c-1 +i2c_address: 64 +sensor_id: default diff --git a/embedded/current_sensor/current_sensor_bq.cc b/embedded/current_sensor/current_sensor_bq.cc new file mode 100644 index 0000000..7a01636 --- /dev/null +++ b/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 + +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 i2c_handle = i2c_open(config["i2c_bus_path"].as().c_str()); + if (i2c_handle == -1) { + std::cerr << "Failed to open i2c" << std::endl; + exit(1); + } + + sensor_ptr_ = + std::make_unique(i2c_handle, i2c_addr, ina219::DriverConfiguration::make_32V_2A()); + power_publisher_ = make_publisher(std::string("current_sensor_sensor") + config["sensor_name"].as()); +} + +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) diff --git a/embedded/current_sensor/current_sensor_bq.hh b/embedded/current_sensor/current_sensor_bq.hh new file mode 100644 index 0000000..db8adac --- /dev/null +++ b/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 + +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 sensor_ptr_; +}; + +} // namespace jet diff --git a/embedded/current_sensor/ina219_driver.cc b/embedded/current_sensor/ina219_driver.cc new file mode 100644 index 0000000..7cf07be --- /dev/null +++ b/embedded/current_sensor/ina219_driver.cc @@ -0,0 +1,107 @@ +#include "ina219_driver.hh" + +#include // 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(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(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 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 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(raw_shunt_voltage_buffer); +} + +std::optional 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(raw_current_buffer); +} + +std::optional 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(raw_power_buffer); +} + +std::optional INA219Driver::get_shunt_voltage_mV() const { + std::optional 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 INA219Driver::get_bus_voltage_V() const { + std::optional 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 INA219Driver::get_current_mA() const { + std::optional raw_current_value = get_raw_current(); + if (!raw_current_value) { + return {}; + } + return raw_current_value.value() / config_.current_divider_mA; +} + +std::optional INA219Driver::get_power_mW() const { + std::optional raw_power_value = get_raw_power(); + if (!raw_power_value) { + return {}; + } + return raw_power_value.value() * config_.power_multiplier_mW; +} + +} // namespace ina219 diff --git a/embedded/current_sensor/ina219_driver.hh b/embedded/current_sensor/ina219_driver.hh new file mode 100644 index 0000000..906c293 --- /dev/null +++ b/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 + +#include + +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 get_bus_voltage_V() const; + + /// @brief Gets the shunt voltage in mV + /// @return Shunt voltage in millivolts + std::optional get_shunt_voltage_mV() const; + + /// @brief Gets the current value in mA + /// @return Current in milliamps + std::optional get_current_mA() const; + + /// @brief Gets the measured power in mW + /// @return Power in milliwatts + std::optional 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 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 get_raw_shunt_voltage() const; + + /// @brief Gets the raw current value + /// @return The value read rom the INA219's current register + std::optional get_raw_current() const; + + /// @brief Gets the raw power value + /// @return The value read rom the INA219's power register + std::optional get_raw_power() const; + + i2c_device i2c_device_; +}; + +} // namespace ina219 diff --git a/embedded/current_sensor/ina219_driver_configuration.cc b/embedded/current_sensor/ina219_driver_configuration.cc new file mode 100644 index 0000000..eccc75d --- /dev/null +++ b/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(bus_voltage_range) | static_cast(pga_gain) | + static_cast(bus_adc_resolution) | static_cast(shunt_adc_resolution) | + static_cast(operating_mode); +} + +} // namespace ina219 diff --git a/embedded/current_sensor/ina219_driver_configuration.hh b/embedded/current_sensor/ina219_driver_configuration.hh new file mode 100644 index 0000000..4127041 --- /dev/null +++ b/embedded/current_sensor/ina219_driver_configuration.hh @@ -0,0 +1,84 @@ +#pragma once + +#include + +namespace ina219 { + +struct DriverConfiguration { + /// @brief Bus voltage ranges + enum class BusVoltageRange : uint16_t { + RANGE_16V = (0x0000), + RANGE_32V = (0x2000), + }; + + /// @brief PGA gains + enum class PGAGain : uint16_t { + GAIN_1_40MV = (0x0000), + GAIN_2_80MV = (0x0800), + GAIN_4_160MV = (0x1000), + GAIN_8_320MV = (0x1800), + }; + + /// @brief Bus ADC resolutions + enum class BusADCResolution : uint16_t { + RES_9BIT = (0x0000), + RES_10BIT = (0x0080), + RES_11BIT = (0x0100), + RES_12BIT = (0x0180), + }; + + /// @brief Shunt ADC resolutions + enum class ShuntADCResolution : uint8_t { + RES_9BIT_1S_84US = (0x0000), + RES_10BIT_1S_148US = (0x0008), + RES_11BIT_1S_276US = (0x0010), + RES_12BIT_1S_532US = (0x0018), + RES_12BIT_2S_1060US = (0x0048), + RES_12BIT_4S_2130US = (0x0050), + RES_12BIT_8S_4260US = (0x0058), + RES_12BIT_16S_8510US = (0x0060), + RES_12BIT_32S_17MS = (0x0068), + RES_12BIT_64S_34MS = (0x0070), + RES_12BIT_128S_69MS = (0x0078), + }; + + /// @brief Operating modes + enum class OperatingMode : uint8_t { + POWERDOWN = (0x0000), + SVOLT_TRIGGERED = (0x0001), + BVOLT_TRIGGERED = (0x0002), + SANDBVOLT_TRIGGERED = (0x0003), + ADCOFF = (0x0004), + SVOLT_CONTINUOUS = (0x0005), + BVOLT_CONTINUOUS = (0x0006), + SANDBVOLT_CONTINUOUS = (0x0007), + }; + + /// @brief Convenience functions for generating common configurations + static DriverConfiguration make_32V_2A(); + static DriverConfiguration make_32V_1A(); + static DriverConfiguration make_16V_400mA(); + + uint32_t current_divider_mA{0}; + float power_multiplier_mW{0}; + uint32_t calibration_value{0}; + uint16_t ina219_configuration{0}; + + private: + 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); + + uint16_t calculate_ina219_configuration_value(BusVoltageRange bus_voltage_range, + PGAGain pga_gain, + BusADCResolution bus_adc_resolution, + ShuntADCResolution shunt_adc_resolution, + OperatingMode operating_mode) const; +}; + +} // namespace ina219 diff --git a/embedded/current_sensor/power_reading.cc b/embedded/current_sensor/power_reading.cc new file mode 100644 index 0000000..4f66f98 --- /dev/null +++ b/embedded/current_sensor/power_reading.cc @@ -0,0 +1 @@ +#include "power_reading.hh" diff --git a/embedded/current_sensor/power_reading.hh b/embedded/current_sensor/power_reading.hh new file mode 100644 index 0000000..1ce579c --- /dev/null +++ b/embedded/current_sensor/power_reading.hh @@ -0,0 +1,18 @@ +#pragma once + +#include "infrastructure/comms/schemas/message.hh" + +#include + +namespace jet { + +// +struct PowerReading : Message { + float bus_voltage_mV; + float current_mA; + float power_mW; + + MESSAGE(PowerReading, bus_voltage_mV, current_mA, power_mW); +}; + +} // namespace jet