simulation organization and cleanup

- new modules/simulation directory to collect all simulators and related modules
 - new Tools/simulation directory to collect and organize scattered simulation submodules, scripts, etc
 - simulation module renamed to simulator_mavlink
 - sih renamed to simulator_sih (not a great name, but I wanted to be clear it was a simulator)
 - ignition_simulator renamed to simulator_ignition_bridge
 - large sitl_target.cmake split by simulation option and in some cases pushed to appropriate modules
 - sitl targets broken down to what's actually available (eg jmavsim only has 1 model and 1 world)
 - new Gazebo consistently referred to as Ignition for now (probably the least confusing thing until we fully drop Gazebo classic support someday)

src/modules/simulation/Kconfig

@@ -0,0 +1,15 @@
+menu "Simulation"
+    menuconfig COMMON_SIMULATION
+        bool "Common simulation modules"
+        default n
+        select MODULES_SIMULATION_BATTERY_SIMULATOR
+        select MODULES_SIMULATION_PWM_OUT_SIM
+        select MODULES_SIMULATION_SENSOR_BARO_SIM
+        select MODULES_SIMULATION_SENSOR_GPS_SIM
+        select MODULES_SIMULATION_SENSOR_MAG_SIM
+        select MODULES_SIMULATION_SIMULATOR_MAVLINK
+        select MODULES_SIMULATION_SIMULATOR_SIH
+        ---help---
+            Enable default set of simulation modules
+    rsource "*/Kconfig"
+endmenu

src/modules/simulation/battery_simulator/BatterySimulator.cpp

@@ -0,0 +1,163 @@
+/****************************************************************************
+ *
+ *   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),
+	_battery(1, this, BATTERY_SIMLATOR_SAMPLE_INTERVAL_US, battery_status_s::BATTERY_SOURCE_POWER_MODULE)
+{
+}
+
+BatterySimulator::~BatterySimulator()
+{
+	perf_free(_loop_perf);
+}
+
+bool BatterySimulator::init()
+{
+	ScheduleOnInterval(BATTERY_SIMLATOR_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 {
+		_battery_percentage = 1.f;
+		_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::interpolate(_battery_percentage, 0.f, 1.f, _battery.empty_cell_voltage(),
+					_battery.full_cell_voltage());
+	vbatt *= _battery.cell_count();
+
+	_battery.setConnected(true);
+	_battery.updateVoltage(vbatt);
+	_battery.updateCurrent(ibatt);
+	_battery.updateAndPublishBatteryStatus(now_us);
+
+	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/simulation/battery_simulator/BatterySimulator.hpp

@@ -0,0 +1,91 @@
+/****************************************************************************
+ *
+ *   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/SubscriptionInterval.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;
+
+	static constexpr uint32_t BATTERY_SIMLATOR_SAMPLE_FREQUENCY_HZ = 100; // Hz
+	static constexpr uint32_t BATTERY_SIMLATOR_SAMPLE_INTERVAL_US = 1_s / BATTERY_SIMLATOR_SAMPLE_FREQUENCY_HZ;
+
+	uORB::Publication<battery_status_s> _battery_pub{ORB_ID(battery_status)};
+
+	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
+
+	Battery _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/simulation/battery_simulator/CMakeLists.txt

@@ -0,0 +1,45 @@
+############################################################################
+#
+#   Copyright (c) 2020-2022 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__simulation__battery_simulator
+	MAIN battery_simulator
+	COMPILE_FLAGS
+	SRCS
+		BatterySimulator.cpp
+		BatterySimulator.hpp
+	DEPENDS
+		battery
+		mathlib
+		px4_work_queue
+	)

src/modules/simulation/battery_simulator/Kconfig

@@ -0,0 +1,5 @@
+menuconfig MODULES_SIMULATION_BATTERY_SIMULATOR
+	bool "battery_simulator"
+	default n
+	---help---
+		Enable support for battery_simulator

src/modules/simulation/battery_simulator/battery_simulator_params.c

@@ -0,0 +1,59 @@
+/****************************************************************************
+ *
+ *   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/simulation/pwm_out_sim/CMakeLists.txt

@@ -0,0 +1,50 @@
+############################################################################
+#
+#   Copyright (c) 2015 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.
+#
+############################################################################
+
+if(${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL} STREQUAL "px4_sitl")
+	set(module_config "module_sim.yaml")
+else()
+	set(module_config "module_hil.yaml")
+endif()
+
+px4_add_module(
+	MODULE modules__simulation__pwm_out_sim
+	MAIN pwm_out_sim
+	SRCS
+		PWMSim.cpp
+	MODULE_CONFIG
+		${module_config}
+	DEPENDS
+		mixer
+		mixer_module
+	)

src/modules/simulation/pwm_out_sim/Kconfig

@@ -0,0 +1,5 @@
+menuconfig MODULES_SIMULATION_PWM_OUT_SIM
+	bool "pwm_out_sim"
+	default n
+	---help---
+		Enable support for pwm_out_sim

src/modules/simulation/pwm_out_sim/PWMSim.cpp

@@ -0,0 +1,330 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2022 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 "PWMSim.hpp"
+
+#include <mathlib/mathlib.h>
+#include <px4_platform_common/getopt.h>
+
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/parameter_update.h>
+
+#include <px4_platform_common/sem.hpp>
+
+PWMSim::PWMSim(bool hil_mode_enabled) :
+	CDev(PWM_OUTPUT0_DEVICE_PATH),
+	OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default)
+{
+	_mixing_output.setAllDisarmedValues(PWM_SIM_DISARMED_MAGIC);
+	_mixing_output.setAllFailsafeValues(PWM_SIM_FAILSAFE_MAGIC);
+	_mixing_output.setAllMinValues(PWM_SIM_PWM_MIN_MAGIC);
+	_mixing_output.setAllMaxValues(PWM_SIM_PWM_MAX_MAGIC);
+
+	_mixing_output.setIgnoreLockdown(hil_mode_enabled);
+
+	CDev::init();
+}
+
+void
+PWMSim::Run()
+{
+	if (should_exit()) {
+		ScheduleClear();
+		_mixing_output.unregister();
+
+		exit_and_cleanup();
+		return;
+	}
+
+	SmartLock lock_guard(_lock);
+
+	_mixing_output.update();
+
+	// check for parameter updates
+	if (_parameter_update_sub.updated()) {
+		parameter_update_s pupdate;
+		_parameter_update_sub.copy(&pupdate);
+		updateParams();
+	}
+
+	// check at end of cycle (updateSubscriptions() can potentially change to a different WorkQueue thread)
+	_mixing_output.updateSubscriptions(true);
+}
+
+bool
+PWMSim::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
+		      unsigned num_control_groups_updated)
+{
+	// Only publish once we receive actuator_controls (important for lock-step to work correctly)
+	if (num_control_groups_updated > 0) {
+		actuator_outputs_s actuator_outputs{};
+		actuator_outputs.noutputs = num_outputs;
+
+		const uint32_t reversible_outputs = _mixing_output.reversibleOutputs();
+
+		for (int i = 0; i < (int)num_outputs; i++) {
+			if (outputs[i] != PWM_SIM_DISARMED_MAGIC) {
+
+				OutputFunction function = _mixing_output.outputFunction(i);
+				bool is_reversible = reversible_outputs & (1u << i);
+				float output = outputs[i];
+
+				if (((int)function >= (int)OutputFunction::Motor1 && (int)function <= (int)OutputFunction::MotorMax)
+				    && !is_reversible) {
+					// Scale non-reversible motors to [0, 1]
+					actuator_outputs.output[i] = (output - PWM_SIM_PWM_MIN_MAGIC) / (PWM_SIM_PWM_MAX_MAGIC - PWM_SIM_PWM_MIN_MAGIC);
+
+				} else {
+					// Scale everything else to [-1, 1]
+					const float pwm_center = (PWM_SIM_PWM_MAX_MAGIC + PWM_SIM_PWM_MIN_MAGIC) / 2.f;
+					const float pwm_delta = (PWM_SIM_PWM_MAX_MAGIC - PWM_SIM_PWM_MIN_MAGIC) / 2.f;
+					actuator_outputs.output[i] = (output - pwm_center) / pwm_delta;
+				}
+			}
+		}
+
+		actuator_outputs.timestamp = hrt_absolute_time();
+		_actuator_outputs_sim_pub.publish(actuator_outputs);
+		return true;
+	}
+
+	return false;
+}
+
+int
+PWMSim::ioctl(device::file_t *filp, int cmd, unsigned long arg)
+{
+	SmartLock lock_guard(_lock);
+
+	int ret = OK;
+
+	switch (cmd) {
+	case PWM_SERVO_ARM:
+		break;
+
+	case PWM_SERVO_DISARM:
+		break;
+
+	case PWM_SERVO_SET_MIN_PWM: {
+			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
+
+			for (unsigned i = 0; i < pwm->channel_count; i++) {
+				if (i < OutputModuleInterface::MAX_ACTUATORS && !_mixing_output.useDynamicMixing()) {
+					_mixing_output.minValue(i) = pwm->values[i];
+				}
+			}
+
+			break;
+		}
+
+	case PWM_SERVO_SET_MAX_PWM: {
+			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
+
+			for (unsigned i = 0; i < pwm->channel_count; i++) {
+				if (i < OutputModuleInterface::MAX_ACTUATORS && !_mixing_output.useDynamicMixing()) {
+					_mixing_output.maxValue(i) = pwm->values[i];
+				}
+			}
+
+			break;
+		}
+
+	case PWM_SERVO_SET_UPDATE_RATE:
+		// PWMSim does not limit the update rate
+		break;
+
+	case PWM_SERVO_SET_SELECT_UPDATE_RATE:
+		break;
+
+	case PWM_SERVO_GET_DEFAULT_UPDATE_RATE:
+		*(uint32_t *)arg = 9999;
+		break;
+
+	case PWM_SERVO_GET_UPDATE_RATE:
+		*(uint32_t *)arg = 9999;
+		break;
+
+	case PWM_SERVO_GET_SELECT_UPDATE_RATE:
+		*(uint32_t *)arg = 0;
+		break;
+
+	case PWM_SERVO_GET_FAILSAFE_PWM: {
+			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
+
+			for (unsigned i = 0; i < OutputModuleInterface::MAX_ACTUATORS; i++) {
+				pwm->values[i] = _mixing_output.failsafeValue(i);
+			}
+
+			pwm->channel_count = OutputModuleInterface::MAX_ACTUATORS;
+			break;
+		}
+
+	case PWM_SERVO_GET_DISARMED_PWM: {
+			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
+
+			for (unsigned i = 0; i < OutputModuleInterface::MAX_ACTUATORS; i++) {
+				pwm->values[i] = _mixing_output.disarmedValue(i);
+			}
+
+			pwm->channel_count = OutputModuleInterface::MAX_ACTUATORS;
+			break;
+		}
+
+	case PWM_SERVO_GET_MIN_PWM: {
+			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
+
+			for (unsigned i = 0; i < OutputModuleInterface::MAX_ACTUATORS; i++) {
+				pwm->values[i] = _mixing_output.minValue(i);
+			}
+
+			pwm->channel_count = OutputModuleInterface::MAX_ACTUATORS;
+			break;
+		}
+
+	case PWM_SERVO_GET_MAX_PWM: {
+			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
+
+			for (unsigned i = 0; i < OutputModuleInterface::MAX_ACTUATORS; i++) {
+				pwm->values[i] = _mixing_output.maxValue(i);
+			}
+
+			pwm->channel_count = OutputModuleInterface::MAX_ACTUATORS;
+			break;
+		}
+
+	case PWM_SERVO_GET_RATEGROUP(0) ... PWM_SERVO_GET_RATEGROUP(PWM_OUTPUT_MAX_CHANNELS - 1): {
+			// no restrictions on output grouping
+			unsigned channel = cmd - PWM_SERVO_GET_RATEGROUP(0);
+
+			*(uint32_t *)arg = (1 << channel);
+			break;
+		}
+
+	case PWM_SERVO_GET_COUNT:
+		*(unsigned *)arg = OutputModuleInterface::MAX_ACTUATORS;
+		break;
+
+	case MIXERIOCRESET:
+		_mixing_output.resetMixer();
+		break;
+
+	case MIXERIOCLOADBUF: {
+			const char *buf = (const char *)arg;
+			unsigned buflen = strlen(buf);
+			ret = _mixing_output.loadMixer(buf, buflen);
+			break;
+		}
+
+	default:
+		ret = -ENOTTY;
+		break;
+	}
+
+	return ret;
+}
+
+int
+PWMSim::task_spawn(int argc, char *argv[])
+{
+	bool hil_mode = false;
+
+	int myoptind = 1;
+	int ch;
+	const char *myoptarg = nullptr;
+
+	while ((ch = px4_getopt(argc, argv, "m:", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+		case 'm':
+			hil_mode = strcmp(myoptarg, "hil") == 0;
+			break;
+
+		default:
+			return print_usage("unrecognized flag");
+		}
+	}
+
+	PWMSim *instance = new PWMSim(hil_mode);
+
+	if (!instance) {
+		PX4_ERR("alloc failed");
+		return -1;
+	}
+
+	_object.store(instance);
+	_task_id = task_id_is_work_queue;
+	instance->ScheduleNow();
+	return 0;
+}
+
+int PWMSim::custom_command(int argc, char *argv[])
+{
+	return print_usage("unknown command");
+}
+
+int PWMSim::print_status()
+{
+	_mixing_output.printStatus();
+	return 0;
+}
+
+int PWMSim::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_WARN("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+Driver for simulated PWM outputs.
+
+Its only function is to take `actuator_control` uORB messages,
+mix them with any loaded mixer and output the result to the
+`actuator_output` uORB topic.
+
+It is used in SITL and HITL.
+
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("pwm_out_sim", "driver");
+	PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the module");
+	PRINT_MODULE_USAGE_PARAM_STRING('m', "sim", "hil|sim", "Mode", true);
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+
+	return 0;
+}
+
+extern "C" __EXPORT int pwm_out_sim_main(int argc, char *argv[])
+{
+	return PWMSim::main(argc, argv);
+}

src/modules/simulation/pwm_out_sim/PWMSim.hpp

@@ -0,0 +1,94 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2019 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 <drivers/device/device.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_mixer.h>
+#include <drivers/drv_pwm_output.h>
+#include <lib/mixer_module/mixer_module.hpp>
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/tasks.h>
+#include <px4_platform_common/time.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/actuator_outputs.h>
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+
+#if defined(CONFIG_ARCH_BOARD_PX4_SITL)
+#define PARAM_PREFIX "PWM_MAIN"
+#else
+#define PARAM_PREFIX "HIL_ACT"
+#endif
+
+
+using namespace time_literals;
+
+class PWMSim : public cdev::CDev, public ModuleBase<PWMSim>, public OutputModuleInterface
+{
+public:
+	PWMSim(bool hil_mode_enabled);
+
+	/** @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);
+
+	/** @see ModuleBase::print_status() */
+	int print_status() override;
+
+	int ioctl(device::file_t *filp, int cmd, unsigned long arg) override;
+
+	bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
+			   unsigned num_outputs, unsigned num_control_groups_updated) override;
+
+private:
+
+	void Run() override;
+
+	static constexpr uint16_t PWM_SIM_DISARMED_MAGIC = 900;
+	static constexpr uint16_t PWM_SIM_FAILSAFE_MAGIC = 600;
+	static constexpr uint16_t PWM_SIM_PWM_MIN_MAGIC = 1000;
+	static constexpr uint16_t PWM_SIM_PWM_MAX_MAGIC = 2000;
+
+	MixingOutput _mixing_output{PARAM_PREFIX, MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
+	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
+
+	uORB::Publication<actuator_outputs_s> _actuator_outputs_sim_pub{ORB_ID(actuator_outputs_sim)};
+};

src/modules/simulation/pwm_out_sim/module_hil.yaml

@@ -0,0 +1,8 @@
+
+module_name: HIL
+actuator_output:
+  show_subgroups_if: 'SYS_HITL>0'
+  output_groups:
+    - param_prefix: HIL_ACT
+      channel_label: Channel
+      num_channels: 16

src/modules/simulation/pwm_out_sim/module_sim.yaml

@@ -0,0 +1,7 @@
+
+module_name: SIM
+actuator_output:
+  output_groups:
+    - param_prefix: PWM_MAIN
+      channel_label: Channel
+      num_channels: 16

src/modules/simulation/sensor_baro_sim/CMakeLists.txt

@@ -0,0 +1,44 @@
+############################################################################
+#
+#   Copyright (c) 2021-2022 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__simulation__sensor_baro_sim
+	MAIN sensor_baro_sim
+	COMPILE_FLAGS
+	SRCS
+		SensorBaroSim.cpp
+		SensorBaroSim.hpp
+	DEPENDS
+		geo
+		px4_work_queue
+	)

src/modules/simulation/sensor_baro_sim/Kconfig

@@ -0,0 +1,5 @@
+menuconfig MODULES_SIMULATION_SENSOR_BARO_SIM
+	bool "sensor_baro_sim"
+	default n
+	---help---
+		Enable support for sensor_baro_sim

src/modules/simulation/sensor_baro_sim/SensorBaroSim.cpp

@@ -0,0 +1,232 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 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 "SensorBaroSim.hpp"
+
+#include <drivers/drv_sensor.h>
+
+using namespace matrix;
+
+SensorBaroSim::SensorBaroSim() :
+	ModuleParams(nullptr),
+	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default)
+{
+	srand(1234); // initialize the random seed once before calling generate_wgn()
+}
+
+SensorBaroSim::~SensorBaroSim()
+{
+	perf_free(_loop_perf);
+}
+
+bool SensorBaroSim::init()
+{
+	ScheduleOnInterval(50_ms); // 20 Hz
+	return true;
+}
+
+float SensorBaroSim::generate_wgn()
+{
+	// generate white Gaussian noise sample with std=1
+
+	// algorithm 1:
+	// float temp=((float)(rand()+1))/(((float)RAND_MAX+1.0f));
+	// return sqrtf(-2.0f*logf(temp))*cosf(2.0f*M_PI_F*rand()/RAND_MAX);
+	// algorithm 2: from BlockRandGauss.hpp
+	static float V1, V2, S;
+	static bool phase = true;
+	float X;
+
+	if (phase) {
+		do {
+			float U1 = (float)rand() / (float)RAND_MAX;
+			float U2 = (float)rand() / (float)RAND_MAX;
+			V1 = 2.0f * U1 - 1.0f;
+			V2 = 2.0f * U2 - 1.0f;
+			S = V1 * V1 + V2 * V2;
+		} while (S >= 1.0f || fabsf(S) < 1e-8f);
+
+		X = V1 * float(sqrtf(-2.0f * float(logf(S)) / S));
+
+	} else {
+		X = V2 * float(sqrtf(-2.0f * float(logf(S)) / S));
+	}
+
+	phase = !phase;
+	return X;
+}
+
+void SensorBaroSim::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_global_position_sub.updated()) {
+		vehicle_global_position_s gpos;
+
+		if (_vehicle_global_position_sub.copy(&gpos)) {
+
+			const float dt = math::constrain((gpos.timestamp - _last_update_time) * 1e-6f, 0.001f, 0.1f);
+
+			const float alt_msl = gpos.alt;
+
+			// calculate abs_pressure using an ISA model for the tropsphere (valid up to 11km above MSL)
+			const float lapse_rate = 0.0065f; // reduction in temperature with altitude (Kelvin/m)
+			const float temperature_msl = 288.0f; // temperature at MSL (Kelvin)
+
+			const float temperature_local = temperature_msl - lapse_rate * alt_msl;
+			const float pressure_ratio = powf(temperature_msl / temperature_local, 5.256f);
+			const float pressure_msl = 101325.0f; // pressure at MSL
+			const float absolute_pressure = pressure_msl / pressure_ratio;
+
+			// generate Gaussian noise sequence using polar form of Box-Muller transformation
+			double y1;
+			{
+				double x1;
+				double x2;
+				double w;
+
+				if (!_baro_rnd_use_last) {
+					do {
+						x1 = 2. * (double)generate_wgn() - 1.;
+						x2 = 2. * (double)generate_wgn() - 1.;
+						w = x1 * x1 + x2 * x2;
+					} while (w >= 1.0);
+
+					w = sqrt((-2.0 * log(w)) / w);
+					// calculate two values - the second value can be used next time because it is uncorrelated
+					y1 = x1 * w;
+					_baro_rnd_y2 = x2 * w;
+					_baro_rnd_use_last = true;
+
+				} else {
+					// no need to repeat the calculation - use the second value from last update
+					y1 = _baro_rnd_y2;
+					_baro_rnd_use_last = false;
+				}
+			}
+
+			// Apply noise and drift
+			const float abs_pressure_noise = 1.f * (float)y1;  // 1 Pa RMS noise
+			_baro_drift_pa += _baro_drift_pa_per_sec * dt;
+			const float absolute_pressure_noisy = absolute_pressure + abs_pressure_noise + _baro_drift_pa;
+
+			// convert to hPa
+			float pressure = absolute_pressure_noisy + _sim_baro_off_p.get();
+
+			// calculate temperature in Celsius
+			float temperature = temperature_local - 273.0f + _sim_baro_off_t.get();
+
+			// publish
+			sensor_baro_s sensor_baro{};
+			sensor_baro.timestamp_sample = gpos.timestamp;
+			sensor_baro.device_id = 6620172; // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION
+			sensor_baro.pressure = pressure;
+			sensor_baro.temperature = temperature;
+			sensor_baro.timestamp = hrt_absolute_time();
+			_sensor_baro_pub.publish(sensor_baro);
+
+
+			_last_update_time = gpos.timestamp;
+		}
+	}
+
+	perf_end(_loop_perf);
+}
+
+int SensorBaroSim::task_spawn(int argc, char *argv[])
+{
+	SensorBaroSim *instance = new SensorBaroSim();
+
+	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 SensorBaroSim::custom_command(int argc, char *argv[])
+{
+	return print_usage("unknown command");
+}
+
+int SensorBaroSim::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("sensor_baro_sim", "system");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+
+	return 0;
+}
+
+extern "C" __EXPORT int sensor_baro_sim_main(int argc, char *argv[])
+{
+	return SensorBaroSim::main(argc, argv);
+}

src/modules/simulation/sensor_baro_sim/SensorBaroSim.hpp

