Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-28 10:46:33 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

refactor(power_monitor): extract shared ina_common library from ina226/ina231

Browse Source 
The INA226 and INA231 drivers share an identical register map and nearly
identical measurement logic. This extracts the common code into a shared
library at src/lib/drivers/ina_common, reducing duplication and making it
easier to add support for additional INA-family devices.

The ina_common library provides shared register definitions, I2C
read/write helpers (via function pointer callback), device initialization,
measurement triggering, data collection, and battery connection state
management. Each driver retains only its device-specific constants,
probe logic, and scheduling.

Co-Authored-By: Claude Opus 4.6 (1M context) <noreply@anthropic.com>
This commit is contained in:
Eric Katzfey
2026-03-23 11:34:10 -07:00
parent 0cc603b88d
commit 116501f9c1
10 changed files with 531 additions and 607 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/power_monitor/ina226/CMakeLists.txt
+1
View File
@@ -42,5 +42,6 @@ px4_add_module(
ina226_params.yaml ina226_params.yaml
DEPENDS DEPENDS
battery battery
ina_common
px4_work_queue px4_work_queue
) )
src/drivers/power_monitor/ina226/ina226.cpp
+29 -182
View File
@@ -36,6 +36,9 @@
* @author David Sidrane <david_s5@usa.net> * @author David Sidrane <david_s5@usa.net>
* *
* Driver for the I2C attached INA226 * Driver for the I2C attached INA226
*
* Shared register definitions, I2C read/write, and common measurement logic
* are provided by the ina_common library (src/lib/drivers/ina_common).
*/ */
#include "ina226.h" #include "ina226.h"
@@ -49,41 +52,14 @@ INA226::INA226(const I2CSPIDriverConfig &config, int battery_index) :
_comms_errors(perf_alloc(PC_COUNT, "ina226_com_err")), _comms_errors(perf_alloc(PC_COUNT, "ina226_com_err")),
_collection_errors(perf_alloc(PC_COUNT, "ina226_collection_err")), _collection_errors(perf_alloc(PC_COUNT, "ina226_collection_err")),
_measure_errors(perf_alloc(PC_COUNT, "ina226_measurement_err")), _measure_errors(perf_alloc(PC_COUNT, "ina226_measurement_err")),
_battery(battery_index, this, INA226_SAMPLE_INTERVAL_US, battery_status_s::SOURCE_POWER_MODULE) _battery(battery_index, this, INA_COMMON_SAMPLE_INTERVAL_US, battery_status_s::SOURCE_POWER_MODULE),
_common(i2c_transfer_wrapper, this, _battery, _sample_perf, _comms_errors)
{ {
float fvalue = MAX_CURRENT; _common.loadParams("INA226_CURRENT", "INA226_SHUNT", "INA226_CONFIG",
_max_current = fvalue; INA226_MAX_CURRENT, INA226_SHUNT, INA226_DEFAULT_CONFIG);
param_t ph = param_find("INA226_CURRENT");
if (ph != PARAM_INVALID && param_get(ph, &fvalue) == PX4_OK) {
_max_current = fvalue;
}
fvalue = INA226_SHUNT;
_rshunt = fvalue;
ph = param_find("INA226_SHUNT");
if (ph != PARAM_INVALID && param_get(ph, &fvalue) == PX4_OK) {
_rshunt = fvalue;
}
ph = param_find("INA226_CONFIG");
int32_t value = INA226_CONFIG;
_config = (uint16_t)value;
if (ph != PARAM_INVALID && param_get(ph, &value) == PX4_OK) {
_config = (uint16_t)value;
}
_mode_triggered = ((_config & INA226_MODE_MASK) >> INA226_MODE_SHIFTS) <=
((INA226_MODE_SHUNT_BUS_TRIG & INA226_MODE_MASK) >>
INA226_MODE_SHIFTS);
_current_lsb = _max_current / DN_MAX;
_power_lsb = 25 * _current_lsb;
// We need to publish immediately, to guarantee that the first instance of the driver publishes to uORB instance 0 // We need to publish immediately, to guarantee that the first instance of the driver publishes to uORB instance 0
setConnected(false); _common.setConnected(false);
_battery.updateAndPublishBatteryStatus(hrt_absolute_time()); _battery.updateAndPublishBatteryStatus(hrt_absolute_time());
I2C::_retries = 5; I2C::_retries = 5;
@@ -91,41 +67,12 @@ INA226::INA226(const I2CSPIDriverConfig &config, int battery_index) :
INA226::~INA226() INA226::~INA226()
{ {
/* free perf counters */
perf_free(_sample_perf); perf_free(_sample_perf);
perf_free(_comms_errors); perf_free(_comms_errors);
perf_free(_collection_errors); perf_free(_collection_errors);
perf_free(_measure_errors); perf_free(_measure_errors);
} }
int INA226::read(uint8_t address, int16_t &data)
{
// read desired little-endian value via I2C
uint16_t received_bytes;
int ret = PX4_ERROR;
for (size_t i = 0; i < 3; i++) {
ret = transfer(&address, 1, (uint8_t *)&received_bytes, sizeof(received_bytes));
if (ret == PX4_OK) {
data = swap16(received_bytes);
break;
} else {
perf_count(_comms_errors);
PX4_DEBUG("i2c::transfer returned %d", ret);
}
}
return ret;
}
int INA226::write(uint8_t address, uint16_t value)
{
uint8_t data[3] = {address, ((uint8_t)((value & 0xff00) >> 8)), (uint8_t)(value & 0xff)};
return transfer(data, sizeof(data), nullptr, 0);
}
int int
INA226::init() INA226::init()
{ {
@@ -136,27 +83,10 @@ INA226::init()
return ret; return ret;
} }
write(INA226_REG_CONFIGURATION, INA226_RST); ret = _common.init();
_cal = INA226_CONST / (_current_lsb * _rshunt);
if (write(INA226_REG_CALIBRATION, _cal) < 0) {
return -3;
}
// If we run in continuous mode then start it here
if (!_mode_triggered) {
ret = write(INA226_REG_CONFIGURATION, _config);
} else {
ret = PX4_OK;
}
start(); start();
_sensor_ok = true;
_initialized = ret == PX4_OK;
return ret; return ret;
} }
@@ -175,12 +105,12 @@ INA226::probe()
{ {
int16_t value{0}; int16_t value{0};
if (read(INA226_MFG_ID, value) != PX4_OK || value != INA226_MFG_ID_TI) { if (_common.read(INA226_MFG_ID, value) != PX4_OK || value != INA226_MFG_ID_TI) {
PX4_DEBUG("probe mfgid %d", value); PX4_DEBUG("probe mfgid %d", value);
return -1; return -1;
} }
if (read(INA226_MFG_DIEID, value) != PX4_OK || value != INA226_MFG_DIE) { if (_common.read(INA226_MFG_DIEID, value) != PX4_OK || value != INA226_MFG_DIE) {
PX4_DEBUG("probe die id %d", value); PX4_DEBUG("probe die id %d", value);
return -1; return -1;
} }
@@ -188,150 +118,67 @@ INA226::probe()
return PX4_OK; return PX4_OK;
} }
int
INA226::measure()
{
int ret = PX4_OK;
if (_mode_triggered) {
ret = write(INA226_REG_CONFIGURATION, _config);
if (ret < 0) {
perf_count(_comms_errors);
PX4_DEBUG("i2c::transfer returned %d", ret);
}
}
return ret;
}
int
INA226::collect()
{
perf_begin(_sample_perf);
if (_parameter_update_sub.updated()) {
// Read from topic to clear updated flag
parameter_update_s parameter_update;
_parameter_update_sub.copy(&parameter_update);
updateParams();
}
// read from the sensor
// Note: If the power module is connected backwards, then the values of _power, _current, and _shunt will be negative but otherwise valid.
bool success{true};
success = success && (read(INA226_REG_BUSVOLTAGE, _bus_voltage) == PX4_OK);
// success = success && (read(INA226_REG_POWER, _power) == PX4_OK);
success = success && (read(INA226_REG_CURRENT, _current) == PX4_OK);
// success = success && (read(INA226_REG_SHUNTVOLTAGE, _shunt) == PX4_OK);
if (setConnected(success)) {
_battery.updateVoltage(static_cast<float>(_bus_voltage * INA226_VSCALE));
_battery.updateCurrent(static_cast<float>(_current * _current_lsb));
}
_battery.updateAndPublishBatteryStatus(hrt_absolute_time());
perf_end(_sample_perf);
if (success) {
return PX4_OK;
} else {
return PX4_ERROR;
}
}
void void
INA226::start() INA226::start()
{ {
ScheduleClear(); ScheduleClear();
/* reset the report ring and state machine */
_collect_phase = false; _collect_phase = false;
_measure_interval = INA226_CONVERSION_INTERVAL; _measure_interval = INA_COMMON_CONVERSION_INTERVAL;
/* schedule a cycle to start things */
ScheduleDelayed(5); ScheduleDelayed(5);
} }
void void
INA226::RunImpl() INA226::RunImpl()
{ {
if (_initialized) { if (_common._initialized) {
if (_collect_phase) { if (_collect_phase) {
/* perform collection */ if (_parameter_update_sub.updated()) {
if (collect() != PX4_OK) { parameter_update_s parameter_update;
_parameter_update_sub.copy(&parameter_update);
updateParams();
}
if (_common.collect() != PX4_OK) {
perf_count(_collection_errors); perf_count(_collection_errors);
/* if error restart the measurement state machine */
start(); start();
return; return;
} }
/* next phase is measurement */ _collect_phase = !_common._mode_triggered;
_collect_phase = !_mode_triggered;
if (_measure_interval > INA226_CONVERSION_INTERVAL) { if (_measure_interval > INA_COMMON_CONVERSION_INTERVAL) {
/* schedule a fresh cycle call when we are ready to measure again */ ScheduleDelayed(_measure_interval - INA_COMMON_CONVERSION_INTERVAL);
ScheduleDelayed(_measure_interval - INA226_CONVERSION_INTERVAL);
return; return;
} }
} }
/* Measurement phase */ if (_common.measure() != PX4_OK) {
/* Perform measurement */
if (measure() != PX4_OK) {
perf_count(_measure_errors); perf_count(_measure_errors);
} }
/* next phase is collection */
_collect_phase = true; _collect_phase = true;
/* schedule a fresh cycle call when the measurement is done */ ScheduleDelayed(INA_COMMON_CONVERSION_INTERVAL);
ScheduleDelayed(INA226_CONVERSION_INTERVAL);
} else { } else {
setConnected(false); _common.setConnected(false);
_battery.updateAndPublishBatteryStatus(hrt_absolute_time()); _battery.updateAndPublishBatteryStatus(hrt_absolute_time());
if (init() != PX4_OK) { if (init() != PX4_OK) {
ScheduleDelayed(INA226_INIT_RETRY_INTERVAL_US); ScheduleDelayed(INA_COMMON_INIT_RETRY_INTERVAL_US);
} }
} }
} }
bool INA226::setConnected(bool state)
{
// Filter out brief I2C failures for 2s
if (state) {
_connected = INA226_SAMPLE_FREQUENCY_HZ * 2;
} else if (_connected > 0) {
_connected--;
}
if (_connected > 0) {
_battery.setConnected(true);
} else {
_battery.setConnected(false);
_battery.updateVoltage(0);
_battery.updateCurrent(0);
}
return state;
}
void void
INA226::print_status() INA226::print_status()
{ {
I2CSPIDriverBase::print_status(); I2CSPIDriverBase::print_status();
if (_initialized) { if (_common._initialized) {
perf_print_counter(_sample_perf); perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors); perf_print_counter(_comms_errors);
@@ -339,6 +186,6 @@ INA226::print_status()
} else { } else {
PX4_INFO("Device not initialized. Retrying every %d ms until battery is plugged in.", PX4_INFO("Device not initialized. Retrying every %d ms until battery is plugged in.",
INA226_INIT_RETRY_INTERVAL_US / 1000); INA_COMMON_INIT_RETRY_INTERVAL_US / 1000);
} }
} }
src/drivers/power_monitor/ina226/ina226.h
+20 -124
View File
@@ -38,7 +38,6 @@
#pragma once #pragma once
#include <px4_platform_common/px4_config.h> #include <px4_platform_common/px4_config.h>
#include <px4_platform_common/getopt.h> #include <px4_platform_common/getopt.h>
#include <drivers/device/i2c.h> #include <drivers/device/i2c.h>
@@ -48,104 +47,21 @@
#include <uORB/SubscriptionInterval.hpp> #include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/parameter_update.h> #include <uORB/topics/parameter_update.h>
#include <px4_platform_common/i2c_spi_buses.h> #include <px4_platform_common/i2c_spi_buses.h>
#include <lib/drivers/ina_common/ina_common.h>
using namespace time_literals; using namespace time_literals;
/* Configuration Constants */ /* INA226-specific constants */
#define INA226_BASEADDR 0x41 /* 7-bit address. 8-bit address is 0x41 */ #define INA226_BASEADDR 0x41 /* 7-bit address */
// If initialization is forced (with the -f flag on the command line), but it fails, the drive will try again to #define INA226_MAX_CURRENT 164.0f /* 164 Amps */
// connect to the INA226 every this many microseconds #define INA226_SHUNT 0.0005f /* Shunt is 500 uOhm */
#define INA226_INIT_RETRY_INTERVAL_US 500000 #define INA226_DEFAULT_CONFIG (INA_COMMON_MODE_SHUNT_BUS_CONT | INA_COMMON_VSHCT_588US | INA_COMMON_VBUSCT_588US | INA_COMMON_AVERAGES_64)
/* INA226 Registers addresses */ /* INA226-specific probe registers */
#define INA226_REG_CONFIGURATION (0x00) #define INA226_MFG_ID (0xfe)
#define INA226_REG_SHUNTVOLTAGE (0x01) #define INA226_MFG_DIEID (0xff)
#define INA226_REG_BUSVOLTAGE (0x02) #define INA226_MFG_ID_TI (0x5449) // TI
#define INA226_REG_POWER (0x03) #define INA226_MFG_DIE (0x2260) // INA2260
#define INA226_REG_CURRENT (0x04)
#define INA226_REG_CALIBRATION (0x05)
#define INA226_REG_MASKENABLE (0x06)
#define INA226_REG_ALERTLIMIT (0x07)
#define INA226_MFG_ID (0xfe)
#define INA226_MFG_DIEID (0xff)
#define INA226_MFG_ID_TI (0x5449) // TI
#define INA226_MFG_DIE (0x2260) // INA2260
/* INA226 Configuration Register */
#define INA226_MODE_SHIFTS (0)
#define INA226_MODE_MASK (7 << INA226_MODE_SHIFTS)
#define INA226_MODE_SHUTDOWN (0 << INA226_MODE_SHIFTS)
#define INA226_MODE_SHUNT_TRIG (1 << INA226_MODE_SHIFTS)
#define INA226_MODE_BUS_TRIG (2 << INA226_MODE_SHIFTS)
#define INA226_MODE_SHUNT_BUS_TRIG (3 << INA226_MODE_SHIFTS)
#define INA226_MODE_ADC_OFF (4 << INA226_MODE_SHIFTS)
#define INA226_MODE_SHUNT_CONT (5 << INA226_MODE_SHIFTS)
#define INA226_MODE_BUS_CONT (6 << INA226_MODE_SHIFTS)
#define INA226_MODE_SHUNT_BUS_CONT (7 << INA226_MODE_SHIFTS)
#define INA226_VSHCT_SHIFTS (3)
#define INA226_VSHCT_MASK (7 << INA226_VSHCT_SHIFTS)
#define INA226_VSHCT_140US (0 << INA226_VSHCT_SHIFTS)
#define INA226_VSHCT_204US (1 << INA226_VSHCT_SHIFTS)
#define INA226_VSHCT_332US (2 << INA226_VSHCT_SHIFTS)
#define INA226_VSHCT_588US (3 << INA226_VSHCT_SHIFTS)
#define INA226_VSHCT_1100US (4 << INA226_VSHCT_SHIFTS)
#define INA226_VSHCT_2116US (5 << INA226_VSHCT_SHIFTS)
#define INA226_VSHCT_4156US (6 << INA226_VSHCT_SHIFTS)
#define INA226_VSHCT_8244US (7 << INA226_VSHCT_SHIFTS)
#define INA226_VBUSCT_SHIFTS (6)
#define INA226_VBUSCT_MASK (7 << INA226_VBUSCT_SHIFTS)
#define INA226_VBUSCT_140US (0 << INA226_VBUSCT_SHIFTS)
#define INA226_VBUSCT_204US (1 << INA226_VBUSCT_SHIFTS)
#define INA226_VBUSCT_332US (2 << INA226_VBUSCT_SHIFTS)
#define INA226_VBUSCT_588US (3 << INA226_VBUSCT_SHIFTS)
#define INA226_VBUSCT_1100US (4 << INA226_VBUSCT_SHIFTS)
#define INA226_VBUSCT_2116US (5 << INA226_VBUSCT_SHIFTS)
#define INA226_VBUSCT_4156US (6 << INA226_VBUSCT_SHIFTS)
#define INA226_VBUSCT_8244US (7 << INA226_VBUSCT_SHIFTS)
#define INA226_AVERAGES_SHIFTS (9)
#define INA226_AVERAGES_MASK (7 << INA226_AVERAGES_SHIFTS)
#define INA226_AVERAGES_1 (0 << INA226_AVERAGES_SHIFTS)
#define INA226_AVERAGES_4 (1 << INA226_AVERAGES_SHIFTS)
#define INA226_AVERAGES_16 (2 << INA226_AVERAGES_SHIFTS)
#define INA226_AVERAGES_64 (3 << INA226_AVERAGES_SHIFTS)
#define INA226_AVERAGES_128 (4 << INA226_AVERAGES_SHIFTS)
#define INA226_AVERAGES_256 (5 << INA226_AVERAGES_SHIFTS)
#define INA226_AVERAGES_512 (6 << INA226_AVERAGES_SHIFTS)
#define INA226_AVERAGES_1024 (7 << INA226_AVERAGES_SHIFTS)
#define INA226_CONFIG (INA226_MODE_SHUNT_BUS_CONT | INA226_VSHCT_588US | INA226_VBUSCT_588US | INA226_AVERAGES_64)
#define INA226_RST (1 << 15)
/* INA226 Enable / Mask Register */
#define INA226_LEN (1 << 0)
#define INA226_APOL (1 << 1)
#define INA226_OVF (1 << 2)
#define INA226_CVRF (1 << 3)
#define INA226_AFF (1 << 4)
#define INA226_CNVR (1 << 10)
#define INA226_POL (1 << 11)
#define INA226_BUL (1 << 12)
#define INA226_BOL (1 << 13)
#define INA226_SUL (1 << 14)
#define INA226_SOL (1 << 15)
#define INA226_SAMPLE_FREQUENCY_HZ 10
#define INA226_SAMPLE_INTERVAL_US (1_s / INA226_SAMPLE_FREQUENCY_HZ)
#define INA226_CONVERSION_INTERVAL (INA226_SAMPLE_INTERVAL_US - 7)
#define MAX_CURRENT 164.0f /* 164 Amps */
#define DN_MAX 32768.0f /* 2^15 */
#define INA226_CONST 0.00512f /* is an internal fixed value used to ensure scaling is maintained properly */
#define INA226_SHUNT 0.0005f /* Shunt is 500 uOhm */
#define INA226_VSCALE 0.00125f /* LSB of voltage is 1.25 mV */
#define swap16(w) __builtin_bswap16((w))
class INA226 : public device::I2C, public ModuleParams, public I2CSPIDriver<INA226> class INA226 : public device::I2C, public ModuleParams, public I2CSPIDriver<INA226>
{ {
@@ -162,7 +78,7 @@ public:
/** /**
* Tries to call the init() function. If it fails, then it will schedule to retry again in * Tries to call the init() function. If it fails, then it will schedule to retry again in
* INA226_INIT_RETRY_INTERVAL_US microseconds. It will keep retrying at this interval until initialization succeeds. * INA_COMMON_INIT_RETRY_INTERVAL_US microseconds. It will keep retrying at this interval until initialization succeeds.
* *
* @return PX4_OK if initialization succeeded on the first try. Negative value otherwise. * @return PX4_OK if initialization succeeded on the first try. Negative value otherwise.
*/ */
@@ -177,48 +93,28 @@ protected:
int probe() override; int probe() override;
private: private:
bool _sensor_ok{false};
unsigned _measure_interval{0};
bool _collect_phase{false};
bool _initialized{false};
perf_counter_t _sample_perf; perf_counter_t _sample_perf;
perf_counter_t _comms_errors; perf_counter_t _comms_errors;
perf_counter_t _collection_errors; perf_counter_t _collection_errors;
perf_counter_t _measure_errors; perf_counter_t _measure_errors;
int16_t _bus_voltage{0}; unsigned _measure_interval{0};
int16_t _power{0}; bool _collect_phase{false};
int16_t _current{0};
int16_t _shunt{0};
int16_t _cal{0};
bool _mode_triggered{false};
float _max_current{MAX_CURRENT};
float _rshunt{INA226_SHUNT};
uint16_t _config{INA226_CONFIG};
float _current_lsb{_max_current / DN_MAX};
float _power_lsb{25.0f * _current_lsb};
Battery _battery; Battery _battery;
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
int read(uint8_t address, int16_t &data); INACommon _common;
int write(uint8_t address, uint16_t data);
uint8_t _connected{0}; static int i2c_transfer_wrapper(void *context, const uint8_t *send, unsigned send_len,
// returns state unchanged uint8_t *recv, unsigned recv_len)
bool setConnected(bool state); {
return static_cast<INA226 *>(context)->transfer(send, send_len, recv, recv_len);
}
/** /**
* Initialise the automatic measurement state machine and start it. * Initialise the automatic measurement state machine and start it.
*
* @note This function is called at open and error time. It might make sense
* to make it more aggressive about resetting the bus in case of errors.
*/ */
void start(); void start();
int measure();
int collect();
}; };
src/drivers/power_monitor/ina231/CMakeLists.txt
+1
View File
@@ -42,5 +42,6 @@ px4_add_module(
ina231_params.yaml ina231_params.yaml
DEPENDS DEPENDS
battery battery
ina_common
px4_work_queue px4_work_queue
) )
src/drivers/power_monitor/ina231/ina231.cpp
+28 -181
View File
@@ -35,6 +35,9 @@
* @file ina231.cpp * @file ina231.cpp
* *
* Driver for the I2C attached INA231 * Driver for the I2C attached INA231
*
* Shared register definitions, I2C read/write, and common measurement logic
* are provided by the ina_common library (src/lib/drivers/ina_common).
*/ */
#include "ina231.h" #include "ina231.h"
@@ -48,41 +51,14 @@ INA231::INA231(const I2CSPIDriverConfig &config, int battery_index) :
_comms_errors(perf_alloc(PC_COUNT, "ina231_com_err")), _comms_errors(perf_alloc(PC_COUNT, "ina231_com_err")),
_collection_errors(perf_alloc(PC_COUNT, "ina231_collection_err")), _collection_errors(perf_alloc(PC_COUNT, "ina231_collection_err")),
_measure_errors(perf_alloc(PC_COUNT, "ina231_measurement_err")), _measure_errors(perf_alloc(PC_COUNT, "ina231_measurement_err")),
_battery(battery_index, this, INA231_SAMPLE_INTERVAL_US, battery_status_s::SOURCE_POWER_MODULE) _battery(battery_index, this, INA_COMMON_SAMPLE_INTERVAL_US, battery_status_s::SOURCE_POWER_MODULE),
_common(i2c_transfer_wrapper, this, _battery, _sample_perf, _comms_errors)
{ {
float fvalue = MAX_CURRENT; _common.loadParams("INA231_CURRENT", "INA231_SHUNT", "INA231_CONFIG",
_max_current = fvalue; INA231_MAX_CURRENT, INA231_SHUNT, INA231_DEFAULT_CONFIG);
param_t ph = param_find("INA231_CURRENT");
if (ph != PARAM_INVALID && param_get(ph, &fvalue) == PX4_OK) {
_max_current = fvalue;
}
fvalue = INA231_SHUNT;
_rshunt = fvalue;
ph = param_find("INA231_SHUNT");
if (ph != PARAM_INVALID && param_get(ph, &fvalue) == PX4_OK) {
_rshunt = fvalue;
}
ph = param_find("INA231_CONFIG");
int32_t value = INA231_CONFIG;
_config = (uint16_t)value;
if (ph != PARAM_INVALID && param_get(ph, &value) == PX4_OK) {
_config = (uint16_t)value;
}
_mode_triggered = ((_config & INA231_MODE_MASK) >> INA231_MODE_SHIFTS) <=
((INA231_MODE_SHUNT_BUS_TRIG & INA231_MODE_MASK) >>
INA231_MODE_SHIFTS);
_current_lsb = _max_current / DN_MAX;
_power_lsb = 25 * _current_lsb;
// We need to publish immediately, to guarantee that the first instance of the driver publishes to uORB instance 0 // We need to publish immediately, to guarantee that the first instance of the driver publishes to uORB instance 0
setConnected(false); _common.setConnected(false);
_battery.updateAndPublishBatteryStatus(hrt_absolute_time()); _battery.updateAndPublishBatteryStatus(hrt_absolute_time());
I2C::_retries = 5; I2C::_retries = 5;
@@ -90,41 +66,12 @@ INA231::INA231(const I2CSPIDriverConfig &config, int battery_index) :
INA231::~INA231() INA231::~INA231()
{ {
/* free perf counters */
perf_free(_sample_perf); perf_free(_sample_perf);
perf_free(_comms_errors); perf_free(_comms_errors);
perf_free(_collection_errors); perf_free(_collection_errors);
perf_free(_measure_errors); perf_free(_measure_errors);
} }
int INA231::read(uint8_t address, int16_t &data)
{
// read desired little-endian value via I2C
uint16_t received_bytes;
int ret = PX4_ERROR;
for (size_t i = 0; i < 3; i++) {
ret = transfer(&address, 1, (uint8_t *)&received_bytes, sizeof(received_bytes));
if (ret == PX4_OK) {
data = swap16(received_bytes);
break;
} else {
perf_count(_comms_errors);
PX4_DEBUG("i2c::transfer returned %d", ret);
}
}
return ret;
}
int INA231::write(uint8_t address, uint16_t value)
{
uint8_t data[3] = {address, ((uint8_t)((value & 0xff00) >> 8)), (uint8_t)(value & 0xff)};
return transfer(data, sizeof(data), nullptr, 0);
}
int int
INA231::init() INA231::init()
{ {
@@ -135,27 +82,10 @@ INA231::init()
return ret; return ret;
} }
write(INA231_REG_CONFIGURATION, INA231_RST); ret = _common.init();
_cal = INA231_CONST / (_current_lsb * _rshunt);
if (write(INA231_REG_CALIBRATION, _cal) < 0) {
return -3;
}
// If we run in continuous mode then start it here
if (!_mode_triggered) {
ret = write(INA231_REG_CONFIGURATION, _config);
} else {
ret = PX4_OK;
}
start(); start();
_sensor_ok = true;
_initialized = ret == PX4_OK;
return ret; return ret;
} }
@@ -174,7 +104,7 @@ INA231::probe()
{ {
int16_t value{0}; int16_t value{0};
if (read(INA231_REG_CONFIGURATION, value) != PX4_OK) { if (_common.read(INA_COMMON_REG_CONFIGURATION, value) != PX4_OK) {
PX4_DEBUG("probe failed to read config register"); PX4_DEBUG("probe failed to read config register");
return -1; return -1;
} }
@@ -182,150 +112,67 @@ INA231::probe()
return PX4_OK; return PX4_OK;
} }
int
INA231::measure()
{
int ret = PX4_OK;
if (_mode_triggered) {
ret = write(INA231_REG_CONFIGURATION, _config);
if (ret < 0) {
perf_count(_comms_errors);
PX4_DEBUG("i2c::transfer returned %d", ret);
}
}
return ret;
}
int
INA231::collect()
{
perf_begin(_sample_perf);
if (_parameter_update_sub.updated()) {
// Read from topic to clear updated flag
parameter_update_s parameter_update;
_parameter_update_sub.copy(&parameter_update);
updateParams();
}
// read from the sensor
// Note: If the power module is connected backwards, then the values of _power, _current, and _shunt will be negative but otherwise valid.
bool success{true};
success = success && (read(INA231_REG_BUSVOLTAGE, _bus_voltage) == PX4_OK);
// success = success && (read(INA231_REG_POWER, _power) == PX4_OK);
success = success && (read(INA231_REG_CURRENT, _current) == PX4_OK);
// success = success && (read(INA231_REG_SHUNTVOLTAGE, _shunt) == PX4_OK);
if (setConnected(success)) {
_battery.updateVoltage(static_cast<float>(_bus_voltage * INA231_VSCALE));
_battery.updateCurrent(static_cast<float>(_current * _current_lsb));
}
_battery.updateAndPublishBatteryStatus(hrt_absolute_time());
perf_end(_sample_perf);
if (success) {
return PX4_OK;
} else {
return PX4_ERROR;
}
}
void void
INA231::start() INA231::start()
{ {
ScheduleClear(); ScheduleClear();
/* reset the report ring and state machine */
_collect_phase = false; _collect_phase = false;
_measure_interval = INA231_CONVERSION_INTERVAL; _measure_interval = INA_COMMON_CONVERSION_INTERVAL;
/* schedule a cycle to start things */
ScheduleDelayed(5); ScheduleDelayed(5);
} }
void void
INA231::RunImpl() INA231::RunImpl()
{ {
if (_initialized) { if (_common._initialized) {
if (_collect_phase) { if (_collect_phase) {
/* perform collection */ if (_parameter_update_sub.updated()) {
if (collect() != PX4_OK) { parameter_update_s parameter_update;
_parameter_update_sub.copy(&parameter_update);
updateParams();
}
if (_common.collect() != PX4_OK) {
perf_count(_collection_errors); perf_count(_collection_errors);
/* if error restart the measurement state machine */
start(); start();
return; return;
} }
/* next phase is measurement */ _collect_phase = !_common._mode_triggered;
_collect_phase = !_mode_triggered;
if (_measure_interval > INA231_CONVERSION_INTERVAL) { if (_measure_interval > INA_COMMON_CONVERSION_INTERVAL) {
/* schedule a fresh cycle call when we are ready to measure again */ ScheduleDelayed(_measure_interval - INA_COMMON_CONVERSION_INTERVAL);
ScheduleDelayed(_measure_interval - INA231_CONVERSION_INTERVAL);
return; return;
} }
} }
/* Measurement phase */ if (_common.measure() != PX4_OK) {
/* Perform measurement */
if (measure() != PX4_OK) {
perf_count(_measure_errors); perf_count(_measure_errors);
} }
/* next phase is collection */
_collect_phase = true; _collect_phase = true;
/* schedule a fresh cycle call when the measurement is done */ ScheduleDelayed(INA_COMMON_CONVERSION_INTERVAL);
ScheduleDelayed(INA231_CONVERSION_INTERVAL);
} else { } else {
setConnected(false); _common.setConnected(false);
_battery.updateAndPublishBatteryStatus(hrt_absolute_time()); _battery.updateAndPublishBatteryStatus(hrt_absolute_time());
if (init() != PX4_OK) { if (init() != PX4_OK) {
ScheduleDelayed(INA231_INIT_RETRY_INTERVAL_US); ScheduleDelayed(INA_COMMON_INIT_RETRY_INTERVAL_US);
} }
} }
} }
bool INA231::setConnected(bool state)
{
// Filter out brief I2C failures for 2s
if (state) {
_connected = INA231_SAMPLE_FREQUENCY_HZ * 2;
} else if (_connected > 0) {
_connected--;
}
if (_connected > 0) {
_battery.setConnected(true);
} else {
_battery.setConnected(false);
_battery.updateVoltage(0);
_battery.updateCurrent(0);
}
return state;
}
void void
INA231::print_status() INA231::print_status()
{ {
I2CSPIDriverBase::print_status(); I2CSPIDriverBase::print_status();
if (_initialized) { if (_common._initialized) {
perf_print_counter(_sample_perf); perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors); perf_print_counter(_comms_errors);
@@ -333,6 +180,6 @@ INA231::print_status()
} else { } else {
PX4_INFO("Device not initialized. Retrying every %d ms until battery is plugged in.", PX4_INFO("Device not initialized. Retrying every %d ms until battery is plugged in.",
INA231_INIT_RETRY_INTERVAL_US / 1000); INA_COMMON_INIT_RETRY_INTERVAL_US / 1000);
} }
} }
src/drivers/power_monitor/ina231/ina231.h
+15 -120
View File
@@ -38,7 +38,6 @@
#pragma once #pragma once
#include <px4_platform_common/px4_config.h> #include <px4_platform_common/px4_config.h>
#include <px4_platform_common/getopt.h> #include <px4_platform_common/getopt.h>
#include <drivers/device/i2c.h> #include <drivers/device/i2c.h>
@@ -48,99 +47,15 @@
#include <uORB/SubscriptionInterval.hpp> #include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/parameter_update.h> #include <uORB/topics/parameter_update.h>
#include <px4_platform_common/i2c_spi_buses.h> #include <px4_platform_common/i2c_spi_buses.h>
#include <lib/drivers/ina_common/ina_common.h>
using namespace time_literals; using namespace time_literals;
/* Configuration Constants */ /* INA231-specific constants */
#define INA231_BASEADDR 0x44 /* 7-bit address. 8-bit address is 0x88 */ #define INA231_BASEADDR 0x44 /* 7-bit address. 8-bit address is 0x88 */
// If initialization is forced (with the -f flag on the command line), but it fails, the drive will try again to #define INA231_MAX_CURRENT 90.0f /* 90 Amps */
// connect to the INA231 every this many microseconds #define INA231_SHUNT 0.0005f /* Shunt is 500 uOhm */
#define INA231_INIT_RETRY_INTERVAL_US 500000 #define INA231_DEFAULT_CONFIG (INA_COMMON_MODE_SHUNT_BUS_CONT | INA_COMMON_VSHCT_1100US | INA_COMMON_VBUSCT_1100US | INA_COMMON_AVERAGES_16)
/* INA231 Registers addresses */
#define INA231_REG_CONFIGURATION (0x00)
#define INA231_REG_SHUNTVOLTAGE (0x01)
#define INA231_REG_BUSVOLTAGE (0x02)
#define INA231_REG_POWER (0x03)
#define INA231_REG_CURRENT (0x04)
#define INA231_REG_CALIBRATION (0x05)
#define INA231_REG_MASKENABLE (0x06)
#define INA231_REG_ALERTLIMIT (0x07)
/* INA231 Configuration Register */
#define INA231_MODE_SHIFTS (0)
#define INA231_MODE_MASK (7 << INA231_MODE_SHIFTS)
#define INA231_MODE_SHUTDOWN (0 << INA231_MODE_SHIFTS)
#define INA231_MODE_SHUNT_TRIG (1 << INA231_MODE_SHIFTS)
#define INA231_MODE_BUS_TRIG (2 << INA231_MODE_SHIFTS)
#define INA231_MODE_SHUNT_BUS_TRIG (3 << INA231_MODE_SHIFTS)
#define INA231_MODE_ADC_OFF (4 << INA231_MODE_SHIFTS)
#define INA231_MODE_SHUNT_CONT (5 << INA231_MODE_SHIFTS)
#define INA231_MODE_BUS_CONT (6 << INA231_MODE_SHIFTS)
#define INA231_MODE_SHUNT_BUS_CONT (7 << INA231_MODE_SHIFTS)
#define INA231_VSHCT_SHIFTS (3)
#define INA231_VSHCT_MASK (7 << INA231_VSHCT_SHIFTS)
#define INA231_VSHCT_140US (0 << INA231_VSHCT_SHIFTS)
#define INA231_VSHCT_204US (1 << INA231_VSHCT_SHIFTS)
#define INA231_VSHCT_332US (2 << INA231_VSHCT_SHIFTS)
#define INA231_VSHCT_588US (3 << INA231_VSHCT_SHIFTS)
#define INA231_VSHCT_1100US (4 << INA231_VSHCT_SHIFTS)
#define INA231_VSHCT_2116US (5 << INA231_VSHCT_SHIFTS)
#define INA231_VSHCT_4156US (6 << INA231_VSHCT_SHIFTS)
#define INA231_VSHCT_8244US (7 << INA231_VSHCT_SHIFTS)
#define INA231_VBUSCT_SHIFTS (6)
#define INA231_VBUSCT_MASK (7 << INA231_VBUSCT_SHIFTS)
#define INA231_VBUSCT_140US (0 << INA231_VBUSCT_SHIFTS)
#define INA231_VBUSCT_204US (1 << INA231_VBUSCT_SHIFTS)
#define INA231_VBUSCT_332US (2 << INA231_VBUSCT_SHIFTS)
#define INA231_VBUSCT_588US (3 << INA231_VBUSCT_SHIFTS)
#define INA231_VBUSCT_1100US (4 << INA231_VBUSCT_SHIFTS)
#define INA231_VBUSCT_2116US (5 << INA231_VBUSCT_SHIFTS)
#define INA231_VBUSCT_4156US (6 << INA231_VBUSCT_SHIFTS)
#define INA231_VBUSCT_8244US (7 << INA231_VBUSCT_SHIFTS)
#define INA231_AVERAGES_SHIFTS (9)
#define INA231_AVERAGES_MASK (7 << INA231_AVERAGES_SHIFTS)
#define INA231_AVERAGES_1 (0 << INA231_AVERAGES_SHIFTS)
#define INA231_AVERAGES_4 (1 << INA231_AVERAGES_SHIFTS)
#define INA231_AVERAGES_16 (2 << INA231_AVERAGES_SHIFTS)
#define INA231_AVERAGES_64 (3 << INA231_AVERAGES_SHIFTS)
#define INA231_AVERAGES_128 (4 << INA231_AVERAGES_SHIFTS)
#define INA231_AVERAGES_256 (5 << INA231_AVERAGES_SHIFTS)
#define INA231_AVERAGES_512 (6 << INA231_AVERAGES_SHIFTS)
#define INA231_AVERAGES_1024 (7 << INA231_AVERAGES_SHIFTS)
#define INA231_CONFIG (INA231_MODE_SHUNT_BUS_CONT | INA231_VSHCT_1100US | INA231_VBUSCT_1100US | INA231_AVERAGES_16)
#define INA231_RST (1 << 15)
/* INA231 Enable / Mask Register */
#define INA231_LEN (1 << 0)
#define INA231_APOL (1 << 1)
#define INA231_OVF (1 << 2)
#define INA231_CVRF (1 << 3)
#define INA231_AFF (1 << 4)
#define INA231_CNVR (1 << 10)
#define INA231_POL (1 << 11)
#define INA231_BUL (1 << 12)
#define INA231_BOL (1 << 13)
#define INA231_SUL (1 << 14)
#define INA231_SOL (1 << 15)
#define INA231_SAMPLE_FREQUENCY_HZ 10
#define INA231_SAMPLE_INTERVAL_US (1_s / INA231_SAMPLE_FREQUENCY_HZ)
#define INA231_CONVERSION_INTERVAL (INA231_SAMPLE_INTERVAL_US - 7)
#define MAX_CURRENT 90.0f /* 90 Amps */
#define DN_MAX 32768.0f /* 2^15 */
#define INA231_CONST 0.00512f /* is an internal fixed value used to ensure scaling is maintained properly */
#define INA231_SHUNT 0.0005f /* Shunt is 500 uOhm */
#define INA231_VSCALE 0.00125f /* LSB of voltage is 1.25 mV */
#define swap16(w) __builtin_bswap16((w))
class INA231 : public device::I2C, public ModuleParams, public I2CSPIDriver<INA231> class INA231 : public device::I2C, public ModuleParams, public I2CSPIDriver<INA231>
{ {
@@ -157,7 +72,7 @@ public:
/** /**
* Tries to call the init() function. If it fails, then it will schedule to retry again in * Tries to call the init() function. If it fails, then it will schedule to retry again in
* INA231_INIT_RETRY_INTERVAL_US microseconds. It will keep retrying at this interval until initialization succeeds. * INA_COMMON_INIT_RETRY_INTERVAL_US microseconds. It will keep retrying at this interval until initialization succeeds.
* *
* @return PX4_OK if initialization succeeded on the first try. Negative value otherwise. * @return PX4_OK if initialization succeeded on the first try. Negative value otherwise.
*/ */
@@ -172,48 +87,28 @@ protected:
int probe() override; int probe() override;
private: private:
bool _sensor_ok{false};
unsigned _measure_interval{0};
bool _collect_phase{false};
bool _initialized{false};
perf_counter_t _sample_perf; perf_counter_t _sample_perf;
perf_counter_t _comms_errors; perf_counter_t _comms_errors;
perf_counter_t _collection_errors; perf_counter_t _collection_errors;
perf_counter_t _measure_errors; perf_counter_t _measure_errors;
int16_t _bus_voltage{0}; unsigned _measure_interval{0};
int16_t _power{0}; bool _collect_phase{false};
int16_t _current{0};
int16_t _shunt{0};
int16_t _cal{0};
bool _mode_triggered{false};
float _max_current{MAX_CURRENT};
float _rshunt{INA231_SHUNT};
uint16_t _config{INA231_CONFIG};
float _current_lsb{_max_current / DN_MAX};
float _power_lsb{25.0f * _current_lsb};
Battery _battery; Battery _battery;
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
int read(uint8_t address, int16_t &data); INACommon _common;
int write(uint8_t address, uint16_t data);
uint8_t _connected{0}; static int i2c_transfer_wrapper(void *context, const uint8_t *send, unsigned send_len,
// returns state unchanged uint8_t *recv, unsigned recv_len)
bool setConnected(bool state); {
return static_cast<INA231 *>(context)->transfer(send, send_len, recv, recv_len);
}
/** /**
* Initialise the automatic measurement state machine and start it. * Initialise the automatic measurement state machine and start it.
*
* @note This function is called at open and error time. It might make sense
* to make it more aggressive about resetting the bus in case of errors.
*/ */
void start(); void start();
int measure();
int collect();
}; };
src/lib/drivers/CMakeLists.txt
+1
View File
@@ -34,6 +34,7 @@
add_subdirectory(accelerometer) add_subdirectory(accelerometer)
add_subdirectory(device) add_subdirectory(device)
add_subdirectory(gyroscope) add_subdirectory(gyroscope)
add_subdirectory(ina_common)
add_subdirectory(led) add_subdirectory(led)
add_subdirectory(magnetometer) add_subdirectory(magnetometer)
add_subdirectory(rangefinder) add_subdirectory(rangefinder)
src/lib/drivers/ina_common/CMakeLists.txt
+34
View File
@@ -0,0 +1,34 @@
############################################################################
#
# Copyright (c) 2026 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
px4_add_library(ina_common ina_common.cpp)
src/lib/drivers/ina_common/ina_common.cpp
+201
View File
@@ -0,0 +1,201 @@
/****************************************************************************
*
* Copyright (c) 2019-2026 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file ina_common.cpp
*
* Shared implementation for INA226/INA228/INA231 power monitor drivers.
*/
#include "ina_common.h"
#include <px4_platform_common/log.h>
#include <parameters/param.h>
INACommon::INACommon(transfer_func_t transfer_func, void *transfer_context, Battery &battery,
perf_counter_t sample_perf, perf_counter_t comms_errors) :
_transfer_func(transfer_func),
_transfer_context(transfer_context),
_battery(battery),
_sample_perf(sample_perf),
_comms_errors(comms_errors)
{
}
void INACommon::loadParams(const char *current_param, const char *shunt_param, const char *config_param,
float default_max_current, float default_shunt, uint16_t default_config)
{
float fvalue = default_max_current;
_max_current = fvalue;
param_t ph = param_find(current_param);
if (ph != PARAM_INVALID && param_get(ph, &fvalue) == PX4_OK) {
_max_current = fvalue;
}
fvalue = default_shunt;
_rshunt = fvalue;
ph = param_find(shunt_param);
if (ph != PARAM_INVALID && param_get(ph, &fvalue) == PX4_OK) {
_rshunt = fvalue;
}
ph = param_find(config_param);
int32_t value = default_config;
_config = (uint16_t)value;
if (ph != PARAM_INVALID && param_get(ph, &value) == PX4_OK) {
_config = (uint16_t)value;
}
_mode_triggered = ((_config & INA_COMMON_MODE_MASK) >> INA_COMMON_MODE_SHIFTS) <=
((INA_COMMON_MODE_SHUNT_BUS_TRIG & INA_COMMON_MODE_MASK) >>
INA_COMMON_MODE_SHIFTS);
_current_lsb = _max_current / INA_COMMON_DN_MAX;
_power_lsb = 25 * _current_lsb;
}
int INACommon::read(uint8_t address, int16_t &data)
{
uint16_t received_bytes;
int ret = PX4_ERROR;
for (size_t i = 0; i < 3; i++) {
ret = _transfer_func(_transfer_context, &address, 1, (uint8_t *)&received_bytes, sizeof(received_bytes));
if (ret == PX4_OK) {
data = ina_common_swap16(received_bytes);
break;
} else {
perf_count(_comms_errors);
PX4_DEBUG("i2c::transfer returned %d", ret);
}
}
return ret;
}
int INACommon::write(uint8_t address, uint16_t value)
{
uint8_t data[3] = {address, ((uint8_t)((value & 0xff00) >> 8)), (uint8_t)(value & 0xff)};
return _transfer_func(_transfer_context, data, sizeof(data), nullptr, 0);
}
int INACommon::init()
{
write(INA_COMMON_REG_CONFIGURATION, INA_COMMON_RST);
_cal = INA_COMMON_CONST / (_current_lsb * _rshunt);
if (write(INA_COMMON_REG_CALIBRATION, _cal) < 0) {
return -3;
}
int ret;
if (!_mode_triggered) {
ret = write(INA_COMMON_REG_CONFIGURATION, _config);
} else {
ret = PX4_OK;
}
_sensor_ok = true;
_initialized = ret == PX4_OK;
return ret;
}
int INACommon::measure()
{
int ret = PX4_OK;
if (_mode_triggered) {
ret = write(INA_COMMON_REG_CONFIGURATION, _config);
if (ret < 0) {
perf_count(_comms_errors);
PX4_DEBUG("i2c::transfer returned %d", ret);
}
}
return ret;
}
int INACommon::collect()
{
perf_begin(_sample_perf);
// Note: If the power module is connected backwards, then the values of _current will be negative but otherwise valid.
bool success{true};
success = success && (read(INA_COMMON_REG_BUSVOLTAGE, _bus_voltage) == PX4_OK);
success = success && (read(INA_COMMON_REG_CURRENT, _current) == PX4_OK);
if (setConnected(success)) {
_battery.updateVoltage(static_cast<float>(_bus_voltage * INA_COMMON_VSCALE));
_battery.updateCurrent(static_cast<float>(_current * _current_lsb));
}
_battery.updateAndPublishBatteryStatus(hrt_absolute_time());
perf_end(_sample_perf);
if (success) {
return PX4_OK;
} else {
return PX4_ERROR;
}
}
bool INACommon::setConnected(bool state)
{
if (state) {
_connected = INA_COMMON_SAMPLE_FREQUENCY_HZ * 2;
} else if (_connected > 0) {
_connected--;
}
if (_connected > 0) {
_battery.setConnected(true);
} else {
_battery.setConnected(false);
_battery.updateVoltage(0);
_battery.updateCurrent(0);
}
return state;
}
src/lib/drivers/ina_common/ina_common.h
+201
View File
@@ -0,0 +1,201 @@
/****************************************************************************
*
* Copyright (c) 2019-2026 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file ina_common.h
*
* Shared register definitions and utility class for INA226/INA228/INA231
* power monitor drivers. These devices share the same register map.
*/
#pragma once
#include <px4_platform_common/px4_config.h>
#include <lib/perf/perf_counter.h>
#include <battery/battery.h>
#include <drivers/drv_hrt.h>
using namespace time_literals;
/* INA Common Register Addresses */
#define INA_COMMON_REG_CONFIGURATION (0x00)
#define INA_COMMON_REG_SHUNTVOLTAGE (0x01)
#define INA_COMMON_REG_BUSVOLTAGE (0x02)
#define INA_COMMON_REG_POWER (0x03)
#define INA_COMMON_REG_CURRENT (0x04)
#define INA_COMMON_REG_CALIBRATION (0x05)
#define INA_COMMON_REG_MASKENABLE (0x06)
#define INA_COMMON_REG_ALERTLIMIT (0x07)
/* INA Common Configuration Register */
#define INA_COMMON_MODE_SHIFTS (0)
#define INA_COMMON_MODE_MASK (7 << INA_COMMON_MODE_SHIFTS)
#define INA_COMMON_MODE_SHUTDOWN (0 << INA_COMMON_MODE_SHIFTS)
#define INA_COMMON_MODE_SHUNT_TRIG (1 << INA_COMMON_MODE_SHIFTS)
#define INA_COMMON_MODE_BUS_TRIG (2 << INA_COMMON_MODE_SHIFTS)
#define INA_COMMON_MODE_SHUNT_BUS_TRIG (3 << INA_COMMON_MODE_SHIFTS)
#define INA_COMMON_MODE_ADC_OFF (4 << INA_COMMON_MODE_SHIFTS)
#define INA_COMMON_MODE_SHUNT_CONT (5 << INA_COMMON_MODE_SHIFTS)
#define INA_COMMON_MODE_BUS_CONT (6 << INA_COMMON_MODE_SHIFTS)
#define INA_COMMON_MODE_SHUNT_BUS_CONT (7 << INA_COMMON_MODE_SHIFTS)
#define INA_COMMON_VSHCT_SHIFTS (3)
#define INA_COMMON_VSHCT_MASK (7 << INA_COMMON_VSHCT_SHIFTS)
#define INA_COMMON_VSHCT_140US (0 << INA_COMMON_VSHCT_SHIFTS)
#define INA_COMMON_VSHCT_204US (1 << INA_COMMON_VSHCT_SHIFTS)
#define INA_COMMON_VSHCT_332US (2 << INA_COMMON_VSHCT_SHIFTS)
#define INA_COMMON_VSHCT_588US (3 << INA_COMMON_VSHCT_SHIFTS)
#define INA_COMMON_VSHCT_1100US (4 << INA_COMMON_VSHCT_SHIFTS)
#define INA_COMMON_VSHCT_2116US (5 << INA_COMMON_VSHCT_SHIFTS)
#define INA_COMMON_VSHCT_4156US (6 << INA_COMMON_VSHCT_SHIFTS)
#define INA_COMMON_VSHCT_8244US (7 << INA_COMMON_VSHCT_SHIFTS)
#define INA_COMMON_VBUSCT_SHIFTS (6)
#define INA_COMMON_VBUSCT_MASK (7 << INA_COMMON_VBUSCT_SHIFTS)
#define INA_COMMON_VBUSCT_140US (0 << INA_COMMON_VBUSCT_SHIFTS)
#define INA_COMMON_VBUSCT_204US (1 << INA_COMMON_VBUSCT_SHIFTS)
#define INA_COMMON_VBUSCT_332US (2 << INA_COMMON_VBUSCT_SHIFTS)
#define INA_COMMON_VBUSCT_588US (3 << INA_COMMON_VBUSCT_SHIFTS)
#define INA_COMMON_VBUSCT_1100US (4 << INA_COMMON_VBUSCT_SHIFTS)
#define INA_COMMON_VBUSCT_2116US (5 << INA_COMMON_VBUSCT_SHIFTS)
#define INA_COMMON_VBUSCT_4156US (6 << INA_COMMON_VBUSCT_SHIFTS)
#define INA_COMMON_VBUSCT_8244US (7 << INA_COMMON_VBUSCT_SHIFTS)
#define INA_COMMON_AVERAGES_SHIFTS (9)
#define INA_COMMON_AVERAGES_MASK (7 << INA_COMMON_AVERAGES_SHIFTS)
#define INA_COMMON_AVERAGES_1 (0 << INA_COMMON_AVERAGES_SHIFTS)
#define INA_COMMON_AVERAGES_4 (1 << INA_COMMON_AVERAGES_SHIFTS)
#define INA_COMMON_AVERAGES_16 (2 << INA_COMMON_AVERAGES_SHIFTS)
#define INA_COMMON_AVERAGES_64 (3 << INA_COMMON_AVERAGES_SHIFTS)
#define INA_COMMON_AVERAGES_128 (4 << INA_COMMON_AVERAGES_SHIFTS)
#define INA_COMMON_AVERAGES_256 (5 << INA_COMMON_AVERAGES_SHIFTS)
#define INA_COMMON_AVERAGES_512 (6 << INA_COMMON_AVERAGES_SHIFTS)
#define INA_COMMON_AVERAGES_1024 (7 << INA_COMMON_AVERAGES_SHIFTS)
#define INA_COMMON_RST (1 << 15)
/* INA Common Enable / Mask Register */
#define INA_COMMON_LEN (1 << 0)
#define INA_COMMON_APOL (1 << 1)
#define INA_COMMON_OVF (1 << 2)
#define INA_COMMON_CVRF (1 << 3)
#define INA_COMMON_AFF (1 << 4)
#define INA_COMMON_CNVR (1 << 10)
#define INA_COMMON_POL (1 << 11)
#define INA_COMMON_BUL (1 << 12)
#define INA_COMMON_BOL (1 << 13)
#define INA_COMMON_SUL (1 << 14)
#define INA_COMMON_SOL (1 << 15)
/* Shared constants */
#define INA_COMMON_SAMPLE_FREQUENCY_HZ 10
#define INA_COMMON_SAMPLE_INTERVAL_US (1_s / INA_COMMON_SAMPLE_FREQUENCY_HZ)
#define INA_COMMON_CONVERSION_INTERVAL (INA_COMMON_SAMPLE_INTERVAL_US - 7)
#define INA_COMMON_INIT_RETRY_INTERVAL_US 500000
#define INA_COMMON_DN_MAX 32768.0f /* 2^15 */
#define INA_COMMON_CONST 0.00512f /* internal fixed value for scaling */
#define INA_COMMON_VSCALE 0.00125f /* LSB of voltage is 1.25 mV */
#define ina_common_swap16(w) __builtin_bswap16((w))
class INACommon
{
public:
typedef int (*transfer_func_t)(void *context, const uint8_t *send, unsigned send_len,
uint8_t *recv, unsigned recv_len);
INACommon(transfer_func_t transfer_func, void *transfer_context, Battery &battery,
perf_counter_t sample_perf, perf_counter_t comms_errors);
/**
* Load device parameters (max current, shunt resistance, config register)
* from the PX4 parameter system and compute current/power LSBs.
*/
void loadParams(const char *current_param, const char *shunt_param, const char *config_param,
float default_max_current, float default_shunt, uint16_t default_config);
/**
* Read a 16-bit register via I2C with retry.
*/
int read(uint8_t address, int16_t &data);
/**
* Write a 16-bit register via I2C.
*/
int write(uint8_t address, uint16_t data);
/**
* Initialize the INA device: reset, write calibration, optionally write config.
* Call this after I2C::init() succeeds.
*
* @return PX4_OK on success.
*/
int init();
/**
* Write config register when in triggered mode.
*/
int measure();
/**
* Read bus voltage and current from the device, update battery status.
*/
int collect();
/**
* Manage the connected state with debounce filtering.
*/
bool setConnected(bool state);
bool _mode_triggered{false};
bool _sensor_ok{false};
bool _initialized{false};
float _current_lsb{0};
float _power_lsb{0};
float _max_current{0};
float _rshunt{0};
uint16_t _config{0};
int16_t _cal{0};
private:
transfer_func_t _transfer_func;
void *_transfer_context;
Battery &_battery;
perf_counter_t _sample_perf;
perf_counter_t _comms_errors;
int16_t _bus_voltage{0};
int16_t _current{0};
uint8_t _connected{0};
};