diff --git a/src/drivers/power_monitor/ina220/ina220.cpp b/src/drivers/power_monitor/ina220/ina220.cpp index c8f52bf71c..bde68ce875 100644 --- a/src/drivers/power_monitor/ina220/ina220.cpp +++ b/src/drivers/power_monitor/ina220/ina220.cpp @@ -329,8 +329,6 @@ INA220::RunImpl() if (_ch_type == PM_CH_TYPE_VBATT) { _battery.setConnected(false); - _battery.updateVoltage(0.f); - _battery.updateCurrent(0.f); _battery.updateAndPublishBatteryStatus(hrt_absolute_time()); } diff --git a/src/drivers/power_monitor/ina226/ina226.cpp b/src/drivers/power_monitor/ina226/ina226.cpp index f61a7c7b79..60fb333699 100644 --- a/src/drivers/power_monitor/ina226/ina226.cpp +++ b/src/drivers/power_monitor/ina226/ina226.cpp @@ -83,10 +83,10 @@ INA226::INA226(const I2CSPIDriverConfig &config, int battery_index) : _power_lsb = 25 * _current_lsb; // We need to publish immediately, to guarantee that the first instance of the driver publishes to uORB instance 0 - _battery.setConnected(false); - _battery.updateVoltage(0.f); - _battery.updateCurrent(0.f); + setConnected(false); _battery.updateAndPublishBatteryStatus(hrt_absolute_time()); + + I2C::_retries = 5; } INA226::~INA226() @@ -226,14 +226,11 @@ INA226::collect() success = success && (read(INA226_REG_CURRENT, _current) == PX4_OK); // success = success && (read(INA226_REG_SHUNTVOLTAGE, _shunt) == PX4_OK); - if (!success) { - PX4_DEBUG("error reading from sensor"); - _bus_voltage = _power = _current = _shunt = 0; + if (setConnected(success)) { + _battery.updateVoltage(static_cast(_bus_voltage * INA226_VSCALE)); + _battery.updateCurrent(static_cast(_current * _current_lsb)); } - _battery.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); @@ -246,6 +243,7 @@ INA226::collect() } } + void INA226::start() { @@ -297,9 +295,7 @@ INA226::RunImpl() ScheduleDelayed(INA226_CONVERSION_INTERVAL); } else { - _battery.setConnected(false); - _battery.updateVoltage(0.f); - _battery.updateCurrent(0.f); + setConnected(false); _battery.updateAndPublishBatteryStatus(hrt_absolute_time()); if (init() != PX4_OK) { @@ -308,6 +304,28 @@ INA226::RunImpl() } } +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() { diff --git a/src/drivers/power_monitor/ina226/ina226.h b/src/drivers/power_monitor/ina226/ina226.h index a402a079ab..a69695a1cc 100644 --- a/src/drivers/power_monitor/ina226/ina226.h +++ b/src/drivers/power_monitor/ina226/ina226.h @@ -206,6 +206,10 @@ private: 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. * diff --git a/src/drivers/power_monitor/ina228/ina228.cpp b/src/drivers/power_monitor/ina228/ina228.cpp index e586590470..fd7494d3f3 100644 --- a/src/drivers/power_monitor/ina228/ina228.cpp +++ b/src/drivers/power_monitor/ina228/ina228.cpp @@ -98,11 +98,10 @@ INA228::INA228(const I2CSPIDriverConfig &config, int battery_index) : _power_lsb = 3.2f * _current_lsb; // We need to publish immediately, to guarantee that the first instance of the driver publishes to uORB instance 0 - _battery.setConnected(false); - _battery.updateVoltage(0.f); - _battery.updateCurrent(0.f); - _battery.updateTemperature(0.f); + setConnected(false); _battery.updateAndPublishBatteryStatus(hrt_absolute_time()); + + I2C::_retries = 5; } INA228::~INA228() @@ -322,15 +321,12 @@ INA228::collect() //success = success && (read(INA228_REG_VSHUNT, _shunt) == PX4_OK); success = success && (read(INA228_REG_DIETEMP, _temperature) == PX4_OK); - if (!success) { - PX4_DEBUG("error reading from sensor"); - _bus_voltage = _power = _current = _shunt = 0; + if (setConnected(success)) { + _battery.updateVoltage(static_cast(_bus_voltage * INA228_VSCALE)); + _battery.updateCurrent(static_cast(_current * _current_lsb)); + _battery.updateTemperature(static_cast(_temperature * INA228_TSCALE)); } - _battery.setConnected(success); - _battery.updateVoltage(static_cast(_bus_voltage * INA228_VSCALE)); - _battery.updateCurrent(static_cast(_current * _current_lsb)); - _battery.updateTemperature(static_cast(_temperature * INA228_TSCALE)); _battery.updateAndPublishBatteryStatus(hrt_absolute_time()); perf_end(_sample_perf); @@ -394,10 +390,7 @@ INA228::RunImpl() ScheduleDelayed(INA228_CONVERSION_INTERVAL); } else { - _battery.setConnected(false); - _battery.updateVoltage(0.f); - _battery.updateCurrent(0.f); - _battery.updateTemperature(0.f); + setConnected(false); _battery.updateAndPublishBatteryStatus(hrt_absolute_time()); if (init() != PX4_OK) { @@ -406,6 +399,29 @@ INA228::RunImpl() } } +bool INA228::setConnected(bool state) +{ + // Filter out brief I2C failures for 2s + if (state) { + _connected = INA228_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); + _battery.updateTemperature(0); + } + + return state; +} + void INA228::print_status() { diff --git a/src/drivers/power_monitor/ina228/ina228.h b/src/drivers/power_monitor/ina228/ina228.h index 46e152ff00..72a89e6dcb 100644 --- a/src/drivers/power_monitor/ina228/ina228.h +++ b/src/drivers/power_monitor/ina228/ina228.h @@ -356,6 +356,9 @@ private: 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, int16_t &data); int write(uint8_t address, int16_t data); diff --git a/src/drivers/power_monitor/ina238/ina238.cpp b/src/drivers/power_monitor/ina238/ina238.cpp index a55f71b893..f42ae87764 100644 --- a/src/drivers/power_monitor/ina238/ina238.cpp +++ b/src/drivers/power_monitor/ina238/ina238.cpp @@ -90,11 +90,10 @@ INA238::INA238(const I2CSPIDriverConfig &config, int battery_index) : _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); - _battery.updateVoltage(0.f); - _battery.updateCurrent(0.f); - _battery.updateTemperature(0.f); + setConnected(false); _battery.updateAndPublishBatteryStatus(hrt_absolute_time()); + + I2C::_retries = 5; } INA238::~INA238() @@ -178,11 +177,8 @@ int INA238::probe() int INA238::Reset() { - int ret = PX4_ERROR; - _retries = 3; - if (RegisterWrite(Register::CONFIG, (uint16_t)(ADC_RESET_BIT)) != PX4_OK) { return ret; } @@ -254,13 +250,11 @@ 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) { + 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)); - _battery.setConnected(success); - _battery.updateAndPublishBatteryStatus(hrt_absolute_time()); } @@ -276,8 +270,7 @@ int INA238::collect() perf_count(_bad_register_perf); success = false; - _battery.setConnected(success); - + setConnected(false); _battery.updateAndPublishBatteryStatus(hrt_absolute_time()); } } @@ -338,10 +331,7 @@ void INA238::RunImpl() ScheduleDelayed(INA238_CONVERSION_INTERVAL); } else { - _battery.setConnected(false); - _battery.updateVoltage(0.f); - _battery.updateCurrent(0.f); - _battery.updateTemperature(0.f); + setConnected(false); _battery.updateAndPublishBatteryStatus(hrt_absolute_time()); if (init() != PX4_OK) { @@ -354,6 +344,29 @@ void INA238::RunImpl() } } +bool INA238::setConnected(bool state) +{ + // Filter out brief I2C failures for 2s + if (state) { + _connected = INA238_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); + _battery.updateTemperature(0); + } + + return state; +} + void INA238::print_status() { I2CSPIDriverBase::print_status(); diff --git a/src/drivers/power_monitor/ina238/ina238.h b/src/drivers/power_monitor/ina238/ina238.h index 7d888aef54..62750c44c4 100644 --- a/src/drivers/power_monitor/ina238/ina238.h +++ b/src/drivers/power_monitor/ina238/ina238.h @@ -154,6 +154,9 @@ private: 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);