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