Skip to content

Commit 73ce55a

Browse files
benkroopbenkroop
committed
Adding current sensor driver + BQ
1 parent c69fada commit 73ce55a

9 files changed

Lines changed: 437 additions & 0 deletions

‎config/hoverjet/default_current_sensor.yaml‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,3 @@
1+
i2c_bus_path: /dev/i2c-1
2+
i2c_address: 64
3+
sensor_id: default

‎embedded/current_sensor/current_sensor_bq.cc‎

Lines changed: 50 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,50 @@
1+
//%bin(current_sensor_bq_main)
2+
//%deps(balsa_queue)
3+
//%deps(message)
4+
5+
#include "embedded/current_sensor/current_sensor_bq.hh"
6+
#include "embedded/current_sensor/power_reading.hh"
7+
#include "infrastructure/balsa_queue/bq_main_macro.hh"
8+
9+
#include <iostream>
10+
11+
namespace jet {
12+
13+
void CurrentSensorBq::init(const Config& config) {
14+
assert(config["i2c_bus_path"]);
15+
assert(config["i2c_address"]);
16+
assert(config["sensor_name"]);
17+
18+
const int i2c_addr = config["i2c_address"].as<int>();
19+
int i2c_handle = i2c_open(config["i2c_bus_path"].as<std::string>().c_str());
20+
if (i2c_handle == -1) {
21+
std::cerr << "Failed to open i2c" << std::endl;
22+
exit(1);
23+
}
24+
25+
sensor_ptr_ =
26+
std::make_unique<ina219::INA219Driver>(i2c_handle, i2c_addr, ina219::DriverConfiguration::make_32V_2A());
27+
power_publisher_ = make_publisher(std::string("current_sensor_sensor") + config["sensor_name"].as<std::string>());
28+
}
29+
30+
void CurrentSensorBq::loop() {
31+
PowerReading power_reading_message;
32+
if (const auto shunt_voltage_mV = sensor_ptr_->get_shunt_voltage_mV()) {
33+
power_reading_message.bus_voltage_mV = shunt_voltage_mV.value();
34+
}
35+
if (const auto shunt_voltage_mV = sensor_ptr_->get_current_mA()) {
36+
power_reading_message.current_mA = shunt_voltage_mV.value();
37+
}
38+
if (const auto shunt_voltage_mV = sensor_ptr_->get_power_mW()) {
39+
power_reading_message.power_mW = shunt_voltage_mV.value();
40+
}
41+
power_publisher_->publish(power_reading_message);
42+
}
43+
44+
void CurrentSensorBq::shutdown() {
45+
std::cout << "Shutting down!" << std::endl;
46+
}
47+
48+
} // namespace jet
49+
50+
BALSA_QUEUE_MAIN_FUNCTION(jet::CurrentSensorBq)

‎embedded/current_sensor/current_sensor_bq.hh‎

Lines changed: 22 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,22 @@
1+
#pragma once
2+
3+
#include "embedded/current_sensor/ina219_driver.hh"
4+
#include "infrastructure/balsa_queue/balsa_queue.hh"
5+
6+
#include <memory>
7+
8+
namespace jet {
9+
10+
class CurrentSensorBq : public BalsaQ {
11+
public:
12+
CurrentSensorBq() = default;
13+
void init(const Config& config);
14+
void loop();
15+
void shutdown();
16+
17+
private:
18+
PublisherPtr power_publisher_;
19+
std::unique_ptr<ina219::INA219Driver> sensor_ptr_;
20+
};
21+
22+
} // namespace jet

‎embedded/current_sensor/ina219_driver.cc‎

