Add jsbsim bridge to enable jsbsim for px4 SITL on jsbsim (#15748)

* Add jsbsim bridge to enable jsbsim for px4 SITL/HIL on jsbsim

This is a PX4 HIL/SITL integration into JSBSim. JSBSim is an open source flight dynamics model (http://jsbsim.sourceforge.net/)

Currently there are three models available which is the rascal, quadrotor_x, hexarotor_x integrated into the bridge.

The simulation can be run with the firmware with the following command for example
```
make px4_sitl jsbsim_rascal
```

The visualization is done flightgear and is done by the bridge sending UDP packets to flightgear. To disable the visualization `HEADLESS=1` when running the make command.

The simulation can be configured through the configuration files under the `config` directory through a xml file. Senor configurations,  The xml file name should match the name of the model.

* Update Tools/sitl_run.sh

Co-authored-by: Beat Küng <beat-kueng@gmx.net>

Co-authored-by: Beat Küng <beat-kueng@gmx.net>

.gitmodules

@@ -48,3 +48,6 @@
 [submodule "Tools/flightgear_bridge"]
 	path = Tools/flightgear_bridge
 	url = https://github.com/PX4/PX4-FlightGear-Bridge.git
+[submodule "Tools/jsbsim_bridge"]
+	path = Tools/jsbsim_bridge
+	url = https://github.com/PX4/px4-jsbsim-bridge.git

ROMFS/px4fmu_common/init.d-posix/10017_quadrotor_x

@@ -0,0 +1,12 @@
+#!/bin/sh
+#
+# @name Quadrotor SITL model for JSBSim
+#
+# @type Quadrotor Wide
+#
+# @maintainer Jaeyoung Lim <jaeyoung@auterion.com>
+#
+
+sh /etc/init.d/rc.mc_defaults
+
+set MIXER quad_w

ROMFS/px4fmu_common/init.d-posix/10018_hexarotor_x

@@ -0,0 +1,35 @@
+#!/bin/sh
+#
+# @name Hexacopter SITL model for JSBSim
+#
+# @type Hexarotor x
+#
+# @maintainer Jaeyoung Lim <jaeyoung@auterion.com>
+#
+
+sh /etc/init.d/rc.mc_defaults
+
+if [ $AUTOCNF = yes ]
+then
+	param set MC_PITCHRATE_P 0.1
+	param set MC_PITCHRATE_I 0.05
+	param set MC_PITCH_P 6.0
+	param set MC_ROLLRATE_P 0.15
+	param set MC_ROLLRATE_I 0.1
+	param set MC_ROLL_P 6.0
+	param set MPC_XY_VEL_I_ACC 4
+	param set MPC_XY_VEL_P_ACC 3
+
+	param set RTL_DESCEND_ALT 10
+	param set RTL_LAND_DELAY 0
+
+	param set TRIG_INTERFACE 3
+	param set TRIG_MODE 4
+	param set MNT_MODE_IN 4
+	param set MNT_DO_STAB 2
+	param set MAV_PROTO_VER 2
+fi
+
+set MAV_TYPE 13
+
+set MIXER hexa_x

Tools/jsbsim_bridge

@@ -0,0 +1 @@
+Subproject commit efe19dbec7e82784a48c6b6e7710f54468c8a8d4

Tools/setup_jsbsim.bash

@@ -0,0 +1,40 @@
+#!/bin/bash
+#
+# Setup environment to make JSBSim visible to PX4.
+#
+# License: according to LICENSE.md in the root directory of the PX4 Firmware repository
+
+if [ "$#" != 3 ]; then
+    echo -e "usage: source setup_jsbsim.bash src_dir build_dir model\n"
+    return 1
+fi
+
+SRC_DIR="$1"
+BUILD_DIR="$2"
+MODEL="$3"
+
+export FG_AIRCRAFT="${SRC_DIR}/Tools/jsbsim_bridge/models"
+
+# This is needed for aircraft namespace mapping
+# Need more architectural discussions to make this more scalable
+case "$MODEL" in
+        rascal)
+            AIRCRAFT_DIR="Rascal"
+            MODEL_NAME="Rascal110-JSBSim"
+            ;;
+        quadrotor_x)
+            AIRCRAFT_DIR="quadrotor_x"
+            MODEL_NAME="quadrotor_x"
+            ;;
+        hexarotor_x)
+            AIRCRAFT_DIR="hexarotor_x"
+            MODEL_NAME="hexarotor_x"
+            ;;
+        *)
+            echo "Unknown Model"
+            exit 1
+
+esac
+
+export JSBSIM_AIRCRAFT_DIR="$AIRCRAFT_DIR"
+export JSBSIM_AIRCRAFT_MODEL="$MODEL_NAME"

Tools/sitl_run.sh

@@ -58,7 +58,11 @@ fi
 
 if [ "$model" == "" ] || [ "$model" == "none" ]; then
 	echo "empty model, setting iris as default"
-	model="iris"
+	if [ "$program" == "jsbsim" ]; then
+		model="rascal"
+	else
+		model="iris"
+	fi
 fi
 
 # kill process names that might stil
