diff --git a/src/drivers/power_monitor/ina238/CMakeLists.txt b/src/drivers/power_monitor/ina238/CMakeLists.txt index 7b5545dcc0..d983f29d5d 100644 --- a/src/drivers/power_monitor/ina238/CMakeLists.txt +++ b/src/drivers/power_monitor/ina238/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2021 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 @@ -33,10 +33,7 @@ px4_add_module( MODULE drivers__ina238 MAIN ina238 - COMPILE_FLAGS - -Wno-cast-align # TODO: fix and enable SRCS - ina238_main.cpp ina238.cpp MODULE_CONFIG ina238_params.yaml diff --git a/src/drivers/power_monitor/ina238/ina238.cpp b/src/drivers/power_monitor/ina238/ina238.cpp index f42ae87764..fa95ab6085 100644 --- a/src/drivers/power_monitor/ina238/ina238.cpp +++ b/src/drivers/power_monitor/ina238/ina238.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2021 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,354 +31,387 @@ * ****************************************************************************/ -/** - * Driver for the I2C attached INA238 - */ - #include "ina238.h" +#include +#include -INA238::INA238(const I2CSPIDriverConfig &config, int battery_index) : - I2C(config), - ModuleParams(nullptr), - I2CSPIDriver(config), - _sample_perf(perf_alloc(PC_ELAPSED, "ina238_read")), - _comms_errors(perf_alloc(PC_COUNT, "ina238_com_err")), - _collection_errors(perf_alloc(PC_COUNT, "ina238_collection_err")), - _battery(battery_index, this, INA238_SAMPLE_INTERVAL_US, battery_status_s::SOURCE_POWER_MODULE) +using namespace ina238; + +INA238::INA238(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, "ina238_read")), + _comms_errors(perf_alloc(PC_COUNT, "ina238_com_err")), + _collection_errors(perf_alloc(PC_COUNT, "ina238_collection_err")), + _bad_register_perf(perf_alloc(PC_COUNT, "ina238_bad_register")), + _reinit_perf(perf_alloc(PC_COUNT, "ina238_reinit")) { - float fvalue = DEFAULT_MAX_CURRENT; - _max_current = fvalue; - param_t ph = param_find("INA238_CURRENT"); + float max_current = _param_ina238_current.get(); + float shunt_resistance = _param_ina238_shunt.get(); - if (ph != PARAM_INVALID && param_get(ph, &fvalue) == PX4_OK) { - _max_current = fvalue; - } + // Pick the ADC range so the device doesn't clip at the configured max current. + // Datasheet §8.2.2.1: R_SHUNT * I_MAX < V_SENSE_MAX. + const float v_sense_max = shunt_resistance * max_current; + const bool use_low_range = (v_sense_max <= ADCRANGE_LOW_V_SENSE); + _config_value = use_low_range ? RANGE_LOW : RANGE_HIGH; - fvalue = DEFAULT_SHUNT; - _rshunt = fvalue; - ph = param_find("INA238_SHUNT"); + _current_lsb = max_current / 32768.f; // From datasheet: current_lsb = max_current / 2^15 + _shunt_calibration = static_cast(SHUNT_CAL_K * _current_lsb * shunt_resistance); - if (ph != PARAM_INVALID && param_get(ph, &fvalue) == PX4_OK) { - _rshunt = fvalue; - } - - // According to page 8.2.2.1, page 33/48 of the INA238 interface datasheet (Rev. A), - // the requirement is: R_SHUNT < V_SENSE_MAX / I_MAX - // therefore: R_SHUNT * I_MAX < V_SENSE_MAX - // and so if V_SENSE_MAX is bigger, we need to use the bigger ADC range to avoid - // the device from capping the measured current. - - const float v_sense_max = _rshunt * _max_current; - - if (v_sense_max > INA238_ADCRANGE_LOW_V_SENSE) { - _range = INA238_ADCRANGE_HIGH; - - } else { - _range = INA238_ADCRANGE_LOW; - } - - _current_lsb = _max_current / INA238_DN_MAX; - _shunt_calibration = static_cast(INA238_CONST * _current_lsb * _rshunt); - - if (_range == INA238_ADCRANGE_LOW) { + if (use_low_range) { _shunt_calibration *= 4; } - _register_cfg[0].set_bits = (uint16_t)(_range); - _register_cfg[2].set_bits = _shunt_calibration; - _register_cfg[2].clear_bits = ~_shunt_calibration; + // Continuous conversion, 540us conversion per channel x 3 channels x 64-sample average = 103.7 ms per output sample = ~10Hz + _adc_config_value = MODE_TEMP_SHUNT_BUS_CONT | VBUSCT_540US | VSHCT_540US | VTCT_540US | 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; } INA238::~INA238() { - /* free perf counters */ perf_free(_sample_perf); perf_free(_comms_errors); perf_free(_collection_errors); -} - -int INA238::read(uint8_t address, uint16_t &data) -{ - // read desired little-endian value via I2C - uint16_t received_bytes; - const int ret = transfer(&address, 1, (uint8_t *)&received_bytes, sizeof(received_bytes)); - - if (ret == PX4_OK) { - data = swap16(received_bytes); - - } else { - perf_count(_comms_errors); - PX4_DEBUG("i2c::transfer returned %d", ret); - } - - return ret; -} - -int INA238::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); + perf_free(_bad_register_perf); + perf_free(_reinit_perf); } int INA238::init() { - int ret = PX4_ERROR; - - /* do I2C init (and probe) first */ if (I2C::init() != PX4_OK) { - return ret; - } - - ret = Reset(); - - if (ret) { - return ret; - } - - start(); - - return 0; -} - -int INA238::force_init() -{ - int ret = init(); - - start(); - - return ret; -} - -int INA238::probe() -{ - uint16_t value{0}; - - if (RegisterRead(Register::MANUFACTURER_ID, value) != PX4_OK || value != INA238_MFG_ID_TI) { - PX4_DEBUG("probe mfgid %d", value); - return -1; - } - - if (RegisterRead(Register::DEVICE_ID, value) != PX4_OK || ( - INA238_DEVICEID(value) != INA238_MFG_DIE - )) { - PX4_DEBUG("probe die id %d", value); - return -1; + return PX4_ERROR; } + _state = State::RESET; return PX4_OK; } -int INA238::Reset() +void INA238::RunImpl() { - int ret = PX4_ERROR; + const hrt_abstime start_time = hrt_absolute_time(); - if (RegisterWrite(Register::CONFIG, (uint16_t)(ADC_RESET_BIT)) != PX4_OK) { - return ret; + switch (_state) { + case State::UNINITIALIZED: { + if (init() != PX4_OK) { + _battery.updateAndPublishBatteryStatus(start_time); + ScheduleDelayed(INIT_RETRY_INTERVAL_US); + return; + } + + // init() advanced us to State::RESET + ScheduleNow(); + return; + } + + case State::RESET: { + _battery.setConnected(false); + _battery.updateVoltage(0.f); + _battery.updateCurrent(-1.f); + _battery.updateTemperature(NAN); + _battery.updateAndPublishBatteryStatus(start_time); + + if (registerWrite(Register::CONFIG, RST) != PX4_OK) { + ScheduleDelayed(INIT_RETRY_INTERVAL_US); + return; + } + + _state = State::CONFIGURE; + ScheduleDelayed(RESET_DELAY_US); + return; + } + + case State::CONFIGURE: { + const bool ok = (probe() == PX4_OK) && + (registerWrite(Register::SHUNT_CAL, _shunt_calibration) == PX4_OK) && + (registerWrite(Register::CONFIG, _config_value) == PX4_OK) && + (registerWrite(Register::ADCCONFIG, _adc_config_value) == PX4_OK); + + if (!ok) { + _state = State::RESET; + ScheduleDelayed(INIT_RETRY_INTERVAL_US); + return; + } + + // Communication success + _consecutive_failures = 0; + _next_reg_to_check = 0; + _state = State::MEASURE; + + // Wait one full sample period + some margin + ScheduleDelayed(SAMPLE_INTERVAL_US + 5_ms); + return; + } + + case State::MEASURE: { + if (collect() == PX4_OK) { + _consecutive_failures = 0; + + } else { + perf_count(_collection_errors); + + if (++_consecutive_failures >= MAX_CONSECUTIVE_FAILURES) { + perf_count(_reinit_perf); + _state = State::RESET; + _consecutive_failures = 0; + PX4_WARN("consecutive failures, resetting"); + ScheduleNow(); + return; + } + } + + const hrt_abstime elapsed = hrt_elapsed_time(&start_time); + const hrt_abstime scheduled_time = elapsed < SAMPLE_INTERVAL_US ? SAMPLE_INTERVAL_US - elapsed : 0; + + ScheduleDelayed(scheduled_time); + return; + } } - - if (RegisterWrite(Register::SHUNT_CAL, uint16_t(_shunt_calibration)) < 0) { - return -3; - } - - // Set the CONFIG for max I - if (RegisterWrite(Register::CONFIG, (uint16_t) _range) != PX4_OK) { - return ret; - } - - // Start ADC continous mode here - ret = write((uint16_t)_register_cfg[1].reg, (uint16_t)_register_cfg[1].set_bits); - - return ret; -} - -bool INA238::RegisterCheck(const register_config_t ®_cfg) -{ - bool success = true; - - uint16_t reg_value = 0; - RegisterRead(reg_cfg.reg, reg_value); - - if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) { - PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits); - success = false; - } - - if (reg_cfg.clear_bits && ((reg_value & reg_cfg.clear_bits) != 0)) { - PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits); - success = false; - } - - return success; -} - -int INA238::RegisterWrite(Register reg, uint16_t value) -{ - return write((uint8_t)reg, value); -} -int INA238::RegisterRead(Register reg, uint16_t &value) -{ - return read((uint8_t)reg, value); } int INA238::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); + // Verify one config register per cycle + bool config_ok = checkConfigurationRotating(); - updateParams(); + if (!config_ok) { + perf_count(_bad_register_perf); + perf_end(_sample_perf); + return PX4_ERROR; } - // read from the sensor - // Note: If the power module is connected backwards, then the values of _current will be negative but otherwise valid. - bool success{true}; - int16_t bus_voltage{0}; - int16_t current{0}; - int16_t temperature{0}; + int16_t bus_voltage = 0; + int16_t current = 0; + int16_t temperature = 0; - success = (RegisterRead(Register::VS_BUS, (uint16_t &)bus_voltage) == PX4_OK); - success = success && (RegisterRead(Register::CURRENT, (uint16_t &)current) == PX4_OK); - success = success && (RegisterRead(Register::DIETEMP, (uint16_t &)temperature) == PX4_OK); - - if (setConnected(success)) { - _battery.updateVoltage(static_cast(bus_voltage * INA238_VSCALE)); - _battery.updateCurrent(static_cast(current * _current_lsb)); - _battery.updateTemperature(static_cast(temperature * INA238_TSCALE)); + const bool reads_ok = (registerRead(Register::VS_BUS, (uint16_t &)bus_voltage) == PX4_OK) + && (registerRead(Register::CURRENT, (uint16_t &)current) == PX4_OK) + && (registerRead(Register::DIETEMP, (uint16_t &)temperature) == PX4_OK); + if (reads_ok) { + _battery.setConnected(true); + _battery.updateVoltage(static_cast(bus_voltage) * V_LSB); + _battery.updateCurrent(static_cast(current) * _current_lsb); + _battery.updateTemperature(static_cast(temperature) * T_LSB); _battery.updateAndPublishBatteryStatus(hrt_absolute_time()); } - if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) { - // check configuration registers periodically or immediately following any failure - if (RegisterCheck(_register_cfg[_checked_register])) { - _last_config_check_timestamp = hrt_absolute_time(); - _checked_register = (_checked_register + 1) % size_register_cfg; - - } else { - // register check failed, force reset - PX4_DEBUG("register check failed"); - perf_count(_bad_register_perf); - success = false; - - setConnected(false); - _battery.updateAndPublishBatteryStatus(hrt_absolute_time()); - } - } - perf_end(_sample_perf); + return reads_ok ? PX4_OK : PX4_ERROR; +} - if (success) { - return PX4_OK; - - } else { - PX4_DEBUG("error reading from sensor"); +int INA238::probe() +{ + uint16_t value = 0; + if (registerRead(Register::MANUFACTURER_ID, value) != PX4_OK || value != MANFID) { + PX4_DEBUG("probe mfgid %d", value); return PX4_ERROR; } + + if (registerRead(Register::DEVICE_ID, value) != PX4_OK || deviceId(value) != DIEID) { + PX4_DEBUG("probe die id %d", value); + return PX4_ERROR; + } + + return PX4_OK; } -void INA238::start() +bool INA238::checkConfigurationRotating() { - ScheduleClear(); + const struct { + Register reg; + uint16_t expected; + } checks[] = { + { Register::CONFIG, _config_value }, + { Register::ADCCONFIG, _adc_config_value }, + { Register::SHUNT_CAL, _shunt_calibration }, + }; - /* reset the report ring and state machine */ - _collect_phase = false; + const auto &check = checks[_next_reg_to_check]; + uint16_t actual = 0; - _measure_interval = INA238_CONVERSION_INTERVAL; + if (registerRead(check.reg, actual) != PX4_OK) { + return false; + } - /* schedule a cycle to start things */ - ScheduleDelayed(5); + if (actual != check.expected) { + return false; + } + + _next_reg_to_check = (_next_reg_to_check + 1) % (sizeof(checks) / sizeof(checks[0])); + return true; } -void INA238::RunImpl() +int INA238::registerRead(Register reg, uint16_t &value) { - if (_initialized) { - if (_collect_phase) { - /* perform collection */ - if (collect() != PX4_OK) { - perf_count(_collection_errors); - /* if error restart the measurement state machine */ - ScheduleClear(); - _initialized = false; - ScheduleNow(); - return; - } + uint8_t address = static_cast(reg); + uint16_t raw = 0; - /* next phase is measurement */ - _collect_phase = true; + const int ret = transfer(&address, 1, (uint8_t *)&raw, sizeof(raw)); - if (_measure_interval > INA238_CONVERSION_INTERVAL) { - /* schedule a fresh cycle call when we are ready to measure again */ - ScheduleDelayed(_measure_interval - INA238_CONVERSION_INTERVAL); - return; - } - } - - /* next phase is collection */ - _collect_phase = true; - - /* schedule a fresh cycle call when the measurement is done */ - ScheduleDelayed(INA238_CONVERSION_INTERVAL); + if (ret == PX4_OK) { + value = __builtin_bswap16(raw); } else { - setConnected(false); - _battery.updateAndPublishBatteryStatus(hrt_absolute_time()); - - if (init() != PX4_OK) { - ScheduleDelayed(INA238_INIT_RETRY_INTERVAL_US); - - } else { - _initialized = true; - start(); - } + perf_count(_comms_errors); } + + return ret; } -bool INA238::setConnected(bool state) +int INA238::registerWrite(Register reg, uint16_t value) { - // Filter out brief I2C failures for 2s - if (state) { - _connected = INA238_SAMPLE_FREQUENCY_HZ * 2; + const uint8_t buf[3] = { + static_cast(reg), + static_cast((value >> 8) & 0xff), + static_cast(value & 0xff), + }; - } else if (_connected > 0) { - _connected--; + const int ret = transfer(buf, sizeof(buf), nullptr, 0); + + if (ret != PX4_OK) { + perf_count(_comms_errors); } - if (_connected > 0) { - _battery.setConnected(true); - - } else { - _battery.setConnected(false); - _battery.updateVoltage(0); - _battery.updateCurrent(0); - _battery.updateTemperature(0); - } - - return state; + return ret; } void INA238::print_status() { I2CSPIDriverBase::print_status(); - if (_initialized) { - perf_print_counter(_sample_perf); - perf_print_counter(_comms_errors); + const char *state_str = "?"; - printf("poll interval: %u \n", _measure_interval); + switch (_state) { + case State::UNINITIALIZED: + state_str = "UNINITIALIZED"; + break; + + case State::RESET: + state_str = "RESET"; + break; + + case State::CONFIGURE: + state_str = "CONFIGURE"; + break; + + case State::MEASURE: + state_str = "MEASURE"; + break; + } + + 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); +} + +I2CSPIDriverBase *INA238::instantiate(const I2CSPIDriverConfig &config, int /*runtime_instance*/) +{ + INA238 *instance = new INA238(config, config.custom1); + + if (instance == nullptr) { + PX4_ERR("alloc failed"); + return nullptr; + } + + if (instance->init() == PX4_OK) { + instance->ScheduleNow(); + + } else if (config.keep_running) { + // Driver stays alive even if the device isn't powered yet; RunImpl will retry. + PX4_INFO("ina238 init failed on bus %d, will retry every %u ms.", config.bus, static_cast(INIT_RETRY_INTERVAL_US / 1000)); + instance->ScheduleDelayed(INIT_RETRY_INTERVAL_US); } else { - PX4_INFO("Device not initialized. Retrying every %d ms until battery is plugged in.", - INA238_INIT_RETRY_INTERVAL_US / 1000); + delete instance; + return nullptr; } + + return instance; +} + +void INA238::print_usage() +{ + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Driver for the Texas Instruments INA237 / INA238 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("ina238", "driver"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); + PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x45); + 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 ina238_main(int argc, char *argv[]) +{ + using ThisDriver = INA238; + BusCLIArguments cli{true, false}; + cli.i2c_address = 0x45; + 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; + } + + break; + } + } + + const char *verb = cli.optArg(); + + if (!verb) { + ThisDriver::print_usage(); + return -1; + } + + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_POWER_DEVTYPE_INA238); + + 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/ina238/ina238.h b/src/drivers/power_monitor/ina238/ina238.h index 62750c44c4..935e5d6933 100644 --- a/src/drivers/power_monitor/ina238/ina238.h +++ b/src/drivers/power_monitor/ina238/ina238.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2021 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,154 +31,144 @@ * ****************************************************************************/ - #pragma once - -#include -#include #include -#include -#include #include -#include -#include +#include +#include #include -#include "ina238_registers.hpp" +#include +#include using namespace time_literals; -using namespace ina238; -/* Configuration Constants */ -#define INA238_BASEADDR 0x45 /* 7-bit address. 8-bit address is 0x45 */ -// If initialization is forced (with the -f flag on the command line), but it fails, the drive will try again to -// connect to the INA238 every this many microseconds -#define INA238_INIT_RETRY_INTERVAL_US 500000 +namespace ina238 +{ +static constexpr uint32_t BUS_CLOCK_HZ = 100'000; -#define INA238_MFG_ID_TI (0x5449) // TI -#define INA238_MFG_DIE (0x238) // INA237, INA238 +static constexpr uint16_t MANFID = 0x5449; +static constexpr uint16_t DIEID = 0x238; -#define INA238_ADCRANGE_SHIFTS (4) -#define INA238_ADCRANGE_LOW (1 << INA238_ADCRANGE_SHIFTS) // ± 40.96 mV -#define INA238_ADCRANGE_HIGH (0 << INA238_ADCRANGE_SHIFTS) // ±163.84 mV +// Measurement scaling (from datasheet) +static constexpr float V_LSB = 3.125e-3f; // V per LSB +static constexpr float T_LSB = 7.8125e-3f; // °C per LSB +static constexpr float SHUNT_CAL_K = 819.2e6f; // shunt-cal scaling constant +static constexpr float ADCRANGE_LOW_V_SENSE = 0.04096f; // ±40.96 mV -#define INA238_DEVICE_ID_SHIFTS (4) -#define INA238_DEVICE_ID_MASK (0xfff << INA238_DEVICE_ID_SHIFTS) -#define INA238_DEVICEID(v) (((v) & INA238_DEVICE_ID_MASK) >> INA238_DEVICE_ID_SHIFTS) +// Sample timing +// ADC produces one averaged sample every 540us x 3 channels x 64 avg = 103.68 ms. +// Poll a hair slower than that so we always read a fresh sample rather than aliasing. +static constexpr hrt_abstime SAMPLE_INTERVAL_US = 105_ms; -#define INA238_SAMPLE_FREQUENCY_HZ 10 -#define INA238_SAMPLE_INTERVAL_US (1_s / INA238_SAMPLE_FREQUENCY_HZ) -#define INA238_CONVERSION_INTERVAL (INA238_SAMPLE_INTERVAL_US - 7) -#define INA238_DN_MAX 32768.0f /* 2^15 */ -#define INA238_CONST 819.2e6f /* is an internal fixed value used to ensure scaling is maintained properly */ -#define INA238_VSCALE 3.125e-03f /* LSB of voltage is 3.1255 mV/LSB */ -#define INA238_TSCALE 7.8125e-03f /* LSB of temperature is 7.8125 mDegC/LSB */ +// Recovery / robustness timing +static constexpr hrt_abstime INIT_RETRY_INTERVAL_US = 500_ms; +static constexpr hrt_abstime RESET_DELAY_US = 1_ms; // datasheet specifies 300us. Give some margin +static constexpr hrt_abstime DISCONNECT_DEBOUNCE_US = 2_s; +static constexpr uint8_t MAX_CONSECUTIVE_FAILURES = DISCONNECT_DEBOUNCE_US / SAMPLE_INTERVAL_US; -#define INA238_ADCRANGE_LOW_V_SENSE 0.04096f // ± 40.96 mV +// Register map (subset used by this driver) +enum class Register : uint8_t { + CONFIG = 0x00, + ADCCONFIG = 0x01, + SHUNT_CAL = 0x02, + VS_BUS = 0x05, + DIETEMP = 0x06, + CURRENT = 0x07, + MANUFACTURER_ID = 0x3e, + DEVICE_ID = 0x3f, +}; -#define DEFAULT_MAX_CURRENT 327.68f /* Amps */ -#define DEFAULT_SHUNT 0.0003f /* Shunt is 300 uOhm */ +// CONFIG register bits +enum CONFIG_BIT : uint16_t { + RST = (1u << 15), + RANGE_HIGH = (0u << 4), // ±163.84 mV — used when R_SHUNT * I_MAX > 40.96 mV + RANGE_LOW = (1u << 4), // ±40.96 mV +}; + +// DEVICE_ID register field accessor +static constexpr uint16_t DEVICE_ID_MASK = 0xfff0u; +static inline constexpr uint16_t deviceId(uint16_t v) { return (v & DEVICE_ID_MASK) >> 4; } + +// ADCCONFIG register bits +enum ADCCONFIG_BIT : uint16_t { + AVERAGES_1 = (0u << 0), + AVERAGES_4 = (1u << 0), + AVERAGES_16 = (2u << 0), + AVERAGES_64 = (3u << 0), + AVERAGES_128 = (4u << 0), + AVERAGES_256 = (5u << 0), + AVERAGES_512 = (6u << 0), + AVERAGES_1024 = (7u << 0), + + VTCT_540US = (4u << 3), + VSHCT_540US = (4u << 6), + VBUSCT_540US = (4u << 9), + + MODE_TEMP_SHUNT_BUS_CONT = (15u << 12), +}; + +} // namespace ina238 -#define swap16(w) __builtin_bswap16((w)) -#define swap32(d) __builtin_bswap32((d)) -#define swap64(q) __builtin_bswap64((q)) class INA238 : public device::I2C, public ModuleParams, public I2CSPIDriver { public: INA238(const I2CSPIDriverConfig &config, int battery_index); - virtual ~INA238(); + ~INA238() override; static I2CSPIDriverBase *instantiate(const I2CSPIDriverConfig &config, int runtime_instance); static void print_usage(); + int init() override; void RunImpl(); - int init() override; - - /** - * Tries to call the init() function. If it fails, then it will schedule to retry again in - * INA238_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; protected: int probe() override; private: - // Sensor Configuration - struct register_config_t { - Register reg; - uint16_t set_bits{0}; - uint16_t clear_bits{0}; + 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 SHUNT_CAL / CONFIG / ADCCONFIG, then transition to MEASURE + MEASURE, // steady-state: read VS_BUS / CURRENT / DIETEMP, publish, repeat }; - bool RegisterCheck(const register_config_t ®_cfg); - int RegisterWrite(Register reg, uint16_t value); - int RegisterRead(Register reg, uint16_t &value); - int Reset(); - - unsigned int _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 _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")}; - - // Configuration state, computed from params - float _max_current; - float _rshunt; - float _current_lsb; - int16_t _range; - uint16_t _shunt_calibration{0}; - - - hrt_abstime _last_config_check_timestamp{0}; - uint8_t _checked_register{0}; - static constexpr uint8_t size_register_cfg{3}; - register_config_t _register_cfg[size_register_cfg] { - // Register | Set bits, Clear bits - { Register::CONFIG, 0, 0}, // will be set dynamically - { Register::ADCCONFIG, MODE_TEMP_SHUNT_BUS_CONT | VBUSCT_540US | VSHCT_540US | VTCT_540US | AVERAGES_64}, - { Register::SHUNT_CAL, 0, 0} // will be set dynamically - }; - - Battery _battery; - uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - uint8_t _connected{0}; - // returns state unchanged - bool setConnected(bool state); - - int read(uint8_t address, uint16_t &data); - int write(uint8_t address, uint16_t data); - - int read(uint8_t address, int16_t &data) - { - return read(address, (uint16_t &)data); - } - - int write(uint8_t address, int16_t data) - { - return write(address, (uint16_t)data); - } - - /** - * 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 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(ina238::Register reg, uint16_t &value); + int registerWrite(ina238::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 _shunt_calibration{0}; + uint16_t _config_value{0}; // CONFIG register value we wrote + uint16_t _adc_config_value{0}; // ADCCONFIG 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_ina238_current, + (ParamFloat) _param_ina238_shunt + ); }; diff --git a/src/drivers/power_monitor/ina238/ina238_main.cpp b/src/drivers/power_monitor/ina238/ina238_main.cpp deleted file mode 100644 index 608c033130..0000000000 --- a/src/drivers/power_monitor/ina238/ina238_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 "ina238.h" - -I2CSPIDriverBase *INA238::instantiate(const I2CSPIDriverConfig &config, int runtime_instance) -{ - INA238 *instance = new INA238(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 INA238 on bus %d, but will try again periodically.", config.bus); - } - - } else if (instance->init() != PX4_OK) { - delete instance; - return nullptr; - } - - return instance; -} - -void -INA238::print_usage() -{ - PRINT_MODULE_DESCRIPTION( - R"DESCR_STR( -### Description -Driver for the INA238 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 0x45, and one can run on Bus 2, address 0x45. - -If the INA238 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("ina238", "driver"); - - PRINT_MODULE_USAGE_COMMAND("start"); - PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); - PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x45); - 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 -ina238_main(int argc, char *argv[]) -{ - int ch; - using ThisDriver = INA238; - BusCLIArguments cli{true, false}; - cli.i2c_address = INA238_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_INA238); - - 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/ina238/ina238_registers.hpp b/src/drivers/power_monitor/ina238/ina238_registers.hpp deleted file mode 100644 index 8f170a7cb6..0000000000 --- a/src/drivers/power_monitor/ina238/ina238_registers.hpp +++ /dev/null @@ -1,91 +0,0 @@ -// -// Created by roman on 10/18/23. -// - -#ifndef PX4_SRC_DRIVERS_POWER_MONITOR_INA238_INA238_REGISTERS_HPP_ -#define PX4_SRC_DRIVERS_POWER_MONITOR_INA238_INA238_REGISTERS_HPP_ - - -namespace ina238 -{ - -enum class Register : uint8_t { - CONFIG = 0x00, // Configuration Register - ADCCONFIG = 0x01, // ADC Configuration Register - SHUNT_CAL = 0x02, // Shunt Calibration Register - VS_BUS = 0x05, - DIETEMP = 0x06, - CURRENT = 0x07, - MANUFACTURER_ID = 0x3e, - DEVICE_ID = 0x3f -}; - -enum CONFIG_BIT : uint16_t { - RANGE_BIT = (1 << 4), - ADC_RESET_BIT = (1 << 15) -}; - -enum ADCCONFIG_BIT : uint16_t { - AVERAGES_1 = (0 << 0), - AVERAGES_4 = (1 << 0), - AVERAGES_16 = (2 << 0), - AVERAGES_64 = (3 << 0), - AVERAGES_128 = (4 << 0), - AVERAGES_256 = (5 << 0), - AVERAGES_512 = (6 << 0), - AVERAGES_1024 = (7 << 0), - - VTCT_50US = (0 << 3), - VTCT_84US = (1 << 3), - VTCT_150US = (2 << 3), - VTCT_280US = (3 << 3), - VTCT_540US = (4 << 3), - VTCT_1052US = (5 << 3), - VTCT_2074US = (6 << 3), - VTCT_4170US = (7 << 3), - - - VSHCT_MASK = (7 << 6), - VSHCT_50US = (0 << 6), - VSHCT_84US = (1 << 6), - VSHCT_150US = (2 << 6), - VSHCT_280US = (3 << 6), - VSHCT_540US = (4 << 6), - VSHCT_1052US = (5 << 6), - VSHCT_2074US = (6 << 6), - VSHCT_4170US = (7 << 6), - - - VBUSCT_MASK = (7 << 9), - VBUSCT_50US = (0 << 9), - VBUSCT_84US = (1 << 9), - VBUSCT_150US = (2 << 9), - VBUSCT_280US = (3 << 9), - VBUSCT_540US = (4 << 9), - VBUSCT_1052US = (5 << 9), - VBUSCT_2074US = (6 << 9), - VBUSCT_4170US = (7 << 9), - - - MODE_SHUTDOWN_TRIG = (0 << 12), - MODE_BUS_TRIG = (1 << 12), - MODE_SHUNT_TRIG = (2 << 12), - MODE_SHUNT_BUS_TRIG = (3 << 12), - MODE_TEMP_TRIG = (4 << 12), - MODE_TEMP_BUS_TRIG = (5 << 12), - MODE_TEMP_SHUNT_TRIG = (6 << 12), - MODE_TEMP_SHUNT_BUS_TRIG = (7 << 12), - - MODE_SHUTDOWN_CONT = (8 << 12), - MODE_BUS_CONT = (9 << 12), - MODE_SHUNT_CONT = (10 << 12), - MODE_SHUNT_BUS_CONT = (11 << 12), - MODE_TEMP_CONT = (12 << 12), - MODE_TEMP_BUS_CONT = (13 << 12), - MODE_TEMP_SHUNT_CONT = (14 << 12), - MODE_TEMP_SHUNT_BUS_CONT = (15 << 12) - -}; - -} -#endif //PX4_SRC_DRIVERS_POWER_MONITOR_INA238_INA238_REGISTERS_HPP_