simulator: break out standalone battery simulator module

- this is a small step towards unifying SITL & HITL

ROMFS/px4fmu_common/init.d-posix/rcS

@@ -245,6 +245,7 @@ if ! replay tryapplyparams
 then
 	simulator start -c $simulator_tcp_port
 fi
+battery_simulator start
 tone_alarm start
 rc_update start
 sensors start

posix-configs/SITL/init/test/cmd_template.in

@@ -11,6 +11,7 @@ param set SYS_RESTART_TYPE 0
 
 dataman start
 
+battery_simulator start
 simulator start
 tone_alarm start
 pwm_out_sim start

posix-configs/SITL/init/test/test_mavlink

@@ -11,6 +11,7 @@ param set SYS_RESTART_TYPE 0
 
 dataman start
 
+battery_simulator start
 simulator start
 tone_alarm start
 pwm_out_sim start

posix-configs/SITL/init/test/test_shutdown

@@ -15,6 +15,7 @@ param set SYS_RESTART_TYPE 0
 
 dataman start
 
+battery_simulator start
 simulator start
 tone_alarm start
 pwm_out_sim start

posix-configs/SITL/init/test/test_template.in

@@ -11,6 +11,7 @@ param set SYS_RESTART_TYPE 0
 
 dataman start
 
+battery_simulator start
 simulator start
 tone_alarm start
 pwm_out_sim start

posix-configs/eagle/hil/mainapphil.config

@@ -9,4 +9,5 @@ sleep 1
 mavlink stream -u 14556 -s HIGHRES_IMU -r 50
 mavlink stream -u 14556 -s ATTITUDE -r 50
 mavlink boot_complete
+battery_simulator start
 simulator start

src/modules/simulator/CMakeLists.txt

@@ -66,7 +66,6 @@ px4_add_module(
 		${SIMULATOR_SRCS}
 	DEPENDS
 		git_mavlink_v2
-		battery
 		conversion
 		git_ecl
 		ecl_geo
@@ -76,3 +75,5 @@ px4_add_module(
 		drivers_magnetometer
 	)
 target_include_directories(modules__simulator INTERFACE ${PX4_SOURCE_DIR}/mavlink/include/mavlink)
+
+add_subdirectory(battery_simulator)

src/modules/simulator/battery_simulator/BatterySimulator.cpp

@@ -0,0 +1,158 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "BatterySimulator.hpp"
+
+BatterySimulator::BatterySimulator() :
+	ModuleParams(nullptr),
+	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default)
+{
+}
+
+BatterySimulator::~BatterySimulator()
+{
+	perf_free(_loop_perf);
+}
+
+bool BatterySimulator::init()
+{
+	ScheduleOnInterval(SimulatorBattery::SIMLATOR_BATTERY_SAMPLE_INTERVAL_US);
+	return true;
+}
+
+void BatterySimulator::Run()
+{
+	if (should_exit()) {
+		ScheduleClear();
+		exit_and_cleanup();
+		return;
+	}
+
+	perf_begin(_loop_perf);
+
+	// Check if parameters have changed
+	if (_parameter_update_sub.updated()) {
+		// clear update
+		parameter_update_s param_update;
+		_parameter_update_sub.copy(&param_update);
+
+		updateParams();
+	}
+
+	if (_vehicle_status_sub.updated()) {
+		vehicle_status_s vehicle_status;
+
+		if (_vehicle_status_sub.copy(&vehicle_status)) {
+			_armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
+		}
+	}
+
+	const hrt_abstime now_us = hrt_absolute_time();
+
+	const float discharge_interval_us = _param_sim_bat_drain.get() * 1000 * 1000;
+
+	if (_armed) {
+		if (_last_integration_us != 0) {
+			_battery_percentage -= (now_us - _last_integration_us) / discharge_interval_us;
+		}
+
+		_last_integration_us = now_us;
+
+	} else {
+		_last_integration_us = 0;
+	}
+
+	float ibatt = -1.0f; // no current sensor in simulation
+
+	_battery_percentage = math::max(_battery_percentage, _param_bat_min_pct.get() / 100.f);
+	float vbatt = math::gradual(_battery_percentage, 0.f, 1.f, _battery.empty_cell_voltage(), _battery.full_cell_voltage());
+	vbatt *= _battery.cell_count();
+
+	const float throttle = 0.0f; // simulate no throttle compensation to make the estimate predictable
+	_battery.updateBatteryStatus(now_us, vbatt, ibatt, true, battery_status_s::BATTERY_SOURCE_POWER_MODULE, 0, throttle);
+
+	perf_end(_loop_perf);
+}
+
+int BatterySimulator::task_spawn(int argc, char *argv[])
+{
+	BatterySimulator *instance = new BatterySimulator();
+
+	if (instance) {
+		_object.store(instance);
+		_task_id = task_id_is_work_queue;
+
+		if (instance->init()) {
+			return PX4_OK;
+		}
+
+	} else {
+		PX4_ERR("alloc failed");
+	}
+
+	delete instance;
+	_object.store(nullptr);
+	_task_id = -1;
+
+	return PX4_ERROR;
+}
+
+int BatterySimulator::custom_command(int argc, char *argv[])
+{
+	return print_usage("unknown command");
+}
+
+int BatterySimulator::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_WARN("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+
+
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("battery_simulator", "system");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+
+	return 0;
+}
+
+extern "C" __EXPORT int battery_simulator_main(int argc, char *argv[])
+{
+	return BatterySimulator::main(argc, argv);
+}