@@ -0,0 +1,92 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 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/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/PublicationMulti.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionInterval.hpp>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/sensor_baro.h>
+#include <uORB/topics/vehicle_global_position.h>
+
+using namespace time_literals;
+
+class SensorBaroSim : public ModuleBase<SensorBaroSim>, public ModuleParams, public px4::ScheduledWorkItem
+{
+public:
+	SensorBaroSim();
+	~SensorBaroSim() 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;
+
+	// generate white Gaussian noise sample with std=1
+	static float generate_wgn();
+
+	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
+	uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position_groundtruth)};
+
+	bool _baro_rnd_use_last{false};
+	double _baro_rnd_y2{0.0};
+	float _baro_drift_pa_per_sec{0.0};
+	float _baro_drift_pa{0.0};
+
+	hrt_abstime _last_update_time{0};
+
+	uORB::PublicationMulti<sensor_baro_s> _sensor_baro_pub{ORB_ID(sensor_baro)};
+
+	perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::SIM_BARO_OFF_P>) _sim_baro_off_p,
+		(ParamFloat<px4::params::SIM_BARO_OFF_T>) _sim_baro_off_t
+	)
+};

src/modules/simulation/sensor_baro_sim/parameters.c

@@ -0,0 +1,47 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 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.
+ *
+ ****************************************************************************/
+
+/**
+ * simulated barometer pressure offset
+ *
+ * @group Simulator
+ */
+PARAM_DEFINE_FLOAT(SIM_BARO_OFF_P, 0.0f);
+
+/**
+ * simulated barometer temperature offset
+ *
+ * @group Simulator
+ * @unit celcius
+ */
+PARAM_DEFINE_FLOAT(SIM_BARO_OFF_T, 0.0f);

src/modules/simulation/sensor_gps_sim/CMakeLists.txt

@@ -0,0 +1,43 @@
+############################################################################
+#
+#   Copyright (c) 2021-2022 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__simulation__sensor_gps_sim
+	MAIN sensor_gps_sim
+	COMPILE_FLAGS
+	SRCS
+		SensorGpsSim.cpp
+		SensorGpsSim.hpp
+	DEPENDS
+		px4_work_queue
+	)

src/modules/simulation/sensor_gps_sim/Kconfig

@@ -0,0 +1,5 @@
+menuconfig MODULES_SIMULATION_SENSOR_GPS_SIM
+	bool "sensor_gps_sim"
+	default n
+	---help---
+		Enable support for sensor_gps_sim

src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp

@@ -0,0 +1,235 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 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 "SensorGpsSim.hpp"
+
+#include <drivers/drv_sensor.h>
+#include <lib/drivers/device/Device.hpp>
+#include <lib/geo/geo.h>
+
+using namespace matrix;
+
+SensorGpsSim::SensorGpsSim() :
+	ModuleParams(nullptr),
+	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default)
+{
+}
+
+SensorGpsSim::~SensorGpsSim()
+{
+	perf_free(_loop_perf);
+}
+
+bool SensorGpsSim::init()
+{
+	ScheduleOnInterval(125_ms); // 8 Hz
+	return true;
+}
+
+float SensorGpsSim::generate_wgn()
+{
+	// generate white Gaussian noise sample with std=1
+
+	// algorithm 1:
+	// float temp=((float)(rand()+1))/(((float)RAND_MAX+1.0f));
+	// return sqrtf(-2.0f*logf(temp))*cosf(2.0f*M_PI_F*rand()/RAND_MAX);
+	// algorithm 2: from BlockRandGauss.hpp
+	static float V1, V2, S;
+	static bool phase = true;
+	float X;
+
+	if (phase) {
+		do {
+			float U1 = (float)rand() / (float)RAND_MAX;
+			float U2 = (float)rand() / (float)RAND_MAX;
+			V1 = 2.0f * U1 - 1.0f;
+			V2 = 2.0f * U2 - 1.0f;
+			S = V1 * V1 + V2 * V2;
+		} while (S >= 1.0f || fabsf(S) < 1e-8f);
+
+		X = V1 * float(sqrtf(-2.0f * float(logf(S)) / S));
+
+	} else {
+		X = V2 * float(sqrtf(-2.0f * float(logf(S)) / S));
+	}
+
+	phase = !phase;
+	return X;
+}
+
+void SensorGpsSim::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_local_position_sub.updated() && _vehicle_global_position_sub.updated()) {
+
+		vehicle_local_position_s lpos{};
+		_vehicle_local_position_sub.copy(&lpos);
+
+		vehicle_global_position_s gpos{};
+		_vehicle_global_position_sub.copy(&gpos);
+
+		double latitude = gpos.lat + math::degrees((double)generate_wgn() * 0.2 / CONSTANTS_RADIUS_OF_EARTH);
+		double longitude = gpos.lon + math::degrees((double)generate_wgn() * 0.2 / CONSTANTS_RADIUS_OF_EARTH);
+		float altitude = gpos.alt + (generate_wgn() * 0.5f);
+
+		Vector3f gps_vel = Vector3f{lpos.vx, lpos.vy, lpos.vz} + noiseGauss3f(0.06f, 0.077f, 0.158f);
+
+		// device id
+		device::Device::DeviceId device_id;
+		device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION;
+		device_id.devid_s.bus = 0;
+		device_id.devid_s.address = 0;
+		device_id.devid_s.devtype = DRV_GPS_DEVTYPE_SIM;
+
+		sensor_gps_s sensor_gps{};
+
+		if (_sim_gps_used.get() >= 4) {
+			// fix
+			sensor_gps.fix_type = 3; // 3D fix
+			sensor_gps.s_variance_m_s = 0.5f;
+			sensor_gps.c_variance_rad = 0.1f;
+			sensor_gps.eph = 0.9f;
+			sensor_gps.epv = 1.78f;
+			sensor_gps.hdop = 0.7f;
+			sensor_gps.vdop = 1.1f;
+
+		} else {
+			// no fix
+			sensor_gps.fix_type = 0; // No fix
+			sensor_gps.s_variance_m_s = 100.f;
+			sensor_gps.c_variance_rad = 100.f;
+			sensor_gps.eph = 100.f;
+			sensor_gps.epv = 100.f;
+			sensor_gps.hdop = 100.f;
+			sensor_gps.vdop = 100.f;
+		}
+
+		sensor_gps.timestamp_sample = gpos.timestamp_sample;
+		sensor_gps.time_utc_usec = 0;
+		sensor_gps.device_id = device_id.devid;
+		sensor_gps.lat = roundf(latitude * 1e7); // Latitude in 1E-7 degrees
+		sensor_gps.lon = roundf(longitude * 1e7); // Longitude in 1E-7 degrees
+		sensor_gps.alt = roundf(altitude * 1000.f); // Altitude in 1E-3 meters above MSL, (millimetres)
+		sensor_gps.alt_ellipsoid = sensor_gps.alt;
+		sensor_gps.noise_per_ms = 0;
+		sensor_gps.jamming_indicator = 0;
+		sensor_gps.vel_m_s = sqrtf(gps_vel(0) * gps_vel(0) + gps_vel(1) * gps_vel(1)); // GPS ground speed, (metres/sec)
+		sensor_gps.vel_n_m_s = gps_vel(0);
+		sensor_gps.vel_e_m_s = gps_vel(1);
+		sensor_gps.vel_d_m_s = gps_vel(2);
+		sensor_gps.cog_rad = atan2(gps_vel(1),
+					   gps_vel(0)); // Course over ground (NOT heading, but direction of movement), -PI..PI, (radians)
+		sensor_gps.timestamp_time_relative = 0;
+		sensor_gps.heading = NAN;
+		sensor_gps.heading_offset = NAN;
+		sensor_gps.heading_accuracy = 0;
+		sensor_gps.automatic_gain_control = 0;
+		sensor_gps.jamming_state = 0;
+		sensor_gps.vel_ned_valid = true;
+		sensor_gps.satellites_used = _sim_gps_used.get();
+
+		sensor_gps.timestamp = hrt_absolute_time();
+		_sensor_gps_pub.publish(sensor_gps);
+	}
+
+	perf_end(_loop_perf);
+}
+
+int SensorGpsSim::task_spawn(int argc, char *argv[])
+{
+	SensorGpsSim *instance = new SensorGpsSim();
+
+	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 SensorGpsSim::custom_command(int argc, char *argv[])
+{
+	return print_usage("unknown command");
+}
+
+int SensorGpsSim::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("sensor_gps_sim", "system");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+
+	return 0;
+}
+
+extern "C" __EXPORT int sensor_gps_sim_main(int argc, char *argv[])
+{
+	return SensorGpsSim::main(argc, argv);
+}

src/modules/simulation/sensor_gps_sim/SensorGpsSim.hpp

@@ -0,0 +1,88 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 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/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/PublicationMulti.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionInterval.hpp>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/sensor_gps.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_local_position.h>
+
+using namespace time_literals;
+
+class SensorGpsSim : public ModuleBase<SensorGpsSim>, public ModuleParams, public px4::ScheduledWorkItem
+{
+public:
+	SensorGpsSim();
+	~SensorGpsSim() 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;
+
+	// generate white Gaussian noise sample with std=1
+	static float generate_wgn();
+
+	// generate white Gaussian noise sample as a 3D vector with specified std
+	matrix::Vector3f noiseGauss3f(float stdx, float stdy, float stdz) { return matrix::Vector3f(generate_wgn() * stdx, generate_wgn() * stdy, generate_wgn() * stdz); }
+
+	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
+	uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position_groundtruth)};
+	uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position_groundtruth)};
+
+	uORB::PublicationMulti<sensor_gps_s> _sensor_gps_pub{ORB_ID(sensor_gps)};
+
+	perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
+
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::SIM_GPS_USED>) _sim_gps_used
+	)
+};

src/modules/simulation/sensor_gps_sim/parameters.c

@@ -0,0 +1,41 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 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.
+ *
+ ****************************************************************************/
+
+/**
+ * simulated GPS number of satellites used
+ *
+ * @min 0
+ * @max  50
+ * @group Simulator
+ */
+PARAM_DEFINE_INT32(SIM_GPS_USED, 10);

src/modules/simulation/sensor_mag_sim/CMakeLists.txt

@@ -0,0 +1,45 @@
+############################################################################
+#
+#   Copyright (c) 2021 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__simulation__senosr_mag_sim
+	MAIN sensor_mag_sim
+	COMPILE_FLAGS
+	SRCS
+		SensorMagSim.cpp
+		SensorMagSim.hpp
+	DEPENDS
+		drivers_magnetometer
+		geo
+		px4_work_queue
+	)

src/modules/simulation/sensor_mag_sim/Kconfig

@@ -0,0 +1,5 @@
+menuconfig MODULES_SIMULATION_SENSOR_MAG_SIM
+	bool "sensor_mag_sim"
+	default n
+	---help---
+		Enable support for sensor_mag_sim

src/modules/simulation/sensor_mag_sim/SensorMagSim.cpp

@@ -0,0 +1,196 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 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 "SensorMagSim.hpp"
+
+#include <drivers/drv_sensor.h>
+#include <lib/world_magnetic_model/geo_mag_declination.h>
+
+using namespace matrix;
+
+SensorMagSim::SensorMagSim() :
+	ModuleParams(nullptr),
+	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default)
+{
+	_px4_mag.set_device_type(DRV_MAG_DEVTYPE_MAGSIM);
+}
+
+SensorMagSim::~SensorMagSim()
+{
+	perf_free(_loop_perf);
+}
+
+bool SensorMagSim::init()
+{
+	ScheduleOnInterval(20_ms); // 50 Hz
+	return true;
+}
+
+float SensorMagSim::generate_wgn()
+{
+	// generate white Gaussian noise sample with std=1
+
+	// algorithm 1:
+	// float temp=((float)(rand()+1))/(((float)RAND_MAX+1.0f));
+	// return sqrtf(-2.0f*logf(temp))*cosf(2.0f*M_PI_F*rand()/RAND_MAX);
+	// algorithm 2: from BlockRandGauss.hpp
+	static float V1, V2, S;
+	static bool phase = true;
+	float X;
+
+	if (phase) {
+		do {
+			float U1 = (float)rand() / (float)RAND_MAX;
+			float U2 = (float)rand() / (float)RAND_MAX;
+			V1 = 2.0f * U1 - 1.0f;
+			V2 = 2.0f * U2 - 1.0f;
+			S = V1 * V1 + V2 * V2;
+		} while (S >= 1.0f || fabsf(S) < 1e-8f);
+
+		X = V1 * float(sqrtf(-2.0f * float(logf(S)) / S));
+
+	} else {
+		X = V2 * float(sqrtf(-2.0f * float(logf(S)) / S));
+	}
+
+	phase = !phase;
+	return X;
+}
+
+void SensorMagSim::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_global_position_sub.updated()) {
+		vehicle_global_position_s gpos;
+
+		if (_vehicle_global_position_sub.copy(&gpos)) {
+			if (gpos.eph < 1000) {
+
+				// magnetic field data returned by the geo library using the current GPS position
+				const float mag_declination_gps = get_mag_declination_radians(gpos.lat, gpos.lon);
+				const float mag_inclination_gps = get_mag_inclination_radians(gpos.lat, gpos.lon);
+				const float mag_strength_gps = get_mag_strength_gauss(gpos.lat, gpos.lon);
+
+				_mag_earth_pred = Dcmf(Eulerf(0, -mag_inclination_gps, mag_declination_gps)) * Vector3f(mag_strength_gps, 0, 0);
+
+				_mag_earth_available = true;
+			}
+		}
+	}
+
+	if (_mag_earth_available) {
+		vehicle_attitude_s attitude;
+
+		if (_vehicle_attitude_sub.update(&attitude)) {
+			Vector3f expected_field = Dcmf{Quatf{attitude.q}} .transpose() * _mag_earth_pred;
+
+			expected_field += noiseGauss3f(0.02f, 0.02f, 0.03f);
+
+			_px4_mag.update(attitude.timestamp,
+					expected_field(0) + _sim_mag_offset_x.get(),
+					expected_field(1) + _sim_mag_offset_y.get(),
+					expected_field(2) + _sim_mag_offset_z.get());
+		}
+	}
+
+	perf_end(_loop_perf);
+}
+
+int SensorMagSim::task_spawn(int argc, char *argv[])
+{
+	SensorMagSim *instance = new SensorMagSim();
+
+	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 SensorMagSim::custom_command(int argc, char *argv[])
+{
+	return print_usage("unknown command");
+}
+
+int SensorMagSim::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("sensor_mag_sim", "system");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+
+	return 0;
+}
+
+extern "C" __EXPORT int sensor_mag_sim_main(int argc, char *argv[])
+{
+	return SensorMagSim::main(argc, argv);
+}

src/modules/simulation/sensor_mag_sim/SensorMagSim.hpp

@@ -0,0 +1,94 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 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/drivers/magnetometer/PX4Magnetometer.hpp>
+#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/SubscriptionInterval.hpp>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_global_position.h>
+
+using namespace time_literals;
+
+class SensorMagSim : public ModuleBase<SensorMagSim>, public ModuleParams, public px4::ScheduledWorkItem
+{
+public:
+	SensorMagSim();
+	~SensorMagSim() 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;
+
+	// generate white Gaussian noise sample with std=1
+	static float generate_wgn();
+
+	// generate white Gaussian noise sample as a 3D vector with specified std
+	matrix::Vector3f noiseGauss3f(float stdx, float stdy, float stdz) { return matrix::Vector3f(generate_wgn() * stdx, generate_wgn() * stdy, generate_wgn() * stdz); }
+
+	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
+	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude_groundtruth)};
+	uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position_groundtruth)};
+
+	PX4Magnetometer _px4_mag{197388, ROTATION_NONE}; // 197388: DRV_MAG_DEVTYPE_MAGSIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
+
+	bool _mag_earth_available{false};
+
+	matrix::Vector3f _mag_earth_pred{};
+
+	perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::SIM_MAG_OFFSET_X>) _sim_mag_offset_x,
+		(ParamFloat<px4::params::SIM_MAG_OFFSET_Y>) _sim_mag_offset_y,
+		(ParamFloat<px4::params::SIM_MAG_OFFSET_Z>) _sim_mag_offset_z
+	)
+};

src/modules/simulation/sensor_mag_sim/parameters.c

@@ -0,0 +1,56 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 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.
+ *
+ ****************************************************************************/
+
+/**
+ * simulated magnetometer X offset
+ *
+ * @unit gauss
+ * @group Simulator
+ */
+PARAM_DEFINE_FLOAT(SIM_MAG_OFFSET_X, 0.0f);
+
+/**
+ * simulated magnetometer Y offset
+ *
+ * @unit gauss
+ * @group Simulator
+ */
+PARAM_DEFINE_FLOAT(SIM_MAG_OFFSET_Y, 0.0f);
+
+/**
+ * simulated magnetometer Z offset
+ *
+ * @unit gauss
+ * @group Simulator
+ */
+PARAM_DEFINE_FLOAT(SIM_MAG_OFFSET_Z,  0.0f);

src/modules/simulation/simulator_ignition_bridge/CMakeLists.txt

