SIH in SITL with lockstep (#19028)

* sih: Move sih out of work queue
This reverts commit bb7dd0cf00.

* sih-sim: Enable sih in sitl, together with lockstep

* sih-sim: new files for sih: quadx and airplane

* sih: Added tailsitter for sih-sitl simulation

* sitl_target: Added seperate target loop for sih

* jmavsim_run: Allow jmavsim to run in UDP mode

* lockstep: Post semaphore on last lockstep component removed

* sih-sim: Added display for effectively achieved speed

* sih: increase stack size

* sih-sim: Improved sleep time computation, fixes bug of running too fast

* sitl_target: place omnicopter in alphabethic order

Co-authored-by: romain-chiap <romain.chiap@gmail.com>
Co-authored-by: Matthias Grob <maetugr@gmail.com>

ROMFS/px4fmu_common/init.d-posix/airframes/10040_quadx

@@ -0,0 +1,23 @@
+#!/bin/sh
+#
+# @name QuadrotorX SITL for SIH
+#
+# @type Quadrotor
+#
+# @maintainer Romain Chiappinelli <romain.chiap@gmail.com>
+#
+
+. ${R}etc/init.d/rc.mc_defaults
+
+set MIXER quad_x
+
+# disable some checks to allow to fly:
+# - with usb
+param set-default CBRK_USB_CHK 197848
+# - without real battery
+param set-default CBRK_SUPPLY_CHK 894281
+# - without safety switch
+param set-default COM_PREARM_MODE 0
+param set-default CBRK_IO_SAFETY 22027
+
+param set SIH_VEHICLE_TYPE 0

ROMFS/px4fmu_common/init.d-posix/airframes/10041_airplane

@@ -0,0 +1,33 @@
+#!/bin/sh
+#
+# @name Plane SITL for SIH
+#
+# @type Plane
+#
+# @maintainer Romain Chiappinelli <romain.chiap@gmail.com>
+
+. ${R}etc/init.d/rc.fw_defaults
+
+set MIXER AERT
+
+# disable some checks to allow to fly:
+# - with usb
+param set-default CBRK_USB_CHK 197848
+# - without real battery
+param set-default CBRK_SUPPLY_CHK 894281
+# - without safety switch
+param set-default COM_PREARM_MODE 0
+param set-default CBRK_IO_SAFETY 22027
+
+param set-default BAT_N_CELLS 3
+
+param set SIH_T_MAX 6.0
+param set SIH_MASS 0.3
+param set SIH_IXX 0.00402
+param set SIH_IYY 0.0144
+param set SIH_IZZ 0.0177
+param set SIH_IXZ 0.00046
+param set SIH_KDV 0.2
+
+param set SIH_VEHICLE_TYPE 1 	# sih as fixed wing
+param set RWTO_TKOFF 1  # enable takeoff from runway (as opposed to launched)

ROMFS/px4fmu_common/init.d-posix/airframes/10042_xvert

@@ -0,0 +1,45 @@
+#!/bin/sh
+#
+# @name SIH Tailsitter Duo
+#
+# @type VTOL
+#
+# @maintainer Romain Chiappinelli <romain.chiap@gmail.com>
+
+. ${R}etc/init.d/rc.vtol_defaults
+
+param set-default VT_ELEV_MC_LOCK 0
+param set-default VT_TYPE 0
+param set-default VT_FW_DIFTHR_EN 1
+param set-default VT_FW_DIFTHR_SC 0.3
+param set-default MPC_MAN_Y_MAX 60
+param set-default MC_PITCH_P 5
+
+param set-default MAV_TYPE 19
+set MAV_TYPE 19
+set MIXER vtol_tailsitter_duo_sat
+
+# disable some checks to allow to fly:
+# - with usb
+param set-default CBRK_USB_CHK 197848
+# - without real battery
+param set-default CBRK_SUPPLY_CHK 894281
+# - without safety switch
+param set-default COM_PREARM_MODE 0
+param set-default CBRK_IO_SAFETY 22027
+
+param set-default BAT_N_CELLS 3
+
+param set SIH_T_MAX 2.0
+param set SIH_Q_MAX 0.0165
+param set SIH_MASS 0.2
+# IXX and IZZ are inverted from the thesis as the body frame is pitched by 90 deg
+param set SIH_IXX 0.00354
+param set SIH_IYY 0.000625
+param set SIH_IZZ 0.00300
+param set SIH_IXZ 0.0
+param set SIH_KDV 0.2
+param set SIH_L_ROLL 0.145
+
+# sih as tailsitter
+param set SIH_VEHICLE_TYPE 2

ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt

@@ -38,6 +38,9 @@ px4_add_romfs_files(
 	10019_omnicopter
 	10020_if750a
 	10030_px4vision
+	10040_quadx
+	10041_airplane
+	10042_xvert
 	1010_iris_opt_flow
 	1010_iris_opt_flow.post
 	1011_iris_irlock

ROMFS/px4fmu_common/init.d-posix/px4-rc.mavlink

@@ -30,3 +30,12 @@ mavlink start -x -u $udp_onboard_payload_port_local -r 4000 -f -m onboard -o $ud
 
 # Onboard link to gimbal
 mavlink start -x -u $udp_onboard_gimbal_port_local -r 400000 -m gimbal -o $udp_onboard_gimbal_port_remote
+
+# To display for SIH sitl
+if [ "$SIM_MODE" = "sihsim" ]; then
+	udp_sihsim_port_local=$((19450+px4_instance))
+	udp_sihsim_port_remote=$((19410+px4_instance))
+	mavlink start -x -u $udp_sihsim_port_local -r 400000 -m custom -o $udp_sihsim_port_remote
+	mavlink stream -r 200 -s HIL_ACTUATOR_CONTROLS -u  $udp_sihsim_port_local
+	mavlink stream -r 25 -s HIL_STATE_QUATERNION -u  $udp_sihsim_port_local
+fi

ROMFS/px4fmu_common/init.d-posix/rcS

@@ -204,8 +204,11 @@ param set IMU_INTEG_RATE 250
 . px4-rc.params
 
 dataman start
+# start sih in sih_sim mode, otherwise simulator module
+if [ "$SIM_MODE" = "sihsim" ]; then
+	sih start
 # only start the simulator if not in replay mode, as both control the lockstep time
-if ! replay tryapplyparams
+elif ! replay tryapplyparams
 then
   . px4-rc.simulator
 fi

Tools/jmavsim_run.sh

@@ -5,12 +5,13 @@ set -e
 SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"
 cd "$SCRIPT_DIR/jMAVSim"
 
-tcp_port=4560
+port=4560
 extra_args=
 baudrate=921600
 device=
 ip="127.0.0.1"
-while getopts ":b:d:p:qsr:f:i:loat" opt; do
+protocol="tcp"
+while getopts ":b:d:u:p:qsr:f:i:loat" opt; do
 	case $opt in
 		b)
 			baudrate=$OPTARG
@@ -18,11 +19,14 @@ while getopts ":b:d:p:qsr:f:i:loat" opt; do
 		d)
 			device="$OPTARG"
 			;;