Lines changed: 108 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,108 @@
1+
#include "ina219_driver.hh"
2+
3+
#include <string.h> // memset
4+
5+
namespace ina219 {
6+
7+
[[nodiscard]] bool INA219Driver::write_register(Register reg, uint16_t value) const {
8+
uint16_t write_buffer = ((value & 0xFF) << 8) + (value >> 8);
9+
if (i2c_write(&i2c_device_, static_cast<uint8_t>(reg), &write_buffer, 2) == -1) {
10+
return false;
11+
}
12+
return true;
13+
}
14+
15+
[[nodiscard]] bool INA219Driver::read_register(Register reg, uint16_t& value) const {
16+
uint16_t read_buffer;
17+
if (i2c_read(&i2c_device_, static_cast<uint8_t>(reg), &read_buffer, 2) == -1) {
18+
return false;
19+
}
20+
value = ((read_buffer & 0xFF) << 8) + (read_buffer >> 8);
21+
return true;
22+
}
23+
24+
INA219Driver::INA219Driver(int i2c_handle, int i2c_address, DriverConfiguration config) : config_(config) {
25+
memset(&i2c_device_, 0, sizeof(i2c_device_));
26+
i2c_device_.bus = i2c_handle;
27+
i2c_device_.addr = i2c_address;
28+
i2c_device_.iaddr_bytes = I2C_ADDR_BYTES;
29+
i2c_device_.page_bytes = I2C_PAGE_BYTES;
30+
}
31+
32+
std::optional<int16_t> INA219Driver::get_raw_bus_voltage() const {
33+
uint16_t raw_bus_voltage_buffer;
34+
if (!read_register(Register::BUS_VOLTAGE, raw_bus_voltage_buffer)) {
35+
return {};
36+
}
37+
38+
// Shift to the right 3 to drop CNVR and OVF and multiply by LSB
39+
return (int16_t)((raw_bus_voltage_buffer >> 3) * 4);
40+
}
41+
42+
std::optional<int16_t> INA219Driver::get_raw_shunt_voltage() const {
43+
uint16_t raw_shunt_voltage_buffer;
44+
if (!read_register(Register::SHUNT_VOLTAGE, raw_shunt_voltage_buffer)) {
45+
return {};
46+
}
47+
return static_cast<int16_t>(raw_shunt_voltage_buffer);
48+
}
49+
50+
std::optional<int16_t> INA219Driver::get_raw_current() const {
51+
if (!write_register(Register::CALIBRATION, config_.calibration_value)) {
52+
return {};
53+
}
54+
55+
uint16_t raw_current_buffer;
56+
if (!read_register(Register::CURRENT, raw_current_buffer)) {
57+
return {};
58+
}
59+
60+
return static_cast<int16_t>(raw_current_buffer);
61+
}
62+
63+
std::optional<int16_t> INA219Driver::get_raw_power() const {
64+
if (!write_register(Register::CALIBRATION, config_.calibration_value)) {
65+
return {};
66+
}
67+
68+
uint16_t raw_power_buffer;
69+
if (!read_register(Register::POWER, raw_power_buffer)) {
70+
return {};
71+
}
72+
73+
return static_cast<int16_t>(raw_power_buffer);
74+
}
75+
76+
std::optional<float> INA219Driver::get_shunt_voltage_mV() const {
77+
std::optional<int16_t> raw_shunt_voltage_value = get_raw_shunt_voltage();
78+
if (!raw_shunt_voltage_value) {
79+
return {};
80+
}
81+
return raw_shunt_voltage_value.value() * MILLIVOLTS_PER_MICROVOLT;
82+
}
83+
84+
std::optional<float> INA219Driver::get_bus_voltage_V() const {
85+
std::optional<int16_t> raw_bus_voltage_value = get_raw_bus_voltage();
86+
if (!raw_bus_voltage_value) {
87+
return {};
88+
}
89+
return raw_bus_voltage_value.value() * VOLTS_PER_MILLIVOLT;
90+
}
91+
92+
std::optional<float> INA219Driver::get_current_mA() const {
93+
std::optional<float> raw_current_value = get_raw_current();
94+
if (!raw_current_value) {
95+
return {};
96+
}
97+
return raw_current_value.value() / config_.current_divider_mA;
98+
}
99+
100+
std::optional<float> INA219Driver::get_power_mW() const {
101+
std::optional<float> raw_power_value = get_raw_power();
102+
if (!raw_power_value) {
103+
return {};
104+
}
105+
return raw_power_value.value() * config_.power_multiplier_mW;
106+
}
107+
108+
} // namespace ina219

‎embedded/current_sensor/ina219_driver.hh‎

