Browse Source

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
sbg
AlexKlimaj 7 years ago committed by Daniel Agar
parent
commit
08a53a9056
  1. 48
      src/drivers/batt_smbus/batt_smbus.cpp

48
src/drivers/batt_smbus/batt_smbus.cpp

@ -541,19 +541,19 @@ BATT_SMBUS::cycle()
} }
// read remaining capacity // 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) { if (tmp > _batt_capacity) {
PX4_WARN("Remaining Cap greater than total: Cap:%hu RemainingCap:%hu", (uint16_t)_batt_capacity, (uint16_t)tmp); PX4_WARN("Remaining Cap greater than total: Cap:%hu RemainingCap:%hu", (uint16_t)_batt_capacity, (uint16_t)tmp);
_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 // calculate total discharged amount
new_report.discharged_mah = (float)((float)_batt_startup_capacity - (float)tmp); new_report.discharged_mah = (float)((float)_batt_startup_capacity - (float)tmp);
}
} }
// read battery temperature and covert to Celsius // read battery temperature and covert to Celsius
@ -561,18 +561,30 @@ BATT_SMBUS::cycle()
new_report.temperature = (float)(((float)tmp / 10.0f) - 273.15f); new_report.temperature = (float)(((float)tmp / 10.0f) - 273.15f);
} }
// propagate warning state only if the state //Check if remaining % is out of range
if (new_report.remaining < _emergency_thr) { 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; new_report.warning = battery_status_s::BATTERY_WARNING_EMERGENCY;
}
} else if (new_report.remaining < _crit_thr) { // propagate warning state
new_report.warning = battery_status_s::BATTERY_WARNING_CRITICAL; else {
if (new_report.remaining > _low_thr) {
new_report.warning = battery_status_s::BATTERY_WARNING_NONE;
} else if (new_report.remaining < _low_thr) { } else if (new_report.remaining > _crit_thr) {
new_report.warning = battery_status_s::BATTERY_WARNING_LOW; new_report.warning = battery_status_s::BATTERY_WARNING_LOW;
} else { } else if (new_report.remaining > _emergency_thr) {
new_report.warning = battery_status_s::BATTERY_WARNING_NONE; new_report.warning = battery_status_s::BATTERY_WARNING_CRITICAL;
} else {
new_report.warning = battery_status_s::BATTERY_WARNING_EMERGENCY;
}
} }
new_report.capacity = _batt_capacity; new_report.capacity = _batt_capacity;

Loading…
Cancel
Save