Explicit internal battery resistance params

.gitignore

@@ -5,6 +5,7 @@
 *.px4log
 *.dSYM
 *.o
+*.gch
 *.pyc
 *~
 .*.swp

msg/battery_status.msg

@@ -4,7 +4,7 @@ float32 current_a		# Battery current in amperes, -1 if unknown
 float32 current_filtered_a	# Battery current in amperes, filtered, 0 if unknown
 float32 discharged_mah		# Discharged amount in mAh, -1 if unknown
 float32 remaining		# From 1 to 0, -1 if unknown
-float32 scale		# Power scaling factor with >= 1, or -1 if unknown
+float32 scale		# Power scaling factor, >= 1, or -1 if unknown
 int32 cell_count		# Number of cells
 bool connected			# Wether or not a battery is connected
 #bool is_powering_off		# Power off event imminent indication, false if unknown

src/modules/systemlib/battery.cpp

@@ -81,21 +81,16 @@ Battery::reset(battery_status_s *battery_status)
 	battery_status->connected = false;
 }
 
-// TODO: Distinguish between terminal battery voltage and real battery voltage
-
 void
 Battery::updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float current_a, float throttle_normalized,
 			     bool armed, battery_status_s *battery_status)
 {
-
-	// what we should do is
-// TODO: Add in voltage_term_v
 	reset(battery_status);
 	battery_status->timestamp = timestamp;
 	filterVoltage(voltage_v);
 	filterCurrent(current_a);
 	sumDischarged(timestamp, current_a);
-	estimateRemaining(voltage_v, throttle_normalized, armed);
+	estimateRemaining(voltage_v, current_a, throttle_normalized, armed);
 	determineWarning();
 	computeScale();
 
@@ -163,13 +158,22 @@ Battery::sumDischarged(hrt_abstime timestamp, float current_a)
 }
 
 void
-Battery::estimateRemaining(float voltage_v, float throttle_normalized, bool armed)
+Battery::estimateRemaining(float voltage_v, float current_a, float throttle_normalized, bool armed)
 {
+	const float bat_r = _param_r_internal.get();
+
+	// remaining charge estimate based on voltage and internal resistance (drop under load)
+	float bat_v_empty_dynamic = _param_v_empty.get();
+
+	if (bat_r >= 0.0f) {
+		bat_v_empty_dynamic -= current_a * bat_r;
+
+	} else {
 		// assume 10% voltage drop of the full drop range with motors idle
 		const float thr = (armed) ? ((fabsf(throttle_normalized) + 0.1f) / 1.1f) : 0.0f;
 
-	// remaining charge estimate based on voltage and internal resistance (drop under load)
-	const float bat_v_empty_dynamic = _param_v_empty.get() - (_param_v_load_drop.get() * thr);
+		bat_v_empty_dynamic -= _param_v_load_drop.get() * thr;
+	}
 
 	// the range from full to empty is the same for batteries under load and without load,
 	// since the voltage drop applies to both the full and empty state
@@ -226,7 +230,12 @@ Battery::determineWarning()
 void
 Battery::computeScale()
 {
-	_scale = (_param_v_full.get() * _param_n_cells.get()) / _voltage_filtered_v;
+	const float voltage_range = (_param_v_full.get() - _param_v_empty.get());
+
+	// reusing capacity calculation to get single cell voltage before drop
+	const float bat_v = _param_v_empty.get() + (voltage_range * _remaining_voltage);
+
+	_scale = _param_v_full.get() / bat_v;
 
 	if (_scale > 1.3f) { // Allow at most 30% compensation
 		_scale = 1.3f;

src/modules/systemlib/battery.h

@@ -94,7 +94,7 @@ private:
 	void filterVoltage(float voltage_v);
 	void filterCurrent(float current_a);
 	void sumDischarged(hrt_abstime timestamp, float current_a);
-	void estimateRemaining(float voltage_v, float throttle_normalized, bool armed);
+	void estimateRemaining(float voltage_v, float current_a, float throttle_normalized, bool armed);
 	void determineWarning();
 	void computeScale();
 

src/modules/systemlib/battery_params.c

@@ -107,7 +107,8 @@ PARAM_DEFINE_FLOAT(BAT_CRIT_THR, 0.07f);
  * This implicitely defines the internal resistance
  * to maximum current ratio and assumes linearity.
  * A good value to use is the difference between the
- * 5C and 20-25C load.
+ * 5C and 20-25C load. Not used if BAT_R_INTERNAL is
+ * set.
  *
  * @group Battery Calibration
  * @unit V
@@ -118,6 +119,19 @@ PARAM_DEFINE_FLOAT(BAT_CRIT_THR, 0.07f);
  */
 PARAM_DEFINE_FLOAT(BAT_V_LOAD_DROP, 0.3f);
 
+/**
+ * Explicitly defines the per cell internal resistance
+ *
+ * If non-negative, then this will be used in place of
+ * BAT_V_LOAD_DROP for all calculations.
+ *
+ * @group Battery Calibration
+ * @unit Ohms
+ * @min -1.0
+ * @max 0.2
+ */
+PARAM_DEFINE_FLOAT(BAT_R_INTERNAL, -1.0f);
+
 /**
  * Number of cells.
  *