From 08a53a9056ba86a4b3361a99c75115857920591a Mon Sep 17 00:00:00 2001 From: AlexKlimaj Date: Thu, 15 Mar 2018 12:46:27 -0600 Subject: [PATCH] batt_smbus add complementary filter and small fixes (#9080) * Batt_smbus. Added complementary filter to remaining capacity, reversed warning state checks, and added remaining/discharged out of range checks to handle bad battery calibrations * Changed errx to PX4_ERR * Added PX4_ERR returns --- src/drivers/batt_smbus/batt_smbus.cpp | 48 +++++++++++++++++---------- 1 file changed, 30 insertions(+), 18 deletions(-) diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index 3666089176..6716b666e8 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -541,19 +541,19 @@ BATT_SMBUS::cycle() } // read remaining capacity - if (_batt_capacity > 0) { - if (read_reg(BATT_SMBUS_REMAINING_CAPACITY, tmp) == OK) { + if (read_reg(BATT_SMBUS_REMAINING_CAPACITY, tmp) == OK) { - if (tmp > _batt_capacity) { - PX4_WARN("Remaining Cap greater than total: Cap:%hu RemainingCap:%hu", (uint16_t)_batt_capacity, (uint16_t)tmp); - _batt_capacity = (uint16_t)tmp; - } + if (tmp > _batt_capacity) { + PX4_WARN("Remaining Cap greater than total: Cap:%hu RemainingCap:%hu", (uint16_t)_batt_capacity, (uint16_t)tmp); + _batt_capacity = (uint16_t)tmp; + } - new_report.remaining = (float)(1.000f - (((float)_batt_capacity - (float)tmp) / (float)_batt_capacity)); + // Calculate remaining capacity percent with complementary filter + new_report.remaining = (float)(_last_report.remaining * 0.8f) + (float)(0.2f * (float)(1.000f - ((( + float)_batt_capacity - (float)tmp) / (float)_batt_capacity))); - // calculate total discharged amount - new_report.discharged_mah = (float)((float)_batt_startup_capacity - (float)tmp); - } + // calculate total discharged amount + new_report.discharged_mah = (float)((float)_batt_startup_capacity - (float)tmp); } // read battery temperature and covert to Celsius @@ -561,18 +561,30 @@ BATT_SMBUS::cycle() new_report.temperature = (float)(((float)tmp / 10.0f) - 273.15f); } - // propagate warning state only if the state - if (new_report.remaining < _emergency_thr) { + //Check if remaining % is out of range + if ((new_report.remaining > 1.00f) || (new_report.remaining <= 0.00f)) { + new_report.warning = battery_status_s::BATTERY_WARNING_EMERGENCY; + } + + //Check if discharged amount is greater than the starting capacity + else if (new_report.discharged_mah > (float)_batt_startup_capacity) { new_report.warning = battery_status_s::BATTERY_WARNING_EMERGENCY; + } - } else if (new_report.remaining < _crit_thr) { - new_report.warning = battery_status_s::BATTERY_WARNING_CRITICAL; + // propagate warning state + else { + if (new_report.remaining > _low_thr) { + new_report.warning = battery_status_s::BATTERY_WARNING_NONE; - } else if (new_report.remaining < _low_thr) { - new_report.warning = battery_status_s::BATTERY_WARNING_LOW; + } else if (new_report.remaining > _crit_thr) { + new_report.warning = battery_status_s::BATTERY_WARNING_LOW; - } else { - new_report.warning = battery_status_s::BATTERY_WARNING_NONE; + } else if (new_report.remaining > _emergency_thr) { + new_report.warning = battery_status_s::BATTERY_WARNING_CRITICAL; + + } else { + new_report.warning = battery_status_s::BATTERY_WARNING_EMERGENCY; + } } new_report.capacity = _batt_capacity;