src/modules/simulator/battery_simulator/BatterySimulator.hpp

@@ -0,0 +1,109 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <lib/battery/battery.h>
+#include <lib/perf/perf_counter.h>
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/battery_status.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_status.h>
+
+using namespace time_literals;
+
+class BatterySimulator : public ModuleBase<BatterySimulator>, public ModuleParams, public px4::ScheduledWorkItem
+{
+public:
+	BatterySimulator();
+	~BatterySimulator() override;
+
+	/** @see ModuleBase */
+	static int task_spawn(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int custom_command(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);
+
+	bool init();
+
+private:
+	void Run() override;
+
+	uORB::Publication<battery_status_s> _battery_pub{ORB_ID(battery_status)};
+
+	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
+
+	class SimulatorBattery : public Battery
+	{
+	public:
+		static constexpr uint32_t SIMLATOR_BATTERY_SAMPLE_FREQUENCY_HZ = 100; // Hz
+		static constexpr uint32_t SIMLATOR_BATTERY_SAMPLE_INTERVAL_US = 1_s / SIMLATOR_BATTERY_SAMPLE_FREQUENCY_HZ;
+
+		SimulatorBattery() : Battery(1, nullptr, SIMLATOR_BATTERY_SAMPLE_INTERVAL_US) {}
+
+		virtual void updateParams() override
+		{
+			Battery::updateParams();
+			_params.v_empty = 3.5f;
+			_params.v_charged = 4.05f;
+			_params.n_cells = 4;
+			_params.capacity = 10.0f;
+			_params.v_load_drop = 0.0f;
+			_params.r_internal = 0.0f;
+			_params.low_thr = 0.15f;
+			_params.crit_thr = 0.07f;
+			_params.emergen_thr = 0.05f;
+			_params.source = 0;
+		}
+	} _battery;
+
+	uint64_t _last_integration_us{0};
+	float _battery_percentage{1.f};
+	bool _armed{false};
+
+	perf_counter_t	_loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::SIM_BAT_DRAIN>) _param_sim_bat_drain, ///< battery drain interval
+		(ParamFloat<px4::params::SIM_BAT_MIN_PCT>) _param_bat_min_pct //< minimum battery percentage
+	)
+};

src/modules/simulator/battery_simulator/CMakeLists.txt

@@ -0,0 +1,45 @@
+############################################################################
+#
+#   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_module(
+	MODULE modules__simulator__battery_simulator
+	MAIN battery_simulator
+	COMPILE_FLAGS
+	SRCS
+		BatterySimulator.cpp
+		BatterySimulator.hpp
+	DEPENDS
+		battery
+		mathlib
+		px4_work_queue
+	)

src/modules/simulator/battery_simulator/battery_simulator_params.c

@@ -0,0 +1,58 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * Simulator Battery drain interval
+ *
+ * @min 1
+ * @max 86400
+ * @increment 1
+ * @unit s
+ *
+ * @group SITL
+ */
+PARAM_DEFINE_FLOAT(SIM_BAT_DRAIN, 60);
+
+/**
+ * Simulator Battery minimal percentage. Can be used to alter
+ * the battery level during SITL- or HITL-simulation on the fly.
+ * Particularly useful for testing different low-battery behaviour.
+ *
+ * @min 0
+ * @max 100
+ * @increment 0.1
+ * @unit %
+ *
+ * @group SITL
+ */
+PARAM_DEFINE_FLOAT(SIM_BAT_MIN_PCT, 50.0f);

src/modules/simulator/simulator.h

@@ -42,7 +42,6 @@
 
 #pragma once
 
-#include <battery/battery.h>
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_rc_input.h>
 #include <drivers/drv_range_finder.h>
@@ -59,7 +58,6 @@
 #include <uORB/Publication.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/actuator_outputs.h>
-#include <uORB/topics/battery_status.h>
 #include <uORB/topics/differential_pressure.h>
 #include <uORB/topics/distance_sensor.h>
 #include <uORB/topics/ekf2_timestamps.h>
@@ -79,7 +77,6 @@
 
 #include <v2.0/common/mavlink.h>
 #include <v2.0/mavlink_types.h>
-#include <lib/battery/battery.h>
 
 using namespace time_literals;
 
@@ -168,7 +165,6 @@ private:
 	perf_counter_t _perf_sim_interval{perf_alloc(PC_INTERVAL, MODULE_NAME": network interval")};
 
 	// uORB publisher handlers