@@ -0,0 +1,103 @@
+############################################################################
+#
+#   Copyright (c) 2022 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.
+#
+############################################################################
+
+add_compile_options(-frtti -fexceptions)
+
+# Find the Ignition_Transport library
+find_package(ignition-transport
+	REQUIRED COMPONENTS core
+	NAMES
+		ignition-transport8
+		ignition-transport10
+		ignition-transport11
+	#QUIET
+)
+set(IGN_TRANSPORT_VER ${ignition-transport_VERSION_MAJOR})
+
+px4_add_module(
+	MODULE modules__simulation__ignition_bridge
+	MAIN simulator_ignition_bridge
+	COMPILE_FLAGS
+		${MAX_CUSTOM_OPT_LEVEL}
+	SRCS
+		SimulatorIgnitionBridge.cpp
+		SimulatorIgnitionBridge.hpp
+	DEPENDS
+		drivers_accelerometer
+		drivers_gyroscope
+		mixer_module
+		px4_work_queue
+		ignition-transport${IGN_TRANSPORT_VER}::core
+	MODULE_CONFIG
+		module.yaml
+)
+
+file(GLOB ign_models
+	LIST_DIRECTORIES true
+	RELATIVE ${PX4_SOURCE_DIR}/Tools/simulation/ignition/models
+	CONFIGURE_DEPENDS
+	${PX4_SOURCE_DIR}/Tools/simulation/ignition/models/*
+)
+
+file(GLOB ign_worlds
+	CONFIGURE_DEPENDS
+	${PX4_SOURCE_DIR}/Tools/simulation/ignition/worlds/*.sdf
+)
+
+foreach(model ${ign_models})
+	foreach(world ${ign_worlds})
+
+		get_filename_component("world_name" ${world} NAME_WE)
+
+		if(world_name MATCHES "default")
+			add_custom_target(ign_${model}
+				COMMAND ${CMAKE_COMMAND} -E env PX4_SIM_MODEL=${model} $<TARGET_FILE:px4>
+				WORKING_DIRECTORY ${SITL_WORKING_DIR}
+				USES_TERMINAL
+				DEPENDS px4
+			)
+		else()
+			add_custom_target(ign_${model}_${world_name}
+				COMMAND ${CMAKE_COMMAND} -E env PX4_SIM_MODEL=${model} PX4_SIM_WORLD=${world_name} $<TARGET_FILE:px4>
+				WORKING_DIRECTORY ${SITL_WORKING_DIR}
+				USES_TERMINAL
+				DEPENDS px4
+			)
+		endif()
+	endforeach()
+endforeach()
+
+
+# TODO: PX4_IGN_MODELS_PATH
+# PX4_IGN_WORLDS_PATH
+configure_file(gazebo_env.sh.in ${PX4_BINARY_DIR}/rootfs/gazebo_env.sh)

src/modules/simulation/simulator_ignition_bridge/Kconfig

@@ -0,0 +1,5 @@
+menuconfig MODULES_SIMULATION_SIMULATOR_IGNITION_BRIDGE
+	bool "simulator_ignition_bridge"
+	default n
+	---help---
+		Enable support for simulator_ignition_bridge

src/modules/simulation/simulator_ignition_bridge/SimulatorIgnitionBridge.cpp

@@ -0,0 +1,418 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 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 "SimulatorIgnitionBridge.hpp"
+
+#include <uORB/Subscription.hpp>
+
+#include <lib/mathlib/mathlib.h>
+
+#include <px4_platform_common/getopt.h>
+
+#include <iostream>
+#include <string>
+
+SimulatorIgnitionBridge::SimulatorIgnitionBridge(const char *world, const char *model) :
+	OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default),
+	_px4_accel(1310988),  // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
+	_px4_gyro(1310988),   // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
+	_world_name(world),
+	_model_name(model)
+{
+	_px4_accel.set_range(2000.f); // don't care
+
+	updateParams();
+}
+
+SimulatorIgnitionBridge::~SimulatorIgnitionBridge()
+{
+	// TODO: unsubscribe
+
+	for (auto &sub_topic : _node.SubscribedTopics()) {
+		_node.Unsubscribe(sub_topic);
+	}
+}
+
+int SimulatorIgnitionBridge::init()
+{
+	// service call to create model
+	// ign service -s /world/${PX4_SIM_WORLD}/create --reqtype ignition.msgs.EntityFactory --reptype ignition.msgs.Boolean --timeout 1000 --req "sdf_filename: \"${PX4_SIM_MODEL}/model.sdf\""
+	ignition::msgs::EntityFactory req{};
+	req.set_sdf_filename(_model_name + "/model.sdf");
+
+	// TODO: support model instances?
+	// req.set_name("model_instance_name"); // New name for the entity, overrides the name on the SDF.
+	req.set_allow_renaming(false); // allowed to rename the entity in case of overlap with existing entities
+
+	// /world/$WORLD/create service.
+	ignition::msgs::Boolean rep;
+	bool result;
+	std::string create_service = "/world/" + _world_name + "/create";
+
+	if (_node.Request(create_service, req, 1000, rep, result)) {
+		if (!result) {
+			PX4_ERR("Service call failed");
+			return PX4_ERROR;
+		}
+
+	} else {
+		PX4_ERR("Service call timed out");
+		return PX4_ERROR;
+	}
+
+
+	// clock
+	std::string clock_topic = "/world/" + _world_name + "/clock";
+
+	if (!_node.Subscribe(clock_topic, &SimulatorIgnitionBridge::clockCallback, this)) {
+		return PX4_ERROR;
+	}
+
+	// pose: /world/$WORLD/pose/info
+	std::string world_pose_topic = "/world/" + _world_name + "/pose/info";
+
+	if (!_node.Subscribe(world_pose_topic, &SimulatorIgnitionBridge::poseInfoCallback, this)) {
+		return PX4_ERROR;
+	}
+
+	// IMU: /world/$WORLD/model/$MODEL//link/base_link/sensor/imu_sensor/imu
+	std::string imu_topic = "/world/" + _world_name + "/model/" + _model_name + "/link/base_link/sensor/imu_sensor/imu";
+
+	if (!_node.Subscribe(imu_topic, &SimulatorIgnitionBridge::imuCallback, this)) {
+		return PX4_ERROR;
+	}
+
+	for (auto &sub_topic : _node.SubscribedTopics()) {
+		PX4_INFO("subscribed: %s", sub_topic.c_str());
+	}
+
+	// output eg /X3/command/motor_speed
+	std::string actuator_topic = _model_name + "/command/motor_speed";
+	_actuators_pub = _node.Advertise<ignition::msgs::Actuators>(actuator_topic);
+
+	if (!_actuators_pub.Valid()) {
+		PX4_ERR("failed to advertise %s", actuator_topic.c_str());
+		return PX4_ERROR;
+	}
+
+	ScheduleNow();
+	return OK;
+}
+
+int SimulatorIgnitionBridge::task_spawn(int argc, char *argv[])
+{
+	const char *world_name = "default";
+	const char *model_name = nullptr;
+
+	bool error_flag = false;
+	int myoptind = 1;
+	int ch;
+	const char *myoptarg = nullptr;
+
+	while ((ch = px4_getopt(argc, argv, "w:m:", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+		case 'w':
+			// world
+			world_name = myoptarg;
+			break;
+
+		case 'm':
+			// model
+			model_name = myoptarg;
+			break;
+
+		case '?':
+			error_flag = true;
+			break;
+
+		default:
+			PX4_WARN("unrecognized flag");
+			error_flag = true;
+			break;
+		}
+	}
+
+	if (error_flag) {
+		return PX4_ERROR;
+	}
+
+	PX4_INFO("world: %s, model: %s", world_name, model_name);
+
+	SimulatorIgnitionBridge *instance = new SimulatorIgnitionBridge(world_name, model_name);
+
+	if (instance) {
+		_object.store(instance);
+		_task_id = task_id_is_work_queue;
+
+		if (instance->init() == PX4_OK) {
+			return PX4_OK;
+		}
+
+	} else {
+		PX4_ERR("alloc failed");
+	}
+
+	delete instance;
+	_object.store(nullptr);
+	_task_id = -1;
+
+	return PX4_ERROR;
+}
+
+void SimulatorIgnitionBridge::clockCallback(const ignition::msgs::Clock &clock)
+{
+	// uint64_t time_us = clock.sim().sec() * 1e6 + clock.sim().nsec() / 1000;
+	// PX4_INFO("clock %lu ", time_us);
+
+	struct timespec ts;
+	ts.tv_sec = clock.sim().sec();
+	ts.tv_nsec = clock.sim().nsec();
+	px4_clock_settime(CLOCK_MONOTONIC, &ts);
+}
+
+void SimulatorIgnitionBridge::imuCallback(const ignition::msgs::IMU &imu)
+{
+	// FLU -> FRD
+	static const auto q_FLU_to_FRD = ignition::math::Quaterniond(0, 1, 0, 0);
+
+	ignition::math::Vector3d accel_b = q_FLU_to_FRD.RotateVector(ignition::math::Vector3d(
+			imu.linear_acceleration().x(),
+			imu.linear_acceleration().y(),
+			imu.linear_acceleration().z()));
+
+	ignition::math::Vector3d gyro_b = q_FLU_to_FRD.RotateVector(ignition::math::Vector3d(
+			imu.angular_velocity().x(),
+			imu.angular_velocity().y(),
+			imu.angular_velocity().z()));
+
+	//const uint64_t time_us = imu.header().stamp().sec() * 1e6 + imu.header().stamp().nsec() / 1000;
+	const uint64_t time_us = hrt_absolute_time();
+
+	if (time_us != 0) {
+		_px4_accel.update(time_us, accel_b.X(), accel_b.Y(), accel_b.Z());
+		_px4_gyro.update(time_us, gyro_b.X(), gyro_b.Y(), gyro_b.Z());
+	}
+}
+
+void SimulatorIgnitionBridge::poseInfoCallback(const ignition::msgs::Pose_V &pose)
+{
+	//const uint64_t time_us = pose.header().stamp().sec() * 1e6 + pose.header().stamp().nsec() / 1000;
+	const uint64_t time_us = hrt_absolute_time();
+
+	if (time_us == 0) {
+		return;
+	}
+
+	for (int p = 0; p < pose.pose_size(); p++) {
+		if (pose.pose(p).name() == _model_name) {
+
+			const double dt = math::constrain((time_us - _timestamp_prev) * 1e-6, 0.001, 0.1);
+			_timestamp_prev = time_us;
+
+			ignition::msgs::Vector3d pose_position = pose.pose(p).position();
+			ignition::msgs::Quaternion pose_orientation = pose.pose(p).orientation();
+
+			static const auto q_FLU_to_FRD = ignition::math::Quaterniond(0, 1, 0, 0);
+
+			/**
+			 * @brief Quaternion for rotation between ENU and NED frames
+			 *
+			 * NED to ENU: +PI/2 rotation about Z (Down) followed by a +PI rotation around X (old North/new East)
+			 * ENU to NED: +PI/2 rotation about Z (Up) followed by a +PI rotation about X (old East/new North)
+			 * This rotation is symmetric, so q_ENU_to_NED == q_NED_to_ENU.
+			 */
+			static const auto q_ENU_to_NED = ignition::math::Quaterniond(0, 0.70711, 0.70711, 0);
+
+			// ground truth
+			ignition::math::Quaterniond q_gr = ignition::math::Quaterniond(
+					pose_orientation.w(),
+					pose_orientation.x(),
+					pose_orientation.y(),
+					pose_orientation.z());
+
+			ignition::math::Quaterniond q_gb = q_gr * q_FLU_to_FRD.Inverse();
+			ignition::math::Quaterniond q_nb = q_ENU_to_NED * q_gb;
+
+			// publish attitude groundtruth
+			vehicle_attitude_s vehicle_attitude_groundtruth{};
+			vehicle_attitude_groundtruth.timestamp_sample = time_us;
+			vehicle_attitude_groundtruth.q[0] = q_nb.W();
+			vehicle_attitude_groundtruth.q[1] = q_nb.X();
+			vehicle_attitude_groundtruth.q[2] = q_nb.Y();
+			vehicle_attitude_groundtruth.q[3] = q_nb.Z();
+			vehicle_attitude_groundtruth.timestamp = hrt_absolute_time();
+			_attitude_ground_truth_pub.publish(vehicle_attitude_groundtruth);
+
+			// publish angular velocity groundtruth
+			const matrix::Eulerf euler{matrix::Quatf(vehicle_attitude_groundtruth.q)};
+			vehicle_angular_velocity_s vehicle_angular_velocity_groundtruth{};
+			vehicle_angular_velocity_groundtruth.timestamp_sample = time_us;
+
+			const matrix::Vector3f angular_velocity = (euler - _euler_prev) / dt;
+			_euler_prev = euler;
+			angular_velocity.copyTo(vehicle_angular_velocity_groundtruth.xyz);
+
+			vehicle_angular_velocity_groundtruth.timestamp = hrt_absolute_time();
+			_angular_velocity_ground_truth_pub.publish(vehicle_angular_velocity_groundtruth);
+
+			if (!_pos_ref.isInitialized()) {
+				_pos_ref.initReference((double)_param_sim_home_lat.get(), (double)_param_sim_home_lon.get(), hrt_absolute_time());
+			}
+
+			vehicle_local_position_s local_position_groundtruth{};
+			local_position_groundtruth.timestamp_sample = time_us;
+
+			// position ENU -> NED
+			const matrix::Vector3d position{pose_position.y(), pose_position.x(), -pose_position.z()};
+			const matrix::Vector3d velocity{(position - _position_prev) / dt};
+			const matrix::Vector3d acceleration{(velocity - _velocity_prev) / dt};
+
+			_position_prev = position;
+			_velocity_prev = velocity;
+
+			local_position_groundtruth.ax = acceleration(0);
+			local_position_groundtruth.ay = acceleration(1);
+			local_position_groundtruth.az = acceleration(2);
+			local_position_groundtruth.vx = velocity(0);
+			local_position_groundtruth.vy = velocity(1);
+			local_position_groundtruth.vz = velocity(2);
+			local_position_groundtruth.x = position(0);
+			local_position_groundtruth.y = position(1);
+			local_position_groundtruth.z = position(2);
+
+			local_position_groundtruth.ref_lat = _pos_ref.getProjectionReferenceLat(); // Reference point latitude in degrees
+			local_position_groundtruth.ref_lon = _pos_ref.getProjectionReferenceLon(); // Reference point longitude in degrees
+			local_position_groundtruth.ref_alt = _param_sim_home_alt.get();
+			local_position_groundtruth.ref_timestamp = _pos_ref.getProjectionReferenceTimestamp();
+
+			local_position_groundtruth.timestamp = hrt_absolute_time();
+			_lpos_ground_truth_pub.publish(local_position_groundtruth);
+
+			if (_pos_ref.isInitialized()) {
+				// publish position groundtruth
+				vehicle_global_position_s global_position_groundtruth{};
+				global_position_groundtruth.timestamp_sample = time_us;
+
+				_pos_ref.reproject(local_position_groundtruth.x, local_position_groundtruth.y,
+						   global_position_groundtruth.lat, global_position_groundtruth.lon);
+
+				global_position_groundtruth.alt = _param_sim_home_alt.get() - static_cast<float>(position(2));
+				global_position_groundtruth.timestamp = hrt_absolute_time();
+				_gpos_ground_truth_pub.publish(global_position_groundtruth);
+			}
+
+			return;
+		}
+	}
+}
+
+bool SimulatorIgnitionBridge::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
+		unsigned num_control_groups_updated)
+{
+	ignition::msgs::Actuators rotor_velocity_message;
+	rotor_velocity_message.mutable_velocity()->Resize(num_outputs, 0);
+
+	for (unsigned i = 0; i < num_outputs; i++) {
+		rotor_velocity_message.set_velocity(i, outputs[i]);
+	}
+
+	if (_actuators_pub.Valid()) {
+		return _actuators_pub.Publish(rotor_velocity_message);
+	}
+
+	return false;
+}
+
+void SimulatorIgnitionBridge::Run()
+{
+	if (should_exit()) {
+		ScheduleClear();
+		_mixing_output.unregister();
+
+		exit_and_cleanup();
+		return;
+	}
+
+	if (_parameter_update_sub.updated()) {
+		parameter_update_s pupdate;
+		_parameter_update_sub.copy(&pupdate);
+
+		updateParams();
+	}
+
+	_mixing_output.update();
+
+	//ScheduleDelayed(1000_us);
+
+	// check at end of cycle (updateSubscriptions() can potentially change to a different WorkQueue thread)
+	_mixing_output.updateSubscriptions(true);
+}
+
+int SimulatorIgnitionBridge::print_status()
+{
+	//perf_print_counter(_cycle_perf);
+	_mixing_output.printStatus();
+
+	return 0;
+}
+
+int SimulatorIgnitionBridge::custom_command(int argc, char *argv[])
+{
+	return print_usage("unknown command");
+}
+
+int SimulatorIgnitionBridge::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("simulator_ignition_bridge", "driver");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_PARAM_STRING('m', nullptr, nullptr, "Model name", false);
+	PRINT_MODULE_USAGE_PARAM_STRING('w', nullptr, nullptr, "World name", true);
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+
+	return 0;
+}
+
+extern "C" __EXPORT int simulator_ignition_bridge_main(int argc, char *argv[])
+{
+	return SimulatorIgnitionBridge::main(argc, argv);
+}

src/modules/simulation/simulator_ignition_bridge/SimulatorIgnitionBridge.hpp

@@ -0,0 +1,125 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 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 <px4_platform_common/defines.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/posix.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
+#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
+#include <lib/geo/geo.h>
+#include <lib/mixer_module/mixer_module.hpp>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionInterval.hpp>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_angular_velocity.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_local_position.h>
+
+#include <ignition/msgs.hh>
+#include <ignition/transport.hh>
+
+#include <ignition/msgs/imu.pb.h>
+
+using namespace time_literals;
+
+class SimulatorIgnitionBridge : public ModuleBase<SimulatorIgnitionBridge>, public OutputModuleInterface
+{
+public:
+	SimulatorIgnitionBridge(const char *world, const char *model);
+	~SimulatorIgnitionBridge() override;
+
+	/** @see ModuleBase */
+	static int custom_command(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);
+
+	/** @see ModuleBase */
+	static int task_spawn(int argc, char *argv[]);
+
+	int init();
+
+	/** @see ModuleBase::print_status() */
+	int print_status() override;
+
+private:
+
+	bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
+			   unsigned num_outputs, unsigned num_control_groups_updated) override;
+
+	void mixerChanged() override {}
+
+	void Run() override;
+
+	void clockCallback(const ignition::msgs::Clock &clock);
+	void imuCallback(const ignition::msgs::IMU &imu);
+	void poseInfoCallback(const ignition::msgs::Pose_V &pose);
+
+	// Subscriptions
+	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
+
+	uORB::Publication<vehicle_angular_velocity_s> _angular_velocity_ground_truth_pub{ORB_ID(vehicle_angular_velocity_groundtruth)};
+	uORB::Publication<vehicle_attitude_s>         _attitude_ground_truth_pub{ORB_ID(vehicle_attitude_groundtruth)};
+	uORB::Publication<vehicle_global_position_s>  _gpos_ground_truth_pub{ORB_ID(vehicle_global_position_groundtruth)};
+	uORB::Publication<vehicle_local_position_s>   _lpos_ground_truth_pub{ORB_ID(vehicle_local_position_groundtruth)};
+
+	PX4Accelerometer _px4_accel;
+	PX4Gyroscope _px4_gyro;
+
+	MapProjection _pos_ref{};
+
+	matrix::Vector3d _position_prev{};
+	matrix::Vector3d _velocity_prev{};
+	matrix::Vector3f _euler_prev{};
+	hrt_abstime _timestamp_prev{};
+
+	const std::string _world_name;
+	const std::string _model_name;
+
+	MixingOutput _mixing_output{"SIM_IGN", 8, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
+
+	ignition::transport::Node _node;
+	ignition::transport::Node::Publisher _actuators_pub;
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::SIM_IGN_HOME_LAT>) _param_sim_home_lat,
+		(ParamFloat<px4::params::SIM_IGN_HOME_LON>) _param_sim_home_lon,
+		(ParamFloat<px4::params::SIM_IGN_HOME_ALT>) _param_sim_home_alt
+	)
+};

src/modules/simulation/simulator_ignition_bridge/gazebo_env.sh.in

@@ -0,0 +1,6 @@
+#!/usr/bin/env bash
+
+export PX4_IGN_GAZEBO_MODELS=@PX4_SOURCE_DIR@/Tools/simulation/ignition/models
+export PX4_IGN_GAZEBO_WORLDS=@PX4_SOURCE_DIR@/Tools/simulation/ignition/worlds
+
+export IGN_GAZEBO_RESOURCE_PATH=$IGN_GAZEBO_RESOURCE_PATH:$PX4_IGN_GAZEBO_MODELS

src/modules/simulation/simulator_ignition_bridge/module.yaml

@@ -0,0 +1,10 @@
+module_name: SIM_IGN
+actuator_output:
+  output_groups:
+    - param_prefix: SIM_IGN
+      channel_label: Channel
+      num_channels: 8
+      standard_params:
+        disarmed: { min: 0, max: 1000, default: 0 }
+        min: { min: 0, max: 1000, default: 10 }
+        max: { min: 100, max: 1000, default: 1000 }

src/modules/simulation/simulator_ignition_bridge/parameters.c

@@ -0,0 +1,56 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 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 origin latitude
+ *
+ * @unit deg
+ * @group Simulator
+ */
+PARAM_DEFINE_FLOAT(SIM_IGN_HOME_LAT, 47.397742f);
+
+/**
+ * simulator origin longitude
+ *
+ * @unit deg
+ * @group Simulator
+ */
+PARAM_DEFINE_FLOAT(SIM_IGN_HOME_LON, 8.545594);
+
+/**
+ * simulator origin altitude
+ *
+ * @unit m
+ * @group Simulator
+ */
+PARAM_DEFINE_FLOAT(SIM_IGN_HOME_ALT, 488.0);

src/modules/simulation/simulator_mavlink/CMakeLists.txt

@@ -0,0 +1,59 @@
+############################################################################
+#
+#   Copyright (c) 2015-2022 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__simulation__simulator_mavlink
+	MAIN simulator_mavlink
+	COMPILE_FLAGS
+		-Wno-double-promotion
+		-Wno-cast-align
+		-Wno-address-of-packed-member # TODO: fix in c_library_v2
+	INCLUDES
+		${CMAKE_BINARY_DIR}/mavlink
+		${CMAKE_BINARY_DIR}/mavlink/development
+	SRCS
+		SimulatorMavlink.cpp
+		SimulatorMavlink.hpp
+	DEPENDS
+		mavlink_c_generate
+		conversion
+		geo
+		drivers_accelerometer
+		drivers_gyroscope
+		drivers_magnetometer
+	)
+
+include(sitl_targets_flightgear.cmake)
+include(sitl_targets_gazebo.cmake)
+include(sitl_targets_jmavsim.cmake)
+include(sitl_targets_jsbsim.cmake)

src/modules/simulation/simulator_mavlink/Kconfig

@@ -0,0 +1,5 @@
+menuconfig MODULES_SIMULATION_SIMULATOR_MAVLINK
+	bool "simulator_mavlink"
+	default n
+	---help---
+		Enable support for mavlink simulator

src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp

