From 714f5ea634a184ac80254e2a415221f738d2ecd6 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 11 Nov 2013 22:02:55 +0400 Subject: Track raw battery voltage and filtered battery voltage separately. Estimate remaining battery as min(voltage_estimate, discharged_estimate). Battery voltage LPF time increased. --- src/modules/commander/commander_helper.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) (limited to 'src/modules/commander/commander_helper.cpp') diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 49fe5ea4d..21a1c4c2c 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -44,6 +44,7 @@ #include #include #include +#include #include #include @@ -282,12 +283,15 @@ float battery_remaining_estimate_voltage(float voltage, float discharged) counter++; + /* remaining charge estimate based on voltage */ + float remaining_voltage = (voltage - bat_n_cells * bat_v_empty) / (bat_n_cells * (bat_v_full - bat_v_empty)); + if (bat_capacity > 0.0f) { - /* if battery capacity is known, use it to estimate remaining charge */ - ret = 1.0f - discharged / bat_capacity; + /* if battery capacity is known, use discharged current for estimate, but don't show more than voltage estimate */ + ret = fminf(remaining_voltage, 1.0f - discharged / bat_capacity); } else { /* else use voltage */ - ret = (voltage - bat_n_cells * bat_v_empty) / (bat_n_cells * (bat_v_full - bat_v_empty)); + ret = remaining_voltage; } /* limit to sane values */ -- cgit v1.2.3