-	uORB::Publication<battery_status_s>		_battery_pub{ORB_ID(battery_status)};
 	uORB::Publication<differential_pressure_s>	_differential_pressure_pub{ORB_ID(differential_pressure)};
 	uORB::PublicationMulti<optical_flow_s>		_flow_pub{ORB_ID(optical_flow)};
 	uORB::Publication<irlock_report_s>		_irlock_report_pub{ORB_ID(irlock_report)};
@@ -188,31 +184,7 @@ private:
 
 	hrt_abstime _last_sim_timestamp{0};
 	hrt_abstime _last_sitl_timestamp{0};
-	hrt_abstime _last_battery_timestamp{0};
 
-	class SimulatorBattery : public Battery
-	{
-	public:
-		static constexpr uint32_t SIMLATOR_BATTERY_SAMPLE_FREQUENCY_HZ = 100; // Hz
-		static constexpr uint32_t SIMLATOR_BATTERY_SAMPLE_INTERVAL_US = 1_s / SIMLATOR_BATTERY_SAMPLE_FREQUENCY_HZ;
-
-		SimulatorBattery() : Battery(1, nullptr, SIMLATOR_BATTERY_SAMPLE_INTERVAL_US) {}
-
-		virtual void updateParams() override
-		{
-			Battery::updateParams();
-			_params.v_empty = 3.5f;
-			_params.v_charged = 4.05f;
-			_params.n_cells = 4;
-			_params.capacity = 10.0f;
-			_params.v_load_drop = 0.0f;
-			_params.r_internal = 0.0f;
-			_params.low_thr = 0.15f;
-			_params.crit_thr = 0.07f;
-			_params.emergen_thr = 0.05f;
-			_params.source = 0;
-		}
-	} _battery;
 
 	void run();
 	void handle_message(const mavlink_message_t *msg);
@@ -276,8 +248,6 @@ private:
 #endif
 
 	DEFINE_PARAMETERS(
-		(ParamFloat<px4::params::SIM_BAT_DRAIN>) _param_sim_bat_drain, ///< battery drain interval
-		(ParamFloat<px4::params::SIM_BAT_MIN_PCT>) _param_bat_min_pct, //< minimum battery percentage
 		(ParamBool<px4::params::SIM_GPS_BLOCK>) _param_sim_gps_block,
 		(ParamBool<px4::params::SIM_ACCEL_BLOCK>) _param_sim_accel_block,
 		(ParamBool<px4::params::SIM_GYRO_BLOCK>) _param_sim_gyro_block,

src/modules/simulator/simulator_mavlink.cpp

@@ -355,40 +355,6 @@ void Simulator::handle_message_hil_sensor(const mavlink_message_t *msg)
 
 	update_sensors(now_us, imu);
 
-	static float battery_percentage = 1.0f;
-	static uint64_t last_integration_us = 0;
-
-	// battery simulation (limit update to 100Hz)
-	if (hrt_elapsed_time(&_last_battery_timestamp) >= SimulatorBattery::SIMLATOR_BATTERY_SAMPLE_INTERVAL_US) {
-
-		const float discharge_interval_us = _param_sim_bat_drain.get() * 1000 * 1000;
-
-		bool armed = (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
-
-		if (armed) {
-			if (last_integration_us != 0) {
-				battery_percentage -= (now_us - last_integration_us) / discharge_interval_us;
-			}
-
-			last_integration_us = now_us;
-
-		} else {
-			last_integration_us = 0;
-		}
-
-		float ibatt = -1.0f; // no current sensor in simulation
-
-		battery_percentage = math::max(battery_percentage, _param_bat_min_pct.get() / 100.f);
-		float vbatt = math::gradual(battery_percentage, 0.f, 1.f, _battery.empty_cell_voltage(), _battery.full_cell_voltage());
-		vbatt *= _battery.cell_count();
-
-		const float throttle = 0.0f; // simulate no throttle compensation to make the estimate predictable
-		_battery.updateBatteryStatus(now_us, vbatt, ibatt, true, battery_status_s::BATTERY_SOURCE_POWER_MODULE,
-					     0, throttle);
-
-		_last_battery_timestamp = now_us;
-	}
-
 #if defined(ENABLE_LOCKSTEP_SCHEDULER)
 
 	if (!_has_initialized.load()) {

src/modules/simulator/simulator_params.c

@@ -39,32 +39,6 @@
  * @author Mohamed Abdelkader <mohamedashraf123@gmail.com>
  */
 
-/**
- * Simulator Battery drain interval
- *
- * @min 1
- * @max 86400
- * @increment 1
- * @unit s
- *
- * @group SITL
- */
-PARAM_DEFINE_FLOAT(SIM_BAT_DRAIN, 60);
-
-/**
- * Simulator Battery minimal percentage. Can be used to alter
- * the battery level during SITL- or HITL-simulation on the fly.
- * Particularly useful for testing different low-battery behaviour.
- *
- * @min 0
- * @max 100
- * @increment 0.1
- * @unit %
- *
- * @group SITL
- */
-PARAM_DEFINE_FLOAT(SIM_BAT_MIN_PCT, 50.0f);
-
 /**
  * Simulator block GPS data.
  *