@@ -0,0 +1,317 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 Mark Charlebois. All rights reserved.
+ *   Copyright (c) 2016-2019 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.
+ *
+ ****************************************************************************/
+
+
+/**
+ * @file simulator.h
+ *
+ * This module interfaces via MAVLink to a software in the loop simulator (SITL)
+ * such as jMAVSim or Gazebo.
+ */
+
+#pragma once
+
+#include <drivers/drv_hrt.h>
+#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
+#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
+#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
+#include <lib/geo/geo.h>
+#include <lib/perf/perf_counter.h>
+#include <px4_platform_common/atomic.h>
+#include <px4_platform_common/bitmask.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/posix.h>
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionInterval.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/input_rc.h>
+#include <uORB/topics/esc_status.h>
+#include <uORB/topics/esc_report.h>
+#include <uORB/topics/irlock_report.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/sensor_baro.h>
+#include <uORB/topics/sensor_gps.h>
+#include <uORB/topics/sensor_optical_flow.h>
+#include <uORB/topics/vehicle_angular_velocity.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_odometry.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_command_ack.h>
+#include <uORB/topics/rpm.h>
+
+#include <random>
+
+#include <mavlink.h>
+#include <mavlink_types.h>
+
+using namespace time_literals;
+
+//! Enumeration to use on the bitmask in HIL_SENSOR
+enum class SensorSource {
+	ACCEL		= 0b111,
+	GYRO		= 0b111000,
+	MAG		= 0b111000000,
+	BARO		= 0b1101000000000,
+	DIFF_PRESS	= 0b10000000000
+};
+ENABLE_BIT_OPERATORS(SensorSource)
+
+//! AND operation for the enumeration and unsigned types that returns the bitmask
+template<typename A, typename B>
+static inline SensorSource operator &(A lhs, B rhs)
+{
+	// make it type safe
+	static_assert((std::is_same<A, uint32_t>::value || std::is_same<A, SensorSource>::value),
+		      "first argument is not uint32_t or SensorSource enum type");
+	static_assert((std::is_same<B, uint32_t>::value || std::is_same<B, SensorSource>::value),
+		      "second argument is not uint32_t or SensorSource enum type");
+
+	typedef typename std::underlying_type<SensorSource>::type underlying;
+
+	return static_cast<SensorSource>(
+		       static_cast<underlying>(lhs) &
+		       static_cast<underlying>(rhs)
+	       );
+}
+
+class SimulatorMavlink : public ModuleParams
+{
+public:
+	static SimulatorMavlink *getInstance() { return _instance; }
+
+	enum class InternetProtocol {
+		TCP,
+		UDP
+	};
+
+	static int start(int argc, char *argv[]);
+
+	void set_ip(InternetProtocol ip) { _ip = ip; }
+	void set_port(unsigned port) { _port = port; }
+	void set_hostname(const char *hostname) { _hostname = hostname; }
+	void set_tcp_remote_ipaddr(char *tcp_remote_ipaddr) { _tcp_remote_ipaddr = tcp_remote_ipaddr; }
+
+#if defined(ENABLE_LOCKSTEP_SCHEDULER)
+	bool has_initialized() { return _has_initialized.load(); }
+#endif
+
+private:
+	SimulatorMavlink();
+
+	~SimulatorMavlink()
+	{
+		// free perf counters
+		perf_free(_perf_sim_delay);
+		perf_free(_perf_sim_interval);
+
+		for (size_t i = 0; i < sizeof(_dist_pubs) / sizeof(_dist_pubs[0]); i++) {
+			delete _dist_pubs[i];
+		}
+
+		px4_lockstep_unregister_component(_lockstep_component);
+
+		for (size_t i = 0; i < sizeof(_sensor_gps_pubs) / sizeof(_sensor_gps_pubs[0]); i++) {
+			delete _sensor_gps_pubs[i];
+		}
+
+		_instance = nullptr;
+	}
+
+
+	void check_failure_injections();
+
+	int publish_distance_topic(const mavlink_distance_sensor_t *dist);
+
+	static SimulatorMavlink *_instance;
+
+	// simulated sensor instances
+	static constexpr uint8_t ACCEL_COUNT_MAX = 3;
+	PX4Accelerometer _px4_accel[ACCEL_COUNT_MAX] {
+		{1310988, ROTATION_NONE}, // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
+		{1310996, ROTATION_NONE}, // 1310996: DRV_IMU_DEVTYPE_SIM, BUS: 2, ADDR: 1, TYPE: SIMULATION
+		{1311004, ROTATION_NONE}, // 1311004: DRV_IMU_DEVTYPE_SIM, BUS: 3, ADDR: 1, TYPE: SIMULATION
+	};
+
+	static constexpr uint8_t GYRO_COUNT_MAX = 3;
+	PX4Gyroscope _px4_gyro[GYRO_COUNT_MAX] {
+		{1310988, ROTATION_NONE}, // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
+		{1310996, ROTATION_NONE}, // 1310996: DRV_IMU_DEVTYPE_SIM, BUS: 2, ADDR: 1, TYPE: SIMULATION
+		{1311004, ROTATION_NONE}, // 1311004: DRV_IMU_DEVTYPE_SIM, BUS: 3, ADDR: 1, TYPE: SIMULATION
+	};
+
+	PX4Magnetometer		_px4_mag_0{197388, ROTATION_NONE}; // 197388: DRV_MAG_DEVTYPE_MAGSIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
+	PX4Magnetometer		_px4_mag_1{197644, ROTATION_NONE}; // 197644: DRV_MAG_DEVTYPE_MAGSIM, BUS: 2, ADDR: 1, TYPE: SIMULATION
+
+	uORB::PublicationMulti<sensor_baro_s> _sensor_baro_pubs[2] {{ORB_ID(sensor_baro)}, {ORB_ID(sensor_baro)}};
+
+	float _sensors_temperature{0};
+
+	perf_counter_t _perf_sim_delay{perf_alloc(PC_ELAPSED, MODULE_NAME": network delay")};
+	perf_counter_t _perf_sim_interval{perf_alloc(PC_INTERVAL, MODULE_NAME": network interval")};
+
+	// uORB publisher handlers
+	uORB::Publication<differential_pressure_s>	_differential_pressure_pub{ORB_ID(differential_pressure)};
+	uORB::PublicationMulti<sensor_optical_flow_s>	_sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)};
+	uORB::Publication<irlock_report_s>		_irlock_report_pub{ORB_ID(irlock_report)};
+	uORB::Publication<esc_status_s>			_esc_status_pub{ORB_ID(esc_status)};
+	uORB::Publication<vehicle_odometry_s>		_visual_odometry_pub{ORB_ID(vehicle_visual_odometry)};
+	uORB::Publication<vehicle_odometry_s>		_mocap_odometry_pub{ORB_ID(vehicle_mocap_odometry)};
+
+	uORB::Publication<vehicle_command_ack_s>	_command_ack_pub{ORB_ID(vehicle_command_ack)};
+
+	uORB::PublicationMulti<distance_sensor_s>	*_dist_pubs[ORB_MULTI_MAX_INSTANCES] {};
+	uint32_t _dist_sensor_ids[ORB_MULTI_MAX_INSTANCES] {};
+
+	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
+
+	unsigned int _port{14560};
+
+	InternetProtocol _ip{InternetProtocol::UDP};
+
+	std::string _hostname{""};
+
+	char *_tcp_remote_ipaddr{nullptr};
+
+	double _realtime_factor{1.0};		///< How fast the simulation runs in comparison to real system time
+
+	hrt_abstime _last_sim_timestamp{0};
+	hrt_abstime _last_sitl_timestamp{0};
+
+	void run();
+
+	void handle_message(const mavlink_message_t *msg);
+	void handle_message_distance_sensor(const mavlink_message_t *msg);
+	void handle_message_hil_gps(const mavlink_message_t *msg);
+	void handle_message_hil_sensor(const mavlink_message_t *msg);
+	void handle_message_hil_state_quaternion(const mavlink_message_t *msg);
+	void handle_message_landing_target(const mavlink_message_t *msg);
+	void handle_message_odometry(const mavlink_message_t *msg);
+	void handle_message_optical_flow(const mavlink_message_t *msg);
+	void handle_message_rc_channels(const mavlink_message_t *msg);
+	void handle_message_vision_position_estimate(const mavlink_message_t *msg);
+
+	void parameters_update(bool force);
+	void poll_for_MAVLink_messages();
+	void request_hil_state_quaternion();
+	void send();
+	void send_controls();
+	void send_heartbeat();
+	void send_esc_telemetry(mavlink_hil_actuator_controls_t hil_act_control);
+	void send_mavlink_message(const mavlink_message_t &aMsg);
+	void update_sensors(const hrt_abstime &time, const mavlink_hil_sensor_t &sensors);
+
+	static void *sending_trampoline(void *);
+
+	void actuator_controls_from_outputs(mavlink_hil_actuator_controls_t *msg);
+
+	// uORB publisher handlers
+	uORB::Publication<vehicle_angular_velocity_s>	_vehicle_angular_velocity_ground_truth_pub{ORB_ID(vehicle_angular_velocity_groundtruth)};
+	uORB::Publication<vehicle_attitude_s>		_attitude_ground_truth_pub{ORB_ID(vehicle_attitude_groundtruth)};
+	uORB::Publication<vehicle_global_position_s>	_gpos_ground_truth_pub{ORB_ID(vehicle_global_position_groundtruth)};
+	uORB::Publication<vehicle_local_position_s>	_lpos_ground_truth_pub{ORB_ID(vehicle_local_position_groundtruth)};
+	uORB::Publication<input_rc_s>			_input_rc_pub{ORB_ID(input_rc)};
+
+	//rpm
+	uORB::Publication<rpm_s>			_rpm_pub{ORB_ID(rpm)};
+
+	// HIL GPS
+	static constexpr int MAX_GPS = 3;
+	uORB::PublicationMulti<sensor_gps_s>	*_sensor_gps_pubs[MAX_GPS] {};
+	uint8_t _gps_ids[MAX_GPS] {};
+	std::default_random_engine _gen{};
+
+	// uORB subscription handlers
+	int _actuator_outputs_sub{-1};
+	actuator_outputs_s _actuator_outputs{};
+
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
+	uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
+	uORB::Subscription _battery_status_sub{ORB_ID(battery_status)};
+
+	// hil map_ref data
+	MapProjection _global_local_proj_ref{};
+	float _global_local_alt0{NAN};
+
+	vehicle_status_s _vehicle_status{};
+	battery_status_s _battery_status{};
+
+	bool _accel_blocked[ACCEL_COUNT_MAX] {};
+	bool _accel_stuck[ACCEL_COUNT_MAX] {};
+	sensor_accel_fifo_s _last_accel_fifo{};
+	matrix::Vector3f _last_accel[GYRO_COUNT_MAX] {};
+
+	bool _gyro_blocked[GYRO_COUNT_MAX] {};
+	bool _gyro_stuck[GYRO_COUNT_MAX] {};
+	sensor_gyro_fifo_s _last_gyro_fifo{};
+	matrix::Vector3f _last_gyro[GYRO_COUNT_MAX] {};
+
+	bool _baro_blocked{false};
+	bool _baro_stuck{false};
+
+	bool _mag_blocked{false};
+	bool _mag_stuck{false};
+
+	bool _gps_blocked{false};
+	bool _airspeed_blocked{false};
+
+	float _last_magx{0.0f};
+	float _last_magy{0.0f};
+	float _last_magz{0.0f};
+	bool _use_dynamic_mixing{false};
+
+	float _last_baro_pressure{0.0f};
+	float _last_baro_temperature{0.0f};
+
+	int32_t _output_functions[actuator_outputs_s::NUM_ACTUATOR_OUTPUTS] {};
+
+#if defined(ENABLE_LOCKSTEP_SCHEDULER)
+	px4::atomic<bool> _has_initialized {false};
+#endif
+
+	int _lockstep_component{-1};
+
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::MAV_TYPE>) _param_mav_type,
+		(ParamInt<px4::params::MAV_SYS_ID>) _param_mav_sys_id,
+		(ParamInt<px4::params::MAV_COMP_ID>) _param_mav_comp_id
+	)
+};

src/modules/simulation/simulator_mavlink/sitl_targets_flightgear.cmake

@@ -0,0 +1,43 @@
+if(ENABLE_LOCKSTEP_SCHEDULER STREQUAL "no")
+
+	px4_add_git_submodule(TARGET git_flightgear_bridge PATH "${PX4_SOURCE_DIR}/Tools/simulation/flightgear/flightgear_bridge")
+
+	include(ExternalProject)
+	ExternalProject_Add(flightgear_bridge
+		SOURCE_DIR ${PX4_SOURCE_DIR}/Tools/simulation/flightgear/flightgear_bridge
+		CMAKE_ARGS -DCMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX}
+		BINARY_DIR ${PX4_BINARY_DIR}/build_flightgear_bridge
+		INSTALL_COMMAND ""
+		DEPENDS git_flightgear_bridge
+		USES_TERMINAL_CONFIGURE true
+		USES_TERMINAL_BUILD true
+		EXCLUDE_FROM_ALL true
+		BUILD_ALWAYS 1
+	)
+
+	# flighgear targets
+	set(models
+		rascal
+		rascal-electric
+		tf-g1
+		tf-g2
+		tf-r1
+	)
+
+	# default flightgear target
+	add_custom_target(flightgear
+		COMMAND ${PX4_SOURCE_DIR}/Tools/simulation/flightgear/sitl_run.sh $<TARGET_FILE:px4> "rascal" ${PX4_SOURCE_DIR} ${PX4_BINARY_DIR}
+		WORKING_DIRECTORY ${SITL_WORKING_DIR}
+		USES_TERMINAL
+		DEPENDS px4 flightgear_bridge
+	)
+
+	foreach(model ${models})
+		add_custom_target(flightgear_${model}
+			COMMAND ${PX4_SOURCE_DIR}/Tools/simulation/flightgear/sitl_run.sh $<TARGET_FILE:px4> ${model} ${PX4_SOURCE_DIR} ${PX4_BINARY_DIR}
+			WORKING_DIRECTORY ${SITL_WORKING_DIR}
+			USES_TERMINAL
+			DEPENDS px4 flightgear_bridge
+		)
+	endforeach()
+endif()

src/modules/simulation/simulator_mavlink/sitl_targets_gazebo.cmake

@@ -0,0 +1,171 @@
+# Estimate an appropriate number of parallel jobs
+cmake_host_system_information(RESULT AVAILABLE_PHYSICAL_MEMORY QUERY AVAILABLE_PHYSICAL_MEMORY)
+cmake_host_system_information(RESULT NUMBER_OF_LOGICAL_CORES QUERY NUMBER_OF_LOGICAL_CORES)
+
+set(parallel_jobs 1)
+
+if(NOT NUMBER_OF_LOGICAL_CORES)
+	include(ProcessorCount)
+	ProcessorCount(NUMBER_OF_LOGICAL_CORES)
+endif()
+
+if(NOT AVAILABLE_PHYSICAL_MEMORY AND NUMBER_OF_LOGICAL_CORES GREATER_EQUAL 4)
+	# Memory estimate unavailable, use N-2 jobs
+	math(EXPR parallel_jobs "${NUMBER_OF_LOGICAL_CORES} - 2")
+endif()
+
+if(AVAILABLE_PHYSICAL_MEMORY)
+	# Allow an additional job for every 1.5GB of available physical memory
+	math(EXPR parallel_jobs "${AVAILABLE_PHYSICAL_MEMORY}/(3*1024/2)")
+else()
+	set(AVAILABLE_PHYSICAL_MEMORY "?")
+endif()
+
+if(parallel_jobs GREATER NUMBER_OF_LOGICAL_CORES)
+	set(parallel_jobs ${NUMBER_OF_LOGICAL_CORES})
+endif()
+
+if(parallel_jobs LESS 1)
+	set(parallel_jobs 1)
+endif()
+
+message(DEBUG "${NUMBER_OF_LOGICAL_CORES} logical cores detected and ${AVAILABLE_PHYSICAL_MEMORY} megabytes of memory available.
+		Limiting sitl_gazebo concurrent jobs to ${parallel_jobs}")
+
+# project to build sitl_gazebo if necessary
+px4_add_git_submodule(TARGET git_gazebo PATH "${PX4_SOURCE_DIR}/Tools/simulation/gazebo/sitl_gazebo")
+include(ExternalProject)
+ExternalProject_Add(sitl_gazebo
+	SOURCE_DIR ${PX4_SOURCE_DIR}/Tools/simulation/gazebo/sitl_gazebo
+	CMAKE_ARGS
+		-DCMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX}
+		-DSEND_ODOMETRY_DATA=ON
+		-DGENERATE_ROS_MODELS=ON
+	BINARY_DIR ${PX4_BINARY_DIR}/build_gazebo
+	INSTALL_COMMAND ""
+	DEPENDS git_gazebo
+	USES_TERMINAL_CONFIGURE true
+	USES_TERMINAL_BUILD true
+	EXCLUDE_FROM_ALL true
+	BUILD_ALWAYS 1
+	BUILD_COMMAND ${CMAKE_COMMAND} --build <BINARY_DIR> -- -j ${parallel_jobs}
+)
+
+# create targets for each model/world/debugger combination
+
+set(debuggers
+	none
+	gdb
+	lldb
+	valgrind
+	callgrind
+)
+
+set(models
+	none
+	believer
+	boat
+	cloudship
+	glider
+	iris
+	iris_dual_gps
+	iris_foggy_lidar
+	iris_irlock
+	iris_obs_avoid
+	iris_opt_flow
+	iris_opt_flow_mockup
+	iris_rplidar
+	iris_vision
+	nxp_cupcar
+	omnicopter
+	plane
+	plane_cam
+	plane_catapult
+	plane_lidar
+	px4vision
+	r1_rover
+	rover
+	shell
+	standard_vtol
+	standard_vtol_drop
+	tailsitter
+	techpod
+	tiltrotor
+	typhoon_h480
+	uuv_bluerov2_heavy
+	uuv_hippocampus
+)
+
+set(worlds
+	none
+	baylands
+	empty
+	ksql_airport
+	mcmillan_airfield
+	sonoma_raceway
+	warehouse
+	windy
+	yosemite
+)
+
+foreach(debugger ${debuggers})
+	foreach(model ${models})
+		foreach(world ${worlds})
+			if(world STREQUAL "none")
+				if(debugger STREQUAL "none")
+					if(model STREQUAL "none")
+						set(_targ_name "gazebo")
+					else()
+						set(_targ_name "gazebo_${model}")
+					endif()
+				else()
+					if(model STREQUAL "none")
+						set(_targ_name "gazebo___${debugger}")
+					else()
+						set(_targ_name "gazebo_${model}_${debugger}")
+					endif()
+				endif()
+
+				add_custom_target(${_targ_name}
+					COMMAND ${PX4_SOURCE_DIR}/Tools/simulation/gazebo/sitl_run.sh $<TARGET_FILE:px4> ${debugger} ${model} ${world} ${PX4_SOURCE_DIR} ${PX4_BINARY_DIR}
+					WORKING_DIRECTORY ${SITL_WORKING_DIR}
+					USES_TERMINAL
+					DEPENDS px4 sitl_gazebo
+				)
+			else()
+				if(debugger STREQUAL "none")
+					if(model STREQUAL "none")
+						set(_targ_name "gazebo___${world}")
+					else()
+						set(_targ_name "gazebo_${model}__${world}")
+					endif()
+				else()
+					if(model STREQUAL "none")
+						set(_targ_name "gazebo__${debugger}_${world}")
+					else()
+						set(_targ_name "gazebo_${model}_${debugger}_${world}")
+					endif()
+				endif()
+
+				add_custom_target(${_targ_name}
+					COMMAND ${PX4_SOURCE_DIR}/Tools/simulation/gazebo/sitl_run.sh $<TARGET_FILE:px4> ${debugger} ${model} ${world} ${PX4_SOURCE_DIR} ${PX4_BINARY_DIR}
+					WORKING_DIRECTORY ${SITL_WORKING_DIR}
+					USES_TERMINAL
+					DEPENDS px4 sitl_gazebo
+				)
+			endif()
+		endforeach()
+	endforeach()
+endforeach()
+
+# mavsdk tests currently depend on sitl_gazebo
+ExternalProject_Add(mavsdk_tests
+	SOURCE_DIR ${PX4_SOURCE_DIR}/test/mavsdk_tests
+	CMAKE_ARGS -DCMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX}
+	BINARY_DIR ${PX4_BINARY_DIR}/mavsdk_tests
+	INSTALL_COMMAND ""
+	USES_TERMINAL_CONFIGURE true
+	USES_TERMINAL_BUILD true
+	EXCLUDE_FROM_ALL true
+	BUILD_ALWAYS 1
+)

src/modules/simulation/simulator_mavlink/sitl_targets_jmavsim.cmake

@@ -0,0 +1,25 @@
+px4_add_git_submodule(TARGET git_jmavsim PATH "${PX4_SOURCE_DIR}/Tools/simulation/jmavsim/jMAVSim")
+
+# create targets for each viewer/model/debugger combination
+set(debuggers
+	none
+	gdb
+	lldb
+	valgrind
+	callgrind
+)
+
+foreach(debugger ${debuggers})
+	if(debugger STREQUAL "none")
+		set(_targ_name "jmavsim")
+	else()
+		set(_targ_name "jmavsim_${debugger}")
+	endif()
+
+	add_custom_target(${_targ_name}
+		COMMAND ${PX4_SOURCE_DIR}/Tools/simulation/jmavsim/sitl_run.sh $<TARGET_FILE:px4> ${debugger} ${PX4_SOURCE_DIR} ${PX4_BINARY_DIR}
+		WORKING_DIRECTORY ${SITL_WORKING_DIR}
+		USES_TERMINAL
+		DEPENDS px4 git_jmavsim
+	)
+endforeach()

src/modules/simulation/simulator_mavlink/sitl_targets_jsbsim.cmake

@@ -0,0 +1,56 @@
+
+px4_add_git_submodule(TARGET git_jsbsim_bridge PATH "${PX4_SOURCE_DIR}/Tools/simulation/jsbsim/jsbsim_bridge")
+
+include(ExternalProject)
+ExternalProject_Add(jsbsim_bridge
+	SOURCE_DIR ${PX4_SOURCE_DIR}/Tools/simulation/jsbsim/jsbsim_bridge
+	CMAKE_ARGS -DCMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX}
+	BINARY_DIR ${PX4_BINARY_DIR}/build_jsbsim_bridge
+	INSTALL_COMMAND ""
+	DEPENDS git_jsbsim_bridge
+	USES_TERMINAL_CONFIGURE true
+	USES_TERMINAL_BUILD true
+	EXCLUDE_FROM_ALL true
+	BUILD_ALWAYS 1
+)
+
+# jsbsim: create targets for jsbsim
+set(models
+	rascal
+	quadrotor_x
+	hexarotor_x
+	malolo
+)
+
+set(worlds
+	none
+	LSZH
+)
+
+# default jsbsim target
+add_custom_target(jsbsim
+	COMMAND ${PX4_SOURCE_DIR}/Tools/simulation/jsbsim/sitl_run.sh $<TARGET_FILE:px4> "rascal" "LSZH" ${PX4_SOURCE_DIR} ${PX4_BINARY_DIR}
+	WORKING_DIRECTORY ${SITL_WORKING_DIR}
+	USES_TERMINAL
+	DEPENDS px4 jsbsim_bridge
+)
+
+foreach(model ${models})
+	foreach(world ${worlds})
+		if(world STREQUAL "none")
+			add_custom_target(jsbsim_${model}
+				COMMAND ${PX4_SOURCE_DIR}/Tools/simulation/jsbsim/sitl_run.sh $<TARGET_FILE:px4> ${model} "LSZH" ${PX4_SOURCE_DIR} ${PX4_BINARY_DIR}
+				WORKING_DIRECTORY ${SITL_WORKING_DIR}
+				USES_TERMINAL
+				DEPENDS px4 jsbsim_bridge
+			)
+		else()
+			add_custom_target(jsbsim_${model}__${world}
+				COMMAND ${PX4_SOURCE_DIR}/Tools/simulation/jsbsim/sitl_run.sh $<TARGET_FILE:px4> ${model} ${world} ${PX4_SOURCE_DIR} ${PX4_BINARY_DIR}
+				WORKING_DIRECTORY ${SITL_WORKING_DIR}
+				USES_TERMINAL
+				DEPENDS px4 jsbsim_bridge
+			)
+		endif()
+	endforeach()
+endforeach()

src/modules/simulation/simulator_sih/CMakeLists.txt

@@ -0,0 +1,67 @@
+############################################################################
+#
+#   Copyright (c) 2019-2022 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__simulation__simulator_sih
+	MAIN simulator_sih
+	COMPILE_FLAGS
+		${MAX_CUSTOM_OPT_LEVEL}
+	SRCS
+		aero.hpp
+		sih.cpp
+		sih.hpp
+	DEPENDS
+		mathlib
+		drivers_accelerometer
+		drivers_gyroscope
+		drivers_magnetometer
+	)
+
+if(PX4_PLATFORM MATCHES "posix")
+	# create targets for sihsim
+	set(models
+		airplane
+		quadx
+		xvert
+	)
+
+	foreach(model ${models})
+		add_custom_target(sihsim_${model}
+			COMMAND ${CMAKE_COMMAND} -E env PX4_SIM_MODEL=${model} PX4_SIMULATOR=sihsim $<TARGET_FILE:px4>
+			WORKING_DIRECTORY ${SITL_WORKING_DIR}
+			USES_TERMINAL
+			DEPENDS px4
+		)
+	endforeach()
+
+endif()

src/modules/simulation/simulator_sih/Kconfig

@@ -0,0 +1,12 @@
+menuconfig MODULES_SIMULATION_SIMULATOR_SIH
+	bool "simulator_sih"
+	default n
+	---help---
+		Enable support for simulator_sih
+
+menuconfig USER_SIH
+	bool "simulator_sih running as userspace module"
+	default y
+	depends on BOARD_PROTECTED && MODULES_SIMULATION_SIMULATOR_SIH
+	---help---
+		Put sih in userspace memory

src/modules/simulation/simulator_sih/aero.hpp