@@ -146,6 +150,21 @@ elif [ "$program" == "flightgear" ] && [ -z "$no_sim" ]; then
 	"${src_path}/Tools/flightgear_bridge/FG_run.py" "models/"${model}".json" 0
 	"${build_path}/build_flightgear_bridge/flightgear_bridge" 0 `./get_FGbridge_params.py "models/"${model}".json"` &
 	FG_BRIDGE_PID=$!
+elif [ "$program" == "jsbsim" ] && [ -z "$no_sim" ]; then
+	source "$src_path/Tools/setup_jsbsim.bash" "${src_path}" "${build_path}" ${model}
+	if [[ -n "$HEADLESS" ]]; then
+		echo "not running flightgear gui"
+	else
+		fgfs --fdm=null \
+			--native-fdm=socket,in,60,,5550,udp \
+			--aircraft=$JSBSIM_AIRCRAFT_MODEL \
+			--airport=${world} \
+			--disable-hud \
+			--disable-ai-models &> /dev/null &
+		FGFS_PID=$!
+	fi
+	"${build_path}/build_jsbsim_bridge/jsbsim_bridge" "models/${JSBSIM_AIRCRAFT_DIR}" $JSBSIM_AIRCRAFT_MODEL ${model} "${src_path}/Tools/jsbsim_bridge/scene/${world}.xml" $HEADLESS 2> /dev/null &
+	JSBSIM_PID=$!
 fi
 
 pushd "$rootfs" >/dev/null
@@ -199,4 +218,7 @@ elif [ "$program" == "gazebo" ]; then
 elif [ "$program" == "flightgear" ]; then
 	kill $FG_BRIDGE_PID
 	kill -9 `cat /tmp/px4fgfspid_0`
+elif [ "$program" == "jsbsim" ]; then
+	kill $JSBSIM_PID
+	kill $FGFS_PID
 fi

platforms/posix/cmake/sitl_target.cmake

@@ -26,6 +26,7 @@ add_custom_target(run_config
 px4_add_git_submodule(TARGET git_gazebo PATH "${PX4_SOURCE_DIR}/Tools/sitl_gazebo")
 px4_add_git_submodule(TARGET git_jmavsim PATH "${PX4_SOURCE_DIR}/Tools/jMAVSim")
 px4_add_git_submodule(TARGET git_flightgear_bridge PATH "${PX4_SOURCE_DIR}/Tools/flightgear_bridge")
+px4_add_git_submodule(TARGET git_jsbsim_bridge PATH "${PX4_SOURCE_DIR}/Tools/jsbsim_bridge")
 
 # Add support for external project building
 include(ExternalProject)
@@ -73,6 +74,20 @@ ExternalProject_Add(flightgear_bridge
 	BUILD_ALWAYS 1
 )
 
+ExternalProject_Add(jsbsim_bridge
+	SOURCE_DIR ${PX4_SOURCE_DIR}/Tools/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
+)
+
 # create targets for each viewer/model/debugger combination
 set(viewers none jmavsim gazebo)
 set(debuggers none ide gdb lldb ddd valgrind callgrind)
@@ -161,6 +176,79 @@ foreach(viewer ${viewers})
 	endforeach()
 endforeach()
 
+# create targets for jsbsim
+set(models_jsbsim none rascal quadrotor_x hexarotor_x)
+set(worlds_jsbsim none LSZH)
+foreach(debugger ${debuggers})
+	foreach(model ${models_jsbsim})
+		foreach(world ${worlds_jsbsim})
+			if (world STREQUAL "none")
+				if (debugger STREQUAL "none")
+					if (model STREQUAL "none")
+						set(_targ_name "jsbsim")
+					else()
+						set(_targ_name "jsbsim_${model}")
+					endif()
+				else()
+					if (model STREQUAL "none")
+						set(_targ_name "jsbsim__${debugger}_${world}")
+					else()
+						set(_targ_name "jsbsim_${model}_${debugger}_${world}")
+					endif()
+				endif()
+
+				add_custom_target(${_targ_name}
+				COMMAND ${PX4_SOURCE_DIR}/Tools/sitl_run.sh
+					$<TARGET_FILE:px4>
+					${debugger}
+					jsbsim
+					${model}
+					"LSZH"
+					${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})
+				add_dependencies(${_targ_name} px4 jsbsim_bridge)
+			else()
+				if (debugger STREQUAL "none")
+					if (model STREQUAL "none")
+						set(_targ_name "jsbsim___${world}")
+					else()
+						set(_targ_name "jsbsim_${model}__${world}")
+					endif()
+				else()
+					if (model STREQUAL "none")
+						set(_targ_name "jsbsim___${debugger}_${world}")
+					else()
+						set(_targ_name "jsbsim_${model}_${debugger}_${world}")
+					endif()
+				endif()
+
+				add_custom_target(${_targ_name}
+				COMMAND ${PX4_SOURCE_DIR}/Tools/sitl_run.sh
+					$<TARGET_FILE:px4>
+					${debugger}
+					jsbsim
+					${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})
+				add_dependencies(${_targ_name} px4 jsbsim_bridge)
+			endif()
+		endforeach()
+	endforeach()
+endforeach()
+
 #add flighgear targets
 if( ENABLE_LOCKSTEP_SCHEDULER STREQUAL "no")
 	set(models