From 9a09c5af5c132074de2722b963b7e6f1d679fc67 Mon Sep 17 00:00:00 2001 From: sander Date: Tue, 10 May 2016 10:08:15 +0200 Subject: [PATCH] Add low pass filtered current draw --- msg/battery_status.msg | 1 + src/modules/sdlog2/sdlog2.c | 4 +++- src/modules/sdlog2/sdlog2_messages.h | 3 ++- src/modules/systemlib/battery.cpp | 18 ++++++++++++++++++ src/modules/systemlib/battery.h | 2 ++ 5 files changed, 26 insertions(+), 2 deletions(-) diff --git a/msg/battery_status.msg b/msg/battery_status.msg index ff4485378a..420a76fc8b 100644 --- a/msg/battery_status.msg +++ b/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 diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 5c2959e7c1..92d7da3e71 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/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); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 67e4b5ab97..9a8d958d6c 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/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"), diff --git a/src/modules/systemlib/battery.cpp b/src/modules/systemlib/battery.cpp index 859de86dd4..b971b10d87 100644 --- a/src/modules/systemlib/battery.cpp +++ b/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) { diff --git a/src/modules/systemlib/battery.h b/src/modules/systemlib/battery.h index 2b10c2df03..f1bb12e80f 100644 --- a/src/modules/systemlib/battery.h +++ b/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