@@ -0,0 +1,441 @@
+/****************************************************************************
+*
+*   Copyright (c) 2019-2022 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.
+*
+****************************************************************************/
+
+/**
+ * @file aero.hpp
+ * Aerodynamic class for modeling wing, tailaplane, fin.
+ * Captures the effect of partial stall, low aspect ratio, control surfaces deflection,
+ * propeller slipstream.
+ *
+ * @author Romain Chiappinelli      <romain.chiap@gmail.com>
+ *
+ * Altitude R&D inc, Montreal - July 2021
+ *
+ * The aerodynamic model is inspired from [2]
+ * [2] Khan, Waqas, supervised by Meyer Nahon "Dynamics modeling of agile fixed-wing unmanned aerial vehicles."
+ *  McGill University, PhD thesis, 2016. Sections 3.1, 3.2, and 3.3
+ *
+ * Capabilities and limitations
+ * This class can model
+ * - full 360 deg angle of attack lift, drag, and pitching moment.
+ * - wings with aspect ratio from 0.1666 up to 6, (gliders would perform poorly for instance).
+ * - control surface (flap) deflection up to 70 degrees.
+ * - unlimited flap chord, (the elevator could take the entire tailplane).
+ * - stall angle function of the flap deflection.
+ * - effect of the flap deflection on lift, drag, and pitching moment.
+ * - any dihedral angle, the fin is modeled as a wing with dihedral angle of 90 deg.
+ * - slipstream velocity (velocity from the propeller) using momentum theory.
+ */
+
+
+
+#pragma once
+
+#include <matrix/matrix/math.hpp>   	// matrix, vectors, dcm, quaterions
+#include <conversion/rotation.h>    	// math::radians,
+// #include <lib/mathlib/mathlib.h>
+#include <math.h>
+
+// class Aerodynamic Segment ------------------------------------------------------------------------
+class AeroSeg
+{
+private:
+	// Table 3.1 SEMI-EMPIRICAL COEFFICIENTS FOR RECTANGULAR FLAT PLATES
+	static constexpr const int N_TAB = 12;
+	// static constexpr const float AR_tab[N_TAB] = {0.1666f,0.333f,0.4f,0.5f,1.0f,1.25f, 2.0f, 3.0f, 4.0f, 6.0f};
+	// static constexpr const float ale_tab[N_TAB] = {3.00f,3.64f,4.48f,7.18f,10.20f,13.38f,14.84f,14.49f,9.95f,12.93f,15.00f,15.00f};
+	// static constexpr const float ate_tab[N_TAB] = {5.90f,15.51f,32.57f,39.44f,48.22f,59.29f,21.55f,7.74f,7.05f,5.26f,6.50f,6.50f};
+	// static constexpr const float afle_tab[N_TAB] = {59.00f,58.60f,58.20f,50.00f,41.53f,26.70f,23.44f,21.00f,18.63f,14.28f,11.60f,10.00f};
+	// static constexpr const float afte_tab[N_TAB] = {59.00f,58.60f,58.20f,51.85f,41.46f,28.09f,39.40f,35.86f,26.76f,19.76f,16.43f,14.00f};
+	// static constexpr const float afs_tab[N_TAB] = {49.00f,54.00f,56.00f,48.00f,40.00f,29.00f,27.00f,25.00f,24.00f,22.00f,22.00f,20.00f};
+
+	// deflection effectiveness curve fitted as second order
+	static constexpr const float ETA_POLY[] = {0.0535f, -0.2688f, 0.5817f}; 	// 1/rad
+
+	// aerodynamic and physical constants
+	static constexpr const float P0 = 101325.0f;	// _pressure at sea level [N/m^2]=[Pa]
+	static constexpr const float R = 287.04f;		// real gas constant for air [J/kg/K]
+	static constexpr const float T0_K = 288.15f;	// _temperature at sea level [K]
+	static constexpr const float TEMP_GRADIENT  = -6.5e-3f;    // _temperature gradient in degrees per metre
+
+	static constexpr const float KV = M_PI_F; 	// total vortex lift parameter
+	static constexpr float CD0 = 0.04f; 		// no lift drag coefficient
+	static constexpr float CD90 = 1.98f; 		// 90 deg angle of attack drag coefficient
+	static constexpr float AF_DOT_MAX = M_PI_F / 2.0f;
+	static constexpr float ALPHA_BLEND = M_PI_F / 18.0f; 	// 10 degrees
+
+	// here we make the distinction of the plate (i.e. wing, or tailplane, or fin) and the segment
+	// the segment can be a portion of the wing, but the aspect ratio (AR) of the wing needs to be used
+	float _alpha; 		// angle of attack [rad]
+	float _CL, _CD, _CM; 	// aerodynamic coefficients
+	float _CL_, _CD_, _CM_; 	// low aoa coeffs
+	float _f_blend;		// blending function
+	matrix::Vector3f _p_B; 	// position of the aerodynamic center of the segment from _CM in body frame [m]
+	matrix::Dcmf _C_BS;	// dcm from segment frame to body frame
+	float _ar;		// aspect ratio of the plate
+	float _span;		// _span of the segment
+	float _mac;		// mean aerodynamic chord of the segment
+	float _alpha_0;		// zero lift angle of attack [rad]
+	float _kp, _kn;
+	float _ate, _ale, _afte, _afle;	// semi empirical coefficients for flat plates function of AR
+	float _tau_te, _tau_le, _fte, _fle; 	// leading and trailing edge functions
+	float _rho = 1.225f; 	// air density at current altitude [kg/m^3]
+	float _kD;		// for parabolic drag model
+	const float K0 = 0.87f;	// Oswald efficiency factor
+	// variables for flap model
+	float _eta_f;		// flap effectiveness
+	float _def_a;		// absolute value of the deflection angle
+	float _cf;		// flap chord (control surface chord length)
+	float _theta_f, _tau_f;	// check 3.2.3 in [2]
+	float _deltaCL, _dCLmax;	// increase in lift coefficient
+	float _CLmax, _CLmin;	// max and min lift value
+	float _alpha_eff_min; 	// min effective angle of attack
+	float _alpha_eff_max;	// max effective angle of attack
+	float _alpha_min; 	// min angle of attack (stall angle)
+	float _alpha_max;	// min angle of attack (stall angle)
+	float _alf0eff;		// effective zero lift angle of attack
+	// float _alfmeff;		// effective maximum lift angle of attack
+	float _alpha_eff;	// effectie angle of attack
+	// float _alpha_eff_dot;	// effectie angle of attack derivative
+	float _alpha_eff_old;	// angle of attack [rad]
+
+	float _pressure; 	// pressure in Pa at current altitude
+	float _temperature;	// temperature in K at current altitude
+	float _prop_radius;	// propeller radius [m], used to create the slipstream
+	// float _v_slipstream;	// slipstream velocity [m/s], computed from momentum theory
+
+	matrix::Vector3f _Fa;	// aerodynamic force
+	matrix::Vector3f _Ma;	// aerodynamic moment computed at _CM directly
+	matrix::Vector3f _v_S;	// velocity in segment frame
+
+public:
+	/** public constructor
+	 * AeroSeg(float span, float mac, float alpha_0_deg, matrix::Vector3f p_B, float dihedral_deg = 0.0f,
+	 * float AR = -1.0f, float cf = 0.0f, float prop_radius=-1.0f, float cl_alpha=2.0f*M_PI_F, float alpha_max_deg=0.0f, float alpha_min_deg=0.0f)
+	 *
+	 * span_: span of the segment [m]
+	 * mac_: mean aerodynamic chord of the segment [m]
+	 * alpha_0_deg: zero lift angle of attack of the segment [deg], negative number represents a segment oriented up
+	 * p_B_: position of the segment (mean aerodynamic center) in the body frame from the center of mass [m,m,m]
+	 * dihedral_deg: dihedral angle of the segment [deg], set to 0 for tailplane, set to -90 for the fin. default is 0.
+	 * AR: Aspect Ratio of the wing, or tailplane, or fin (not the segment).
+	 *     If the aspect ratio is negative, the aspect ratio is computed from the _span and MAC
+	 * cf_: flap chord [m], this is the chord length of the control surface, default is zero (no flap).
+	 * prop_radius_: radius of the propeller for slipstream computation. Setting to -1 (default) will assume no slipstream.
+	 * cl_alpha: 2D lift curve slope (1/rad), default it 2*pi for a flat plate. This can be computed from http://airfoiltools.com for instance.
+	 * alpha_max_deg: maximum angle of attack before stall. Setting to 0 (default) will compute it from a table for flat plate.
+	 * alpha_min_deg: maximum negative angle of attack before stall. Setting to 0 (default) will compute it from a table for flat plate.
+	 */
+	AeroSeg(float span, float mac, float alpha_0_deg, const matrix::Vector3f &p_B, float dihedral_deg = 0.0f,
+		float AR = -1.0f, float cf = 0.0f, float prop_radius = -1.0f, float cl_alpha = 2.0f * M_PI_F,
+		float alpha_max_deg = 0.0f, float alpha_min_deg = 0.0f)
+	{
+		static const float AR_tab[N_TAB] = {0.1666f, 0.333f, 0.4f, 0.5f, 1.0f, 1.25f, 2.0f, 3.0f, 4.0f, 6.0f};
+		static const float ale_tab[N_TAB] = {3.00f, 3.64f, 4.48f, 7.18f, 10.20f, 13.38f, 14.84f, 14.49f, 9.95f, 12.93f, 15.00f, 15.00f};
+		static const float ate_tab[N_TAB] = {5.90f, 15.51f, 32.57f, 39.44f, 48.22f, 59.29f, 21.55f, 7.74f, 7.05f, 5.26f, 6.50f, 6.50f};
+		static const float afle_tab[N_TAB] = {59.00f, 58.60f, 58.20f, 50.00f, 41.53f, 26.70f, 23.44f, 21.00f, 18.63f, 14.28f, 11.60f, 10.00f};
+		static const float afte_tab[N_TAB] = {59.00f, 58.60f, 58.20f, 51.85f, 41.46f, 28.09f, 39.40f, 35.86f, 26.76f, 19.76f, 16.43f, 14.00f};
+		static const float afs_tab[N_TAB] = {49.00f, 54.00f, 56.00f, 48.00f, 40.00f, 29.00f, 27.00f, 25.00f, 24.00f, 22.00f, 22.00f, 20.00f};
+
+		_span = span;
+		_mac = mac;
+		_alpha_0 = math::radians(alpha_0_deg);
+		_p_B = p_B;
+		_ar = (AR <= 0.0f) ? _span / _mac : AR; // setting AR<=0 will compute it from _span and _mac
+		_alpha_eff = 0.0f;
+		_alpha_eff_old = 0.0f;
+		_kp = cl_alpha / (1.0f + 2.0f * (_ar + 4.0f) / (_ar * (_ar + 2.0f)));
+		_kn = 0.41f * (1.0f - expf(-17.0f / _ar));
+		_ale = lin_interp_lkt(AR_tab, ale_tab, _ar, N_TAB);
+		_ate = lin_interp_lkt(AR_tab, ate_tab, _ar, N_TAB);
+		_afle = lin_interp_lkt(AR_tab, afle_tab, _ar, N_TAB);
+		_afte = lin_interp_lkt(AR_tab, afte_tab, _ar, N_TAB);
+		float afs_rad = math::radians(lin_interp_lkt(AR_tab, afs_tab, _ar, N_TAB));
+
+		if (fabsf(alpha_max_deg) < 1.0e-3f) {
+			_alpha_max = afs_rad;
+
+		} else {
+			_alpha_max = math::radians(alpha_max_deg);
+		}
+
+		if (fabsf(alpha_min_deg) < 1.0e-3f) {
+			_alpha_min = -afs_rad;
+
+		} else {
+			_alpha_min = math::radians(alpha_min_deg);
+		}
+
+		_cf = math::constrain(cf, 0.0f, mac);
+		_C_BS = matrix::Dcmf(matrix::Eulerf(math::radians(dihedral_deg), 0.0f, 0.0f));
+		_prop_radius = prop_radius;
+		_kD = 1.0f / (M_PI_F * K0 * _ar);
+	}
+
+
+	AeroSeg() : AeroSeg(1.0f, 0.2f, 0.0f, matrix::Vector3f())
+	{}
+
+	/** aerodynamic force and moments of a generic flate plate segment
+	 * void update_aero(matrix::Vector3f v_B, matrix::Vector3f w_B, float alt = 0.0f,
+	 *                  float def = 0.0f, float thrust=0.0f, float dt = -1.0f)
+	 *
+	 * v_B: 3D velocity in body frame [m/s], (front, right, down FRD frame)
+	 * w_B: 3D body rates in body frame [rad/s], FRD frame.
+	 * alt: altitude above mean sea level for computing air density [m], default is 0.
+	 * def: flap deflection angle [rad], default is 0.
+	 * thrust: thrust force [N] from the propeller to compute the slipstream velocity, default is 0.
+	 */
+	void update_aero(const matrix::Vector3f &v_B, const matrix::Vector3f &w_B, float alt = 0.0f, float def = 0.0f,
+			 float thrust = 0.0f)
+	{
+		// ISA model taken from Mustafa Cavcar, Anadolu University, Turkey
+		_pressure = P0 * powf(1.0f - 0.0065f * alt / T0_K, 5.2561f);
+		_temperature = T0_K + TEMP_GRADIENT * alt;
+		_rho = _pressure / R / _temperature;
+
+		_v_S = _C_BS.transpose() * (v_B + w_B % _p_B); 	// velocity in segment frame
+
+		if (_prop_radius > 1e-4f) {
+			// Add velocity generated from the propeller and thrust force.
+			// Computed from momentum theory.
+			// For info, the diameter of the slipstream is sqrt(2)*_prop_radius,
+			// this should be the width of the segment in the slipstream.
+			_v_S(0) += sqrtf(2.0f * thrust / (_rho * M_PI_F * _prop_radius * _prop_radius));
+		}
+
+		float vxz2 = _v_S(0) * _v_S(0) + _v_S(2) * _v_S(2);
+
+		if (vxz2 < 0.01f) {
+			_Fa = matrix::Vector3f();
+			_Ma = matrix::Vector3f();
+			_alpha = 0.0f;
+			return;
+		}
+
+		_alpha = matrix::wrap_pi(atan2f(_v_S(2), _v_S(0)) - _alpha_0);
+		// _alpha = atan2f(_v_S(2), _v_S(0));
+		aoa_coeff(_alpha, sqrtf(vxz2), def);
+		_Fa = _C_BS * (0.5f * _rho * vxz2 * _span * _mac) * matrix::Vector3f(_CL * sinf(_alpha) - _CD * cosf(_alpha),
+				0.0f,
+				-_CL * cosf(_alpha) - _CD * sinf(_alpha));
+		_Ma = _C_BS * (0.5f * _rho * vxz2 * _span * _mac * _mac) * matrix::Vector3f(0.0f, _CM,
+				0.0f) + _p_B % _Fa; 	// computed at vehicle _CM
+	}
+
+	// return the air density at current altitude, must be called after update_aero()
+	float get_rho() const { return _rho; }
+
+	// return angle of attack in radians
+	float get_aoa() const {return _alpha;}
+
+	// return the aspect ratio
+	float get_ar() const {return _ar;}
+
+	// return the sum of aerodynamic forces of the segment in the body frame, taken at the _CM,
+	// must be called after update_aero()
+	matrix::Vector3f get_Fa() const { return _Fa; }
+
+	// return the sum of aerodynamic moments of the segment in the body frame, taken at the _CM,
+	// must be called after update_aero()
+	matrix::Vector3f get_Ma() const { return _Ma; }
+
+	// return the velocity in segment frame
+	matrix::Vector3f get_vS() const { return _v_S; }
+
+private:
+
+	// low angle of attack and stalling region coefficient based on flat plate
+	void aoa_coeff(float a, float vxz, float def)
+	{
+		_alpha_eff_old = _alpha_eff;
+		_tau_te = (vxz > 0.01f) ? 4.5f * _mac / vxz : 0.0f;
+		_tau_le = (vxz > 0.01f) ? 0.5f * _mac / vxz : 0.0f;
+
+		// model for the control surface deflection
+		if (_cf / _mac < 0.999f) {
+			_def_a = fminf(fabsf(def), math::radians(70.0f));
+			_eta_f = _def_a * _def_a * ETA_POLY[0] + _def_a * ETA_POLY[1] + ETA_POLY[2];	// second order fit
+			_theta_f = acosf(2.0f * _cf / _mac - 1.0f);
+			_tau_f = 1.0f - (_theta_f - sinf(_theta_f)) / M_PI_F;
+			_deltaCL = _kp * _tau_f * _eta_f * def;
+			_dCLmax = (1.0f - _cf / _mac) * _deltaCL;
+			_alf0eff = solve_alpha_eff(_kp, KV, _deltaCL, _alpha_0);
+			_alpha_eff = a - _alf0eff;
+
+			// this doesn't seem to work, so let's comment it
+			// _alpha_eff_dot = math::constrain((_alpha_eff - _alpha_eff_old) / dt, -AF_DOT_MAX, AF_DOT_MAX);
+			// _fte = 0.5f * (1.0f - tanhf(_ate * ((_alpha_eff) - _tau_te * (_alpha_eff_dot)
+			// 	- math::radians(_afte))));	// normalized trailing edge separation
+			// _fle = 0.5f * (1.0f - tanhf(_ale * ((_alpha_eff) - _tau_le * (_alpha_eff_dot)
+			// 	- math::radians(_afle))));	// normalized leading edge separation
+
+			_fte = 1.0f;
+			_fle = 1.0f;
+			_CLmax = fCL(_alpha_max - _alpha_0) + _dCLmax;
+			_alpha_eff_max = _alf0eff - solve_alpha_eff(_kp, KV * _fle * _fle,
+					 _CLmax / (0.25f * (1.0f + sqrtf(_fte)) * (1.0f + sqrtf(_fte))), _alpha_max - _alpha_0);
+			_CLmin = fCL(_alpha_min - _alpha_0) + _dCLmax;
+			_alpha_eff_min = _alf0eff - solve_alpha_eff(_kp, KV * _fle * _fle,
+					 _CLmin / (0.25f * (1.0f + sqrtf(_fte)) * (1.0f + sqrtf(_fte))), _alpha_min - _alpha_0);
+
+		} else { 	// this segment is a full flap
+			_alpha_eff = a + def;
+
+			// this doesn't seem to work, so let's comment it
+			// _alpha_eff_dot = math::constrain((_alpha_eff - _alpha_eff_old) / dt, -AF_DOT_MAX, AF_DOT_MAX);
+			// _fte = 0.5f * (1.0f - tanhf(_ate * ((_alpha_eff) - _tau_te * (_alpha_eff_dot)
+			// 	- math::radians(_afte))));	// normalized trailing edge separation
+			// _fle = 0.5f * (1.0f - tanhf(_ale * ((_alpha_eff) - _tau_le * (_alpha_eff_dot)
+			// 	- math::radians(_afle))));	// normalized leading edge separation
+
+			_fte = 1.0f;
+			_fle = 1.0f;
+			_alpha_eff_max = _alpha_max;
+			_alpha_eff_min = _alpha_min;
+		}
+
+		// compute the aerodynamic coefficients
+		high_aoa_coeff(_alpha_eff, def);
+		_CL_ = fCL(_alpha_eff);
+		_CD_ = CD0 + fabsf(_CL * tanf(_alpha_eff));
+		// _CD_ = CD0 + _kD*_CL_*_CL_; 	// alternative method
+		_CM_ = -fCM(_alpha_eff);
+
+		// blending function
+		if (_alpha_eff > 0.0f) {
+			_f_blend = 0.5f * (1.0f - tanhf(4.0f * (_alpha_eff - _alpha_eff_max) / ALPHA_BLEND));
+
+		} else {
+			_f_blend = 0.5f * (1.0f - tanhf(-4.0f * (_alpha_eff - _alpha_eff_min) / ALPHA_BLEND));
+		}
+
+		_CL = _CL_ * _f_blend + _CL * (1.0f - _f_blend);
+		_CD = _CD_ * _f_blend + _CD * (1.0f - _f_blend);
+		_CM = _CM_ * _f_blend + _CM * (1.0f - _f_blend);
+	}
+
+	// high angle of attack coefficient based on flat plate
+	void high_aoa_coeff(float a, float def = 0.0f)
+	{
+		float mac_eff = sqrtf((_mac - _cf) * (_mac - _cf) + _cf * _cf + 2.0f * (_mac - _cf) * _cf * cosf(fabsf(def)));
+		a += asinf(_cf / mac_eff * sinf(def));
+		float cd90_eff = CD90 + 0.21f * def - 0.0426f * def *
+				 def; // this might not be accurate for lower flap chord to chord ratio
+		// normal coeff
+		float CN = cd90_eff * sinf(a) * (1.0f / (0.56f + 0.44f * sinf(fabsf(a))) - _kn);
+		// tengential coeff
+		float CT = 0.5f * CD0 * cosf(a);
+		_CL = CN * cosf(a) - CT * sinf(a);
+		_CD = CN * sinf(a) + CT * cosf(a);
+		_CM = -CN * (0.25f - 7.0f / 40.0f * (1.0f - 2.0f / M_PI_F * fabsf(a)));
+	}
+
+	// linear interpolation between 2 points
+	static float lin_interp(const float x0, const float y0, const float x1, const float y1, const float x)
+	{
+		if (x < x0) {
+			return y0;
+		}
+
+		if (x > x1) {
+			return y1;
+		}
+
+		float slope = (y1 - y0) / (x1 - x0);
+		return y0 + slope * (x - x0);
+	}
+
+	// lookup table linear interpolation
+	static float lin_interp_lkt(const float x_tab [], const float y_tab [], const float x, const int length)
+	{
+		if (x < x_tab[0]) {
+			return y_tab[0];
+		}
+
+		if (x > x_tab[length - 1]) {
+			return y_tab [length - 1];
+		}
+
+		int i = length - 2;
+
+		while (x_tab[i] > x) {
+			i--;
+		}
+
+		return lin_interp(x_tab[i], y_tab[i], x_tab[i + 1], y_tab[i + 1], x);
+	}
+
+	float solve_alpha_eff(const float Kp, const float Kv, const float dCL, const float a0)
+	{
+		// we use here the Newton method with explicit derivative to find the root of equation 3.15
+		float a = a0; 	// init the search
+
+		for (int i = 0; i < 3; i++) {
+			a = a - (-Kp * sinf(a) * cosf(a) * cosf(a) - Kv * fabsf(sinf(a)) * sinf(a) * cosf(a) - dCL) /
+			    (Kv * fabsf(sinf(a)) * sinf(a) * sinf(a) - Kv * fabsf(sinf(a)) * cosf(a) * cosf(a) - Kp * cosf(a) * cosf(a) * cosf(
+				     a) + 2 * Kp * cosf(a) * sinf(a) * sinf(a) - Kv * matrix::sign(sinf(a)) * cosf(a) * cosf(a) * sinf(a));
+		}
+
+		return a;
+	}
+
+	float fCL(float a)
+	{
+		return 0.25f * (1.0f + sqrtf(_fte)) * (1.0f + sqrtf(_fte)) * (_kp * sinf(a) * cosf(a) * cosf(a)
+				+ _fle * _fle * KV * fabsf(sinf(a)) * sinf(a) * cosf(a));
+	}
+
+	float fCM(float a)
+	{
+		return -0.25f * (1.0f + sqrtf(_fte)) * (1.0f + sqrtf(_fte)) * 0.0625f * (-1.0f + 6.0f * sqrtf(
+					_fte) - 5.0f * _fte) * _kp * sinf(a) * cosf(a)
+		       + 0.17f * _fle * _fle * KV * fabsf(sinf(a)) * sinf(a);
+	}
+
+	// AeroSeg operator=(const AeroSeg&) const {
+	// 	return this;
+	// }
+
+	// AeroSeg operator+(const States other) const {
+	// 	return AeroSeg(p_I+other.p_I, v_I+other.v_I, q+other.q, w_B+other.w_B);
+	// }
+
+	// void unwrap_states(matrix::Vector3f &p, matrix::Vector3f &v, matrix::Quatf &q_, matrix::Vector3f &w) {
+	// 	p=p_I;
+	// 	v=v_I;
+	// 	q_=q;
+	// 	w=w_B;
+	// }
+
+}; // ---------------------------------------------------------------------------------------------------------

