Merge pull request #1211 from PX4/battload

Consider the throttle load for battery voltage calculation

7 files changed, 127 insertions, 29 deletions

diff --git a/Tools/tests-host/data/fit_linear_voltage.m b/Tools/tests-host/data/fit_linear_voltage.m
new file mode 100644
index 000000000..7d0c2c27f
--- /dev/null
+++ b/Tools/tests-host/data/fit_linear_voltage.m
@@ -0,0 +1,14 @@
+close all;
+clear all;
+M = importdata('px4io_v1.3.csv');
+voltage = M.data(:, 1);
+counts = M.data(:, 2);
+plot(counts, voltage, 'b*-', 'LineWidth', 2, 'MarkerSize', 15);
+coeffs = polyfit(counts, voltage, 1);
+fittedC = linspace(min(counts), max(counts), 500);
+fittedV = polyval(coeffs, fittedC);
+hold on
+plot(fittedC, fittedV, 'r-', 'LineWidth', 3);
+
+slope = coeffs(1)
+y_intersection = coeffs(2)
diff --git a/Tools/tests-host/data/px4io_v1.3.csv b/Tools/tests-host/data/px4io_v1.3.csv
new file mode 100644
index 000000000..b41ee8f1f
--- /dev/null
+++ b/Tools/tests-host/data/px4io_v1.3.csv
@@ -0,0 +1,70 @@
+voltage, counts
+4.3, 950
+4.4, 964
+4.5, 986
+4.6, 1009
+4.7, 1032
+4.8, 1055
+4.9, 1078
+5.0, 1101
+5.2, 1124
+5.3, 1148
+5.4, 1171
+5.5, 1195
+6.0, 1304
+6.1, 1329
+6.2, 1352
+7.0, 1517
+7.1, 1540
+7.2, 1564
+7.3, 1585
+7.4, 1610
+7.5, 1636
+8.0, 1728
+8.1, 1752
+8.2, 1755
+8.3, 1798
+8.4, 1821
+9.0, 1963
+9.1, 1987
+9.3, 2010
+9.4, 2033
+10.0, 2174
+10.1, 2198
+10.2, 2221
+10.3, 2245
+10.4, 2268
+11.0, 2385
+11.1, 2409
+11.2, 2432
+11.3, 2456
+11.4, 2480
+11.5, 2502
+11.6, 2526
+11.7, 2550
+11.8, 2573
+11.9, 2597
+12.0, 2621
+12.1, 2644
+12.3, 2668
+12.4, 2692
+12.5, 2716
+12.6, 2737
+12.7, 2761
+13.0, 2832
+13.5, 2950
+13.6, 2973
+14.1, 3068
+14.2, 3091
+14.7, 3209
+15.0, 3280
+15.1, 3304
+15.5, 3374
+15.6, 3397
+15.7, 3420
+16.0, 3492
+16.1, 3514
+16.2, 3538
+16.9, 3680
+17.0, 3704
+17.1, 3728
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 48cbc254f..a0c896178 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -913,6 +913,11 @@ int commander_thread_main(int argc, char *argv[])
 	struct system_power_s system_power;
 	memset(&system_power, 0, sizeof(system_power));
 
+	/* Subscribe to actuator controls (outputs) */
+	int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
+	struct actuator_controls_s actuator_controls;
+	memset(&actuator_controls, 0, sizeof(actuator_controls));
+
 	control_status_leds(&status, &armed, true);
 
 	/* now initialized */
@@ -1194,13 +1199,17 @@ int commander_thread_main(int argc, char *argv[])
 
 		if (updated) {
 			orb_copy(ORB_ID(battery_status), battery_sub, &battery);
+			orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls);
 
 			/* only consider battery voltage if system has been running 2s and battery voltage is valid */
 			if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) {
 				status.battery_voltage = battery.voltage_filtered_v;
 				status.battery_current = battery.current_a;
 				status.condition_battery_voltage_valid = true;
-				status.battery_remaining = battery_remaining_estimate_voltage(battery.voltage_filtered_v, battery.discharged_mah);
+
+				/* get throttle (if armed), as we only care about energy negative throttle also counts */
+				float throttle = (armed.armed) ? fabsf(actuator_controls.control[3]) : 0.0f;
+				status.battery_remaining = battery_remaining_estimate_voltage(battery.voltage_filtered_v, battery.discharged_mah, throttle);
 			}
 		}
 
@@ -1262,13 +1271,13 @@ int commander_thread_main(int argc, char *argv[])
 		}
 
 		/* if battery voltage is getting lower, warn using buzzer, etc. */
-		if (status.condition_battery_voltage_valid && status.battery_remaining < 0.25f && !low_battery_voltage_actions_done) {
+		if (status.condition_battery_voltage_valid && status.battery_remaining < 0.18f && !low_battery_voltage_actions_done) {
 			low_battery_voltage_actions_done = true;
 			mavlink_log_critical(mavlink_fd, "LOW BATTERY, RETURN TO LAND ADVISED");
 			status.battery_warning = VEHICLE_BATTERY_WARNING_LOW;
 			status_changed = true;
 
-		} else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.1f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) {
+		} else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.09f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) {
 			/* critical battery voltage, this is rather an emergency, change state machine */
 			critical_battery_voltage_actions_done = true;
 			mavlink_log_emergency(mavlink_fd, "CRITICAL BATTERY, LAND IMMEDIATELY");
diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp
index d5fe122cb..2022e99fb 100644
--- a/src/modules/commander/commander_helper.cpp
+++ b/src/modules/commander/commander_helper.cpp
@@ -281,15 +281,17 @@ void rgbled_set_pattern(rgbled_pattern_t *pattern)
 	}
 }
 
