diff --git a/src/drivers/auterion_autostarter/AuterionAutostarter.cpp b/src/drivers/auterion_autostarter/AuterionAutostarter.cpp index 7b229498916a..f3a09079f32f 100644 --- a/src/drivers/auterion_autostarter/AuterionAutostarter.cpp +++ b/src/drivers/auterion_autostarter/AuterionAutostarter.cpp @@ -173,19 +173,19 @@ int AuterionAutostarter::ina226_probe(const uint32_t instance) const msgv[1].buffer = rxdata; msgv[1].length = sizeof(rxdata); - txdata[0] = {INA226_MFG_ID}; + txdata[0] = static_cast(ina226::Register::MFG_ID); ret = I2C_TRANSFER(i2c.get(), msgv, 2); uint16_t value = static_cast(rxdata[1] | rxdata[0] << 8); - if (ret != PX4_OK || value != INA226_MFG_ID_TI) { + if (ret != PX4_OK || value != ina226::MANFID) { ret = PX4_ERROR; } else { - txdata[0] = {INA226_MFG_DIEID}; + txdata[0] = static_cast(ina226::Register::DIE_ID); ret = I2C_TRANSFER(i2c.get(), msgv, 2); value = static_cast(rxdata[1] | rxdata[0] << 8); - if (ret != PX4_OK || value != INA226_MFG_DIE) { + if (ret != PX4_OK || value != ina226::DIEID) { ret = PX4_ERROR; } } diff --git a/src/drivers/power_monitor/ina226/CMakeLists.txt b/src/drivers/power_monitor/ina226/CMakeLists.txt index 26fd043379e2..744404fcbd22 100644 --- a/src/drivers/power_monitor/ina226/CMakeLists.txt +++ b/src/drivers/power_monitor/ina226/CMakeLists.txt @@ -33,10 +33,7 @@ px4_add_module( MODULE drivers__ina226 MAIN ina226 - COMPILE_FLAGS - -Wno-cast-align # TODO: fix and enable SRCS - ina226_main.cpp ina226.cpp MODULE_CONFIG ina226_params.yaml diff --git a/src/drivers/power_monitor/ina226/ina226.cpp b/src/drivers/power_monitor/ina226/ina226.cpp index 60fb33369900..8fa4a1dd8a00 100644 --- a/src/drivers/power_monitor/ina226/ina226.cpp +++ b/src/drivers/power_monitor/ina226/ina226.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019-2020 PX4 Development Team. All rights reserved. + * 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 @@ -31,314 +31,372 @@ * ****************************************************************************/ -/** - * @file ina226.cpp - * @author David Sidrane - * - * Driver for the I2C attached INA226 - */ - #include "ina226.h" - -INA226::INA226(const I2CSPIDriverConfig &config, int battery_index) : - I2C(config), - ModuleParams(nullptr), - I2CSPIDriver(config), - _sample_perf(perf_alloc(PC_ELAPSED, "ina226_read")), - _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) +#include +#include + +using namespace ina226; + +INA226::INA226(const I2CSPIDriverConfig &config, int battery_index) + : I2C(config), + ModuleParams(nullptr), + I2CSPIDriver(config), + _battery(battery_index, this, SAMPLE_INTERVAL_US, battery_status_s::SOURCE_POWER_MODULE), + _sample_perf(perf_alloc(PC_ELAPSED, "ina226_read")), + _comms_errors(perf_alloc(PC_COUNT, "ina226_com_err")), + _collection_errors(perf_alloc(PC_COUNT, "ina226_collection_err")), + _bad_register_perf(perf_alloc(PC_COUNT, "ina226_bad_register")), + _reinit_perf(perf_alloc(PC_COUNT, "ina226_reinit")) { - 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; - } + const float max_current = _param_ina226_current.get(); + const float shunt_resistance = _param_ina226_shunt.get(); - _mode_triggered = ((_config & INA226_MODE_MASK) >> INA226_MODE_SHIFTS) <= - ((INA226_MODE_SHUNT_BUS_TRIG & INA226_MODE_MASK) >> - INA226_MODE_SHIFTS); + _current_lsb = max_current / 32768.f; // From datasheet: current_lsb = max_current / 2^15 + _calibration = static_cast(CAL_K / (_current_lsb * shunt_resistance)); - _current_lsb = _max_current / DN_MAX; - _power_lsb = 25 * _current_lsb; + _config_value = MODE_SHUNT_BUS_CONT | VSHCT_588US | VBUSCT_588US | AVERAGES_64; - // We need to publish immediately, to guarantee that the first instance of the driver publishes to uORB instance 0 - setConnected(false); + // Publish an initial disconnected status so the first instance grabs uORB instance 0 immediately. _battery.updateAndPublishBatteryStatus(hrt_absolute_time()); + // Let the lower I2C layer absorb transient bus errors before we see them. I2C::_retries = 5; } INA226::~INA226() { - /* free perf counters */ perf_free(_sample_perf); perf_free(_comms_errors); perf_free(_collection_errors); - perf_free(_measure_errors); + perf_free(_bad_register_perf); + perf_free(_reinit_perf); } -int INA226::read(uint8_t address, int16_t &data) +int INA226::init() { - // read desired little-endian value via I2C - uint16_t received_bytes; - int ret = PX4_ERROR; + if (I2C::init() != PX4_OK) { + return PX4_ERROR; + } - for (size_t i = 0; i < 3; i++) { - ret = transfer(&address, 1, (uint8_t *)&received_bytes, sizeof(received_bytes)); + _state = State::RESET; + return PX4_OK; +} - if (ret == PX4_OK) { - data = swap16(received_bytes); - break; +void INA226::RunImpl() +{ + const hrt_abstime start_time = hrt_absolute_time(); + + switch (_state) { + case State::UNINITIALIZED: { + if (init() != PX4_OK) { + _battery.updateAndPublishBatteryStatus(start_time); + ScheduleDelayed(INIT_RETRY_INTERVAL_US); + return; + } - } else { - perf_count(_comms_errors); - PX4_DEBUG("i2c::transfer returned %d", ret); + // init() advanced us to State::RESET + ScheduleNow(); + return; } - } - return ret; -} + case State::RESET: { + _battery.setConnected(false); + _battery.updateVoltage(0.f); + _battery.updateCurrent(-1.f); + _battery.updateAndPublishBatteryStatus(start_time); -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); -} + if (registerWrite(Register::CONFIGURATION, RST) != PX4_OK) { + ScheduleDelayed(INIT_RETRY_INTERVAL_US); + return; + } -int -INA226::init() -{ - int ret = PX4_ERROR; + _state = State::CONFIGURE; + ScheduleDelayed(RESET_DELAY_US); + return; + } - /* do I2C init (and probe) first */ - if (I2C::init() != PX4_OK) { - return ret; - } + case State::CONFIGURE: { + const bool ok = (probe() == PX4_OK) && + (registerWrite(Register::CALIBRATION, _calibration) == PX4_OK) && + (registerWrite(Register::CONFIGURATION, _config_value) == PX4_OK); - write(INA226_REG_CONFIGURATION, INA226_RST); + if (!ok) { + _state = State::RESET; + ScheduleDelayed(INIT_RETRY_INTERVAL_US); + return; + } - _cal = INA226_CONST / (_current_lsb * _rshunt); + // Communication success + _consecutive_failures = 0; + _next_reg_to_check = 0; + _state = State::MEASURE; - if (write(INA226_REG_CALIBRATION, _cal) < 0) { - return -3; - } + // Wait one full sample period + some margin + ScheduleDelayed(SAMPLE_INTERVAL_US + 5_ms); + return; + } - // If we run in continuous mode then start it here + case State::MEASURE: { + if (collect() == PX4_OK) { + _consecutive_failures = 0; - if (!_mode_triggered) { - ret = write(INA226_REG_CONFIGURATION, _config); + } else { + perf_count(_collection_errors); - } else { - ret = PX4_OK; - } + if (++_consecutive_failures >= MAX_CONSECUTIVE_FAILURES) { + perf_count(_reinit_perf); + _state = State::RESET; + _consecutive_failures = 0; + PX4_WARN("consecutive failures, resetting"); + ScheduleNow(); + return; + } + } - start(); - _sensor_ok = true; + const hrt_abstime elapsed = hrt_elapsed_time(&start_time); + const hrt_abstime scheduled_time = elapsed < SAMPLE_INTERVAL_US ? SAMPLE_INTERVAL_US - elapsed : 0; - _initialized = ret == PX4_OK; - return ret; + ScheduleDelayed(scheduled_time); + return; + } + } } -int -INA226::force_init() +int INA226::collect() { - int ret = init(); + perf_begin(_sample_perf); - start(); + // Verify one config register per cycle + const bool config_ok = checkConfigurationRotating(); - return ret; + if (!config_ok) { + perf_count(_bad_register_perf); + perf_end(_sample_perf); + return PX4_ERROR; + } + + int16_t bus_voltage = 0; + int16_t current = 0; + + const bool reads_ok = (registerRead(Register::BUSVOLTAGE, (uint16_t &)bus_voltage) == PX4_OK) + && (registerRead(Register::CURRENT, (uint16_t &)current) == PX4_OK); + + if (reads_ok) { + _battery.setConnected(true); + _battery.updateVoltage(static_cast(bus_voltage) * V_LSB); + _battery.updateCurrent(static_cast(current) * _current_lsb); + _battery.updateAndPublishBatteryStatus(hrt_absolute_time()); + } + + perf_end(_sample_perf); + return reads_ok ? PX4_OK : PX4_ERROR; } -int -INA226::probe() +int INA226::probe() { - int16_t value{0}; + uint16_t value = 0; - if (read(INA226_MFG_ID, value) != PX4_OK || value != INA226_MFG_ID_TI) { + if (registerRead(Register::MFG_ID, value) != PX4_OK || value != MANFID) { PX4_DEBUG("probe mfgid %d", value); - return -1; + return PX4_ERROR; } - if (read(INA226_MFG_DIEID, value) != PX4_OK || value != INA226_MFG_DIE) { + if (registerRead(Register::DIE_ID, value) != PX4_OK || value != DIEID) { PX4_DEBUG("probe die id %d", value); - return -1; + return PX4_ERROR; } return PX4_OK; } -int -INA226::measure() +bool INA226::checkConfigurationRotating() { - int ret = PX4_OK; - - if (_mode_triggered) { - ret = write(INA226_REG_CONFIGURATION, _config); + const struct { + Register reg; + uint16_t expected; + uint16_t mask; + } checks[] = { + // CONFIGURATION D15 (RST) self-clears; D14:D12 are reserved and D14 reads back as 1. + { Register::CONFIGURATION, _config_value, 0x0FFF }, + { Register::CALIBRATION, _calibration, 0xFFFF }, + }; + + const auto &check = checks[_next_reg_to_check]; + uint16_t actual = 0; + + if (registerRead(check.reg, actual) != PX4_OK) { + return false; + } - if (ret < 0) { - perf_count(_comms_errors); - PX4_DEBUG("i2c::transfer returned %d", ret); - } + if ((actual & check.mask) != (check.expected & check.mask)) { + return false; } - return ret; + _next_reg_to_check = (_next_reg_to_check + 1) % (sizeof(checks) / sizeof(checks[0])); + return true; } -int -INA226::collect() +int INA226::registerRead(Register reg, uint16_t &value) { - perf_begin(_sample_perf); + uint8_t address = static_cast(reg); + uint16_t raw = 0; - if (_parameter_update_sub.updated()) { - // Read from topic to clear updated flag - parameter_update_s parameter_update; - _parameter_update_sub.copy(¶meter_update); + const int ret = transfer(&address, 1, (uint8_t *)&raw, sizeof(raw)); - updateParams(); - } + if (ret == PX4_OK) { + value = __builtin_bswap16(raw); - // 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)); + } else { + perf_count(_comms_errors); } - _battery.updateAndPublishBatteryStatus(hrt_absolute_time()); + return ret; +} - perf_end(_sample_perf); +int INA226::registerWrite(Register reg, uint16_t value) +{ + const uint8_t buf[3] = { + static_cast(reg), + static_cast((value >> 8) & 0xff), + static_cast(value & 0xff), + }; - if (success) { - return PX4_OK; + const int ret = transfer(buf, sizeof(buf), nullptr, 0); - } else { - return PX4_ERROR; + if (ret != PX4_OK) { + perf_count(_comms_errors); } -} + return ret; +} -void -INA226::start() +void INA226::print_status() { - ScheduleClear(); + I2CSPIDriverBase::print_status(); - /* reset the report ring and state machine */ - _collect_phase = false; + const char *state_str = "?"; - _measure_interval = INA226_CONVERSION_INTERVAL; + switch (_state) { + case State::UNINITIALIZED: + state_str = "UNINITIALIZED"; + break; - /* schedule a cycle to start things */ - ScheduleDelayed(5); -} + case State::RESET: + state_str = "RESET"; + break; -void -INA226::RunImpl() -{ - if (_initialized) { - if (_collect_phase) { - /* perform collection */ - if (collect() != PX4_OK) { - perf_count(_collection_errors); - /* if error restart the measurement state machine */ - start(); - return; - } + case State::CONFIGURE: + state_str = "CONFIGURE"; + break; - /* next phase is measurement */ - _collect_phase = !_mode_triggered; + case State::MEASURE: + state_str = "MEASURE"; + break; + } - if (_measure_interval > INA226_CONVERSION_INTERVAL) { - /* schedule a fresh cycle call when we are ready to measure again */ - ScheduleDelayed(_measure_interval - INA226_CONVERSION_INTERVAL); - return; - } - } + PX4_INFO("state: %s", state_str); + PX4_INFO("sample interval: %llu us", SAMPLE_INTERVAL_US); + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); + perf_print_counter(_collection_errors); + perf_print_counter(_bad_register_perf); + perf_print_counter(_reinit_perf); +} - /* Measurement phase */ +I2CSPIDriverBase *INA226::instantiate(const I2CSPIDriverConfig &config, int /*runtime_instance*/) +{ + INA226 *instance = new INA226(config, config.custom1); - /* Perform measurement */ - if (measure() != PX4_OK) { - perf_count(_measure_errors); - } + if (instance == nullptr) { + PX4_ERR("alloc failed"); + return nullptr; + } - /* next phase is collection */ - _collect_phase = true; + if (instance->init() == PX4_OK) { + instance->ScheduleNow(); - /* schedule a fresh cycle call when the measurement is done */ - ScheduleDelayed(INA226_CONVERSION_INTERVAL); + } else if (config.keep_running) { + // Driver stays alive even if the device isn't powered yet; RunImpl will retry. + PX4_INFO("ina226 init failed on bus %d, will retry every %llu ms.", config.bus, INIT_RETRY_INTERVAL_US / 1000); + instance->ScheduleDelayed(INIT_RETRY_INTERVAL_US); } else { - setConnected(false); - _battery.updateAndPublishBatteryStatus(hrt_absolute_time()); - - if (init() != PX4_OK) { - ScheduleDelayed(INA226_INIT_RETRY_INTERVAL_US); - } + delete instance; + return nullptr; } + + return instance; } -bool INA226::setConnected(bool state) +void INA226::print_usage() { - // Filter out brief I2C failures for 2s - if (state) { - _connected = INA226_SAMPLE_FREQUENCY_HZ * 2; + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Driver for the Texas Instruments INA226 power monitor. + +Multiple instances can run simultaneously on separate buses or different I2C addresses. + +If the device is not powered at startup, pass `-k` (keep_running) and the driver +will retry initialization every 500 ms so the battery can be plugged in later. +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("ina226", "driver"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); + PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x41); + PRINT_MODULE_USAGE_PARAMS_I2C_KEEP_RUNNING_FLAG(); + PRINT_MODULE_USAGE_PARAM_INT('t', 1, 1, 3, "battery index for calibration values (1-3)", true); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); +} + +extern "C" int ina226_main(int argc, char *argv[]) +{ + using ThisDriver = INA226; + BusCLIArguments cli{true, false}; + cli.i2c_address = 0x41; + cli.default_i2c_frequency = BUS_CLOCK_HZ; + cli.support_keep_running = true; + cli.custom1 = 1; + + int ch; + + while ((ch = cli.getOpt(argc, argv, "t:")) != EOF) { + switch (ch) { + case 't': + cli.custom1 = static_cast(strtol(cli.optArg(), nullptr, 0)); + + if (cli.custom1 < 1 || cli.custom1 > 3) { + PX4_ERR("index must be 1-3"); + return -1; + } - } else if (_connected > 0) { - _connected--; + break; + } } - if (_connected > 0) { - _battery.setConnected(true); + const char *verb = cli.optArg(); - } else { - _battery.setConnected(false); - _battery.updateVoltage(0); - _battery.updateCurrent(0); + if (!verb) { + ThisDriver::print_usage(); + return -1; } - return state; -} + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_POWER_DEVTYPE_INA226); -void -INA226::print_status() -{ - I2CSPIDriverBase::print_status(); - - if (_initialized) { - perf_print_counter(_sample_perf); - perf_print_counter(_comms_errors); + if (!strcmp(verb, "start")) { + return ThisDriver::module_start(cli, iterator); + } - printf("poll interval: %u \n", _measure_interval); + if (!strcmp(verb, "stop")) { + return ThisDriver::module_stop(iterator); + } - } else { - PX4_INFO("Device not initialized. Retrying every %d ms until battery is plugged in.", - INA226_INIT_RETRY_INTERVAL_US / 1000); + if (!strcmp(verb, "status")) { + return ThisDriver::module_status(iterator); } + + ThisDriver::print_usage(); + return -1; } diff --git a/src/drivers/power_monitor/ina226/ina226.h b/src/drivers/power_monitor/ina226/ina226.h index a69695a1ccef..afe27d2dfeb0 100644 --- a/src/drivers/power_monitor/ina226/ina226.h +++ b/src/drivers/power_monitor/ina226/ina226.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2019 PX4 Development Team. All rights reserved. + * 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 @@ -31,194 +31,158 @@ * ****************************************************************************/ -/** - * @file ina226.h - * - */ - #pragma once - -#include -#include #include -#include -#include #include -#include -#include +#include +#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 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)) +namespace ina226 +{ + +static constexpr uint32_t BUS_CLOCK_HZ = 100'000; + +static constexpr uint16_t MANFID = 0x5449; // TI +static constexpr uint16_t DIEID = 0x2260; // INA226 die + revision 0 + +// Measurement scaling (from datasheet) +static constexpr float V_LSB = 1.25e-3f; // bus voltage: V per LSB +static constexpr float CAL_K = 5.12e-3f; // CALIBRATION = CAL_K / (current_lsb * R_SHUNT) + +// Sample timing +// Default ADC config produces one averaged sample every (588us + 588us) * 64 = 75.264 ms. Poll a hair slower. +static constexpr hrt_abstime SAMPLE_INTERVAL_US = 100_ms; + +// Recovery / robustness timing +static constexpr hrt_abstime INIT_RETRY_INTERVAL_US = 500_ms; +static constexpr hrt_abstime RESET_DELAY_US = 1_ms; +static constexpr hrt_abstime DISCONNECT_DEBOUNCE_US = 2_s; +static constexpr uint8_t MAX_CONSECUTIVE_FAILURES = DISCONNECT_DEBOUNCE_US / SAMPLE_INTERVAL_US; + +// Register map (subset used by this driver) +enum class Register : uint8_t { + CONFIGURATION = 0x00, + SHUNTVOLTAGE = 0x01, + BUSVOLTAGE = 0x02, + POWER = 0x03, + CURRENT = 0x04, + CALIBRATION = 0x05, + MASKENABLE = 0x06, + ALERTLIMIT = 0x07, + MFG_ID = 0xfe, + DIE_ID = 0xff, +}; + +// CONFIGURATION register bits +enum CONFIG_BIT : uint16_t { + RST = (1u << 15), + + // Averaging count (bits 9-11) + AVERAGES_1 = (0u << 9), + AVERAGES_4 = (1u << 9), + AVERAGES_16 = (2u << 9), + AVERAGES_64 = (3u << 9), + AVERAGES_128 = (4u << 9), + AVERAGES_256 = (5u << 9), + AVERAGES_512 = (6u << 9), + AVERAGES_1024 = (7u << 9), + + // Bus voltage conversion time (bits 6-8) + VBUSCT_140US = (0u << 6), + VBUSCT_204US = (1u << 6), + VBUSCT_332US = (2u << 6), + VBUSCT_588US = (3u << 6), + VBUSCT_1100US = (4u << 6), + VBUSCT_2116US = (5u << 6), + VBUSCT_4156US = (6u << 6), + VBUSCT_8244US = (7u << 6), + + // Shunt voltage conversion time (bits 3-5) + VSHCT_140US = (0u << 3), + VSHCT_204US = (1u << 3), + VSHCT_332US = (2u << 3), + VSHCT_588US = (3u << 3), + VSHCT_1100US = (4u << 3), + VSHCT_2116US = (5u << 3), + VSHCT_4156US = (6u << 3), + VSHCT_8244US = (7u << 3), + + // Mode (bits 0-2) + MODE_SHUTDOWN = (0u << 0), + MODE_SHUNT_TRIG = (1u << 0), + MODE_BUS_TRIG = (2u << 0), + MODE_SHUNT_BUS_TRIG = (3u << 0), + MODE_ADC_OFF = (4u << 0), + MODE_SHUNT_CONT = (5u << 0), + MODE_BUS_CONT = (6u << 0), + MODE_SHUNT_BUS_CONT = (7u << 0), +}; + +} // namespace ina226 + class INA226 : public device::I2C, public ModuleParams, public I2CSPIDriver { public: INA226(const I2CSPIDriverConfig &config, int battery_index); - virtual ~INA226(); + ~INA226() override; static I2CSPIDriverBase *instantiate(const I2CSPIDriverConfig &config, int runtime_instance); static void print_usage(); - void RunImpl(); - - int init() override; + int init() override; + void RunImpl(); - /** - * 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. - * - * @return PX4_OK if initialization succeeded on the first try. Negative value otherwise. - */ - int force_init(); - - /** - * Diagnostics - print some basic information about the driver. - */ - void print_status() override; + void print_status() override; protected: - int probe() override; + 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}; - - 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); - - uint8_t _connected{0}; - // returns state unchanged - bool setConnected(bool state); - - /** - * 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(); - + enum class State : uint8_t { + UNINITIALIZED, // I2C::init() not yet called successfully — retry until it does + RESET, // soft-reset the device, then transition to CONFIGURE + CONFIGURE, // write CALIBRATION / CONFIGURATION, then transition to MEASURE + MEASURE, // steady-state: read BUSVOLTAGE / CURRENT, publish, repeat + }; + + int collect(); + + // Rotates through the configuration registers one per call. Returns false + // if a read fails or the value doesn't match what we wrote. + bool checkConfigurationRotating(); + + int registerRead(ina226::Register reg, uint16_t &value); + int registerWrite(ina226::Register reg, uint16_t value); + + // --- State ------------------------------------------------------------- + Battery _battery; + + State _state{State::UNINITIALIZED}; + uint8_t _consecutive_failures{0}; + + uint8_t _next_reg_to_check{0}; + + // Configuration computed from params + float _current_lsb{0.f}; + uint16_t _calibration{0}; + uint16_t _config_value{0}; // CONFIGURATION register value we wrote + + // Perf counters + perf_counter_t _sample_perf; + perf_counter_t _comms_errors; + perf_counter_t _collection_errors; + perf_counter_t _bad_register_perf; + perf_counter_t _reinit_perf; + + DEFINE_PARAMETERS( + (ParamFloat) _param_ina226_current, + (ParamFloat) _param_ina226_shunt + ); }; diff --git a/src/drivers/power_monitor/ina226/ina226_main.cpp b/src/drivers/power_monitor/ina226/ina226_main.cpp deleted file mode 100644 index a02095e3fb22..000000000000 --- a/src/drivers/power_monitor/ina226/ina226_main.cpp +++ /dev/null @@ -1,130 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2021 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. - * - ****************************************************************************/ -#include -#include - -#include "ina226.h" - -I2CSPIDriverBase *INA226::instantiate(const I2CSPIDriverConfig &config, int runtime_instance) -{ - INA226 *instance = new INA226(config, config.custom1); - - if (instance == nullptr) { - PX4_ERR("alloc failed"); - return nullptr; - } - - if (config.keep_running) { - if (instance->force_init() != PX4_OK) { - PX4_INFO("Failed to init INA226 on bus %d, but will try again periodically.", config.bus); - } - - } else if (instance->init() != PX4_OK) { - delete instance; - return nullptr; - } - - return instance; -} - -void -INA226::print_usage() -{ - PRINT_MODULE_DESCRIPTION( - R"DESCR_STR( -### Description -Driver for the INA226 power monitor. - -Multiple instances of this driver can run simultaneously, if each instance has a separate bus OR I2C address. - -For example, one instance can run on Bus 2, address 0x41, and one can run on Bus 2, address 0x43. - -If the INA226 module is not powered, then by default, initialization of the driver will fail. To change this, use -the -f flag. If this flag is set, then if initialization fails, the driver will keep trying to initialize again -every 0.5 seconds. With this flag set, you can plug in a battery after the driver starts, and it will work. Without -this flag set, the battery must be plugged in before starting the driver. - -)DESCR_STR"); - - PRINT_MODULE_USAGE_NAME("ina226", "driver"); - - PRINT_MODULE_USAGE_COMMAND("start"); - PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); - PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x41); - PRINT_MODULE_USAGE_PARAMS_I2C_KEEP_RUNNING_FLAG(); - PRINT_MODULE_USAGE_PARAM_INT('t', 1, 1, 3, "battery index for calibration values (1 or 3)", true); - PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); -} - -extern "C" int -ina226_main(int argc, char *argv[]) -{ - int ch; - using ThisDriver = INA226; - BusCLIArguments cli{true, false}; - cli.i2c_address = INA226_BASEADDR; - cli.default_i2c_frequency = 100000; - cli.support_keep_running = true; - cli.custom1 = 1; - - while ((ch = cli.getOpt(argc, argv, "t:")) != EOF) { - switch (ch) { - case 't': // battery index - cli.custom1 = (int)strtol(cli.optArg(), NULL, 0); - break; - } - } - - const char *verb = cli.optArg(); - if (!verb) { - ThisDriver::print_usage(); - return -1; - } - - BusInstanceIterator iterator(MODULE_NAME, cli, DRV_POWER_DEVTYPE_INA226); - - if (!strcmp(verb, "start")) { - return ThisDriver::module_start(cli, iterator); - } - - if (!strcmp(verb, "stop")) { - return ThisDriver::module_stop(iterator); - } - - if (!strcmp(verb, "status")) { - return ThisDriver::module_status(iterator); - } - - ThisDriver::print_usage(); - return -1; -} diff --git a/src/drivers/power_monitor/ina226/ina226_params.yaml b/src/drivers/power_monitor/ina226/ina226_params.yaml index 77807ccd6f38..0f579e8e2399 100644 --- a/src/drivers/power_monitor/ina226/ina226_params.yaml +++ b/src/drivers/power_monitor/ina226/ina226_params.yaml @@ -9,16 +9,6 @@ parameters: type: boolean default: 0 reboot_required: true - INA226_CONFIG: - description: - short: INA226 Power Monitor Config - type: int32 - default: 18139 - min: 0 - max: 65535 - decimal: 1 - increment: 1 - reboot_required: true INA226_CURRENT: description: short: INA226 Power Monitor Max Current