From 380841563f1b8054d2d0f3e95ef5de20adcd062d Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 30 Jan 2024 19:28:05 +0300 Subject: [PATCH] ina238: set shunt calibration to desired value if readback is incorrect (#22237) * refactor driver to dynamically check registers and do reset if register does not match desired value * have seen various times where shunt calibration was reset in air --------- Signed-off-by: RomanBapst --- src/drivers/power_monitor/ina238/ina238.cpp | 119 ++++++--- src/drivers/power_monitor/ina238/ina238.h | 239 ++---------------- .../power_monitor/ina238/ina238_registers.hpp | 90 +++++++ 3 files changed, 203 insertions(+), 245 deletions(-) create mode 100644 src/drivers/power_monitor/ina238/ina238_registers.hpp diff --git a/src/drivers/power_monitor/ina238/ina238.cpp b/src/drivers/power_monitor/ina238/ina238.cpp index d8afc1bd63..6d58e84d8f 100644 --- a/src/drivers/power_monitor/ina238/ina238.cpp +++ b/src/drivers/power_monitor/ina238/ina238.cpp @@ -66,6 +66,15 @@ INA238::INA238(const I2CSPIDriverConfig &config, int battery_index) : } _current_lsb = _max_current / INA238_DN_MAX; + _shunt_calibration = static_cast(INA238_CONST * _current_lsb * _rshunt); + + if (_range == INA238_ADCRANGE_LOW) { + _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; // We need to publish immediately, to guarantee that the first instance of the driver publishes to uORB instance 0 _battery.setConnected(false); @@ -114,33 +123,7 @@ int INA238::init() return ret; } - if (write(INA238_REG_CONFIG, (uint16_t)(INA238_RST_RESET | _range)) != PX4_OK) { - return ret; - } - - uint16_t shunt_calibration = static_cast(INA238_CONST * _current_lsb * _rshunt); - - if (_range == INA238_ADCRANGE_LOW) { - shunt_calibration *= 4; - } - - if (write(INA238_REG_SHUNTCAL, shunt_calibration) < 0) { - return -3; - } - - // Set the CONFIG for max I - if (write(INA238_REG_CONFIG, (uint16_t) _range) != PX4_OK) { - return ret; - } - - // Start ADC continous mode here - ret = write(INA238_REG_ADCCONFIG, (uint16_t)INA238_ADCCONFIG); - - start(); - _sensor_ok = true; - - _initialized = ret == PX4_OK; - return ret; + return Reset(); } int INA238::force_init() @@ -156,12 +139,12 @@ int INA238::probe() { uint16_t value{0}; - if (read(INA238_MANUFACTURER_ID, value) != PX4_OK || value != INA238_MFG_ID_TI) { + if (RegisterRead(Register::MANUFACTURER_ID, value) != PX4_OK || value != INA238_MFG_ID_TI) { PX4_DEBUG("probe mfgid %d", value); return -1; } - if (read(INA238_DEVICE_ID, value) != PX4_OK || ( + if (RegisterRead(Register::DEVICE_ID, value) != PX4_OK || ( INA238_DEVICEID(value) != INA238_MFG_DIE )) { PX4_DEBUG("probe die id %d", value); @@ -171,6 +154,58 @@ int INA238::probe() return PX4_OK; } +int INA238::Reset() +{ + + int ret = PX4_ERROR; + + if (RegisterWrite(Register::CONFIG, (uint16_t)(ADC_RESET_BIT)) != PX4_OK) { + return ret; + } + + 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() { @@ -190,8 +225,22 @@ int INA238::collect() int16_t bus_voltage{0}; int16_t current{0}; - success = success && (read(INA238_REG_VSBUS, bus_voltage) == PX4_OK); - success = success && (read(INA238_REG_CURRENT, current) == PX4_OK); + success = (RegisterRead(Register::VS_BUS, (uint16_t &)bus_voltage) == PX4_OK); + success = success && (RegisterRead(Register::CURRENT, (uint16_t &)current) == PX4_OK); + + 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; + } + } if (!success) { PX4_DEBUG("error reading from sensor"); @@ -234,7 +283,9 @@ void INA238::RunImpl() if (collect() != PX4_OK) { perf_count(_collection_errors); /* if error restart the measurement state machine */ - start(); + ScheduleClear(); + _initialized = false; + ScheduleNow(); return; } @@ -262,6 +313,10 @@ void INA238::RunImpl() if (init() != PX4_OK) { ScheduleDelayed(INA238_INIT_RETRY_INTERVAL_US); + + } else { + _initialized = true; + start(); } } } diff --git a/src/drivers/power_monitor/ina238/ina238.h b/src/drivers/power_monitor/ina238/ina238.h index a9108c258e..c40514ad58 100644 --- a/src/drivers/power_monitor/ina238/ina238.h +++ b/src/drivers/power_monitor/ina238/ina238.h @@ -44,8 +44,10 @@ #include #include #include +#include "ina238_registers.hpp" using namespace time_literals; +using namespace ina238; /* Configuration Constants */ #define INA238_BASEADDR 0x45 /* 7-bit address. 8-bit address is 0x45 */ @@ -53,225 +55,14 @@ using namespace time_literals; // connect to the INA238 every this many microseconds #define INA238_INIT_RETRY_INTERVAL_US 500000 -/* INA238 Registers addresses */ -#define INA238_REG_CONFIG (0x00) -#define INA238_REG_ADCCONFIG (0x01) -#define INA238_REG_SHUNTCAL (0x02) -#define INA238_REG_SHUNTTEMPCO (0x03) -#define INA238_REG_VSHUNT (0x04) -#define INA238_REG_VSBUS (0x05) -#define INA238_REG_DIETEMP (0x06) -#define INA238_REG_CURRENT (0x07) -#define INA238_REG_POWER (0x08) -#define INA238_REG_ENERGY (0x09) -#define INA238_REG_CHARGE (0x0a) -#define INA238_REG_DIAG_ALRT (0x0b) -#define INA238_REG_SOVL (0x0c) -#define INA238_REG_SUVL (0x0d) -#define INA238_REG_BOVL (0x0e) -#define INA238_REG_BUVL (0x0f) -#define INA238_REG_TEMP_LIMIT (0x10) -#define INA238_REG_TPWR_LIMIT (0x11) -#define INA238_MANUFACTURER_ID (0x3e) -#define INA238_DEVICE_ID (0x3f) #define INA238_MFG_ID_TI (0x5449) // TI #define INA238_MFG_DIE (0x238) // INA237, INA238 -/* INA238 Configuration (CONFIG) 16-bit Register (Address = 0h) [reset = 0h] */ #define INA238_ADCRANGE_SHIFTS (4) -#define INA238_ADCRANGE_MASK (1 << INA238_ADCRANGE_SHIFTS) #define INA238_ADCRANGE_LOW (1 << INA238_ADCRANGE_SHIFTS) // ± 40.96 mV #define INA238_ADCRANGE_HIGH (0 << INA238_ADCRANGE_SHIFTS) // ±163.84 mV -#define INA238_TEMPCOMP_SHIFTS (5) -#define INA238_TEMPCOMP_MASK (1 << INA238_TEMPCOMP_SHIFTS) -#define INA238_TEMPCOMP_ENABLE (1 << INA238_TEMPCOMP_SHIFTS) -#define INA238_TEMPCOMP_DISABLE (0 << INA238_TEMPCOMP_SHIFTS) -#define INA238_CONVDLY_SHIFTS (6) -#define INA238_CONVDLY_MASK (0xff << INA238_CONVDLY_SHIFTS) -#define INA238_CONVDLY2MS(n) ((n) << INA238_CONVDLY_SHIFTS) - -#define INA238_RSTACC_SHIFTS (14) -#define INA238_RSTACC_MASK (1 << INA238_RSTACC_SHIFTS) -#define INA238_RSTACC_CLEAR (1 << INA238_RSTACC_SHIFTS) -#define INA238_RSTACC_NORMAL (0 << INA238_RSTACC_SHIFTS) - -#define INA238_RST_SHIFTS (15) -#define INA238_RST_MASK (1 << INA238_RST_SHIFTS) -#define INA238_RST_RESET (1 << INA238_RST_SHIFTS) -#define INA238_RST_NORMAL (0 << INA238_RST_SHIFTS) - -/* INA238 ADC Configuration (ADC_CONFIG) 16-bit Register (Address = 1h) [reset = FB68h] */ -#define INA238_MODE_SHIFTS (12) -#define INA238_MODE_MASK (0xf << INA238_MODE_SHIFTS) -#define INA238_MODE_SHUTDOWN_TRIG (0 << INA238_MODE_SHIFTS) -#define INA238_MODE_BUS_TRIG (1 << INA238_MODE_SHIFTS) -#define INA238_MODE_SHUNT_TRIG (2 << INA238_MODE_SHIFTS) -#define INA238_MODE_SHUNT_BUS_TRIG (3 << INA238_MODE_SHIFTS) -#define INA238_MODE_TEMP_TRIG (4 << INA238_MODE_SHIFTS) -#define INA238_MODE_TEMP_BUS_TRIG (5 << INA238_MODE_SHIFTS) -#define INA238_MODE_TEMP_SHUNT_TRIG (6 << INA238_MODE_SHIFTS) -#define INA238_MODE_TEMP_SHUNT_BUS_TRIG (7 << INA238_MODE_SHIFTS) - -#define INA238_MODE_SHUTDOWN_CONT (8 << INA238_MODE_SHIFTS) -#define INA238_MODE_BUS_CONT (9 << INA238_MODE_SHIFTS) -#define INA238_MODE_SHUNT_CONT (10 << INA238_MODE_SHIFTS) -#define INA238_MODE_SHUNT_BUS_CONT (11 << INA238_MODE_SHIFTS) -#define INA238_MODE_TEMP_CONT (12 << INA238_MODE_SHIFTS) -#define INA238_MODE_TEMP_BUS_CONT (13 << INA238_MODE_SHIFTS) -#define INA238_MODE_TEMP_SHUNT_CONT (14 << INA238_MODE_SHIFTS) -#define INA238_MODE_TEMP_SHUNT_BUS_CONT (15 << INA238_MODE_SHIFTS) - -#define INA238_VBUSCT_SHIFTS (9) -#define INA238_VBUSCT_MASK (7 << INA238_VBUSCT_SHIFTS) -#define INA238_VBUSCT_50US (0 << INA238_VBUSCT_SHIFTS) -#define INA238_VBUSCT_84US (1 << INA238_VBUSCT_SHIFTS) -#define INA238_VBUSCT_150US (2 << INA238_VBUSCT_SHIFTS) -#define INA238_VBUSCT_280US (3 << INA238_VBUSCT_SHIFTS) -#define INA238_VBUSCT_540US (4 << INA238_VBUSCT_SHIFTS) -#define INA238_VBUSCT_1052US (5 << INA238_VBUSCT_SHIFTS) -#define INA238_VBUSCT_2074US (6 << INA238_VBUSCT_SHIFTS) -#define INA238_VBUSCT_4170US (7 << INA238_VBUSCT_SHIFTS) - -#define INA238_VSHCT_SHIFTS (6) -#define INA238_VSHCT_MASK (7 << INA238_VSHCT_SHIFTS) -#define INA238_VSHCT_50US (0 << INA238_VSHCT_SHIFTS) -#define INA238_VSHCT_84US (1 << INA238_VSHCT_SHIFTS) -#define INA238_VSHCT_150US (2 << INA238_VSHCT_SHIFTS) -#define INA238_VSHCT_280US (3 << INA238_VSHCT_SHIFTS) -#define INA238_VSHCT_540US (4 << INA238_VSHCT_SHIFTS) -#define INA238_VSHCT_1052US (5 << INA238_VSHCT_SHIFTS) -#define INA238_VSHCT_2074US (6 << INA238_VSHCT_SHIFTS) -#define INA238_VSHCT_4170US (7 << INA238_VSHCT_SHIFTS) - -#define INA238_VTCT_SHIFTS (3) -#define INA238_VTCT_MASK (7 << INA238_VTCT_SHIFTS) -#define INA238_VTCT_50US (0 << INA238_VTCT_SHIFTS) -#define INA238_VTCT_84US (1 << INA238_VTCT_SHIFTS) -#define INA238_VTCT_150US (2 << INA238_VTCT_SHIFTS) -#define INA238_VTCT_280US (3 << INA238_VTCT_SHIFTS) -#define INA238_VTCT_540US (4 << INA238_VTCT_SHIFTS) -#define INA238_VTCT_1052US (5 << INA238_VTCT_SHIFTS) -#define INA238_VTCT_2074US (6 << INA238_VTCT_SHIFTS) -#define INA238_VTCT_4170US (7 << INA238_VTCT_SHIFTS) - -#define INA238_AVERAGES_SHIFTS (0) -#define INA238_AVERAGES_MASK (7 << INA238_AVERAGES_SHIFTS) -#define INA238_AVERAGES_1 (0 << INA238_AVERAGES_SHIFTS) -#define INA238_AVERAGES_4 (1 << INA238_AVERAGES_SHIFTS) -#define INA238_AVERAGES_16 (2 << INA238_AVERAGES_SHIFTS) -#define INA238_AVERAGES_64 (3 << INA238_AVERAGES_SHIFTS) -#define INA238_AVERAGES_128 (4 << INA238_AVERAGES_SHIFTS) -#define INA238_AVERAGES_256 (5 << INA238_AVERAGES_SHIFTS) -#define INA238_AVERAGES_512 (6 << INA238_AVERAGES_SHIFTS) -#define INA238_AVERAGES_1024 (7 << INA238_AVERAGES_SHIFTS) - -#define INA238_ADCCONFIG (INA238_MODE_TEMP_SHUNT_BUS_CONT | INA238_VBUSCT_540US | INA238_VSHCT_540US | INA238_VTCT_540US |INA238_AVERAGES_64) - -/* INA238 Shunt Calibration (SHUNT_CAL) 16-bit Register (Address = 2h) [reset = 1000h] */ - -#define INA238_CURRLSB_SHIFTS (0) -#define INA238_CURRLSB_MASK (0x7fff << INA238_CURRLSB_SHIFTS) - -/* INA238 Shunt Temperature Coefficient (SHUNT_TEMPCO) 16-bit Register (Address = 3h) [reset = 0h] */ - -#define INA238_TEMPCO_SHIFTS (0) -#define INA238_TEMPCO_MASK (0x1fff << INA238_TEMPCO_SHIFTS) - -/* INA238 Shunt Voltage Measurement (VSHUNT) 24-bit Register (Address = 4h) [reset = 0h] */ - -#define INA238_VSHUNT_SHIFTS (4) -#define INA238_VSHUNT_MASK (UINT32_C(0xffffff) << INA238_VSHUNT_SHIFTS) - -/* INA238 Bus Voltage Measurement (VBUS) 24-bit Register (Address = 5h) [reset = 0h] */ - -#define INA238_VBUS_SHIFTS (4) -#define INA238_VBUS_MASK (UINT32_C(0xffffff) << INA238_VBUS_SHIFTS) - -/* INA238 Temperature Measurement (DIETEMP) 16-bit Register (Address = 6h) [reset = 0h] */ - -#define INA238_DIETEMP_SHIFTS (0) -#define INA238_DIETEMP_MASK (0xffff << INA238_DIETEMP_SHIFTS) - -/* INA238 Current Result (CURRENT) 24-bit Register (Address = 7h) [reset = 0h] */ - -#define INA238_CURRENT_SHIFTS (4) -#define INA238_CURRENT_MASK (UINT32_C(0xffffff) << INA238_CURRENT_SHIFTS) - -/* INA238 Power Result (POWER) 24-bit Register (Address = 8h) [reset = 0h] */ - -#define INA238_POWER_SHIFTS (0) -#define INA238_POWER_MASK (UINT32_C(0xffffff) << INA238_POWER_SHIFTS) - -/* INA238 Energy Result (ENERGY) 40-bit Register (Address = 9h) [reset = 0h] */ - -#define INA238_ENERGY_SHIFTS (0) -#define INA238_ENERGY_MASK (UINT64_C(0xffffffffff) << INA238_ENERGY_SHIFTS) - -/* INA238 Energy Result (CHARGE) 40-bit Register (Address = Ah) [reset = 0h] */ - -#define INA238_CHARGE_SHIFTS (0) -#define INA238_CHARGE_MASK (UINT64_C(0xffffffffff) << INA238_CHARGE_SHIFTS) - - -/* INA238 Diagnostic Flags and Alert (DIAG_ALRT) 16-bit Register (Address = Bh) [reset = 0001h] */ - -#define INA238_MEMSTAT (1 << 0) // This bit is set to 0 if a checksum error is detected in the device trim memory space -#define INA238_CNVRF (1 << 1) // This bit is set to 1 if the conversion is completed. When ALATCH =1 this bit is cleared by reading the register or starting a new triggered conversion. -#define INA238_POL (1 << 2) // This bit is set to 1 if the power measurement exceeds the threshold limit in the power limit register. -#define INA238_BUSUL (1 << 3) // This bit is set to 1 if the bus voltage measurement falls below the threshold limit in the bus under-limit register. -#define INA238_BUSOL (1 << 4) // This bit is set to 1 if the bus voltage measurement exceeds the threshold limit in the bus over-limit register. -#define INA238_SHNTUL (1 << 5) // This bit is set to 1 if the shunt voltage measurement falls below the threshold limit in the shunt under-limit register -#define INA238_SHNTOL (1 << 6) // This bit is set to 1 if the shunt voltage measurement exceeds the threshold limit in the shunt over-limit register. -#define INA238_TMPOL (1 << 7) // This bit is set to 1 if the temperature measurement exceeds the threshold limit in the temperature over-limit register. -#define INA238_MATHOF (1 << 9) // This bit is set to 1 if an arithmetic operation resulted in an overflow error. -#define INA238_CHARGEOF (1 << 10) // This bit indicates the health of the CHARGE register. If the 40 bit CHARGE register has overflowed this bit is set to 1. -#define INA238_ENERGYOF (1 << 11) // This bit indicates the health of the ENERGY register. If the 40 bit ENERGY register has overflowed this bit is set to 1. -#define INA238_APOL (1 << 12) // Alert Polarity bit sets the Alert pin polarity. -#define INA238_SLOWALER (1 << 13) // ALERT function is asserted on the completed averaged value. This gives the flexibility to delay the ALERT after the averaged value. -#define INA238_CNVR (1 << 14) // Setting this bit high configures the Alert pin to be asserted when the Conversion Ready Flag (bit 1) is asserted, indicating that a conversion cycle has completed -#define INA238_ALATCH (1 << 15) // When the Alert Latch Enable bit is set to Transparent mode, the Alert pin and Flag bit reset to the idle state when the fault has been -// cleared. When the Alert Latch Enable bit is set to Latch mode, the Alert pin and Alert Flag bit remain active following a fault until -// the DIAG_ALRT Register has been read. - -/* Shunt Overvoltage Threshold (SOVL) 16-bit Register (Address = Ch) [reset = 7FFFh] */ - -#define INA238_SOVL_SHIFTS (0) -#define INA238_SOVL_MASK (0xffff << INA238_SOVL_SHIFTS) - -/* Shunt Undervoltage Threshold (SUVL) 16-bit Register (Address = Dh) [reset = 8000h] */ - -#define INA238_SUVL_SHIFTS (0) -#define INA238_SUVL_MASK (0xffff << INA238_SUVL_SHIFTS) - -/* Bus Overvoltage Threshold (BOVL) 16-bit Register (Address = Eh) [reset = 7FFFh] */ - -#define INA238_BOVL_SHIFTS (0) -#define INA238_BOVL_MASK (0xffff << INA238_BOVL_SHIFTS) - -/* Bus Undervoltage Threshold (BUVL) 16-bit Register (Address = Fh) [reset = 0h] */ - -#define INA238_BUVL_SHIFTS (0) -#define INA238_BUVL_MASK (0xffff << INA238_BUVL_SHIFTS) - -/* Temperature Over-Limit Threshold (TEMP_LIMIT) 16-bit Register (Address = 10h) [reset = 7FFFh */ - -#define INA238_TEMP_LIMIT_SHIFTS (0) -#define INA238_TEMP_LIMIT_MASK (0xffff << INA238_TEMP_LIMIT_SHIFTS) - -/* Power Over-Limit Threshold (PWR_LIMIT) 16-bit Register (Address = 11h) [reset = FFFFh] */ - -#define INA238_POWER_LIMIT_SHIFTS (0) -#define INA238_POWER_LIMIT_MASK (0xffff << INA238_POWER_LIMIT_SHIFTS) - -/* Manufacturer ID (MANUFACTURER_ID) 16-bit Register (Address = 3Eh) [reset = 5449h] */ - -/* Device ID (DEVICE_ID) 16-bit Register (Address = 3Fh) [reset = 2380h] */ - -#define INA238_DEVICE_REV_ID_SHIFTS (0) -#define INA238_DEVICE_REV_ID_MASK (0xf << INA238_DEVICE_REV_ID_SHIFTS) -#define INA238_DEVICEREV_ID(v) (((v) & INA238_DEVICE_REV_ID_MASK) >> INA238_DEVICE_REV_ID_SHIFTS) #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) @@ -283,7 +74,6 @@ using namespace time_literals; #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 DEFAULT_MAX_CURRENT 327.68f /* Amps */ #define DEFAULT_SHUNT 0.0003f /* Shunt is 300 uOhm */ @@ -321,7 +111,17 @@ protected: int probe() override; private: - bool _sensor_ok{false}; + // Sensor Configuration + struct register_config_t { + Register reg; + uint16_t set_bits{0}; + uint16_t clear_bits{0}; + }; + 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}; @@ -329,12 +129,25 @@ private: 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}; diff --git a/src/drivers/power_monitor/ina238/ina238_registers.hpp b/src/drivers/power_monitor/ina238/ina238_registers.hpp new file mode 100644 index 0000000000..3f1689fcff --- /dev/null +++ b/src/drivers/power_monitor/ina238/ina238_registers.hpp @@ -0,0 +1,90 @@ +// +// 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, + 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_