src/modules/simulation/simulator_sih/sih.cpp

@@ -0,0 +1,826 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019-2022 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file sih.cpp
+ * Simulator in Hardware
+ *
+ * @author Romain Chiappinelli      <romain.chiap@gmail.com>
+ *
+ * Coriolis g Corporation - January 2019
+ */
+
+#include "aero.hpp"
+#include "sih.hpp"
+
+#include <px4_platform_common/getopt.h>
+#include <px4_platform_common/log.h>
+
+#include <drivers/drv_pwm_output.h>         // to get PWM flags
+#include <lib/drivers/device/Device.hpp>
+
+using namespace math;
+using namespace matrix;
+using namespace time_literals;
+
+Sih::Sih() :
+	ModuleParams(nullptr)
+{}
+
+Sih::~Sih()
+{
+	perf_free(_loop_perf);
+	perf_free(_loop_interval_perf);
+}
+
+void Sih::run()
+{
+	_px4_accel.set_temperature(T1_C);
+	_px4_gyro.set_temperature(T1_C);
+	_px4_mag.set_temperature(T1_C);
+
+	parameters_updated();
+	init_variables();
+	gps_no_fix();
+
+	const hrt_abstime task_start = hrt_absolute_time();
+	_last_run = task_start;
+	_gps_time = task_start;
+	_airspeed_time = task_start;
+	_gt_time = task_start;
+	_dist_snsr_time = task_start;
+	_vehicle = (VehicleType)constrain(_sih_vtype.get(), static_cast<typeof _sih_vtype.get()>(0),
+					  static_cast<typeof _sih_vtype.get()>(2));
+
+	if (_sys_ctrl_alloc.get()) {
+		_actuator_out_sub = uORB::Subscription{ORB_ID(actuator_outputs_sim)};
+	}
+
+#if defined(ENABLE_LOCKSTEP_SCHEDULER)
+	lockstep_loop();
+#else
+	realtime_loop();
+#endif
+	exit_and_cleanup();
+}
+
+#if defined(ENABLE_LOCKSTEP_SCHEDULER)
+// Get current timestamp in microseconds
+uint64_t micros()
+{
+	struct timeval t;
+	gettimeofday(&t, nullptr);
+	return t.tv_sec * ((uint64_t)1000000) + t.tv_usec;
+}
+
+void Sih::lockstep_loop()
+{
+
+	int rate = math::min(_imu_gyro_ratemax.get(), _imu_integration_rate.get());
+
+	// default to 400Hz (2500 us interval)
+	if (rate <= 0) {
+		rate = 400;
+	}
+
+	// 200 - 2000 Hz
+	int sim_interval_us = math::constrain(int(roundf(1e6f / rate)), 500, 5000);
+
+	float speed_factor = 1.f;
+	const char *speedup = getenv("PX4_SIM_SPEED_FACTOR");
+
+	if (speedup) {
+		speed_factor = atof(speedup);
+	}
+
+	int rt_interval_us = int(roundf(sim_interval_us / speed_factor));
+
+	PX4_INFO("Simulation loop with %d Hz (%d us sim time interval)", rate, sim_interval_us);
+	PX4_INFO("Simulation with %.1fx speedup. Loop with (%d us wall time interval)", (double)speed_factor, rt_interval_us);
+	uint64_t pre_compute_wall_time_us;
+
+	while (!should_exit()) {
+		pre_compute_wall_time_us = micros();
+		perf_count(_loop_interval_perf);
+
+		_current_simulation_time_us += sim_interval_us;
+		struct timespec ts;
+		abstime_to_ts(&ts, _current_simulation_time_us);
+		px4_clock_settime(CLOCK_MONOTONIC, &ts);
+
+		perf_begin(_loop_perf);
+		sensor_step();
+		perf_end(_loop_perf);
+
+		// Only do lock-step once we received the first actuator output
+		int sleep_time;
+		uint64_t current_wall_time_us;
+
+		if (_last_actuator_output_time <= 0) {
+			PX4_DEBUG("SIH starting up - no lockstep yet");
+			current_wall_time_us = micros();
+			sleep_time = math::max(0, sim_interval_us - (int)(current_wall_time_us - pre_compute_wall_time_us));
+
+		} else {
+			px4_lockstep_wait_for_components();
+			current_wall_time_us = micros();
+			sleep_time = math::max(0, rt_interval_us - (int)(current_wall_time_us - pre_compute_wall_time_us));
+		}
+
+		_achieved_speedup = 0.99f * _achieved_speedup + 0.01f * ((float)sim_interval_us / (float)(
+					    current_wall_time_us - pre_compute_wall_time_us + sleep_time));
+		usleep(sleep_time);
+	}
+}
+#endif
+
+void Sih::realtime_loop()
+{
+	int rate = _imu_gyro_ratemax.get();
+
+	// default to 250 Hz (4000 us interval)
+	if (rate <= 0) {
+		rate = 250;
+	}
+
+	// 200 - 2000 Hz
+	int interval_us = math::constrain(int(roundf(1e6f / rate)), 500, 5000);
+
+	px4_sem_init(&_data_semaphore, 0, 0);
+	hrt_call_every(&_timer_call, interval_us, interval_us, timer_callback, &_data_semaphore);
+
+	while (!should_exit()) {
+		px4_sem_wait(&_data_semaphore);     // periodic real time wakeup
+		perf_begin(_loop_perf);
+		sensor_step();
+		perf_end(_loop_perf);
+	}
+
+	hrt_cancel(&_timer_call);
+	px4_sem_destroy(&_data_semaphore);
+}
+
+
+void Sih::timer_callback(void *sem)
+{
+	px4_sem_post((px4_sem_t *)sem);
+}
+
+void Sih::sensor_step()
+{
+	// check for parameter updates
+	if (_parameter_update_sub.updated()) {
+		// clear update
+		parameter_update_s pupdate;
+		_parameter_update_sub.copy(&pupdate);
+
+		// update parameters from storage
+		updateParams();
+		parameters_updated();
+	}
+
+	perf_begin(_loop_perf);
+
+	_now = hrt_absolute_time();
+	_dt = (_now - _last_run) * 1e-6f;
+	_last_run = _now;
+
+	read_motors();
+
+	generate_force_and_torques();
+
+	equations_of_motion();
+
+	reconstruct_sensors_signals();
+
+	// update IMU every iteration
+	_px4_accel.update(_now, _acc(0), _acc(1), _acc(2));
+	_px4_gyro.update(_now, _gyro(0), _gyro(1), _gyro(2));
+
+	// magnetometer published at 50 Hz
+	if (_now - _mag_time >= 20_ms
+	    && fabs(_mag_offset_x) < 10000
+	    && fabs(_mag_offset_y) < 10000
+	    && fabs(_mag_offset_z) < 10000) {
+		_mag_time = _now;
+		_px4_mag.update(_now, _mag(0), _mag(1), _mag(2));
+	}
+
+	// baro published at 20 Hz
+	if (_now - _baro_time >= 50_ms
+	    && fabs(_baro_offset_m) < 10000) {
+		_baro_time = _now;
+
+		// publish
+		sensor_baro_s sensor_baro{};
+		sensor_baro.timestamp_sample = _now;
+		sensor_baro.device_id = 6620172; // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION
+		sensor_baro.pressure = _baro_p_mBar * 100.f;
+		sensor_baro.temperature = _baro_temp_c;
+		sensor_baro.error_count = 0;
+		sensor_baro.timestamp = hrt_absolute_time();
+		_sensor_baro_pub.publish(sensor_baro);
+	}
+
+	// gps published at 20Hz
+	if (_now - _gps_time >= 50_ms) {
+		_gps_time = _now;
+		send_gps();
+	}
+
+	if ((_vehicle == VehicleType::FW || _vehicle == VehicleType::TS) && _now - _airspeed_time >= 50_ms) {
+		_airspeed_time = _now;
+		send_airspeed();
+	}
+
+	// distance sensor published at 50 Hz
+	if (_now - _dist_snsr_time >= 20_ms
+	    && fabs(_distance_snsr_override) < 10000) {
+		_dist_snsr_time = _now;
+		send_dist_snsr();
+	}
+
+	// send groundtruth message every 40 ms
+	if (_now - _gt_time >= 40_ms) {
+		_gt_time = _now;
+
+		publish_sih();  // publish _sih message for debug purpose
+	}
+
+	perf_end(_loop_perf);
+}
+
+// store the parameters in a more convenient form
+void Sih::parameters_updated()
+{
+	_T_MAX = _sih_t_max.get();
+	_Q_MAX = _sih_q_max.get();
+	_L_ROLL = _sih_l_roll.get();
+	_L_PITCH = _sih_l_pitch.get();
+	_KDV = _sih_kdv.get();
+	_KDW = _sih_kdw.get();
+	_H0 = _sih_h0.get();
+
+	_LAT0 = (double)_sih_lat0.get() * 1.0e-7;
+	_LON0 = (double)_sih_lon0.get() * 1.0e-7;
+	_COS_LAT0 = cosl((long double)radians(_LAT0));
+
+	_MASS = _sih_mass.get();
+
+	_W_I = Vector3f(0.0f, 0.0f, _MASS * CONSTANTS_ONE_G);
+
+	_I = diag(Vector3f(_sih_ixx.get(), _sih_iyy.get(), _sih_izz.get()));
+	_I(0, 1) = _I(1, 0) = _sih_ixy.get();
+	_I(0, 2) = _I(2, 0) = _sih_ixz.get();
+	_I(1, 2) = _I(2, 1) = _sih_iyz.get();
+
+	// guards against too small determinants
+	_Im1 = 100.0f * inv(static_cast<typeof _I>(100.0f * _I));
+
+	_mu_I = Vector3f(_sih_mu_x.get(), _sih_mu_y.get(), _sih_mu_z.get());
+
+	_gps_used = _sih_gps_used.get();
+	_baro_offset_m = _sih_baro_offset.get();
+	_mag_offset_x = _sih_mag_offset_x.get();
+	_mag_offset_y = _sih_mag_offset_y.get();
+	_mag_offset_z = _sih_mag_offset_z.get();
+
+	_distance_snsr_min = _sih_distance_snsr_min.get();
+	_distance_snsr_max = _sih_distance_snsr_max.get();
+	_distance_snsr_override = _sih_distance_snsr_override.get();
+
+	_T_TAU = _sih_thrust_tau.get();
+}
+
+// initialization of the variables for the simulator
+void Sih::init_variables()
+{
+	srand(1234);    // initialize the random seed once before calling generate_wgn()
+
+	_p_I = Vector3f(0.0f, 0.0f, 0.0f);
+	_v_I = Vector3f(0.0f, 0.0f, 0.0f);
+	_q = Quatf(1.0f, 0.0f, 0.0f, 0.0f);
+	_w_B = Vector3f(0.0f, 0.0f, 0.0f);
+
+	_u[0] = _u[1] = _u[2] = _u[3] = 0.0f;
+}
+
+void Sih::gps_fix()
+{
+	_sensor_gps.fix_type = 3;  // 3D fix
+	_sensor_gps.satellites_used = _gps_used;
+	_sensor_gps.heading = NAN;
+	_sensor_gps.heading_offset = NAN;
+	_sensor_gps.s_variance_m_s = 0.5f;
+	_sensor_gps.c_variance_rad = 0.1f;
+	_sensor_gps.eph = 0.9f;
+	_sensor_gps.epv = 1.78f;
+	_sensor_gps.hdop = 0.7f;
+	_sensor_gps.vdop = 1.1f;
+}
+
+void Sih::gps_no_fix()
+{
+	_sensor_gps.fix_type = 0;  // 3D fix
+	_sensor_gps.satellites_used = _gps_used;
+	_sensor_gps.heading = NAN;
+	_sensor_gps.heading_offset = NAN;
+	_sensor_gps.s_variance_m_s = 100.f;
+	_sensor_gps.c_variance_rad = 100.f;
+	_sensor_gps.eph = 100.f;
+	_sensor_gps.epv = 100.f;
+	_sensor_gps.hdop = 100.f;
+	_sensor_gps.vdop = 100.f;
+}
+
+
+// read the motor signals outputted from the mixer
+void Sih::read_motors()
+{
+	actuator_outputs_s actuators_out;
+
+	float pwm_middle = 0.5f * (PWM_DEFAULT_MIN + PWM_DEFAULT_MAX);
+
+	if (_actuator_out_sub.update(&actuators_out)) {
+		_last_actuator_output_time = actuators_out.timestamp;
+
+		if (_sys_ctrl_alloc.get()) {
+			for (int i = 0; i < NB_MOTORS; i++) { // saturate the motor signals
+				if ((_vehicle == VehicleType::FW && i < 3) || (_vehicle == VehicleType::TS && i > 3)) {
+					_u[i] = actuators_out.output[i];
+
+				} else {
+					float u_sp = actuators_out.output[i];
+					_u[i] = _u[i] + _dt / _T_TAU * (u_sp - _u[i]); // first order transfer function with time constant tau
+				}
+			}
+
+		} else {
+			for (int i = 0; i < NB_MOTORS; i++) { // saturate the motor signals
+				if ((_vehicle == VehicleType::FW && i < 3) || (_vehicle == VehicleType::TS
+						&& i > 3)) { // control surfaces in range [-1,1]
+					_u[i] = constrain(2.0f * (actuators_out.output[i] - pwm_middle) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN), -1.0f, 1.0f);
+
+				} else { // throttle signals in range [0,1]
+					float u_sp = constrain((actuators_out.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN), 0.0f, 1.0f);
+					_u[i] = _u[i] + _dt / _T_TAU * (u_sp - _u[i]); // first order transfer function with time constant tau
+				}
+			}
+		}
+	}
+}
+
+// generate the motors thrust and torque in the body frame
+void Sih::generate_force_and_torques()
+{
+	if (_vehicle == VehicleType::MC) {
+		_T_B = Vector3f(0.0f, 0.0f, -_T_MAX * (+_u[0] + _u[1] + _u[2] + _u[3]));
+		_Mt_B = Vector3f(_L_ROLL * _T_MAX * (-_u[0] + _u[1] + _u[2] - _u[3]),
+				 _L_PITCH * _T_MAX * (+_u[0] - _u[1] + _u[2] - _u[3]),
+				 _Q_MAX * (+_u[0] + _u[1] - _u[2] - _u[3]));
+		_Fa_I = -_KDV * _v_I;   // first order drag to slow down the aircraft
+		_Ma_B = -_KDW * _w_B;   // first order angular damper
+
+	} else if (_vehicle == VehicleType::FW) {
+		_T_B = Vector3f(_T_MAX * _u[3], 0.0f, 0.0f); 	// forward thruster
+		// _Mt_B = Vector3f(_Q_MAX*_u[3], 0.0f,0.0f); 	// thruster torque
+		_Mt_B = Vector3f();
+		generate_fw_aerodynamics();
+
+	} else if (_vehicle == VehicleType::TS) {
+		_T_B = Vector3f(0.0f, 0.0f, -_T_MAX * (_u[0] + _u[1]));
+		_Mt_B = Vector3f(_L_ROLL * _T_MAX * (_u[1] - _u[0]), 0.0f, _Q_MAX * (_u[1] - _u[0]));
+		generate_ts_aerodynamics();
+
+		// _Fa_I = -_KDV * _v_I;   // first order drag to slow down the aircraft
+		// _Ma_B = -_KDW * _w_B;   // first order angular damper
+	}
+}
+
+void Sih::generate_fw_aerodynamics()
+{
+	_v_B = _C_IB.transpose() * _v_I; 	// velocity in body frame [m/s]
+	float altitude = _H0 - _p_I(2);
+	_wing_l.update_aero(_v_B, _w_B, altitude, _u[0]*FLAP_MAX);
+	_wing_r.update_aero(_v_B, _w_B, altitude, -_u[0]*FLAP_MAX);
+	_tailplane.update_aero(_v_B, _w_B, altitude, _u[1]*FLAP_MAX, _T_MAX * _u[3]);
+	_fin.update_aero(_v_B, _w_B, altitude, _u[2]*FLAP_MAX, _T_MAX * _u[3]);
+	_fuselage.update_aero(_v_B, _w_B, altitude);
+	_Fa_I = _C_IB * (_wing_l.get_Fa() + _wing_r.get_Fa() + _tailplane.get_Fa() + _fin.get_Fa() + _fuselage.get_Fa())
+		- _KDV * _v_I; 	// sum of aerodynamic forces
+	_Ma_B = _wing_l.get_Ma() + _wing_r.get_Ma() + _tailplane.get_Ma() + _fin.get_Ma() + _fuselage.get_Ma() - _KDW *
+		_w_B; 	// aerodynamic moments
+}
+
+void Sih::generate_ts_aerodynamics()
+{
+	_v_B = _C_IB.transpose() * _v_I; // velocity in body frame [m/s]
+	Vector3f Fa_ts = Vector3f();
+	Vector3f Ma_ts = Vector3f();
+	Vector3f v_ts = _C_BS.transpose() *
+			_v_B; // the aerodynamic is resolved in a frame like a standard aircraft (nose-right-belly)
+	Vector3f w_ts = _C_BS.transpose() * _w_B;
+	float altitude = _H0 - _p_I(2);
+
+	for (int i = 0; i < NB_TS_SEG; i++) {
+		if (i <= NB_TS_SEG / 2) {
+			_ts[i].update_aero(v_ts, w_ts, altitude, _u[5]*TS_DEF_MAX, _T_MAX * _u[1]);
+
+		} else {
+			_ts[i].update_aero(v_ts, w_ts, altitude, -_u[4]*TS_DEF_MAX, _T_MAX * _u[0]);
+		}
+
+		Fa_ts += _ts[i].get_Fa();
+		Ma_ts += _ts[i].get_Ma();
+	}
+
+	_Fa_I = _C_IB * _C_BS * Fa_ts - _KDV * _v_I; 	// sum of aerodynamic forces
+	_Ma_B = _C_BS * Ma_ts - _KDW * _w_B; 	// aerodynamic moments
+}
+
+// apply the equations of motion of a rigid body and integrate one step
+void Sih::equations_of_motion()
+{
+	_C_IB = matrix::Dcm<float>(_q); // body to inertial transformation
+
+	// Equations of motion of a rigid body
+	_p_I_dot = _v_I;                        // position differential
+	_v_I_dot = (_W_I + _Fa_I + _C_IB * _T_B) / _MASS;   // conservation of linear momentum
+	// _q_dot = _q.derivative1(_w_B);              // attitude differential
+	_dq = Quatf::expq(0.5f * _dt * _w_B);
+	_w_B_dot = _Im1 * (_Mt_B + _Ma_B - _w_B.cross(_I * _w_B)); // conservation of angular momentum
+
+	// fake ground, avoid free fall
+	if (_p_I(2) > 0.0f && (_v_I_dot(2) > 0.0f || _v_I(2) > 0.0f)) {
+		if (_vehicle == VehicleType::MC || _vehicle == VehicleType::TS) {
+			if (!_grounded) {    // if we just hit the floor
+				// for the accelerometer, compute the acceleration that will stop the vehicle in one time step
+				_v_I_dot = -_v_I / _dt;
+
+			} else {
+				_v_I_dot.setZero();
+			}
+
+			_v_I.setZero();
+			_w_B.setZero();
+			_grounded = true;
+
+		} else if (_vehicle == VehicleType::FW) {
+			if (!_grounded) {    // if we just hit the floor
+				// for the accelerometer, compute the acceleration that will stop the vehicle in one time step
+				_v_I_dot(2) = -_v_I(2) / _dt;
+
+			} else {
+				// we only allow negative acceleration in order to takeoff
+				_v_I_dot(2) = fminf(_v_I_dot(2), 0.0f);
+			}
+
+			// integration: Euler forward
+			_p_I = _p_I + _p_I_dot * _dt;
+			_v_I = _v_I + _v_I_dot * _dt;
+			Eulerf RPY = Eulerf(_q);
+			RPY(0) = 0.0f;	// no roll
+			RPY(1) = radians(0.0f); // pitch slightly up if needed to get some lift
+			_q = Quatf(RPY);
+			_w_B.setZero();
+			_grounded = true;
+		}
+
+	} else {
+		// integration: Euler forward
+		_p_I = _p_I + _p_I_dot * _dt;
+		_v_I = _v_I + _v_I_dot * _dt;
+		_q = _q * _dq;
+		_q.normalize();
+		// integration Runge-Kutta 4
+		// rk4_update(_p_I, _v_I, _q, _w_B);
+		_w_B = constrain(_w_B + _w_B_dot * _dt, -6.0f * M_PI_F, 6.0f * M_PI_F);
+		_grounded = false;
+	}
+}
+
+// Sih::States Sih::eom_f(States x) 	// equations of motion f: x'=f(x)
+// {
+// 	States x_dot{}; 	// dx/dt
+
+// 	Dcmf C_IB = matrix::Dcm<float>(x.q); // body to inertial transformation
+// 	// Equations of motion of a rigid body
+// 	x_dot.p_I = x.v_I;                        // position differential
+// 	x_dot.v_I = (_W_I + _Fa_I + C_IB * _T_B) / _MASS;   // conservation of linear momentum
+// 	x_dot.q = x.q.derivative1(x.w_B);              // attitude differential
+// 	x_dot.w_B = _Im1 * (_Mt_B + _Ma_B - x.w_B.cross(_I * x.w_B)); // conservation of angular momentum
+
+// 	return x_dot;
+// }
+
+// reconstruct the noisy sensor signals
+void Sih::reconstruct_sensors_signals()
+{
+	// The sensor signals reconstruction and noise levels are from [1]
+	// [1] Bulka, Eitan, and Meyer Nahon. "Autonomous fixed-wing aerobatics: from theory to flight."
+	//     In 2018 IEEE International Conference on Robotics and Automation (ICRA), pp. 6573-6580. IEEE, 2018.
+
+	// IMU
+	_acc = _C_IB.transpose() * (_v_I_dot - Vector3f(0.0f, 0.0f, CONSTANTS_ONE_G)) + noiseGauss3f(0.5f, 1.7f, 1.4f);
+	_gyro = _w_B + noiseGauss3f(0.14f, 0.07f, 0.03f);
+	_mag = _C_IB.transpose() * _mu_I + noiseGauss3f(0.02f, 0.02f, 0.03f);
+	_mag(0) += _mag_offset_x;
+	_mag(1) += _mag_offset_y;
+	_mag(2) += _mag_offset_z;
+
+	// barometer
+	float altitude = (_H0 - _p_I(2)) + _baro_offset_m + generate_wgn() * 0.14f; // altitude with noise
+	_baro_p_mBar = CONSTANTS_STD_PRESSURE_MBAR *        // reconstructed pressure in mBar
+		       powf((1.0f + altitude * TEMP_GRADIENT / T1_K), -CONSTANTS_ONE_G / (TEMP_GRADIENT * CONSTANTS_AIR_GAS_CONST));
+	_baro_temp_c = T1_K + CONSTANTS_ABSOLUTE_NULL_CELSIUS + TEMP_GRADIENT * altitude; // reconstructed temperture in Celsius
+
+	// GPS
+	_gps_lat_noiseless = _LAT0 + degrees((double)_p_I(0) / CONSTANTS_RADIUS_OF_EARTH);
+	_gps_lon_noiseless = _LON0 + degrees((double)_p_I(1) / CONSTANTS_RADIUS_OF_EARTH) / _COS_LAT0;
+	_gps_alt_noiseless = _H0 - _p_I(2);
+
+	_gps_lat = _gps_lat_noiseless + degrees((double)generate_wgn() * 0.2 / CONSTANTS_RADIUS_OF_EARTH);
+	_gps_lon = _gps_lon_noiseless + degrees((double)generate_wgn() * 0.2 / CONSTANTS_RADIUS_OF_EARTH) / _COS_LAT0;
+	_gps_alt = _gps_alt_noiseless + generate_wgn() * 0.5f;
+	_gps_vel = _v_I + noiseGauss3f(0.06f, 0.077f, 0.158f);
+}
+
+void Sih::send_gps()
+{
+	_sensor_gps.timestamp = _now;
+	_sensor_gps.lat = (int32_t)(_gps_lat * 1e7);       // Latitude in 1E-7 degrees
+	_sensor_gps.lon = (int32_t)(_gps_lon * 1e7); // Longitude in 1E-7 degrees
+	_sensor_gps.alt = (int32_t)(_gps_alt * 1000.0f); // Altitude in 1E-3 meters above MSL, (millimetres)
+	_sensor_gps.alt_ellipsoid = (int32_t)(_gps_alt * 1000); // Altitude in 1E-3 meters bove Ellipsoid, (millimetres)
+	_sensor_gps.vel_ned_valid = true;              // True if NED velocity is valid
+	_sensor_gps.vel_m_s = sqrtf(_gps_vel(0) * _gps_vel(0) + _gps_vel(1) * _gps_vel(
+					    1)); // GPS ground speed, (metres/sec)
+	_sensor_gps.vel_n_m_s = _gps_vel(0);           // GPS North velocity, (metres/sec)
+	_sensor_gps.vel_e_m_s = _gps_vel(1);           // GPS East velocity, (metres/sec)
+	_sensor_gps.vel_d_m_s = _gps_vel(2);           // GPS Down velocity, (metres/sec)
+	_sensor_gps.cog_rad = atan2(_gps_vel(1),
+				    _gps_vel(0)); // Course over ground (NOT heading, but direction of movement), -PI..PI, (radians)
+
+	if (_gps_used >= 4) {
+		gps_fix();
+
+	} else {
+		gps_no_fix();
+	}
+
+	// device id
+	device::Device::DeviceId device_id;
+	device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION;
+	device_id.devid_s.bus = 0;
+	device_id.devid_s.address = 0;
+	device_id.devid_s.devtype = DRV_GPS_DEVTYPE_SIM;
+	_sensor_gps.device_id = device_id.devid;
+
+	_sensor_gps_pub.publish(_sensor_gps);
+}
+
+void Sih::send_airspeed()
+{
+	airspeed_s airspeed{};
+	airspeed.timestamp_sample = _now;
+	airspeed.true_airspeed_m_s	= fmaxf(0.1f, _v_B(0) + generate_wgn() * 0.2f);
+	airspeed.indicated_airspeed_m_s = airspeed.true_airspeed_m_s * sqrtf(_wing_l.get_rho() / RHO);
+	airspeed.air_temperature_celsius = _baro_temp_c;
+	airspeed.confidence = 0.7f;
+	airspeed.timestamp = hrt_absolute_time();
+	_airspeed_pub.publish(airspeed);
+}
+
+void Sih::send_dist_snsr()
+{
+	_distance_snsr.timestamp = _now;
+	_distance_snsr.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
+	_distance_snsr.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
+	_distance_snsr.min_distance = _distance_snsr_min;
+	_distance_snsr.max_distance = _distance_snsr_max;
+	_distance_snsr.signal_quality = -1;
+	_distance_snsr.device_id = 0;
+
+	if (_distance_snsr_override >= 0.f) {
+		_distance_snsr.current_distance = _distance_snsr_override;
+
+	} else {
+		_distance_snsr.current_distance = -_p_I(2) / _C_IB(2, 2);
+
+		if (_distance_snsr.current_distance > _distance_snsr_max) {
+			// this is based on lightware lw20 behaviour
+			_distance_snsr.current_distance = UINT16_MAX / 100.f;
+
+		}
+	}
+
+	_distance_snsr_pub.publish(_distance_snsr);
+}
+
+void Sih::publish_sih()
+{
+	// publish angular velocity groundtruth
+	_vehicle_angular_velocity_gt.timestamp = hrt_absolute_time();
+	_vehicle_angular_velocity_gt.xyz[0] = _w_B(0); // rollspeed;
+	_vehicle_angular_velocity_gt.xyz[1] = _w_B(1); // pitchspeed;
+	_vehicle_angular_velocity_gt.xyz[2] = _w_B(2); // yawspeed;
+
+	_vehicle_angular_velocity_gt_pub.publish(_vehicle_angular_velocity_gt);
+
+	// publish attitude groundtruth
+	_att_gt.timestamp = hrt_absolute_time();
+	_att_gt.q[0] = _q(0);
+	_att_gt.q[1] = _q(1);
+	_att_gt.q[2] = _q(2);
+	_att_gt.q[3] = _q(3);
+
+	_att_gt_pub.publish(_att_gt);
+
+	// publish position groundtruth
+	_gpos_gt.timestamp = hrt_absolute_time();
+	_gpos_gt.lat = _gps_lat_noiseless;
+	_gpos_gt.lon = _gps_lon_noiseless;
+	_gpos_gt.alt = _gps_alt_noiseless;
+
+	_gpos_gt_pub.publish(_gpos_gt);
+}
+
+float Sih::generate_wgn()   // generate white Gaussian noise sample with std=1
+{
+	// algorithm 1:
+	// float temp=((float)(rand()+1))/(((float)RAND_MAX+1.0f));
+	// return sqrtf(-2.0f*logf(temp))*cosf(2.0f*M_PI_F*rand()/RAND_MAX);
+	// algorithm 2: from BlockRandGauss.hpp
+	static float V1, V2, S;
+	static bool phase = true;
+	float X;
+
+	if (phase) {
+		do {
+			float U1 = (float)rand() / (float)RAND_MAX;
+			float U2 = (float)rand() / (float)RAND_MAX;
+			V1 = 2.0f * U1 - 1.0f;
+			V2 = 2.0f * U2 - 1.0f;
+			S = V1 * V1 + V2 * V2;
+		} while (S >= 1.0f || fabsf(S) < 1e-8f);
+
+		X = V1 * float(sqrtf(-2.0f * float(logf(S)) / S));
+
+	} else {
+		X = V2 * float(sqrtf(-2.0f * float(logf(S)) / S));
+	}
+
+	phase = !phase;
+	return X;
+}
+
+// generate white Gaussian noise sample vector with specified std
+Vector3f Sih::noiseGauss3f(float stdx, float stdy, float stdz)
+{
+	return Vector3f(generate_wgn() * stdx, generate_wgn() * stdy, generate_wgn() * stdz);
+}
+
+int Sih::print_status()
+{
+#if defined(ENABLE_LOCKSTEP_SCHEDULER)
+	PX4_INFO("Running in lockstep mode");
+	PX4_INFO("Achieved speedup: %.2fX", (double)_achieved_speedup);
+#endif
+
+	if (_vehicle == VehicleType::MC) {
+		PX4_INFO("Running MultiCopter");
+
+	} else if (_vehicle == VehicleType::FW) {
+		PX4_INFO("Running Fixed-Wing");
+
+	} else if (_vehicle == VehicleType::TS) {
+		PX4_INFO("Running TailSitter");
+		PX4_INFO("aoa [deg]: %d", (int)(degrees(_ts[4].get_aoa())));
+		PX4_INFO("v segment (m/s)");
+		_ts[4].get_vS().print();
+	}
+
+	PX4_INFO("vehicle landed: %d", _grounded);
+	PX4_INFO("dt [us]: %d", (int)(_dt * 1e6f));
+	PX4_INFO("inertial position NED (m)");
+	_p_I.print();
+	PX4_INFO("inertial velocity NED (m/s)");
+	_v_I.print();
+	PX4_INFO("attitude roll-pitch-yaw (deg)");
+	(Eulerf(_q) * 180.0f / M_PI_F).print();
+	PX4_INFO("angular acceleration roll-pitch-yaw (deg/s)");
+	(_w_B * 180.0f / M_PI_F).print();
+	PX4_INFO("actuator signals");
+	Vector<float, 8> u = Vector<float, 8>(_u);
+	u.transpose().print();
+	PX4_INFO("Aerodynamic forces NED inertial (N)");
+	_Fa_I.print();
+	PX4_INFO("Aerodynamic moments body frame (Nm)");
+	_Ma_B.print();
+	PX4_INFO("Thruster moments in body frame (Nm)");
+	_Mt_B.print();
+	return 0;
+}
+
+
+int Sih::task_spawn(int argc, char *argv[])
+{
+	_task_id = px4_task_spawn_cmd("sih",
+				      SCHED_DEFAULT,
+				      SCHED_PRIORITY_MAX,
+				      1250,
+				      (px4_main_t)&run_trampoline,
+				      (char *const *)argv);
+
+	if (_task_id < 0) {
+		_task_id = -1;
+		return -errno;
+	}
+
+	return 0;
+}
+
+Sih *Sih::instantiate(int argc, char *argv[])
+{
+	Sih *instance = new Sih();
+
+	if (instance == nullptr) {
+		PX4_ERR("alloc failed");
+	}
+
+	return instance;
+}
+
+int Sih::custom_command(int argc, char *argv[])
+{
+	return print_usage("unknown command");
+}
+
+int Sih::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_WARN("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+This module provide a simulator for quadrotors and fixed-wings running fully
+inside the hardware autopilot.
+
+This simulator subscribes to "actuator_outputs" which are the actuator pwm
+signals given by the mixer.
+
+This simulator publishes the sensors signals corrupted with realistic noise
+in order to incorporate the state estimator in the loop.
+
+### Implementation
+The simulator implements the equations of motion using matrix algebra.
+Quaternion representation is used for the attitude.
+Forward Euler is used for integration.
+Most of the variables are declared global in the .hpp file to avoid stack overflow.
+
+
+)DESCR_STR");
+
+    PRINT_MODULE_USAGE_NAME("simulator_sih", "simulation");
+    PRINT_MODULE_USAGE_COMMAND("start");
+    PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+
+    return 0;
+}
+
+extern "C" __EXPORT int simulator_sih_main(int argc, char *argv[])
+{
+	return Sih::main(argc, argv);
+}

