SIH in SITL with lockstep (PX4#19028)

* sih: Move sih out of work queue This reverts commit bb7dd0c. * 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 <[email protected]> Co-authored-by: Matthias Grob <[email protected]>
dagar · Jun 9, 2022 · 46c9d1e · 46c9d1e
1 parent a7e1146
commit 46c9d1e
Show file tree

Hide file tree

Showing 16 changed files with 378 additions and 61 deletions.
diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10040_quadx b/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 <[email protected]>
+#
+
+. ${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
diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10041_airplane b/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 <[email protected]>
+
+. ${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)
diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10042_xvert b/ROMFS/px4fmu_common/init.d-posix/airframes/10042_xvert
@@ -0,0 +1,45 @@
+#!/bin/sh
+#
+# @name SIH Tailsitter Duo
+#
+# @type VTOL
+#
+# @maintainer Romain Chiappinelli <[email protected]>
+
+. ${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
diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt b/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

diff --git a/ROMFS/px4fmu_common/init.d-posix/px4-rc.mavlink b/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
diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/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

diff --git a/Tools/jmavsim_run.sh b/Tools/jmavsim_run.sh
@@ -5,24 +5,28 @@ 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
 			;;
 		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

diff --git a/Tools/sitl_run.sh b/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

diff --git a/boards/px4/sitl/default.px4board b/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

diff --git a/platforms/posix/cmake/sitl_target.cmake b/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

diff --git a/platforms/posix/src/px4/common/lockstep_scheduler/src/lockstep_components.cpp b/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);
 	}

diff --git a/src/modules/sih/CMakeLists.txt b/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
 	)
diff --git a/src/modules/sih/aero.hpp b/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 @@ class AeroSeg
 	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 @@ class AeroSeg
 	 * 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 @@ class AeroSeg
 		_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 @@ class AeroSeg
 		_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)