Use discharged current to estimate remaining battery charge if capacity is known

4 files changed, 32 insertions, 21 deletions

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index ace13bb78..2499aff08 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -896,8 +896,9 @@ int commander_thread_main(int argc, char *argv[])
 			/* only consider battery voltage if system has been running 2s and battery voltage is higher than 4V */
 			if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_v > 4.0f) {
 				status.battery_voltage = battery.voltage_v;
+				status.battery_current = battery.current_a;
 				status.condition_battery_voltage_valid = true;
-				status.battery_remaining = battery_remaining_estimate_voltage(status.battery_voltage);
+				status.battery_remaining = battery_remaining_estimate_voltage(battery.voltage_v, battery.discharged_mah);
 			}
 		}
 
diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp
index 565b4b66a..49fe5ea4d 100644
--- a/src/modules/commander/commander_helper.cpp
+++ b/src/modules/commander/commander_helper.cpp
@@ -251,36 +251,44 @@ void rgbled_set_pattern(rgbled_pattern_t *pattern)
 		ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern);
 }
 
-float battery_remaining_estimate_voltage(float voltage)
+float battery_remaining_estimate_voltage(float voltage, float discharged)
 {
 	float ret = 0;
-	static param_t bat_volt_empty;
-	static param_t bat_volt_full;
-	static param_t bat_n_cells;
+	static param_t bat_v_empty_h;
+	static param_t bat_v_full_h;
+	static param_t bat_n_cells_h;
+	static param_t bat_capacity_h;
+	static float bat_v_empty = 3.2f;
+	static float bat_v_full = 4.0f;
+	static int bat_n_cells = 3;
+	static float bat_capacity = -1.0f;
 	static bool initialized = false;
 	static unsigned int counter = 0;
-	static float ncells = 3;
-	// XXX change cells to int (and param to INT32)
 
 	if (!initialized) {
-		bat_volt_empty = param_find("BAT_V_EMPTY");
-		bat_volt_full = param_find("BAT_V_FULL");
-		bat_n_cells = param_find("BAT_N_CELLS");
+		bat_v_empty_h = param_find("BAT_V_EMPTY");
+		bat_v_full_h = param_find("BAT_V_FULL");
+		bat_n_cells_h = param_find("BAT_N_CELLS");
+		bat_capacity_h = param_find("BAT_CAPACITY");
 		initialized = true;
 	}
 
-	static float chemistry_voltage_empty = 3.2f;
-	static float chemistry_voltage_full = 4.05f;
-
 	if (counter % 100 == 0) {
-		param_get(bat_volt_empty, &chemistry_voltage_empty);
-		param_get(bat_volt_full, &chemistry_voltage_full);
-		param_get(bat_n_cells, &ncells);
+		param_get(bat_v_empty_h, &bat_v_empty);
+		param_get(bat_v_full_h, &bat_v_full);
+		param_get(bat_n_cells_h, &bat_n_cells);
+		param_get(bat_capacity_h, &bat_capacity);
 	}
 
 	counter++;
 
-	ret = (voltage - ncells * chemistry_voltage_empty) / (ncells * (chemistry_voltage_full - chemistry_voltage_empty));
+	if (bat_capacity > 0.0f) {
+		/* if battery capacity is known, use it to estimate remaining charge */
+		ret = 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));
+	}
 
 	/* limit to sane values */
 	ret = (ret < 0.0f) ? 0.0f : ret;
diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h
index e9514446c..d0393f45a 100644
--- a/src/modules/commander/commander_helper.h
+++ b/src/modules/commander/commander_helper.h
@@ -75,12 +75,13 @@ void rgbled_set_mode(rgbled_mode_t mode);
 void rgbled_set_pattern(rgbled_pattern_t *pattern);
 
 /**
- * Provides a coarse estimate of remaining battery power.
+ * Estimate remaining battery charge.
  *
- * The estimate is very basic and based on decharging voltage curves.
+ * Use integral of current if battery capacity known (BAT_CAPACITY parameter set),
+ * else use simple estimate based on voltage.
  *
  * @return the estimated remaining capacity in 0..1
  */
-float battery_remaining_estimate_voltage(float voltage);
+float battery_remaining_estimate_voltage(float voltage, float discharged);
 
 #endif /* COMMANDER_HELPER_H_ */
diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c
index 40d0386d5..bdb4a0a1c 100644
--- a/src/modules/commander/commander_params.c
+++ b/src/modules/commander/commander_params.c
@@ -52,4 +52,5 @@ PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f);
 PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f);
 PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.2f);
 PARAM_DEFINE_FLOAT(BAT_V_FULL, 4.05f);
-PARAM_DEFINE_FLOAT(BAT_N_CELLS, 3);
+PARAM_DEFINE_INT32(BAT_N_CELLS, 3);
+PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f);