src/modules/simulation/simulator_sih/sih.hpp

@@ -0,0 +1,333 @@
+/****************************************************************************
+*
+*   Copyright (c) 2019-2022 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.
+*
+****************************************************************************/
+
+/**
+ * @file sih.hpp
+ * Simulator in Hardware
+ *
+ * @author Romain Chiappinelli      <romain.chiap@gmail.com>
+ *
+ * Coriolis g Corporation - January 2019
+ */
+
+// The sensor signals reconstruction and noise levels are from [1]
+// [1] Bulka E, and Nahon M, "Autonomous fixed-wing aerobatics: from theory to flight."
+//     In 2018 IEEE International Conference on Robotics and Automation (ICRA), pp. 6573-6580. IEEE, 2018.
+// The aerodynamic model is from [2]
+// [2] Khan W, supervised by Nahon M, "Dynamics modeling of agile fixed-wing unmanned aerial vehicles."
+//     McGill University (Canada), PhD thesis, 2016.
+// The quaternion integration are from [3]
+// [3] Sveier A, Sjøberg AM, Egeland O. "Applied Runge–Kutta–Munthe-Kaas Integration for the Quaternion Kinematics."
+//     Journal of Guidance, Control, and Dynamics. 2019 Dec;42(12):2747-54.
+// The tailsitter model is from [4]
+// [4] Chiappinelli R, supervised by Nahon M, "Modeling and control of a flying wing tailsitter unmanned aerial vehicle."
+//     McGill University (Canada), Masters Thesis, 2018.
+
+#pragma once
+
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/posix.h>
+
+#include <matrix/matrix/math.hpp>   // matrix, vectors, dcm, quaterions
+#include <conversion/rotation.h>    // math::radians,
+#include <lib/geo/geo.h>        // to get the physical constants
+#include <drivers/drv_hrt.h>        // to get the real time
+#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
+#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
+#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
+#include <perf/perf_counter.h>
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionInterval.hpp>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/sensor_gps.h>
+#include <uORB/topics/sensor_baro.h>
+#include <uORB/topics/vehicle_angular_velocity.h>   // to publish groundtruth
+#include <uORB/topics/vehicle_attitude.h>           // to publish groundtruth
+#include <uORB/topics/vehicle_global_position.h>    // to publish groundtruth
+#include <uORB/topics/distance_sensor.h>
+#include <uORB/topics/airspeed.h>
+
+#if defined(ENABLE_LOCKSTEP_SCHEDULER)
+#include <sys/time.h>
+#endif
+
+using namespace time_literals;
+
+extern "C" __EXPORT int sih_main(int argc, char *argv[]);
+
+class Sih : public ModuleBase<Sih>, public ModuleParams
+{
+public:
+	Sih();
+
+	virtual ~Sih();
+
+	/** @see ModuleBase */
+	static int task_spawn(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static Sih *instantiate(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int custom_command(int argc, char *argv[]);
+
+	/** @see ModuleBase::print_status() */
+	int print_status() override;
+
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);
+
+	/** @see ModuleBase::run() */
+	void run() override;
+
+	static float generate_wgn();    // generate white Gaussian noise sample
+
+	// generate white Gaussian noise sample as a 3D vector with specified std
+	static matrix::Vector3f noiseGauss3f(float stdx, float stdy, float stdz);
+
+	// timer called periodically to post the semaphore
+	static void timer_callback(void *sem);
+
+private:
+	void parameters_updated();
+
+	// simulated sensor instances
+	PX4Accelerometer _px4_accel{1310988}; // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
+	PX4Gyroscope     _px4_gyro{1310988};  // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
+	PX4Magnetometer  _px4_mag{197388};    //  197388: DRV_MAG_DEVTYPE_MAGSIM, BUS: 3, ADDR: 1, TYPE: SIMULATION
+
+	uORB::PublicationMulti<sensor_baro_s> _sensor_baro_pub{ORB_ID(sensor_baro)};
+
+	// to publish the gps position
+	sensor_gps_s			_sensor_gps{};
+	uORB::Publication<sensor_gps_s>	_sensor_gps_pub{ORB_ID(sensor_gps)};
+
+	// to publish the distance sensor
+	distance_sensor_s                    _distance_snsr{};
+	uORB::Publication<distance_sensor_s> _distance_snsr_pub{ORB_ID(distance_sensor)};
+
+	// angular velocity groundtruth
+	vehicle_angular_velocity_s			_vehicle_angular_velocity_gt{};
+	uORB::Publication<vehicle_angular_velocity_s>	_vehicle_angular_velocity_gt_pub{ORB_ID(vehicle_angular_velocity_groundtruth)};
+
+	// attitude groundtruth
+	vehicle_attitude_s				_att_gt{};
+	uORB::Publication<vehicle_attitude_s>		_att_gt_pub{ORB_ID(vehicle_attitude_groundtruth)};
+
+	// global position groundtruth
+	vehicle_global_position_s			_gpos_gt{};
+	uORB::Publication<vehicle_global_position_s>	_gpos_gt_pub{ORB_ID(vehicle_global_position_groundtruth)};
+
+	// airspeed
+	uORB::Publication<airspeed_s>				_airspeed_pub{ORB_ID(airspeed)};
+
+	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
+	uORB::Subscription _actuator_out_sub{ORB_ID(actuator_outputs)};
+
+	// hard constants
+	static constexpr uint16_t NB_MOTORS = 6;
+	static constexpr float T1_C = 15.0f;                        // ground temperature in Celsius
+	static constexpr float T1_K = T1_C - CONSTANTS_ABSOLUTE_NULL_CELSIUS;   // ground temperature in Kelvin
+	static constexpr float TEMP_GRADIENT  = -6.5f / 1000.0f;    // temperature gradient in degrees per metre
+	// Aerodynamic coefficients
+	static constexpr float RHO = 1.225f; 		// air density at sea level [kg/m^3]
+	static constexpr float SPAN = 0.86f; 	// wing span [m]
+	static constexpr float MAC = 0.21f; 	// wing mean aerodynamic chord [m]
+	static constexpr float RP = 0.1f; 	// radius of the propeller [m]
+	static constexpr float FLAP_MAX = M_PI_F / 12.0f; // 15 deg, maximum control surface deflection
+
+	void init_variables();
+	void gps_fix();
+	void gps_no_fix();
+	void read_motors();
+	void generate_force_and_torques();
+	void equations_of_motion();
+	void reconstruct_sensors_signals();
+	void send_gps();
+	void send_airspeed();
+	void send_dist_snsr();
+	void publish_sih();
+	void generate_fw_aerodynamics();
+	void generate_ts_aerodynamics();
+	void sensor_step();
+
+#if defined(ENABLE_LOCKSTEP_SCHEDULER)
+	void lockstep_loop();
+	uint64_t _current_simulation_time_us{0};
+	float _achieved_speedup{0.f};
+#endif
+
+	void realtime_loop();
+	px4_sem_t       _data_semaphore;
+	hrt_call 	_timer_call;
+
+	perf_counter_t  _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
+	perf_counter_t  _loop_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": cycle interval")};
+
+	hrt_abstime _last_run{0};
+	hrt_abstime _last_actuator_output_time{0};
+	hrt_abstime _baro_time{0};
+	hrt_abstime _gps_time{0};
+	hrt_abstime _airspeed_time{0};
+	hrt_abstime _mag_time{0};
+	hrt_abstime _gt_time{0};
+	hrt_abstime _dist_snsr_time{0};
+	hrt_abstime _now{0};
+	float       _dt{0};         // sampling time [s]
+	bool        _grounded{true};// whether the vehicle is on the ground
+
+	matrix::Vector3f    _T_B;           // thrust force in body frame [N]
+	matrix::Vector3f    _Fa_I;          // aerodynamic force in inertial frame [N]
+	matrix::Vector3f    _Mt_B;          // thruster moments in the body frame [Nm]
+	matrix::Vector3f    _Ma_B;          // aerodynamic moments in the body frame [Nm]
+	matrix::Vector3f    _p_I;           // inertial position [m]
+	matrix::Vector3f    _v_I;           // inertial velocity [m/s]
+	matrix::Vector3f    _v_B;           // body frame velocity [m/s]
+	matrix::Vector3f    _p_I_dot;       // inertial position differential
+	matrix::Vector3f    _v_I_dot;       // inertial velocity differential
+	matrix::Quatf       _q;             // quaternion attitude
+	matrix::Dcmf        _C_IB;          // body to inertial transformation
+	matrix::Vector3f    _w_B;           // body rates in body frame [rad/s]
+	matrix::Quatf       _dq;            // quaternion differential
+	matrix::Vector3f    _w_B_dot;       // body rates differential
+	float       _u[NB_MOTORS];          // thruster signals
+
+	enum class VehicleType {MC, FW, TS};
+	VehicleType _vehicle = VehicleType::MC;
+
+	// aerodynamic segments for the fixedwing
+	AeroSeg _wing_l = AeroSeg(SPAN / 2.0f, MAC, -4.0f, matrix::Vector3f(0.0f, -SPAN / 4.0f, 0.0f), 3.0f,
+				  SPAN / MAC, MAC / 3.0f);
+	AeroSeg _wing_r = AeroSeg(SPAN / 2.0f, MAC, -4.0f, matrix::Vector3f(0.0f, SPAN / 4.0f, 0.0f), -3.0f,
+				  SPAN / MAC, MAC / 3.0f);
+	AeroSeg _tailplane = AeroSeg(0.3f, 0.1f, 0.0f, matrix::Vector3f(-0.4f, 0.0f, 0.0f), 0.0f, -1.0f, 0.05f, RP);
+	AeroSeg _fin = AeroSeg(0.25, 0.18, 0.0f, matrix::Vector3f(-0.45f, 0.0f, -0.1f), -90.0f, -1.0f, 0.12f, RP);
+	AeroSeg _fuselage = AeroSeg(0.2, 0.8, 0.0f, matrix::Vector3f(0.0f, 0.0f, 0.0f), -90.0f);
+
+	// aerodynamic segments for the tailsitter
+	static constexpr const int NB_TS_SEG = 11;
+	static constexpr const float TS_AR = 3.13f;
+	static constexpr const float TS_CM = 0.115f;	// longitudinal position of the CM from trailing edge
+	static constexpr const float TS_RP = 0.0625f;	// propeller radius [m]
+	static constexpr const float TS_DEF_MAX = math::radians(39.0f); 	// max deflection
+	matrix::Dcmf _C_BS = matrix::Dcmf(matrix::Eulerf(0.0f, math::radians(90.0f), 0.0f)); // segment to body 90 deg pitch
+	AeroSeg _ts[NB_TS_SEG] = {
+		AeroSeg(0.0225f, 0.110f, 0.0f, matrix::Vector3f(0.083f - TS_CM, -0.239f, 0.0f), 0.0f, TS_AR),
+		AeroSeg(0.0383f, 0.125f, 0.0f, matrix::Vector3f(0.094f - TS_CM, -0.208f, 0.0f), 0.0f, TS_AR, 0.063f),
+		// AeroSeg(0.0884f, 0.148f, 0.0f, matrix::Vector3f(0.111f-TS_CM, -0.143f, 0.0f), 0.0f, TS_AR, 0.063f, TS_RP),
+		AeroSeg(0.0884f, 0.085f, 0.0f, matrix::Vector3f(0.158f - TS_CM, -0.143f, 0.0f), 0.0f, TS_AR),
+		AeroSeg(0.0884f, 0.063f, 0.0f, matrix::Vector3f(0.047f - TS_CM, -0.143f, 0.0f), 0.0f, TS_AR, 0.063f, TS_RP),
+		AeroSeg(0.0633f, 0.176f, 0.0f, matrix::Vector3f(0.132f - TS_CM, -0.068f, 0.0f), 0.0f, TS_AR, 0.063f),
+		AeroSeg(0.0750f, 0.231f, 0.0f, matrix::Vector3f(0.173f - TS_CM,  0.000f, 0.0f), 0.0f, TS_AR),
+		AeroSeg(0.0633f, 0.176f, 0.0f, matrix::Vector3f(0.132f - TS_CM,  0.068f, 0.0f), 0.0f, TS_AR, 0.063f),
+		// AeroSeg(0.0884f, 0.148f, 0.0f, matrix::Vector3f(0.111f-TS_CM,  0.143f, 0.0f), 0.0f, TS_AR, 0.063f, TS_RP),
+		AeroSeg(0.0884f, 0.085f, 0.0f, matrix::Vector3f(0.158f - TS_CM,  0.143f, 0.0f), 0.0f, TS_AR),
+		AeroSeg(0.0884f, 0.063f, 0.0f, matrix::Vector3f(0.047f - TS_CM,  0.143f, 0.0f), 0.0f, TS_AR, 0.063f, TS_RP),
+		AeroSeg(0.0383f, 0.125f, 0.0f, matrix::Vector3f(0.094f - TS_CM,  0.208f, 0.0f), 0.0f, TS_AR, 0.063f),
+		AeroSeg(0.0225f, 0.110f, 0.0f, matrix::Vector3f(0.083f - TS_CM,  0.239f, 0.0f), 0.0f, TS_AR)
+	};
+
+	// AeroSeg _ts[NB_TS_SEG] = {
+	// 	AeroSeg(0.0225f, 0.110f, -90.0f, matrix::Vector3f(0.0f, -0.239f, TS_CM-0.083f), 0.0f, TS_AR),
+	// 	AeroSeg(0.0383f, 0.125f, -90.0f, matrix::Vector3f(0.0f, -0.208f, TS_CM-0.094f), 0.0f, TS_AR, 0.063f),
+	// 	AeroSeg(0.0884f, 0.148f, -90.0f, matrix::Vector3f(0.0f, -0.143f, TS_CM-0.111f), 0.0f, TS_AR, 0.063f, TS_RP),
+	// 	AeroSeg(0.0633f, 0.176f, -90.0f, matrix::Vector3f(0.0f, -0.068f, TS_CM-0.132f), 0.0f, TS_AR, 0.063f),
+	// 	AeroSeg(0.0750f, 0.231f, -90.0f, matrix::Vector3f(0.0f,  0.000f, TS_CM-0.173f), 0.0f, TS_AR),
+	// 	AeroSeg(0.0633f, 0.176f, -90.0f, matrix::Vector3f(0.0f,  0.068f, TS_CM-0.132f), 0.0f, TS_AR, 0.063f),
+	// 	AeroSeg(0.0884f, 0.148f, -90.0f, matrix::Vector3f(0.0f,  0.143f, TS_CM-0.111f), 0.0f, TS_AR, 0.063f, TS_RP),
+	// 	AeroSeg(0.0383f, 0.125f, -90.0f, matrix::Vector3f(0.0f,  0.208f, TS_CM-0.094f), 0.0f, TS_AR, 0.063f),
+	// 	AeroSeg(0.0225f, 0.110f, -90.0f, matrix::Vector3f(0.0f,  0.239f, TS_CM-0.083f), 0.0f, TS_AR)
+	// 	};
+
+	// sensors reconstruction
+	matrix::Vector3f    _acc;
+	matrix::Vector3f    _mag;
+	matrix::Vector3f    _gyro;
+	matrix::Vector3f    _gps_vel;
+	double      _gps_lat, _gps_lat_noiseless;
+	double      _gps_lon, _gps_lon_noiseless;
+	float       _gps_alt, _gps_alt_noiseless;
+	float       _baro_p_mBar;   // reconstructed (simulated) pressure in mBar
+	float       _baro_temp_c;   // reconstructed (simulated) barometer temperature in degrees Celsius
+
+	// parameters
+	float _MASS, _T_MAX, _Q_MAX, _L_ROLL, _L_PITCH, _KDV, _KDW, _H0, _T_TAU;
+	double _LAT0, _LON0, _COS_LAT0;
+	matrix::Vector3f _W_I;  // weight of the vehicle in inertial frame [N]
+	matrix::Matrix3f _I;    // vehicle inertia matrix
+	matrix::Matrix3f _Im1;  // inverse of the inertia matrix
+	matrix::Vector3f _mu_I; // NED magnetic field in inertial frame [G]
+
+	int _gps_used;
+	float _baro_offset_m, _mag_offset_x, _mag_offset_y, _mag_offset_z;
+	float _distance_snsr_min, _distance_snsr_max, _distance_snsr_override;
+
+	// parameters defined in sih_params.c
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::IMU_GYRO_RATEMAX>) _imu_gyro_ratemax,
+		(ParamInt<px4::params::IMU_INTEG_RATE>) _imu_integration_rate,
+		(ParamFloat<px4::params::SIH_MASS>) _sih_mass,
+		(ParamFloat<px4::params::SIH_IXX>) _sih_ixx,
+		(ParamFloat<px4::params::SIH_IYY>) _sih_iyy,
+		(ParamFloat<px4::params::SIH_IZZ>) _sih_izz,
+		(ParamFloat<px4::params::SIH_IXY>) _sih_ixy,
+		(ParamFloat<px4::params::SIH_IXZ>) _sih_ixz,
+		(ParamFloat<px4::params::SIH_IYZ>) _sih_iyz,
+		(ParamFloat<px4::params::SIH_T_MAX>) _sih_t_max,
+		(ParamFloat<px4::params::SIH_Q_MAX>) _sih_q_max,
+		(ParamFloat<px4::params::SIH_L_ROLL>) _sih_l_roll,
+		(ParamFloat<px4::params::SIH_L_PITCH>) _sih_l_pitch,
+		(ParamFloat<px4::params::SIH_KDV>) _sih_kdv,
+		(ParamFloat<px4::params::SIH_KDW>) _sih_kdw,
+		(ParamInt<px4::params::SIH_LOC_LAT0>) _sih_lat0,
+		(ParamInt<px4::params::SIH_LOC_LON0>) _sih_lon0,
+		(ParamFloat<px4::params::SIH_LOC_H0>) _sih_h0,
+		(ParamFloat<px4::params::SIH_LOC_MU_X>) _sih_mu_x,
+		(ParamFloat<px4::params::SIH_LOC_MU_Y>) _sih_mu_y,
+		(ParamFloat<px4::params::SIH_LOC_MU_Z>) _sih_mu_z,
+		(ParamInt<px4::params::SIH_GPS_USED>) _sih_gps_used,
+		(ParamFloat<px4::params::SIH_BARO_OFFSET>) _sih_baro_offset,
+		(ParamFloat<px4::params::SIH_MAG_OFFSET_X>) _sih_mag_offset_x,
+		(ParamFloat<px4::params::SIH_MAG_OFFSET_Y>) _sih_mag_offset_y,
+		(ParamFloat<px4::params::SIH_MAG_OFFSET_Z>) _sih_mag_offset_z,
+		(ParamFloat<px4::params::SIH_DISTSNSR_MIN>) _sih_distance_snsr_min,
+		(ParamFloat<px4::params::SIH_DISTSNSR_MAX>) _sih_distance_snsr_max,
+		(ParamFloat<px4::params::SIH_DISTSNSR_OVR>) _sih_distance_snsr_override,
+		(ParamFloat<px4::params::SIH_T_TAU>) _sih_thrust_tau,
+		(ParamInt<px4::params::SIH_VEHICLE_TYPE>) _sih_vtype,
+		(ParamBool<px4::params::SYS_CTRL_ALLOC>) _sys_ctrl_alloc
+	)
+};

