From c5b0650c239f1f106ba8a3d07985b1be2f5a1c45 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 24 Feb 2016 12:31:24 +0100
Subject: [PATCH] Sim: Add batttery sim

---
 src/modules/simulator/simulator.h           | 10 +++-
 src/modules/simulator/simulator_mavlink.cpp | 51 +++++++++++++++++++++
 2 files changed, 60 insertions(+), 1 deletion(-)

diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h
index 5da68d002a..3f4085dd28 100644
--- a/src/modules/simulator/simulator.h
+++ b/src/modules/simulator/simulator.h
@@ -44,6 +44,7 @@
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/battery_status.h>
 #include <drivers/drv_accel.h>
 #include <drivers/drv_gyro.h>
 #include <drivers/drv_baro.h>
@@ -235,6 +236,8 @@ private:
 		_baro_pub(nullptr),
 		_gyro_pub(nullptr),
 		_mag_pub(nullptr),
+		_flow_pub(nullptr),
+		_battery_pub(nullptr),
 		_initialized(false)
 #ifndef __PX4_QURT
 		,
@@ -247,7 +250,9 @@ private:
 		_actuators{},
 		_attitude{},
 		_manual{},
-		_vehicle_status{}
+		_vehicle_status{},
+		_battery_last_timestamp(0),
+		_battery_mamphour_total(0.0f)
 #endif
 	{}
 	~Simulator() { _instance = NULL; }
@@ -279,6 +284,7 @@ private:
 	orb_advert_t _gyro_pub;
 	orb_advert_t _mag_pub;
 	orb_advert_t _flow_pub;
+	orb_advert_t _battery_pub;
 
 	bool _initialized;
 
@@ -302,6 +308,8 @@ 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 560e7d4a0b..2254f53fc2 100644
--- a/src/modules/simulator/simulator_mavlink.cpp
+++ b/src/modules/simulator/simulator_mavlink.cpp
@@ -255,6 +255,57 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish)
 			}
 
 			update_sensors(&imu);
+
+			/* battery */
+			{
+				hrt_abstime now = hrt_absolute_time();
+				const float discharge_interval_us = 60 * 1000 * 1000;
+
+				static hrt_abstime batt_sim_start = now;
+
+				float cellcount = 3.0f;
+
+				float vbatt = 4.2f * cellcount;
+				float ibatt = 20.0f;
+
+				vbatt -= (0.5f * cellcount) * ((now - batt_sim_start) / discharge_interval_us);
+
+				if (vbatt < (cellcount * 1.5f)) {
+					vbatt = cellcount * 1.5f;
+				}
+
+				battery_status_s	battery_status;
+				battery_status.timestamp = hrt_absolute_time();
+
+				/* 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;
+
+				/* 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);
+				}
+			}
 		}
 		break;