+		u)
+			protocol="udp"
+			;;
 		i)
 			ip="$OPTARG"
 			;;
 		p)
-			tcp_port=$OPTARG
+			port=$OPTARG
 			;;
 		q)
 			extra_args="$extra_args -qgc"
@@ -53,7 +57,11 @@ while getopts ":b:d:p:qsr:f:i:loat" opt; do
 done
 
 if [ "$device" == "" ]; then
-	device="-tcp $ip:$tcp_port"
+	if [ "$protocol" == "tcp" ]; then
+		device="-tcp $ip:$port"
+	else
+		device="-udp $port"
+	fi
 else
 	device="-serial $device $baudrate"
 fi

Tools/sitl_run.sh

@@ -74,6 +74,9 @@ if [ "$model" == "" ] || [ "$model" == "none" ]; then
 	if [ "$program" == "jsbsim" ]; then
 		echo "empty model, setting rascal as default for jsbsim"
 		model="rascal"
+	elif [ "$program" == "sihsim" ]; then
+		echo "empty model, setting quadx as default for sihsim"
+		model="quadx"
 	else
 		echo "empty model, setting iris as default"
 		model="iris"
@@ -214,6 +217,12 @@ elif [ "$program" == "jsbsim" ] && [ -z "$no_sim" ]; then
 	fi
 	"${build_path}/build_jsbsim_bridge/jsbsim_bridge" ${model} -s "${src_path}/Tools/jsbsim_bridge/scene/${world}.xml" 2> /dev/null &
 	JSBSIM_PID=$!
