diff --git a/src/drivers/power_monitor/ina226/CMakeLists.txt b/src/drivers/power_monitor/ina226/CMakeLists.txt index 26fd043379..1a95c80c8a 100644 --- a/src/drivers/power_monitor/ina226/CMakeLists.txt +++ b/src/drivers/power_monitor/ina226/CMakeLists.txt @@ -42,5 +42,6 @@ px4_add_module( ina226_params.yaml DEPENDS battery + ina_common px4_work_queue ) diff --git a/src/drivers/power_monitor/ina226/ina226.cpp b/src/drivers/power_monitor/ina226/ina226.cpp index 60fb333699..896a695de1 100644 --- a/src/drivers/power_monitor/ina226/ina226.cpp +++ b/src/drivers/power_monitor/ina226/ina226.cpp @@ -36,6 +36,9 @@ * @author David Sidrane * * 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" @@ -49,41 +52,14 @@ INA226::INA226(const I2CSPIDriverConfig &config, int battery_index) : _comms_errors(perf_alloc(PC_COUNT, "ina226_com_err")), _collection_errors(perf_alloc(PC_COUNT, "ina226_collection_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; - _max_current = fvalue; - 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; + _common.loadParams("INA226_CURRENT", "INA226_SHUNT", "INA226_CONFIG", + INA226_MAX_CURRENT, INA226_SHUNT, INA226_DEFAULT_CONFIG); // 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()); I2C::_retries = 5; @@ -91,41 +67,12 @@ INA226::INA226(const I2CSPIDriverConfig &config, int battery_index) : INA226::~INA226() { - /* free perf counters */ perf_free(_sample_perf); perf_free(_comms_errors); perf_free(_collection_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 INA226::init() { @@ -136,27 +83,10 @@ INA226::init() return ret; } - write(INA226_REG_CONFIGURATION, INA226_RST); - - _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; - } + ret = _common.init(); start(); - _sensor_ok = true; - _initialized = ret == PX4_OK; return ret; } @@ -175,12 +105,12 @@ INA226::probe() { 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); 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); return -1; } @@ -188,150 +118,67 @@ INA226::probe() 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(¶meter_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(_bus_voltage * INA226_VSCALE)); - _battery.updateCurrent(static_cast(_current * _current_lsb)); - } - - _battery.updateAndPublishBatteryStatus(hrt_absolute_time()); - - perf_end(_sample_perf); - - if (success) { - return PX4_OK; - - } else { - return PX4_ERROR; - } -} - - void INA226::start() { ScheduleClear(); - /* reset the report ring and state machine */ _collect_phase = false; - _measure_interval = INA226_CONVERSION_INTERVAL; + _measure_interval = INA_COMMON_CONVERSION_INTERVAL; - /* schedule a cycle to start things */ ScheduleDelayed(5); } void INA226::RunImpl() { - if (_initialized) { + if (_common._initialized) { if (_collect_phase) { - /* perform collection */ - if (collect() != PX4_OK) { + if (_parameter_update_sub.updated()) { + parameter_update_s parameter_update; + _parameter_update_sub.copy(¶meter_update); + updateParams(); + } + + if (_common.collect() != PX4_OK) { perf_count(_collection_errors); - /* if error restart the measurement state machine */ start(); return; } - /* next phase is measurement */ - _collect_phase = !_mode_triggered; + _collect_phase = !_common._mode_triggered; - if (_measure_interval > INA226_CONVERSION_INTERVAL) { - /* schedule a fresh cycle call when we are ready to measure again */ - ScheduleDelayed(_measure_interval - INA226_CONVERSION_INTERVAL); + if (_measure_interval > INA_COMMON_CONVERSION_INTERVAL) { + ScheduleDelayed(_measure_interval - INA_COMMON_CONVERSION_INTERVAL); return; } } - /* Measurement phase */ - - /* Perform measurement */ - if (measure() != PX4_OK) { + if (_common.measure() != PX4_OK) { perf_count(_measure_errors); } - /* next phase is collection */ _collect_phase = true; - /* schedule a fresh cycle call when the measurement is done */ - ScheduleDelayed(INA226_CONVERSION_INTERVAL); + ScheduleDelayed(INA_COMMON_CONVERSION_INTERVAL); } else { - setConnected(false); + _common.setConnected(false); _battery.updateAndPublishBatteryStatus(hrt_absolute_time()); 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 INA226::print_status() { I2CSPIDriverBase::print_status(); - if (_initialized) { + if (_common._initialized) { perf_print_counter(_sample_perf); perf_print_counter(_comms_errors); @@ -339,6 +186,6 @@ INA226::print_status() } else { 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); } } diff --git a/src/drivers/power_monitor/ina226/ina226.h b/src/drivers/power_monitor/ina226/ina226.h index a69695a1cc..812b935d69 100644 --- a/src/drivers/power_monitor/ina226/ina226.h +++ b/src/drivers/power_monitor/ina226/ina226.h @@ -38,7 +38,6 @@ #pragma once - #include #include #include @@ -48,104 +47,21 @@ #include #include #include +#include using namespace time_literals; -/* Configuration Constants */ -#define INA226_BASEADDR 0x41 /* 7-bit address. 8-bit address is 0x41 */ -// If initialization is forced (with the -f flag on the command line), but it fails, the drive will try again to -// connect to the INA226 every this many microseconds -#define INA226_INIT_RETRY_INTERVAL_US 500000 +/* INA226-specific constants */ +#define INA226_BASEADDR 0x41 /* 7-bit address */ +#define INA226_MAX_CURRENT 164.0f /* 164 Amps */ +#define INA226_SHUNT 0.0005f /* Shunt is 500 uOhm */ +#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 */ -#define INA226_REG_CONFIGURATION (0x00) -#define INA226_REG_SHUNTVOLTAGE (0x01) -#define INA226_REG_BUSVOLTAGE (0x02) -#define INA226_REG_POWER (0x03) -#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)) +/* INA226-specific probe registers */ +#define INA226_MFG_ID (0xfe) +#define INA226_MFG_DIEID (0xff) +#define INA226_MFG_ID_TI (0x5449) // TI +#define INA226_MFG_DIE (0x2260) // INA2260 class INA226 : public device::I2C, public ModuleParams, public I2CSPIDriver { @@ -162,7 +78,7 @@ public: /** * 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. */ @@ -177,48 +93,28 @@ protected: int probe() override; private: - bool _sensor_ok{false}; - unsigned _measure_interval{0}; - bool _collect_phase{false}; - bool _initialized{false}; - perf_counter_t _sample_perf; perf_counter_t _comms_errors; perf_counter_t _collection_errors; perf_counter_t _measure_errors; - int16_t _bus_voltage{0}; - int16_t _power{0}; - 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}; + unsigned _measure_interval{0}; + bool _collect_phase{false}; Battery _battery; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - int read(uint8_t address, int16_t &data); - int write(uint8_t address, uint16_t data); + INACommon _common; - uint8_t _connected{0}; - // returns state unchanged - bool setConnected(bool state); + static int i2c_transfer_wrapper(void *context, const uint8_t *send, unsigned send_len, + uint8_t *recv, unsigned recv_len) + { + return static_cast(context)->transfer(send, send_len, recv, recv_len); + } /** * 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(); - int measure(); - int collect(); - }; diff --git a/src/drivers/power_monitor/ina231/CMakeLists.txt b/src/drivers/power_monitor/ina231/CMakeLists.txt index 64b1c2a898..521824c0a6 100644 --- a/src/drivers/power_monitor/ina231/CMakeLists.txt +++ b/src/drivers/power_monitor/ina231/CMakeLists.txt @@ -42,5 +42,6 @@ px4_add_module( ina231_params.yaml DEPENDS battery + ina_common px4_work_queue ) diff --git a/src/drivers/power_monitor/ina231/ina231.cpp b/src/drivers/power_monitor/ina231/ina231.cpp index 420b86c569..789e9ab0e5 100644 --- a/src/drivers/power_monitor/ina231/ina231.cpp +++ b/src/drivers/power_monitor/ina231/ina231.cpp @@ -35,6 +35,9 @@ * @file ina231.cpp * * 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" @@ -48,41 +51,14 @@ INA231::INA231(const I2CSPIDriverConfig &config, int battery_index) : _comms_errors(perf_alloc(PC_COUNT, "ina231_com_err")), _collection_errors(perf_alloc(PC_COUNT, "ina231_collection_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; - _max_current = fvalue; - 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; + _common.loadParams("INA231_CURRENT", "INA231_SHUNT", "INA231_CONFIG", + INA231_MAX_CURRENT, INA231_SHUNT, INA231_DEFAULT_CONFIG); // 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()); I2C::_retries = 5; @@ -90,41 +66,12 @@ INA231::INA231(const I2CSPIDriverConfig &config, int battery_index) : INA231::~INA231() { - /* free perf counters */ perf_free(_sample_perf); perf_free(_comms_errors); perf_free(_collection_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 INA231::init() { @@ -135,27 +82,10 @@ INA231::init() return ret; } - write(INA231_REG_CONFIGURATION, INA231_RST); - - _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; - } + ret = _common.init(); start(); - _sensor_ok = true; - _initialized = ret == PX4_OK; return ret; } @@ -174,7 +104,7 @@ INA231::probe() { 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"); return -1; } @@ -182,150 +112,67 @@ INA231::probe() 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(¶meter_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(_bus_voltage * INA231_VSCALE)); - _battery.updateCurrent(static_cast(_current * _current_lsb)); - } - - _battery.updateAndPublishBatteryStatus(hrt_absolute_time()); - - perf_end(_sample_perf); - - if (success) { - return PX4_OK; - - } else { - return PX4_ERROR; - } -} - - void INA231::start() { ScheduleClear(); - /* reset the report ring and state machine */ _collect_phase = false; - _measure_interval = INA231_CONVERSION_INTERVAL; + _measure_interval = INA_COMMON_CONVERSION_INTERVAL; - /* schedule a cycle to start things */ ScheduleDelayed(5); } void INA231::RunImpl() { - if (_initialized) { + if (_common._initialized) { if (_collect_phase) { - /* perform collection */ - if (collect() != PX4_OK) { + if (_parameter_update_sub.updated()) { + parameter_update_s parameter_update; + _parameter_update_sub.copy(¶meter_update); + updateParams(); + } + + if (_common.collect() != PX4_OK) { perf_count(_collection_errors); - /* if error restart the measurement state machine */ start(); return; } - /* next phase is measurement */ - _collect_phase = !_mode_triggered; + _collect_phase = !_common._mode_triggered; - if (_measure_interval > INA231_CONVERSION_INTERVAL) { - /* schedule a fresh cycle call when we are ready to measure again */ - ScheduleDelayed(_measure_interval - INA231_CONVERSION_INTERVAL); + if (_measure_interval > INA_COMMON_CONVERSION_INTERVAL) { + ScheduleDelayed(_measure_interval - INA_COMMON_CONVERSION_INTERVAL); return; } } - /* Measurement phase */ - - /* Perform measurement */ - if (measure() != PX4_OK) { + if (_common.measure() != PX4_OK) { perf_count(_measure_errors); } - /* next phase is collection */ _collect_phase = true; - /* schedule a fresh cycle call when the measurement is done */ - ScheduleDelayed(INA231_CONVERSION_INTERVAL); + ScheduleDelayed(INA_COMMON_CONVERSION_INTERVAL); } else { - setConnected(false); + _common.setConnected(false); _battery.updateAndPublishBatteryStatus(hrt_absolute_time()); 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 INA231::print_status() { I2CSPIDriverBase::print_status(); - if (_initialized) { + if (_common._initialized) { perf_print_counter(_sample_perf); perf_print_counter(_comms_errors); @@ -333,6 +180,6 @@ INA231::print_status() } else { 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); } } diff --git a/src/drivers/power_monitor/ina231/ina231.h b/src/drivers/power_monitor/ina231/ina231.h index f4a42abcda..8af4dd94a5 100644 --- a/src/drivers/power_monitor/ina231/ina231.h +++ b/src/drivers/power_monitor/ina231/ina231.h @@ -38,7 +38,6 @@ #pragma once - #include #include #include @@ -48,99 +47,15 @@ #include #include #include +#include using namespace time_literals; -/* Configuration Constants */ -#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 -// connect to the INA231 every this many microseconds -#define INA231_INIT_RETRY_INTERVAL_US 500000 - -/* 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)) +/* INA231-specific constants */ +#define INA231_BASEADDR 0x44 /* 7-bit address. 8-bit address is 0x88 */ +#define INA231_MAX_CURRENT 90.0f /* 90 Amps */ +#define INA231_SHUNT 0.0005f /* Shunt is 500 uOhm */ +#define INA231_DEFAULT_CONFIG (INA_COMMON_MODE_SHUNT_BUS_CONT | INA_COMMON_VSHCT_1100US | INA_COMMON_VBUSCT_1100US | INA_COMMON_AVERAGES_16) class INA231 : public device::I2C, public ModuleParams, public I2CSPIDriver { @@ -157,7 +72,7 @@ public: /** * 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. */ @@ -172,48 +87,28 @@ protected: int probe() override; private: - bool _sensor_ok{false}; - unsigned _measure_interval{0}; - bool _collect_phase{false}; - bool _initialized{false}; - perf_counter_t _sample_perf; perf_counter_t _comms_errors; perf_counter_t _collection_errors; perf_counter_t _measure_errors; - int16_t _bus_voltage{0}; - int16_t _power{0}; - 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}; + unsigned _measure_interval{0}; + bool _collect_phase{false}; Battery _battery; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - int read(uint8_t address, int16_t &data); - int write(uint8_t address, uint16_t data); + INACommon _common; - uint8_t _connected{0}; - // returns state unchanged - bool setConnected(bool state); + static int i2c_transfer_wrapper(void *context, const uint8_t *send, unsigned send_len, + uint8_t *recv, unsigned recv_len) + { + return static_cast(context)->transfer(send, send_len, recv, recv_len); + } /** * 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(); - int measure(); - int collect(); - }; diff --git a/src/lib/drivers/CMakeLists.txt b/src/lib/drivers/CMakeLists.txt index 051b40ab0c..59d050d1f2 100644 --- a/src/lib/drivers/CMakeLists.txt +++ b/src/lib/drivers/CMakeLists.txt @@ -34,6 +34,7 @@ add_subdirectory(accelerometer) add_subdirectory(device) add_subdirectory(gyroscope) +add_subdirectory(ina_common) add_subdirectory(led) add_subdirectory(magnetometer) add_subdirectory(rangefinder) diff --git a/src/lib/drivers/ina_common/CMakeLists.txt b/src/lib/drivers/ina_common/CMakeLists.txt new file mode 100644 index 0000000000..4112112f77 --- /dev/null +++ b/src/lib/drivers/ina_common/CMakeLists.txt @@ -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) diff --git a/src/lib/drivers/ina_common/ina_common.cpp b/src/lib/drivers/ina_common/ina_common.cpp new file mode 100644 index 0000000000..24cc68e389 --- /dev/null +++ b/src/lib/drivers/ina_common/ina_common.cpp @@ -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 +#include + +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(_bus_voltage * INA_COMMON_VSCALE)); + _battery.updateCurrent(static_cast(_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; +} diff --git a/src/lib/drivers/ina_common/ina_common.h b/src/lib/drivers/ina_common/ina_common.h new file mode 100644 index 0000000000..320f6bd54c --- /dev/null +++ b/src/lib/drivers/ina_common/ina_common.h @@ -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 +#include +#include +#include + +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}; +};