-float battery_remaining_estimate_voltage(float voltage, float discharged)
+float battery_remaining_estimate_voltage(float voltage, float discharged, float throttle_normalized)
 {
 	float ret = 0;
 	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 param_t bat_v_load_drop_h;
+	static float bat_v_empty = 3.4f;
+	static float bat_v_full = 4.2f;
+	static float bat_v_load_drop = 0.06f;
 	static int bat_n_cells = 3;
 	static float bat_capacity = -1.0f;
 	static bool initialized = false;
@@ -297,23 +299,26 @@ float battery_remaining_estimate_voltage(float voltage, float discharged)
 
 	if (!initialized) {
 		bat_v_empty_h = param_find("BAT_V_EMPTY");
-		bat_v_full_h = param_find("BAT_V_FULL");
+		bat_v_full_h = param_find("BAT_V_CHARGED");
 		bat_n_cells_h = param_find("BAT_N_CELLS");
 		bat_capacity_h = param_find("BAT_CAPACITY");
+		bat_v_load_drop_h = param_find("BAT_V_LOAD_DROP");
 		initialized = true;
 	}
 
 	if (counter % 100 == 0) {
 		param_get(bat_v_empty_h, &bat_v_empty);
 		param_get(bat_v_full_h, &bat_v_full);
+		param_get(bat_v_load_drop_h, &bat_v_load_drop);
 		param_get(bat_n_cells_h, &bat_n_cells);
 		param_get(bat_capacity_h, &bat_capacity);
 	}
 
 	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));
+	/* remaining charge estimate based on voltage and internal resistance (drop under load) */
+	float bat_v_full_dynamic = bat_v_full - (bat_v_load_drop * throttle_normalized);
+	float remaining_voltage = (voltage - (bat_n_cells * bat_v_empty)) / (bat_n_cells * (bat_v_full_dynamic - bat_v_empty));
 
 	if (bat_capacity > 0.0f) {
 		/* if battery capacity is known, use discharged current for estimate, but don't show more than voltage estimate */
diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h
index a49c9e263..4a77fe487 100644
--- a/src/modules/commander/commander_helper.h
+++ b/src/modules/commander/commander_helper.h
@@ -80,8 +80,9 @@ void rgbled_set_pattern(rgbled_pattern_t *pattern);
  *
  * @param voltage the current battery voltage
  * @param discharged the discharged capacity
+ * @param throttle_normalized the normalized throttle magnitude from 0 to 1. Negative throttle should be converted to this range as well, as it consumes energy.
  * @return the estimated remaining capacity in 0..1
  */
-float battery_remaining_estimate_voltage(float voltage, float discharged);
+float battery_remaining_estimate_voltage(float voltage, float discharged, float throttle_normalized);
 
 #endif /* COMMANDER_HELPER_H_ */
diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c
index 4750f9d5c..dba68700b 100644
--- a/src/modules/commander/commander_params.c
+++ b/src/modules/commander/commander_params.c
@@ -65,7 +65,17 @@ PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f);
  *
  * @group Battery Calibration
  */
-PARAM_DEFINE_FLOAT(BAT_V_FULL, 3.9f);
+PARAM_DEFINE_FLOAT(BAT_V_CHARGED, 4.2f);
+
+/**
+ * Voltage drop per cell on 100% load
+ *
+ * This implicitely defines the internal resistance
+ * to maximum current ratio and assumes linearity.
+ *
+ * @group Battery Calibration
+ */
+PARAM_DEFINE_FLOAT(BAT_V_LOAD_DROP, 0.07f);
 
 /**
  * Number of cells.
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index fcd53f1f1..43161aa70 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -739,30 +739,19 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
 		{
 			/*
 			 * Coefficients here derived by measurement of the 5-16V
-			 * range on one unit:
+			 * range on one unit, validated on sample points of another unit
 			 *
-			 * V   counts
-			 *  5  1001
-			 *  6  1219
-			 *  7  1436
-			 *  8  1653
-			 *  9  1870
-			 * 10  2086
-			 * 11  2303
-			 * 12  2522
-			 * 13  2738
-			 * 14  2956
-			 * 15  3172
-			 * 16  3389
+			 * Data in Tools/tests-host/data folder.
 			 *
-			 * slope = 0.0046067
-			 * intercept = 0.3863
+			 * measured slope = 0.004585267878277 (int: 4585)
+			 * nominal theoretic slope: 0.00459340659 (int: 4593)
+			 * intercept = 0.016646394188076 (int: 16646)
+			 * nominal theoretic intercept: 0.00 (int: 0)
 			 *
-			 * Intercept corrected for best results @ 12V.
 			 */
 			unsigned counts = adc_measure(ADC_VBATT);
 			if (counts != 0xffff) {
-				unsigned mV = (4150 + (counts * 46)) / 10 - 200;
+				unsigned mV = (166460 + (counts * 45934)) / 10000;
 				unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VBATT_SCALE]) / 10000;
 
 				r_page_status[PX4IO_P_STATUS_VBATT] = corrected;