fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-16 12:13:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

batt_smbus add complementary filter and small fixes (#9080)

Browse Source 
* 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
This commit is contained in:
AlexKlimaj
2018-03-15 14:46:27 -04:00
committed by Daniel Agar
parent 7776789b7d
commit 08a53a9056
1 changed files with 31 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/batt_smbus/batt_smbus.cpp
+31 -19
View File
@@ -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;
}
new_report.remaining = (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);
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;
}
// 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);
}
// 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;
}
} else if (new_report.remaining < _crit_thr) {
new_report.warning = battery_status_s::BATTERY_WARNING_CRITICAL;
//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 < _low_thr) {
new_report.warning = battery_status_s::BATTERY_WARNING_LOW;
// propagate warning state
else {
if (new_report.remaining > _low_thr) {
new_report.warning = battery_status_s::BATTERY_WARNING_NONE;
} else {
new_report.warning = battery_status_s::BATTERY_WARNING_NONE;
} else if (new_report.remaining > _crit_thr) {
new_report.warning = battery_status_s::BATTERY_WARNING_LOW;
} 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;