+elif [ "$program" == "sihsim" ] && [ ! -n "$no_sim" ]; then
+	export SIM_MODE="sihsim"
+	if [ "$model" != "airplane" ] && [ "$model" != "quadx" ] && [ "$model" != "xvert" ]; then
+		echo "Model ${model} not compatible with with sih. sih supports [quadx,airplane,xvert]."
+		exit 1
+	fi
 fi
 
 pushd "$rootfs" >/dev/null

boards/px4/sitl/default.px4board

@@ -39,6 +39,7 @@ CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_REPLAY=y
 CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
+CONFIG_MODULES_SIH=y
 CONFIG_MODULES_SIMULATOR=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UUV_ATT_CONTROL=y

platforms/posix/cmake/sitl_target.cmake

@@ -9,7 +9,6 @@ add_custom_command(OUTPUT ${PX4_BINARY_DIR}/logs
 )
 add_custom_target(logs_symlink DEPENDS ${PX4_BINARY_DIR}/logs)
 add_dependencies(px4 logs_symlink)
-
 add_custom_target(run_config
 	COMMAND Tools/sitl_run.sh $<TARGET_FILE:px4> ${config_sitl_debugger} ${config_sitl_viewer} ${config_sitl_model} ${PX4_SOURCE_DIR} ${PX4_BINARY_DIR}
 	WORKING_DIRECTORY ${SITL_WORKING_DIR}
@@ -159,6 +158,7 @@ set(models
 	iris_rplidar
 	iris_vision
 	nxp_cupcar
+	omnicopter
 	plane
 	plane_cam
 	plane_catapult
@@ -177,7 +177,6 @@ set(models
 	typhoon_h480_ctrlalloc
 	uuv_bluerov2_heavy
 	uuv_hippocampus
-	omnicopter
 )
 
 set(worlds
@@ -325,6 +324,70 @@ foreach(debugger ${debuggers})
 	endforeach()
 endforeach()
 
+# create targets for sihsim
+set(models_sih
+	none
+	airplane
+	quadx
+	xvert
+)
+
+set(worlds_sih
+	none
+)
+
+foreach(debugger ${debuggers})
+	foreach(model ${models_sih})
+		foreach(world ${worlds_sih})
+			if(world STREQUAL "none")
+				if(debugger STREQUAL "none")
+					if(model STREQUAL "none")
+						set(_targ_name "sihsim")
+					else()
+						set(_targ_name "sihsim_${model}")
+					endif()
+				else()
+					if(model STREQUAL "none")
+						set(_targ_name "sihsim__${debugger}_${world}")
+					else()
+						set(_targ_name "sihsim_${model}_${debugger}_${world}")
+					endif()
+				endif()
+
+				add_custom_target(${_targ_name}
+					COMMAND ${PX4_SOURCE_DIR}/Tools/sitl_run.sh $<TARGET_FILE:px4> ${debugger} sihsim ${model} ${world} ${PX4_SOURCE_DIR} ${PX4_BINARY_DIR}
+					WORKING_DIRECTORY ${SITL_WORKING_DIR}
+					USES_TERMINAL
+					DEPENDS logs_symlink
+				)
+				list(APPEND all_posix_vmd_make_targets ${_targ_name})
+			else()
+				if(debugger STREQUAL "none")
+					if(model STREQUAL "none")
+						set(_targ_name "sihsim___${world}")
+					else()
+						set(_targ_name "sihsim_${model}__${world}")
+					endif()
+				else()
+					if(model STREQUAL "none")
+						set(_targ_name "sihsim___${debugger}_${world}")
+					else()
+						set(_targ_name "sihsim_${model}_${debugger}_${world}")
+					endif()
+				endif()
+
+				add_custom_target(${_targ_name}
+					COMMAND ${PX4_SOURCE_DIR}/Tools/sitl_run.sh $<TARGET_FILE:px4> ${debugger} sihsim ${model} ${world} ${PX4_SOURCE_DIR} ${PX4_BINARY_DIR}
+					WORKING_DIRECTORY ${SITL_WORKING_DIR}
+					USES_TERMINAL
+					DEPENDS logs_symlink
+				)
+				list(APPEND all_posix_vmd_make_targets ${_targ_name})
+			endif()
+		endforeach()
+	endforeach()
+endforeach()
+
 # add flighgear targets
 if(ENABLE_LOCKSTEP_SCHEDULER STREQUAL "no")
 	set(models

platforms/posix/src/px4/common/lockstep_scheduler/src/lockstep_components.cpp

@@ -84,7 +84,7 @@ void LockstepComponents::unregister_component(int component)
 
 	int components_used_bitset = _components_used_bitset;
 
-	if (_components_progress_bitset == components_used_bitset && components_used_bitset != 0) {
+	if (_components_progress_bitset == components_used_bitset) {
 		_components_progress_bitset = 0;
 		px4_sem_post(&_components_sem);
 	}

src/modules/sih/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2019-2020 PX4 Development Team. All rights reserved.
+#   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
@@ -45,5 +45,4 @@ px4_add_module(
 		drivers_accelerometer
 		drivers_gyroscope
 		drivers_magnetometer
-		px4_work_queue
 	)

src/modules/sih/aero.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
 *
-*   Copyright (c) 2019-2020 PX4 Development Team. All rights reserved.
+*   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
@@ -139,12 +139,6 @@ private:
 	matrix::Vector3f _v_S;	// velocity in segment frame
 
 public:
-
-	AeroSeg()
-	{
-		AeroSeg(1.0f, 0.2f, 0.0f, matrix::Vector3f());
-	}
-
 	/** 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)
@@ -162,7 +156,7 @@ public:
 	 * 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, matrix::Vector3f p_B, float dihedral_deg = 0.0f,
+	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)
 	{
@@ -176,7 +170,7 @@ public:
 		_span = span;
 		_mac = mac;
 		_alpha_0 = math::radians(alpha_0_deg);
-		_p_B = matrix::Vector3f(p_B);
+		_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;
@@ -208,6 +202,10 @@ public:
 		_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)

src/modules/sih/sih.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2019-2020 PX4 Development Team. All rights reserved.
+ *   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
@@ -54,8 +54,16 @@ using namespace matrix;
 using namespace time_literals;
 
 Sih::Sih() :
-	ModuleParams(nullptr),
-	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl)
+	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);
@@ -77,15 +85,86 @@ Sih::Sih() :
 	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();
 }
 
-Sih::~Sih()
+#if defined(ENABLE_LOCKSTEP_SCHEDULER)
+// Get current timestamp in microseconds
+uint64_t micros()
 {
-	perf_free(_loop_perf);
-	perf_free(_loop_interval_perf);
+	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
 
-bool Sih::init()
+void Sih::realtime_loop()
 {
 	int rate = _imu_gyro_ratemax.get();
 
@@ -96,20 +175,29 @@ bool Sih::init()
 
 	// 200 - 2000 Hz
 	int interval_us = math::constrain(int(roundf(1e6f / rate)), 500, 5000);
-	ScheduleOnInterval(interval_us);
 
-	return true;
-}
+	px4_sem_init(&_data_semaphore, 0, 0);
+	hrt_call_every(&_timer_call, interval_us, interval_us, timer_callback, &_data_semaphore);
 
-void Sih::Run()
-{
-	if (should_exit()) {
-		exit_and_cleanup();
-		return;
+	while (!should_exit()) {
+		px4_sem_wait(&_data_semaphore);     // periodic real time wakeup
+		perf_begin(_loop_perf);
+		sensor_step();
+		perf_end(_loop_perf);
 	}
 
-	perf_count(_loop_interval_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
@@ -170,7 +258,7 @@ void Sih::Run()
 		send_gps();
 	}
 
-	if (_vehicle == VehicleType::FW && _now - _airspeed_time >= 50_ms) {
+	if ((_vehicle == VehicleType::FW || _vehicle == VehicleType::TS) && _now - _airspeed_time >= 50_ms) {
 		_airspeed_time = _now;
 		send_airspeed();
 	}
@@ -284,6 +372,7 @@ void Sih::read_motors()
 	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
@@ -598,8 +687,8 @@ float Sih::generate_wgn()   // generate white Gaussian noise sample with std=1
 
 	if (phase) {
 		do {
-			float U1 = (float)rand() / RAND_MAX;
-			float U2 = (float)rand() / RAND_MAX;
+			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;
@@ -623,6 +712,11 @@ Vector3f Sih::noiseGauss3f(float stdx, float stdy, float 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");
 
@@ -658,27 +752,33 @@ int Sih::print_status()
 	return 0;
 }
 
+
 int Sih::task_spawn(int argc, char *argv[])
 {
-	Sih *instance = new Sih();
+	_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;
+	}
 
-	if (instance) {
-		_object.store(instance);
-		_task_id = task_id_is_work_queue;
+	return 0;
+}
 
-		if (instance->init()) {
-			return PX4_OK;
-		}
+Sih *Sih::instantiate(int argc, char *argv[])
+{
+	Sih *instance = new Sih();
 
-	} else {
+	if (instance == nullptr) {
 		PX4_ERR("alloc failed");
 	}
 
-	delete instance;
-	_object.store(nullptr);
-	_task_id = -1;
-
-	return PX4_ERROR;
+	return instance;
 }
 
 int Sih::custom_command(int argc, char *argv[])

src/modules/sih/sih.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
 *
-*   Copyright (c) 2019-2020 PX4 Development Team. All rights reserved.
+*   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
@@ -58,7 +58,6 @@
 #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 <matrix/matrix/math.hpp>   // matrix, vectors, dcm, quaterions
 #include <conversion/rotation.h>    // math::radians,
@@ -67,7 +66,7 @@
 #include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
 #include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
 #include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
-#include <lib/perf/perf_counter.h>
+#include <perf/perf_counter.h>
 #include <uORB/Publication.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionInterval.hpp>
@@ -81,17 +80,27 @@
 #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;
 
-class Sih : public ModuleBase<Sih>, public ModuleParams, public px4::ScheduledWorkItem
+extern "C" __EXPORT int sih_main(int argc, char *argv[]);
+
+class Sih : public ModuleBase<Sih>, public ModuleParams
 {
 public:
 	Sih();
-	~Sih() override;
+
+	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[]);
 
@@ -101,16 +110,18 @@ public:
 	/** @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);
 
-	bool init();
+	// timer called periodically to post the semaphore
+	static void timer_callback(void *sem);
 
 private:
-	void Run() override;
-
 	void parameters_updated();
 
 	// simulated sensor instances
@@ -171,11 +182,23 @@ private:
 	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};
@@ -275,7 +298,7 @@ private:
 	// 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,

src/modules/sih/sih_params.c

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2019-2020 PX4 Development Team. All rights reserved.
+ *   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