diff --git a/src/drivers/power_monitor/ina238/ina238.cpp b/src/drivers/power_monitor/ina238/ina238.cpp index 42f5e12858..b46b277a84 100644 --- a/src/drivers/power_monitor/ina238/ina238.cpp +++ b/src/drivers/power_monitor/ina238/ina238.cpp @@ -160,6 +160,8 @@ int INA238::Reset() int ret = PX4_ERROR; + _retries = 3; + if (RegisterWrite(Register::CONFIG, (uint16_t)(ADC_RESET_BIT)) != PX4_OK) { return ret; } @@ -231,6 +233,16 @@ int INA238::collect() success = success && (RegisterRead(Register::CURRENT, (uint16_t &)current) == PX4_OK); success = success && (RegisterRead(Register::DIETEMP, (uint16_t &)temperature) == PX4_OK); + if (success) { + _battery.updateVoltage(static_cast(bus_voltage * INA238_VSCALE)); + _battery.updateCurrent(static_cast(current * _current_lsb)); + _battery.updateTemperature(static_cast(temperature * INA238_TSCALE)); + + _battery.setConnected(success); + + _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])) { @@ -242,26 +254,21 @@ int INA238::collect() PX4_DEBUG("register check failed"); perf_count(_bad_register_perf); success = false; + + _battery.setConnected(success); + + _battery.updateAndPublishBatteryStatus(hrt_absolute_time()); } } - if (!success) { - PX4_DEBUG("error reading from sensor"); - bus_voltage = current = 0; - } - - _battery.setConnected(success); - _battery.updateVoltage(static_cast(bus_voltage * INA238_VSCALE)); - _battery.updateCurrent(static_cast(current * _current_lsb)); - _battery.updateTemperature(static_cast(temperature * INA238_TSCALE)); - _battery.updateAndPublishBatteryStatus(hrt_absolute_time()); - perf_end(_sample_perf); if (success) { return PX4_OK; } else { + PX4_DEBUG("error reading from sensor"); + return PX4_ERROR; } }