Lines changed: 88 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,88 @@
1+
#pragma once
2+
//%deps(i2c)
3+
4+
#include "ina219_driver_configuration.hh"
5+
6+
#include "third_party/i2c/i2c.h"
7+
8+
#include <stdint.h>
9+
10+
#include <optional>
11+
12+
namespace ina219 {
13+
14+
static constexpr float MILLIVOLTS_PER_MICROVOLT{0.01};
15+
static constexpr float VOLTS_PER_MILLIVOLT{0.001};
16+
static constexpr int32_t I2C_ADDR_BYTES{1};
17+
static constexpr int32_t I2C_PAGE_BYTES{16};
18+
19+
/// @brief Register IDs
20+
enum class Register : uint8_t {
21+
CONFIG = 0x00,
22+
SHUNT_VOLTAGE = 0x01,
23+
BUS_VOLTAGE = 0x02,
24+
POWER = 0x03,
25+
CURRENT = 0x04,
26+
CALIBRATION = 0x05
27+
};
28+
29+
/// @brief Class for interacting with the INA219 I2C current sensor
30+
class INA219Driver {
31+
public:
32+
/// @brief INA219Driver constructor
33+
/// @param i2c_handle - The I2C bus file descriptor
34+
/// @param i2c_address - The I2C address of the INA219 device
35+
/// @param config - The configuration for this INA219
36+
INA219Driver(int i2c_handle, int i2c_address, DriverConfiguration config);
37+
38+
/// @brief Gets the shunt voltage in volts
39+
/// @return Bus voltage in volts
40+
std::optional<float> get_bus_voltage_V() const;
41+
42+
/// @brief Gets the shunt voltage in mV
43+
/// @return Shunt voltage in millivolts
44+
std::optional<float> get_shunt_voltage_mV() const;
45+
46+
/// @brief Gets the current value in mA
47+
/// @return Current in milliamps
48+
std::optional<float> get_current_mA() const;
49+
50+
/// @brief Gets the measured power in mW
51+
/// @return Power in milliwatts
52+
std::optional<float> get_power_mW() const;
53+
54+
private:
55+
const DriverConfiguration config_;
56+
57+
/// @brief Writes two bytes to an INA219 register
58+
/// @param reg - ID of the register to write to
59+
/// @param value - The value to write
60+
/// @return Returns true on success, false on failure
61+
[[nodiscard]] bool write_register(Register reg, uint16_t value) const;
62+
63+
/// @brief Reads 16 bits from an INA219 register
64+
/// @param reg - ID of the register to read from
65+
/// @param value - Buffer to write the value to
66+
/// @return Returns true on success, false on failure
67+
[[nodiscard]] bool read_register(Register reg, uint16_t& value) const;
68+
69+
/// @brief Gets the raw bus voltage value
70+
/// @return The value read rom the INA219's bus voltage register
71+
std::optional<int16_t> get_raw_bus_voltage() const;
72+
73+
/// @brief Gets the raw shunt voltage value
74+
/// @return The value read rom the INA219's shunt voltage register
75+
std::optional<int16_t> get_raw_shunt_voltage() const;
76+
77+
/// @brief Gets the raw current value
78+
/// @return The value read rom the INA219's current register
79+
std::optional<int16_t> get_raw_current() const;
80+
81+
/// @brief Gets the raw power value
82+
/// @return The value read rom the INA219's power register
83+
std::optional<int16_t> get_raw_power() const;
84+
85+
i2c_device i2c_device_;
86+
};
87+
88+
} // namespace ina219

‎embedded/current_sensor/ina219_driver_configuration.cc‎

Lines changed: 63 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,63 @@
1+
#include "ina219_driver_configuration.hh"
2+
3+
namespace ina219 {
4+
5+
DriverConfiguration DriverConfiguration::make_32V_2A() {
6+
return DriverConfiguration(10,
7+
2,
8+
4096,
9+
BusVoltageRange::RANGE_32V,
10+
PGAGain::GAIN_8_320MV,
11+
BusADCResolution::RES_12BIT,
12+
ShuntADCResolution::RES_12BIT_1S_532US,
13+
OperatingMode::SANDBVOLT_CONTINUOUS);
14+
}
15+
16+
DriverConfiguration DriverConfiguration::make_32V_1A() {
17+
return DriverConfiguration(25,
18+
0.8,
19+
10240,
20+
BusVoltageRange::RANGE_32V,
21+
PGAGain::GAIN_8_320MV,
22+
BusADCResolution::RES_12BIT,
23+
ShuntADCResolution::RES_12BIT_1S_532US,
24+
OperatingMode::SANDBVOLT_CONTINUOUS);
25+
}
26+
27+
DriverConfiguration DriverConfiguration::make_16V_400mA() {
28+
return DriverConfiguration(20,
29+
1.0,
30+
8192,
31+
BusVoltageRange::RANGE_16V,
32+
PGAGain::GAIN_1_40MV,
33+
BusADCResolution::RES_12BIT,
34+
ShuntADCResolution::RES_12BIT_1S_532US,
35+
OperatingMode::SANDBVOLT_CONTINUOUS);
36+
}
37+
38+
DriverConfiguration::DriverConfiguration(uint32_t current_divider_mA,
39+
float power_multiplier_mW,
40+
uint32_t calibration_value,
41+
BusVoltageRange bus_voltage_range,
42+
PGAGain pga_gain,
43+
BusADCResolution bus_adc_resolution,
44+
ShuntADCResolution shunt_adc_resolution,
45+
OperatingMode operating_mode)
46+
: current_divider_mA(current_divider_mA),
47+
power_multiplier_mW(power_multiplier_mW),
48+
calibration_value(calibration_value),
49+
ina219_configuration(calculate_ina219_configuration_value(
50+
bus_voltage_range, pga_gain, bus_adc_resolution, shunt_adc_resolution, operating_mode)) {
51+
}
52+
53+
uint16_t DriverConfiguration::calculate_ina219_configuration_value(BusVoltageRange bus_voltage_range,
54+
PGAGain pga_gain,
55+
BusADCResolution bus_adc_resolution,
56+
ShuntADCResolution shunt_adc_resolution,
57+
OperatingMode operating_mode) const {
58+
return static_cast<uint16_t>(bus_voltage_range) | static_cast<uint16_t>(pga_gain) |
59+
static_cast<uint16_t>(bus_adc_resolution) | static_cast<uint16_t>(shunt_adc_resolution) |
60+
static_cast<uint16_t>(operating_mode);
61+
}
62+
63+
} // namespace ina219

0 commit comments

Comments
 (0)