From c4638c3da45d8d10514ef6a321774b55a9af098d Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lorenz@px4.io>
Date: Sat, 19 Aug 2017 23:20:07 +0200
Subject: [PATCH] Simulator: Make battery discharging configurable.

---
 src/modules/simulator/simulator.cpp         | 17 +++++++++++++++++
 src/modules/simulator/simulator.h           | 20 +++++++++++++++-----
 src/modules/simulator/simulator_mavlink.cpp |  3 ++-
 src/modules/simulator/simulator_params.c    | 11 +++++++++++
 4 files changed, 45 insertions(+), 6 deletions(-)

diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp
index ed51648bd510..9beea9e2ffaf 100644
--- a/src/modules/simulator/simulator.cpp
+++ b/src/modules/simulator/simulator.cpp
@@ -122,6 +122,23 @@ void Simulator::write_airspeed_data(void *buf)
 	_airspeed.writeData(buf);
 }
 
+void Simulator::parameters_update(bool force)
+{
+	bool updated;
+	struct parameter_update_s param_upd;
+
+	orb_check(_param_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(parameter_update), _param_sub, &param_upd);
+	}
+
+	if (updated || force) {
+		// update C++ param system
+		updateParams();
+	}
+}
+
 int Simulator::start(int argc, char *argv[])
 {
 	int ret = 0;
diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h
index d9db8d8139c9..c555308b9aa1 100644
--- a/src/modules/simulator/simulator.h
+++ b/src/modules/simulator/simulator.h
@@ -54,6 +54,8 @@
 #include <drivers/drv_rc_input.h>
 #include <systemlib/perf_counter.h>
 #include <systemlib/battery.h>
+#include <controllib/blocks.hpp>
+#include <controllib/block/BlockParam.hpp>
 #include <uORB/uORB.h>
 #include <uORB/topics/optical_flow.h>
 #include <uORB/topics/distance_sensor.h>
@@ -185,7 +187,7 @@ template <typename RType> class Report
 
 };
 
-class Simulator
+class Simulator : public control::SuperBlock
 {
 public:
 	static Simulator *getInstance();
@@ -223,7 +225,7 @@ class Simulator
 	bool isInitialized() { return _initialized; }
 
 private:
-	Simulator() :
+	Simulator() : SuperBlock(nullptr, "SIM"),
 		_accel(1),
 		_mpu(1),
 		_baro(1),
@@ -247,6 +249,7 @@ class Simulator
 		_vision_attitude_pub(nullptr),
 		_dist_pub(nullptr),
 		_battery_pub(nullptr),
+		_param_sub(-1),
 		_initialized(false),
 		_realtime_factor(1.0),
 		_system_type(0)
@@ -270,7 +273,8 @@ class Simulator
 		_actuators{},
 		_attitude{},
 		_manual{},
-		_vehicle_status{}
+		_vehicle_status{},
+		_battery_drain_interval_s(this, "BAT_DRAIN")
 #endif
 	{
 		// We need to know the type for the correct mapping from
@@ -278,8 +282,7 @@ class Simulator
 		param_t param_system_type = param_find("MAV_TYPE");
 		param_get(param_system_type, &_system_type);
 
-		for (unsigned i = 0; i < (sizeof(_actuator_outputs_sub) / sizeof(_actuator_outputs_sub[0])); i++)
-		{
+		for (unsigned i = 0; i < (sizeof(_actuator_outputs_sub) / sizeof(_actuator_outputs_sub[0])); i++) {
 			_actuator_outputs_sub[i] = -1;
 		}
 
@@ -287,6 +290,8 @@ class Simulator
 		gps_data.eph = UINT16_MAX;
 		gps_data.epv = UINT16_MAX;
 		_gps.writeData(&gps_data);
+
+		_param_sub = orb_subscribe(ORB_ID(parameter_update));
 	}
 	~Simulator()
 	{
@@ -329,6 +334,8 @@ class Simulator
 	orb_advert_t _dist_pub;
 	orb_advert_t _battery_pub;
 
+	int				_param_sub;
+
 	bool _initialized;
 	double _realtime_factor;		///< How fast the simulation runs in comparison to real system time
 	hrt_abstime _last_sim_timestamp;
@@ -374,6 +381,8 @@ class Simulator
 	struct manual_control_setpoint_s _manual;
 	struct vehicle_status_s _vehicle_status;
 
+	control::BlockParamFloat _battery_drain_interval_s; ///< battery drain interval
+
 	void poll_topics();
 	void handle_message(mavlink_message_t *msg, bool publish);
 	void send_controls();
@@ -383,6 +392,7 @@ class Simulator
 	void send_mavlink_message(const uint8_t msgid, const void *msg, uint8_t component_ID);
 	void update_sensors(mavlink_hil_sensor_t *imu);
 	void update_gps(mavlink_hil_gps_t *gps_sim);
+	void parameters_update(bool force);
 	static void *sending_trampoline(void *);
 	void send();
 #endif
diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp
index 119c28d4d3e7..98c520258c06 100644
--- a/src/modules/simulator/simulator_mavlink.cpp
+++ b/src/modules/simulator/simulator_mavlink.cpp
@@ -356,7 +356,7 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish)
 			update_sensors(&imu);
 
 			// battery simulation
-			const float discharge_interval_us = 60 * 1000 * 1000;
+			const float discharge_interval_us = _battery_drain_interval_s.get() * 1000 * 1000;
 
 			bool armed = (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
 
@@ -625,6 +625,7 @@ void Simulator::send()
 
 		if (fds[0].revents & POLLIN) {
 			// got new data to read, update all topics
+			parameters_update(false);
 			poll_topics();
 			send_controls();
 		}
diff --git a/src/modules/simulator/simulator_params.c b/src/modules/simulator/simulator_params.c
index 818233fe3baa..23ee0280e641 100644
--- a/src/modules/simulator/simulator_params.c
+++ b/src/modules/simulator/simulator_params.c
@@ -46,3 +46,14 @@
  * @group SITL
  */
 PARAM_DEFINE_INT32(SITL_UDP_PRT, 14560);
+
+/**
+ * Simulator Battery drain interval
+ *
+ * @min 1
+ * @max 86400
+ * @increment 1
+ *
+ * @group SITL
+ */
+PARAM_DEFINE_FLOAT(SIM_BAT_DRAIN, 60);