src/modules/simulation/simulator_sih/sih_params.c

@@ -0,0 +1,448 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2022 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file sih_params.c
+ * Parameters for quadcopter X simulator in hardware.
+ *
+ * @author Romain Chiappinelli <romain.chiap@gmail.com>
+ * February 2019
+ */
+
+/**
+ * Vehicle mass
+ *
+ * This value can be measured by weighting the quad on a scale.
+ *
+ * @unit kg
+ * @min 0.0
+ * @decimal 2
+ * @increment 0.1
+ * @group Simulation In Hardware
+ */
+PARAM_DEFINE_FLOAT(SIH_MASS, 1.0f);
+
+/**
+ * Vehicle inertia about X axis
+ *
+ * The inertia is a 3 by 3 symmetric matrix.
+ * It represents the difficulty of the vehicle to modify its angular rate.
+ *
+ * @unit kg m^2
+ * @min 0.0
+ * @decimal 3
+ * @increment 0.005
+ * @group Simulation In Hardware
+ */
+PARAM_DEFINE_FLOAT(SIH_IXX, 0.025f);
+
+/**
+ * Vehicle inertia about Y axis
+ *
+ * The inertia is a 3 by 3 symmetric matrix.
+ * It represents the difficulty of the vehicle to modify its angular rate.
+ *
+ * @unit kg m^2
+ * @min 0.0
+ * @decimal 3
+ * @increment 0.005
+ * @group Simulation In Hardware
+ */
+PARAM_DEFINE_FLOAT(SIH_IYY, 0.025f);
+
+/**
+ * Vehicle inertia about Z axis
+ *
+ * The inertia is a 3 by 3 symmetric matrix.
+ * It represents the difficulty of the vehicle to modify its angular rate.
+ *
+ * @unit kg m^2
+ * @min 0.0
+ * @decimal 3
+ * @increment 0.005
+ * @group Simulation In Hardware
+ */
+PARAM_DEFINE_FLOAT(SIH_IZZ, 0.030f);
+
+/**
+ * Vehicle cross term inertia xy
+ *
+ * The inertia is a 3 by 3 symmetric matrix.
+ * This value can be set to 0 for a quad symmetric about its center of mass.
+ *
+ * @unit kg m^2
+ * @decimal 3
+ * @increment 0.005
+ * @group Simulation In Hardware
+ */
+PARAM_DEFINE_FLOAT(SIH_IXY, 0.0f);
+
+/**
+ * Vehicle cross term inertia xz
+ *
+ * The inertia is a 3 by 3 symmetric matrix.
+ * This value can be set to 0 for a quad symmetric about its center of mass.
+ *
+ * @unit kg m^2
+ * @decimal 3
+ * @increment 0.005
+ * @group Simulation In Hardware
+ */
+PARAM_DEFINE_FLOAT(SIH_IXZ, 0.0f);
+
+/**
+ * Vehicle cross term inertia yz
+ *
+ * The inertia is a 3 by 3 symmetric matrix.
+ * This value can be set to 0 for a quad symmetric about its center of mass.
+ *
+ * @unit kg m^2
+ * @decimal 3
+ * @increment 0.005
+ * @group Simulation In Hardware
+ */
+PARAM_DEFINE_FLOAT(SIH_IYZ, 0.0f);
+
+/**
+ * Max propeller thrust force
+ *
+ * This is the maximum force delivered by one propeller
+ * when the motor is running at full speed.
+ *
+ * This value is usually about 5 times the mass of the quadrotor.
+ *
+ * @unit N
+ * @min 0.0
+ * @decimal 2
+ * @increment 0.5
+ * @group Simulation In Hardware
+ */
+PARAM_DEFINE_FLOAT(SIH_T_MAX, 5.0f);
+
+/**
+ * Max propeller torque
+ *
+ * This is the maximum torque delivered by one propeller
+ * when the motor is running at full speed.
+ *
+ * This value is usually about few percent of the maximum thrust force.
+ *
+ * @unit Nm
+ * @min 0.0
+ * @decimal 3
+ * @increment 0.05
+ * @group Simulation In Hardware
+ */
+PARAM_DEFINE_FLOAT(SIH_Q_MAX, 0.1f);
+
+/**
+ * Roll arm length
+ *
+ * This is the arm length generating the rolling moment
+ *
+ * This value can be measured with a ruler.
+ * This corresponds to half the distance between the left and right motors.
+ *
+ * @unit m
+ * @min 0.0
+ * @decimal 2
+ * @increment 0.05
+ * @group Simulation In Hardware
+ */
+PARAM_DEFINE_FLOAT(SIH_L_ROLL, 0.2f);
+
+/**
+ * Pitch arm length
+ *
+ * This is the arm length generating the pitching moment
+ *
+ * This value can be measured with a ruler.
+ * This corresponds to half the distance between the front and rear motors.
+ *
+ * @unit m
+ * @min 0.0
+ * @decimal 2
+ * @increment 0.05
+ * @group Simulation In Hardware
+ */
+PARAM_DEFINE_FLOAT(SIH_L_PITCH, 0.2f);
+
+/**
+ * First order drag coefficient
+ *
+ * Physical coefficient representing the friction with air particules.
+ * The greater this value, the slower the quad will move.
+ *
+ * Drag force function of velocity: D=-KDV*V.
+ * The maximum freefall velocity can be computed as V=10*MASS/KDV [m/s]
+ *
+ * @unit N/(m/s)
+ * @min 0.0
+ * @decimal 2
+ * @increment 0.05
+ * @group Simulation In Hardware
+ */
+PARAM_DEFINE_FLOAT(SIH_KDV, 1.0f);
+
+/**
+ * First order angular damper coefficient
+ *
+ * Physical coefficient representing the friction with air particules during rotations.
+ * The greater this value, the slower the quad will rotate.
+ *
+ * Aerodynamic moment function of body rate: Ma=-KDW*W_B.
+ * This value can be set to 0 if unknown.
+ *
+ * @unit Nm/(rad/s)
+ * @min 0.0
+ * @decimal 3
+ * @increment 0.005
+ * @group Simulation In Hardware
+ */
+PARAM_DEFINE_FLOAT(SIH_KDW, 0.025f);
+
+/**
+ * Initial geodetic latitude
+ *
+ * This value represents the North-South location on Earth where the simulation begins.
+ * A value of 45 deg should be written 450000000.
+ *
+ * LAT0, LON0, H0, MU_X, MU_Y, and MU_Z should ideally be consistent among each others
+ * to represent a physical ground location on Earth.
+ *
+ * @unit deg*1e7
+ * @min -850000000
+ * @max  850000000
+ * @group Simulation In Hardware
+ */
+PARAM_DEFINE_INT32(SIH_LOC_LAT0, 454671160);
+
+/**
+ * Initial geodetic longitude
+ *
+ * This value represents the East-West location on Earth where the simulation begins.
+ * A value of 45 deg should be written 450000000.
+ *
+ * LAT0, LON0, H0, MU_X, MU_Y, and MU_Z should ideally be consistent among each others
+ * to represent a physical ground location on Earth.
+ *
+ * @unit deg*1e7
+ * @min -1800000000
+ * @max  1800000000
+ * @group Simulation In Hardware
+ */
+PARAM_DEFINE_INT32(SIH_LOC_LON0, -737578370);
+
+/**
+ * Initial AMSL ground altitude
+ *
+ * This value represents the Above Mean Sea Level (AMSL) altitude where the simulation begins.
+ *
+ * If using FlightGear as a visual animation,
+ * this value can be tweaked such that the vehicle lies on the ground at takeoff.
+ *
+ * LAT0, LON0, H0, MU_X, MU_Y, and MU_Z should ideally be consistent among each others
+ * to represent a physical ground location on Earth.
+ *
+ *
+ * @unit m
+ * @min -420.0
+ * @max 8848.0
+ * @decimal 2
+ * @increment 0.01
+ * @group Simulation In Hardware
+ */
+PARAM_DEFINE_FLOAT(SIH_LOC_H0, 32.34f);
+
+/**
+ * North magnetic field at the initial location
+ *
+ * This value represents the North magnetic field at the initial location.
+ *
+ * A magnetic field calculator can be found on the NOAA website
+ * Note, the values need to be converted from nano Tesla to Gauss
+ *
+ * LAT0, LON0, H0, MU_X, MU_Y, and MU_Z should ideally be consistent among each others
+ * to represent a physical ground location on Earth.
+ *
+ * @unit gauss
+ * @min -1.0
+ * @max  1.0
+ * @decimal 2
+ * @increment 0.001
+ * @group Simulation In Hardware
+ */
+PARAM_DEFINE_FLOAT(SIH_LOC_MU_X,  0.179f);
+
+/**
+ * East magnetic field at the initial location
+ *
+ * This value represents the East magnetic field at the initial location.
+ *
+ * A magnetic field calculator can be found on the NOAA website
+ * Note, the values need to be converted from nano Tesla to Gauss
+ *
+ * LAT0, LON0, H0, MU_X, MU_Y, and MU_Z should ideally be consistent among each others
+ * to represent a physical ground location on Earth.
+ *
+ * @unit gauss
+ * @min -1.0
+ * @max  1.0
+ * @decimal 2
+ * @increment 0.001
+ * @group Simulation In Hardware
+ */
+PARAM_DEFINE_FLOAT(SIH_LOC_MU_Y, -0.045f);
+
+/**
+ * Down magnetic field at the initial location
+ *
+ * This value represents the Down magnetic field at the initial location.
+ *
+ * A magnetic field calculator can be found on the NOAA website
+ * Note, the values need to be converted from nano Tesla to Gauss
+ *
+ * LAT0, LON0, H0, MU_X, MU_Y, and MU_Z should ideally be consistent among each others
+ * to represent a physical ground location on Earth.
+ *
+ * @unit gauss
+ * @min -1.0
+ * @max  1.0
+ * @decimal 2
+ * @increment 0.001
+ * @group Simulation In Hardware
+ */
+PARAM_DEFINE_FLOAT(SIH_LOC_MU_Z,  0.504f);
+
+/**
+ * Number of GPS satellites used
+ *
+ * @min 0
+ * @max  50
+ * @group Simulation In Hardware
+ */
+PARAM_DEFINE_INT32(SIH_GPS_USED, 10);
+
+/**
+ * Barometer offset in meters
+ *
+ * Absolute value superior to 10000 will disable barometer
+ *
+ * @unit m
+ * @group Simulation In Hardware
+ */
+PARAM_DEFINE_FLOAT(SIH_BARO_OFFSET,  0.0f);
+
+/**
+ * magnetometer X offset in Gauss
+ *
+ * Absolute value superior to 10000 will disable magnetometer
+ *
+ * @unit gauss
+ * @group Simulation In Hardware
+ */
+PARAM_DEFINE_FLOAT(SIH_MAG_OFFSET_X,  0.0f);
+
+/**
+ * magnetometer Y offset in Gauss
+ *
+ * Absolute value superior to 10000 will disable magnetometer
+ *
+ * @unit gauss
+ * @group Simulation In Hardware
+ */
+PARAM_DEFINE_FLOAT(SIH_MAG_OFFSET_Y,  0.0f);
+/**
+ * magnetometer Z offset in Gauss
+ *
+ * Absolute value superior to 10000 will disable magnetometer
+ *
+ * @unit gauss
+ * @group Simulation In Hardware
+ */
+PARAM_DEFINE_FLOAT(SIH_MAG_OFFSET_Z,  0.0f);
+
+/**
+ * distance sensor minimum range
+ *
+ * @unit m
+ * @min 0.0
+ * @max 10.0
+ * @decimal 4
+ * @increment 0.01
+ * @group Simulation In Hardware
+ */
+PARAM_DEFINE_FLOAT(SIH_DISTSNSR_MIN, 0.0f);
+
+/**
+ * distance sensor maximum range
+ *
+ * @unit m
+ * @min 0.0
+ * @max 1000.0
+ * @decimal 4
+ * @increment 0.01
+ * @group Simulation In Hardware
+ */
+PARAM_DEFINE_FLOAT(SIH_DISTSNSR_MAX, 100.0f);
+
+/**
+ * if >= 0 the distance sensor measures will be overridden by this value
+ *
+ * Absolute value superior to 10000 will disable distance sensor
+ *
+ * @unit m
+ * @group Simulation In Hardware
+ */
+PARAM_DEFINE_FLOAT(SIH_DISTSNSR_OVR, -1.0f);
+
+/**
+ * thruster time constant tau
+ *
+ * the time taken for the thruster to step from 0 to 100% should be about 4 times tau
+ *
+ * @unit s
+ * @group Simulation In Hardware
+ */
+PARAM_DEFINE_FLOAT(SIH_T_TAU, 0.05f);
+
+/**
+ * Vehicle type
+ *
+ * @value 0 Multicopter
+ * @value 1 Fixed-Wing
+ * @value 2 Tailsitter
+ * @reboot_required true
+ * @group Simulation In Hardware
+ */
+PARAM_DEFINE_INT32(SIH_VEHICLE_TYPE, 0);