Add low pass filtered current draw

msg/battery_status.msg

@@ -2,6 +2,7 @@ uint64 timestamp		# microseconds since system boot, needed to integrate
 float32 voltage_v		# Battery voltage in volts, 0 if unknown
 float32 voltage_filtered_v	# Battery voltage in volts, filtered, 0 if unknown
 float32 current_a		# Battery current in amperes, -1 if unknown
+float32 current_filtered_a	# Battery current in amperes, 0 if unknown
 float32 discharged_mah		# Discharged amount in mAh, -1 if unknown
 float32 remaining		# From 1 to 0, -1 if unknown
 int32 cell_count		# Number of cells

src/modules/sdlog2/sdlog2.c

@@ -1833,7 +1833,8 @@ int sdlog2_thread_main(int argc, char *argv[])
 				log_msg.msg_type = LOG_BATT_MSG;
 				log_msg.body.log_BATT.voltage = buf.battery.voltage_v;
 				log_msg.body.log_BATT.voltage_filtered = buf.battery.voltage_filtered_v;
-				log_msg.body.log_BATT.current = buf.battery.current_a;
+				log_msg.body.log_BATT.current = buf.battery.current_a;				
+				log_msg.body.log_BATT.current_filtered = buf.battery.current_filtered_a;
 				log_msg.body.log_BATT.discharged = buf.battery.discharged_mah;
 				log_msg.body.log_BATT.remaining = buf.battery.remaining;
 				log_msg.body.log_BATT.warning = buf.battery.warning;
@@ -1959,6 +1960,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 				log_msg.body.log_BATT.voltage = buf.battery.voltage_v;
 				log_msg.body.log_BATT.voltage_filtered = buf.battery.voltage_filtered_v;
 				log_msg.body.log_BATT.current = buf.battery.current_a;
+				log_msg.body.log_BATT.current_filtered = buf.battery.current_filtered_a;
 				log_msg.body.log_BATT.discharged = buf.battery.discharged_mah;
 				LOGBUFFER_WRITE_AND_COUNT(BATT);
 			}

src/modules/sdlog2/sdlog2_messages.h

@@ -292,6 +292,7 @@ struct log_BATT_s {
 	float voltage;
 	float voltage_filtered;
 	float current;
+	float current_filtered;
 	float discharged;
 	float remaining;
 	uint8_t warning;
@@ -655,7 +656,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(GPSP, "BLLffBfbf",		"NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"),
 	LOG_FORMAT(ESC, "HBBBHHffiffH",		"count,nESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
 	LOG_FORMAT(GVSP, "fff",			"VX,VY,VZ"),
-	LOG_FORMAT(BATT, "fffffB",		"V,VFilt,C,Discharged,Remaining,Warning"),
+	LOG_FORMAT(BATT, "ffffffB",		"V,VFilt,C,CFilt,Discharged,Remaining,Warning"),
 	LOG_FORMAT(DIST, "BBBff",			"Id,Type,Orientation,Distance,Covariance"),
 	LOG_FORMAT_S(TEL0, TEL, "BBBBHHBQ",		"RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"),
 	LOG_FORMAT_S(TEL1, TEL, "BBBBHHBQ",		"RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"),

src/modules/systemlib/battery.cpp

@@ -84,6 +84,7 @@ Battery::updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float curre
 	reset(battery_status);
 	battery_status->timestamp = timestamp;
 	filterVoltage(voltage_v);
+	filterCurrent(current_a);
 	sumDischarged(timestamp, current_a);
 	estimateRemaining(voltage_v, throttle_normalized, armed);
 	determineWarning();
@@ -92,6 +93,7 @@ Battery::updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float curre
 		battery_status->voltage_v = voltage_v;
 		battery_status->voltage_filtered_v = _voltage_filtered_v;
 		battery_status->current_a = current_a;
+		battery_status->current_filtered_a = _current_filtered_a;
 		battery_status->discharged_mah = _discharged_mah;
 		battery_status->warning = _warning;
 		battery_status->remaining = _remaining;
@@ -114,6 +116,22 @@ Battery::filterVoltage(float voltage_v)
 	}
 }
 
+void
+Battery::filterCurrent(float current_a)
+{
+	if (_current_filtered_a < 0.0f) {
+		_current_filtered_a = current_a;
+	}
+
+	// ADC poll is at 100Hz, this will perform a low pass over approx 500ms
+	const float filtered_next = _current_filtered_a * 0.98f + current_a * 0.02f;
+
+	if (PX4_ISFINITE(filtered_next)) {
+		_current_filtered_a = filtered_next;
+	}
+}
+
+
 void
 Battery::sumDischarged(hrt_abstime timestamp, float current_a)
 {

src/modules/systemlib/battery.h

@@ -92,6 +92,7 @@ public:
 
 private:
 	void filterVoltage(float voltage_v);
+	void filterCurrent(float voltage_a);
 	void sumDischarged(hrt_abstime timestamp, float current_a);
 	void estimateRemaining(float voltage_v, float throttle_normalized, bool armed);
 	void determineWarning();
@@ -105,6 +106,7 @@ private:
 	control::BlockParamFloat _param_crit_thr;
 
 	float _voltage_filtered_v;
+	float _current_filtered_a;
 	float _discharged_mah;
 	float _remaining_voltage;		///< normalized battery charge level remaining based on voltage
 	float _remaining_capacity;		///< normalized battery charge level remaining based on capacity