Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-22 06:14:14 +08:00
Code Issues Actions 8 Packages Projects Releases Wiki Activity

INA2xx: Debounce battery connection state (#25786)

Browse Source 
To prevent critical low battery messages on a single I2C issue.
This commit is contained in:
Niklas Hauser
2025-10-31 13:19:10 +01:00
committed by GitHub
parent f9595319b8
commit 6ec106a0ed
7 changed files with 100 additions and 45 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/power_monitor/ina220/ina220.cpp
-2
View File
@@ -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());
}
src/drivers/power_monitor/ina226/ina226.cpp
+30 -12
View File
@@ -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<float>(_bus_voltage * INA226_VSCALE));
_battery.updateCurrent(static_cast<float>(_current * _current_lsb));
}
_battery.setConnected(success);
_battery.updateVoltage(static_cast<float>(_bus_voltage * INA226_VSCALE));
_battery.updateCurrent(static_cast<float>(_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()
{
src/drivers/power_monitor/ina226/ina226.h
+4
View File
@@ -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.
*
src/drivers/power_monitor/ina228/ina228.cpp
+31 -15
View File
@@ -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<float>(_bus_voltage * INA228_VSCALE));
_battery.updateCurrent(static_cast<float>(_current * _current_lsb));
_battery.updateTemperature(static_cast<float>(_temperature * INA228_TSCALE));
}
_battery.setConnected(success);
_battery.updateVoltage(static_cast<float>(_bus_voltage * INA228_VSCALE));
_battery.updateCurrent(static_cast<float>(_current * _current_lsb));
_battery.updateTemperature(static_cast<float>(_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()
{
src/drivers/power_monitor/ina228/ina228.h
+3
View File
@@ -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);
src/drivers/power_monitor/ina238/ina238.cpp
+29 -16
View File
@@ -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<float>(bus_voltage * INA238_VSCALE));
_battery.updateCurrent(static_cast<float>(current * _current_lsb));
_battery.updateTemperature(static_cast<float>(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();
src/drivers/power_monitor/ina238/ina238.h
+3
View File
@@ -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);