From 705979e3c769b0f395d40a0108887b586e2a2921 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Thu, 25 Feb 2016 16:59:58 +0000
Subject: [PATCH] simulator: use new battery systemlib

---
 src/modules/simulator/simulator.h           | 10 ++++----
 src/modules/simulator/simulator_mavlink.cpp | 27 ++++-----------------
 2 files changed, 10 insertions(+), 27 deletions(-)

diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h
index 3f4085dd28..a5afa72c7a 100644
--- a/src/modules/simulator/simulator.h
+++ b/src/modules/simulator/simulator.h
@@ -52,6 +52,7 @@
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_rc_input.h>
 #include <systemlib/perf_counter.h>
+#include <systemlib/battery.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/optical_flow.h>
 #include <v1.0/mavlink_types.h>
@@ -250,9 +251,7 @@ private:
 		_actuators{},
 		_attitude{},
 		_manual{},
-		_vehicle_status{},
-		_battery_last_timestamp(0),
-		_battery_mamphour_total(0.0f)
+		_vehicle_status{}
 #endif
 	{}
 	~Simulator() { _instance = NULL; }
@@ -288,6 +287,9 @@ private:
 
 	bool _initialized;
 
+	// Lib used to do the battery calculations.
+	Battery _battery;
+
 	// class methods
 	int publish_sensor_topics(mavlink_hil_sensor_t *imu);
 	int publish_flow_topic(mavlink_hil_optical_flow_t *flow);
@@ -308,8 +310,6 @@ private:
 	struct vehicle_attitude_s _attitude;
 	struct manual_control_setpoint_s _manual;
 	struct vehicle_status_s _vehicle_status;
-	uint64_t _battery_last_timestamp;
-	float _battery_mamphour_total;
 
 	void poll_topics();
 	void handle_message(mavlink_message_t *msg, bool publish);
diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp
index 3cb189803d..d9a1f2a5c3 100644
--- a/src/modules/simulator/simulator_mavlink.cpp
+++ b/src/modules/simulator/simulator_mavlink.cpp
@@ -262,6 +262,7 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish)
 			/* battery */
 			{
 				hrt_abstime now = hrt_absolute_time();
+
 				const float discharge_interval_us = 60 * 1000 * 1000;
 
 				static hrt_abstime batt_sim_start = now;
@@ -277,33 +278,15 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish)
 					vbatt = cellcount * 3.7f;
 				}
 
-				battery_status_s	battery_status;
-				battery_status.timestamp = hrt_absolute_time();
+				battery_status_s battery_status;
 
-				/* voltage is scaled to mV */
-				battery_status.voltage_v = vbatt;
-				battery_status.voltage_filtered_v = vbatt;
-
-				/*
-				  ibatt contains the raw ADC count, as 12 bit ADC
-				  value, with full range being 3.3v
-				*/
-				battery_status.current_a = ibatt;
-
-				/*
-				  integrate battery over time to get total mAh used
-				*/
-				if (_battery_last_timestamp != 0) {
-					_battery_mamphour_total += battery_status.current_a *
-								   (battery_status.timestamp - _battery_last_timestamp) * 1.0e-3f / 3600;
-				}
-
-				battery_status.discharged_mah = _battery_mamphour_total;
+				// TODO: don't hard-code throttle.
+				const float throttle = 0.5f;
+				_battery.updateBatteryStatus(now, vbatt, ibatt, throttle, &battery_status);
 
 				/* lazily publish the battery voltage */
 				if (_battery_pub != nullptr) {
 					orb_publish(ORB_ID(battery_status), _battery_pub, &battery_status);
-					_battery_last_timestamp = battery_status.timestamp;
 
 				} else {
 					_battery_pub = orb_advertise(ORB_ID(battery_status), &battery_status);