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)

.gitmodules

@@ -6,12 +6,12 @@
 	path = src/drivers/uavcan/libuavcan
 	url = https://github.com/dronecan/libuavcan.git
 	branch = main
-[submodule "Tools/jMAVSim"]
+[submodule "Tools/simulation/jmavsim/jMAVSim"]
-	path = Tools/jMAVSim
+	path = Tools/simulation/jmavsim/jMAVSim
 	url = https://github.com/PX4/jMAVSim.git
 	branch = master
-[submodule "Tools/sitl_gazebo"]
+[submodule "Tools/simulation/gazebo/sitl_gazebo"]
-	path = Tools/sitl_gazebo
+	path = Tools/simulation/gazebo/sitl_gazebo
 	url = https://github.com/PX4/PX4-SITL_gazebo.git
 	branch = master
 [submodule "src/drivers/gps/devices"]
@@ -31,10 +31,10 @@
 	url = https://github.com/PX4/NuttX-apps.git
 	branch = px4_firmware_nuttx-10.1.0+
 [submodule "Tools/flightgear_bridge"]
-	path = Tools/flightgear_bridge
+	path = Tools/simulation/flightgear/flightgear_bridge
 	url = https://github.com/PX4/PX4-FlightGear-Bridge.git
-[submodule "Tools/jsbsim_bridge"]
+[submodule "Tools/simulation/jsbsim/jsbsim_bridge"]
-	path = Tools/jsbsim_bridge
+	path = Tools/simulation/jsbsim/jsbsim_bridge
 	url = https://github.com/PX4/px4-jsbsim-bridge.git
 [submodule "src/drivers/cyphal/libcanard"]
 	path = src/drivers/cyphal/libcanard

.vscode/cmake-variants.yaml

@@ -2,7 +2,7 @@ CONFIG:
   default: px4_sitl_default
   choices:
     px4_sitl_default:
-      short: px4_sitl
+      short: px4_sitl_default
       buildType: RelWithDebInfo
       settings:
         CONFIG: px4_sitl_default

.vscode/tasks.json

@@ -54,7 +54,7 @@
             "type": "shell",
             "command": "ant create_run_jar copy_res",
             "options": {
-                "cwd": "${workspaceFolder}/Tools/jMAVSim"
+                "cwd": "${workspaceFolder}/Tools/simulation/jmavsim/jMAVSim"
             },
             "problemMatcher": [],
             "presentation":{
@@ -72,7 +72,7 @@
             "dependsOn": "jmavsim build",
             "command": "java -Djava.ext.dirs= -jar jmavsim_run.jar -r 250 -lockstep -tcp localhost:4560 -qgc",
             "options": {
-                "cwd": "${workspaceFolder}/Tools/jMAVSim/out/production",
+                "cwd": "${workspaceFolder}/Tools/simulation/jmavsim/jMAVSim/out/production",
                 "env": {
                     "PX4_SIM_SPEED_FACTOR": "1"
                 }
@@ -86,6 +86,62 @@
                 "showReuseMessage": false,
                 "clear": false
             },
+            "problemMatcher": []
+        },
+        {
+            "label": "jmavsim kill",
+            "type": "shell",
+            "command": "kill $(ps aux | grep jmavsim | grep -v 'grep' | awk '{print $2}')",
+            "presentation": {
+                "echo": false,
+                "reveal": "never",
+                "focus": false,
+                "panel": "shared",
+                "showReuseMessage": false,
+                "clear": false
+            },
+            "problemMatcher": [],
+            "dependsOn":["px4_sitl_cleanup"]
+        },
+        {
+            "label": "gazebo build",
+            "type": "shell",
+            "command": "make px4_sitl_default sitl_gazebo",
+            "options": {
+                "cwd": "${workspaceFolder}"
+            },
+            "problemMatcher": [],
+            "presentation":{
+                "echo": true,
+                "reveal": "always",
+                "focus": false,
+                "panel": "shared",
+                "showReuseMessage": false,
+                "clear": false,
+            }
+        },
+        {
+            "label": "gazebo start",
+            "type": "shell",
+            "dependsOn": "gazebo build",
+            "options": {
+                "cwd": "${workspaceFolder}",
+                "env": {
+                    "GAZEBO_PLUGIN_PATH": "${workspaceFolder}/build/px4_sitl_default/build_gazebo",
+                    "GAZEBO_MODEL_PATH": "${workspaceFolder}/Tools/simulation/gazebo/sitl_gazebo/models",
+                    "PX4_SIM_SPEED_FACTOR": "1"
+                }
+            },
+            "command": "gzserver --verbose ${workspaceFolder}/Tools/simulation/gazebo/sitl_gazebo/worlds/${input:gazeboWorld}.world",
+            "isBackground": true,
+            "presentation": {
+                "echo": true,
+                "reveal": "never",
+                "focus": false,
+                "panel": "shared",
+                "showReuseMessage": false,
+                "clear": false
+            },
             "problemMatcher": [
                 {
                     "pattern": [
@@ -105,31 +161,56 @@
             ]
         },
         {
-            "label": "jmavsim kill",
+            "label": "gazebo",
             "type": "shell",
-            "command": "kill $(ps aux | grep jmavsim | grep -v 'grep' | awk '{print $2}')",
+            "dependsOn": "gazebo start",
+            "options": {
+                "cwd": "${workspaceFolder}",
+                "env": {
+                    "GAZEBO_PLUGIN_PATH": "${workspaceFolder}/build/px4_sitl_default/build_gazebo",
+                    "GAZEBO_MODEL_PATH": "${workspaceFolder}/Tools/simulation/gazebo/sitl_gazebo/models",
+                    "PX4_SIM_SPEED_FACTOR": "1"
+                }
+            },
+            "command": "gz model --verbose --spawn-file=${workspaceFolder}/Tools/simulation/gazebo/sitl_gazebo/models/${input:gazeboModel}/${input:gazeboModel}.sdf --model-name=${input:gazeboModel} -x 1.01 -y 0.98 -z 0.83",
+            "isBackground": false,
             "presentation": {
-                "echo": false,
+                "echo": true,
                 "reveal": "never",
                 "focus": false,
                 "panel": "shared",
                 "showReuseMessage": false,
                 "clear": false
             },
-            "problemMatcher": []
+            "problemMatcher": [
+                {
+                    "pattern": [
+                        {
+                            "regexp": ".",
+                            "file": 1,
+                            "location": 2,
+                            "message": 3
+                        }
+                    ],
+                    "background": {
+                        "activeOnStart": true,
+                        "beginsPattern": ".",
+                        "endsPattern": ".",
+                    }
+                }
+            ]
         },
-
         {
-            "label": "gazebo",
+            "label": "ign gazebo",
             "type": "shell",
             "options": {
                 "cwd": "${workspaceFolder}",
                 "env": {
-                    "IGN_GAZEBO_RESOURCE_PATH": "${workspaceFolder}/Tools/simulation/gazebo/models",
+                    "IGN_GAZEBO_RESOURCE_PATH": "${workspaceFolder}/Tools/simulation/ignition/models",
                     "PX4_SIM_SPEED_FACTOR": "1"
                 }
             },
-            "command": "ign gazebo -v 4 -r ${workspaceFolder}/Tools/simulation/gazebo/worlds/${input:gazeboWorld}.sdf",
+            "command": "ign gazebo -v 4 -r ${workspaceFolder}/Tools/simulation/ignition/worlds/${input:ignWorld}.sdf",
             "isBackground": true,
             "presentation": {
                 "echo": true,
@@ -159,10 +240,26 @@
                 }
             ]
         },
-
         {
             "label": "gazebo kill",
             "type": "shell",
+            "command": "pkill -9 -f gzserver || true",
+            "presentation": {
+                "echo": true,
+                "reveal": "never",
+                "revealProblems": "onProblem",
+                "focus": false,
+                "panel": "dedicated",
+                "showReuseMessage": false,
+                "clear": false,
+                "close": true
+            },
+            "problemMatcher": [],
+            "dependsOn":["px4_sitl_cleanup"]
+        },
+        {
+            "label": "ign gazebo kill",
+            "type": "shell",
             "command": "pkill -9 -f ign || true",
             "presentation": {
                 "echo": true,
@@ -247,12 +344,46 @@
         }
     ],
     "inputs": [
+        {
+            "type": "pickString",
+            "id": "ignWorld",
+            "description": "Ignition world",
+            "options": [
+                "default"
+            ],
+            "default": "default"
+        },
+        {
+            "type": "pickString",
+            "id": "gazeboModel",
+            "description": "gazebo model",
+            "options": [
+              "iris",
+              "typhoon_h480",
+              "plane",
+              "plane_catapult",
+              "plane_lidar",
+              "standard_vtol",
+              "tailsitter",
+              "tiltrotor",
+              "r1_rover",
+              "boat"
+            ],
+            "default": "iris"
+        },
         {
             "type": "pickString",
             "id": "gazeboWorld",
             "description": "gazebo world",
             "options": [
-                "empty"
+                "baylands",
+                "empty",
+                "ksql_airport",
+                "mcmillan_airfield",
+                "sonoma_raceway",
+                "warehouse",
+                "windy",
+                "yosemite"
             ],
             "default": "empty"
         }

Jenkinsfile

@@ -26,15 +26,15 @@ pipeline {
         //       echo $0;
         //       mkdir -p catkin_ws/src;
         //       cd catkin_ws;
-        //       git -C ${WORKSPACE}/catkin_ws/src/Firmware submodule update --init --recursive --force Tools/sitl_gazebo
+        //       git -C ${WORKSPACE}/catkin_ws/src/Firmware submodule update --init --recursive --force Tools/simulation/gazebo/sitl_gazebo
-        //       git clone --recursive ${WORKSPACE}/catkin_ws/src/Firmware/Tools/sitl_gazebo src/mavlink_sitl_gazebo;
+        //       git clone --recursive ${WORKSPACE}/catkin_ws/src/Firmware/Tools/simulation/gazebo/sitl_gazebo src/mavlink_sitl_gazebo;
         //       git -C ${WORKSPACE}/catkin_ws/src/Firmware fetch --tags;
         //       source /opt/ros/melodic/setup.bash;
         //       export PYTHONPATH=/opt/ros/$ROS_DISTRO/lib/python2.7/dist-packages:/usr/lib/python2.7/dist-packages:/usr/local/lib/python2.7/dist-packages;
         //       catkin init;
         //       catkin build -j$(nproc) -l$(nproc);
         //     '''
-        //     // test if the binary was correctly installed and runs using 'mavros_posix_silt.launch'
+        //     // test if the binary was correctly installed and runs using 'mavros_posix_sitl.launch'
         //     sh '''#!/bin/bash -l
         //       echo $0;
         //       source catkin_ws/devel/setup.bash;
@@ -68,7 +68,7 @@ pipeline {
               unset ROS_DISTRO;
               mkdir -p colcon_ws/src;
               cd colcon_ws;
-              git -C ${WORKSPACE}/colcon_ws/src/Firmware submodule update --init --recursive --force Tools/sitl_gazebo;
+              git -C ${WORKSPACE}/colcon_ws/src/Firmware submodule update --init --recursive --force Tools/simulation/gazebo/sitl_gazebo;
               git -C ${WORKSPACE}/colcon_ws/src/Firmware fetch --tags;
               source /opt/ros/foxy/setup.sh;
               colcon build --event-handlers console_direct+ --symlink-install;

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

@@ -9,6 +9,8 @@
 
 . ${R}etc/init.d/rc.mc_defaults
 
+PX4_SIMULATOR=${PX4_SIMULATOR:=sihsim}
+PX4_SIM_MODEL=${PX4_SIM_MODEL:=quadx}
 
 # disable some checks to allow to fly:
 # - with usb

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

@@ -8,6 +8,8 @@
 
 . ${R}etc/init.d/rc.fw_defaults
 
+PX4_SIMULATOR=${PX4_SIMULATOR:=sihsim}
+PX4_SIM_MODEL=${PX4_SIM_MODEL:=airplane}
 
 # disable some checks to allow to fly:
 # - with usb

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

@@ -8,6 +8,9 @@
 
 . ${R}etc/init.d/rc.vtol_defaults
 
+PX4_SIMULATOR=${PX4_SIMULATOR:=sihsim}
+PX4_SIM_MODEL=${PX4_SIM_MODEL:=xvert}
+
 param set-default VT_ELEV_MC_LOCK 0
 param set-default VT_TYPE 0
 param set-default VT_FW_DIFTHR_EN 1

ROMFS/px4fmu_common/init.d-posix/airframes/4001_x3

@@ -7,6 +7,10 @@
 
 . ${R}etc/init.d/rc.mc_defaults
 
+PX4_SIMULATOR=${PX4_SIMULATOR:=ignition}
+PX4_SIM_MODEL=${PX4_SIM_MODEL:=x3}
+PX4_SIM_WORLD=${PX4_SIM_WORLD:=default}
+
 param set-default SYS_CTRL_ALLOC 1
 
 param set-default CA_AIRFRAME 0
@@ -28,19 +32,19 @@ param set-default CA_ROTOR3_PX -0.13
 param set-default CA_ROTOR3_PY 0.20
 param set-default CA_ROTOR3_KM -0.05
 
-param set-default SIM_GZ_FUNC1 101
+param set-default SIM_IGN_FUNC1 101
-param set-default SIM_GZ_FUNC2 102
+param set-default SIM_IGN_FUNC2 102
-param set-default SIM_GZ_FUNC3 103
+param set-default SIM_IGN_FUNC3 103
-param set-default SIM_GZ_FUNC4 104
+param set-default SIM_IGN_FUNC4 104
 
-param set-default SIM_GZ_MIN1 150
+param set-default SIM_IGN_MIN1 150
-param set-default SIM_GZ_MIN2 150
+param set-default SIM_IGN_MIN2 150
-param set-default SIM_GZ_MIN3 150
+param set-default SIM_IGN_MIN3 150
-param set-default SIM_GZ_MIN4 150
+param set-default SIM_IGN_MIN4 150
 
-param set-default SIM_GZ_MAX1 1000
+param set-default SIM_IGN_MAX1 1000
-param set-default SIM_GZ_MAX2 1000
+param set-default SIM_IGN_MAX2 1000
-param set-default SIM_GZ_MAX3 1000
+param set-default SIM_IGN_MAX3 1000
-param set-default SIM_GZ_MAX4 1000
+param set-default SIM_IGN_MAX4 1000
 
 param set-default MPC_THR_HOVER 0.60

ROMFS/px4fmu_common/init.d-posix/airframes/6001_x4

@@ -7,6 +7,10 @@
 
 . ${R}etc/init.d/rc.mc_defaults
 
+PX4_SIMULATOR=${PX4_SIMULATOR:=ignition}
+PX4_SIM_MODEL=${PX4_SIM_MODEL:=x4}
+PX4_SIM_WORLD=${PX4_SIM_WORLD:=default}
+
 param set-default SYS_CTRL_ALLOC 1
 
 param set-default CA_AIRFRAME 0
@@ -36,26 +40,26 @@ param set-default CA_ROTOR5_PX  0.25
 param set-default CA_ROTOR5_PY  0.15
 param set-default CA_ROTOR5_KM -0.05
 
-param set-default SIM_GZ_FUNC1 101
+param set-default SIM_IGN_FUNC1 101
-param set-default SIM_GZ_FUNC2 102
+param set-default SIM_IGN_FUNC2 102
-param set-default SIM_GZ_FUNC3 103
+param set-default SIM_IGN_FUNC3 103
-param set-default SIM_GZ_FUNC4 104
+param set-default SIM_IGN_FUNC4 104
-param set-default SIM_GZ_FUNC5 105
+param set-default SIM_IGN_FUNC5 105
-param set-default SIM_GZ_FUNC6 106
+param set-default SIM_IGN_FUNC6 106
 
-param set-default SIM_GZ_MIN1 150
+param set-default SIM_IGN_MIN1 150
-param set-default SIM_GZ_MIN2 150
+param set-default SIM_IGN_MIN2 150
-param set-default SIM_GZ_MIN3 150
+param set-default SIM_IGN_MIN3 150
-param set-default SIM_GZ_MIN4 150
+param set-default SIM_IGN_MIN4 150
-param set-default SIM_GZ_MIN5 150
+param set-default SIM_IGN_MIN5 150
-param set-default SIM_GZ_MIN6 150
+param set-default SIM_IGN_MIN6 150
 
-param set-default SIM_GZ_MAX1 1000
+param set-default SIM_IGN_MAX1 1000
-param set-default SIM_GZ_MAX2 1000
+param set-default SIM_IGN_MAX2 1000
-param set-default SIM_GZ_MAX3 1000
+param set-default SIM_IGN_MAX3 1000
-param set-default SIM_GZ_MAX4 1000
+param set-default SIM_IGN_MAX4 1000
-param set-default SIM_GZ_MAX5 1000
+param set-default SIM_IGN_MAX5 1000
-param set-default SIM_GZ_MAX6 1000
+param set-default SIM_IGN_MAX6 1000
 
 
 param set-default MC_PITCHRATE_D 0.0016

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

@@ -32,7 +32,7 @@ mavlink start -x -u $udp_onboard_payload_port_local -r 4000 -f -m onboard -o $ud
 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
+if [ "$PX4_SIMULATOR" = "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

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

@@ -1,25 +1,50 @@
 #!/bin/sh
 # shellcheck disable=SC2154
 
-PX4_SIM_WORLD="${PX4_SIM_WORLD:="empty"}" # default to empty world?
+# Simulator IMU data provided at 250 Hz
+param set-default IMU_INTEG_RATE 250
 
-echo "PX4_SIM_MODEL: ${PX4_SIM_MODEL}"
+if [ "$PX4_SIMULATOR" = "sihsim" ] || [ "$(param show -q SYS_AUTOSTART)" -eq "0" ]; then
-echo "PX4_SIM_WORLD: ${PX4_SIM_WORLD}"
 
-# TODO: verify if world exists
+	if ! simulator_sih start; then
-ign service --info --service /world/${PX4_SIM_WORLD}/create
+		echo "ERROR  [init] simulator_sih failed to start"
-echo $?
+		exit 1
+	fi
 
-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\""
+elif [ "$PX4_SIMULATOR" = "ignition" ]; then
 
-if ignition_simulator start -m "${PX4_SIM_MODEL}" -w "${PX4_SIM_WORLD}"
+	# source generated gazebo_env.sh for IGN_GAZEBO_RESOURCE_PATH
-then
+	if [ -f gazebo_env.sh ]; then
+		. ./gazebo_env.sh
+	elif [ -f ../gazebo_env.sh ]; then
+		. ../gazebo_env.sh
+	fi
 
-	sensor_baro_sim start
+	if ! ign service --info --service /world/${PX4_SIM_WORLD}/create | grep "ignition.msgs.EntityFactory"; then
-	sensor_gps_sim start
+		# starting ign gazebo with ${PX4_SIM_WORLD} world
-	sensor_mag_sim start
+		echo "INFO  [init] starting ign gazebo"
+
+		if [ -z $HEADLESS ]; then
+			ign gazebo --verbose=1 -r "${PX4_IGN_GAZEBO_WORLDS}/${PX4_SIM_WORLD}.sdf" &
+		else
+			# starting ign gazebo headless
+			ign gazebo --verbose=1 -r -s "${PX4_IGN_GAZEBO_WORLDS}/${PX4_SIM_WORLD}.sdf" &
+		fi
+	else
+		echo "INFO  [init] ign gazebo already running"
+	fi
+
+	if simulator_ignition_bridge start -m "${PX4_SIM_MODEL}" -w "${PX4_SIM_WORLD}"; then
+		sensor_baro_sim start
+		sensor_gps_sim start
+		sensor_mag_sim start
+	else
+		echo "ERROR  [init] ign gazebo failed to start"
+		exit 1
+	fi
 
 else
+	# otherwise start simulator (mavlink) module
 	simulator_tcp_port=$((4560+px4_instance))
 
 	# Check if PX4_SIM_HOSTNAME environment variable is empty
@@ -29,15 +54,15 @@ else
 
 		if [ -z "${PX4_SIM_HOST_ADDR}" ]; then
 			echo "PX4 SIM HOST: localhost"
-			simulator start -c $simulator_tcp_port
+			simulator_mavlink start -c $simulator_tcp_port
 		else
 			echo "PX4 SIM HOST: $PX4_SIM_HOST_ADDR"
-			simulator start -t $PX4_SIM_HOST_ADDR $simulator_tcp_port
+			simulator_mavlink start -t $PX4_SIM_HOST_ADDR $simulator_tcp_port
 		fi
 
 	else
 		echo "PX4 SIM HOST: $PX4_SIM_HOSTNAME"
-		simulator start -h $PX4_SIM_HOSTNAME $simulator_tcp_port
+		simulator_mavlink start -h $PX4_SIM_HOSTNAME $simulator_tcp_port
 	fi
 
 fi

ROMFS/px4fmu_common/init.d-posix/rcS

@@ -38,10 +38,24 @@ set LOGGER_BUF                  1000
 
 set RUN_MINIMAL_SHELL           no
 
-# Use the variable set by sitl_run.sh to choose the model settings.
+set SYS_AUTOSTART=0
-if [ "$PX4_SIM_MODEL" = "shell" ]; then
+
+if [ "$PX4_SIM_MODEL" = "shell" ]
+then
 	set RUN_MINIMAL_SHELL yes
-else
+
+elif [ -n "$PX4_SYS_AUTOSTART" ]
+then
+	echo "env SYS_AUTOSTART: ${PX4_SYS_AUTOSTART}"
+	SYS_AUTOSTART=${PX4_SYS_AUTOSTART}
+
+elif [ "$PX4_SIM_MODEL" = "none" ] || [ -z $PX4_SIM_MODEL ]
+then
+	# no airframe selected
+	SYS_AUTOSTART=0
+
+elif [ -n "$PX4_SIM_MODEL" ]
+then
 	# Find the matching Autostart ID (file name has the form: [0-9]+_${PX4_SIM_MODEL})
 	# TODO: unify with rc.autostart generation
 	# shellcheck disable=SC2012
@@ -50,12 +64,13 @@ else
 		echo "ERROR  [init] Unknown model $PX4_SIM_MODEL (not found by name on ${R}etc/init.d-posix/airframes)"
 		exit 1
 	else
+		SYS_AUTOSTART=$REQUESTED_AUTOSTART
 		echo "INFO  [init] found model autostart file as SYS_AUTOSTART=$REQUESTED_AUTOSTART"
 	fi
 fi
 
 # Load parameters
-set PARAM_FILE eeprom/parameters_"$REQUESTED_AUTOSTART"
+set PARAM_FILE parameters.bson
 param select $PARAM_FILE
 
 if [ -f $PARAM_FILE ]
@@ -66,26 +81,16 @@ then
 	else
 		echo "[param] FAILED loading $PARAM_FILE"
 	fi
-else
-	echo "[param] parameter file not found, creating $PARAM_FILE"
 fi
 
 # exit early when the minimal shell is requested
 [ $RUN_MINIMAL_SHELL = yes ] && exit 0
 
+if param compare SYS_AUTOSTART $SYS_AUTOSTART
+then
+	set AUTOCNF no
 
-# Use environment variable PX4_ESTIMATOR to choose estimator.
+elif [ "$SYS_AUTOSTART" -eq 0 ]
-if [ "$PX4_ESTIMATOR" = "q" ]; then
-	param set SYS_MC_EST_GROUP 3
-elif [ "$PX4_ESTIMATOR" = "ekf2" ]; then
-	param set SYS_MC_EST_GROUP 2
-elif [ "$PX4_ESTIMATOR" = "lpe" ]; then
-	param set SYS_MC_EST_GROUP 1
-elif [ "$PX4_ESTIMATOR" = "inav" ]; then
-	param set SYS_MC_EST_GROUP 0
-fi
-
-if param compare SYS_AUTOSTART $REQUESTED_AUTOSTART
 then
 	set AUTOCNF no
 else
@@ -106,7 +111,7 @@ param set MAV_SYS_ID $((px4_instance+1))
 
 if [ $AUTOCNF = yes ]
 then
-	param set SYS_AUTOSTART $REQUESTED_AUTOSTART
+	param set SYS_AUTOSTART $SYS_AUTOSTART
 
 	param set CAL_ACC0_ID  1310988 # 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
 	param set CAL_GYRO0_ID 1310988 # 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
@@ -190,27 +195,26 @@ do
 			;;
 	esac
 done
-if [ ! -e "$autostart_file" ]; then
+
+if [ -e "$autostart_file" ]
+then
+	. "$autostart_file"
+
+elif [ ! -e "$autostart_file" ] && [ "$SYS_AUTOSTART" -ne "0" ]
+then
 	echo "Error: no autostart file found ($autostart_file)"
 	exit 1
 fi
 
-. "$autostart_file"
-
-# Simulator IMU data provided at 250 Hz
-param set IMU_INTEG_RATE 250
-
 #user defined params for instances can be in PATH
 . 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
-elif ! replay tryapplyparams
+if ! replay tryapplyparams
 then
-  . px4-rc.simulator
+	. px4-rc.simulator
 fi
 
 load_mon start

ROMFS/px4fmu_common/init.d/rcS

@@ -368,7 +368,7 @@ else
 		# start the simulator in hardware if needed
 		if param compare SYS_HITL 2
 		then
-			sih start
+			simulator_sih start
 		fi
 
 	else

Tools/gz_sim.sh

@@ -1,200 +0,0 @@
-#!/bin/bash
-#Author: Benjamin Perseghetti
-#Email: bperseghetti@rudislabs.com
-# This script unifies running gazebo simulation for HITL and SITL
-# You can run multiple instances of the 'px4' binary, with the gazebo SITL simulation
-# This script assumes px4 is already built, with 'make px4_sitl_default gazebo'
-# You can also run HITL with -h flag 
-# Generate world and/or model files with editable json -j [m (model), w (world), mw (model and world), or wm (world and model)]
-
-# The simulator in SITL is expected to send to TCP port 4560+i for i in [0, N-1]
-# For example gazebo can be run like this for multiple SITL:
-# ./Tools/gz_sim.sh -n 10 -m iris
-# Or gazebo can be run like this for HITL:
-# ./Tools/gz_sim.sh -h 1 -m standard_vtol
-
-function cleanup() {
-	echo "running the cleanup"
-	pkill -x px4
-	pkill gazebo
-	pkill gzclient
-	pkill gzserver
-}
-
-trap "cleanup" INT SIGINT SIGTERM EXIT
-
-function spawn_model() {
-	MODEL=$1
-	N=$2 #Instance Number
-	WORLD_FILE=$3
-	MJ=$4
-	SITL_MODEL_NAME="${MODEL}_${N}"
-	sitl_path=${SCRIPT_DIR}/sitl_gazebo
-	jinja_model_script=${sitl_path}/scripts/jinja_model_gen.py
-	base_model="--base_model ${MODEL}"
-	model_json="--json_gen ${MJ}"
-
-	if [ $hitl == true ]; then
-		python3 ${src_path}/Tools/boot_now.py "/dev/ttyACM0"
-		hil_mode="--hil_mode 1"
-		model_name="--model_name ${MODEL}"
-		hitl_launch_command="${model_json} ${base_model} ${hil_mode}  ${model_name}"
-		echo "Generating: ${jinja_model_script} ${hitl_launch_command}"
-		python3 ${jinja_model_script} ${hitl_launch_command}
-		sleep 1
-		source ${src_path}/Tools/setup_gazebo.bash ${src_path} ${src_path}/build/${target}
-		sleep 2
-		gazebo ${sitl_path}/worlds/temp_${WORLD_FILE}.world --verbose
-
-	else
-		mavlink_tcp="--mavlink_tcp_port $((4560+${N}))"
-		mavlink_udp="--mavlink_udp_port $((14560+${N}))"
-		model_name="--model_name ${SITL_MODEL_NAME}"
-		output_path="--output_path /tmp"
-		working_dir="$build_path/instance_$n"
-		sitl_launch_command="${model_json} ${base_model} ${mavlink_tcp} ${mavlink_udp} ${model_name} ${output_path}"
-		[ ! -d "$working_dir" ] && mkdir -p "$working_dir"
-		pushd "$working_dir" &>/dev/null
-		echo "starting instance $N in $(pwd)"
-		../bin/px4 -i $N -d "$build_path/etc" -w sitl_${SITL_MODEL_NAME} -s etc/init.d-posix/rcS >out.log 2>err.log &
-		python3 ${jinja_model_script} ${sitl_launch_command}
-		echo "Generating: ${jinja_model_script} ${sitl_launch_command}" 
-		echo "Spawning ${SITL_MODEL_NAME}"
-		gz model --spawn-file=/tmp/${SITL_MODEL_NAME}.sdf --model-name=${SITL_MODEL_NAME} -x 0.0 -y $((3*${N})) -z 0.2	
-		popd &>/dev/null
-	fi
-	
-}
-
-if [ "$1" == "--help" ]; then
-	echo "Usage: $0 [-n <num_vehicles>] [-m <vehicle_model>] [-h <run_hitl>] [-w <world>] [-s <script>] [-t <num_threads>] [-j <json_params>]"
-	echo "-s flag is used to script spawning vehicles e.g. $0 -s iris:3,plane:2"
-	echo "-h flag is used to launch a single vehicle in HITL mode"
-	echo "-t flag is used to set the number of ODE threads for the world"
-	echo "-j flag is used to enable json parameters from gen_params.json for the world (w), model (m), or both (wm or mw)"
-	exit 1
-fi
-
-while getopts n:m:h:w:s:t:j:p option
-do
-	case "${option}"
-	in
-		n) NUM_VEHICLES=${OPTARG};;
-		m) VEHICLE_MODEL=${OPTARG};;
-		h) HITL=${OPTARG};;
-		w) WORLD=${OPTARG};;
-		s) SCRIPT=${OPTARG};;
-		t) THREADS=${OPTARG};;
-		j) JSON=${OPTARG};;
-		p) TARGET=${OPTARG};;
-	esac
-done
-
-num_vehicles=${NUM_VEHICLES:=1}
-world=${WORLD:=empty}
-hitl=${HITL:=false}
-threads=${THREADS:=1}
-json_opts=${JSON:=0}
-target=${TARGET:=px4_sitl_default}
-system_threads=`grep -Pc '^processor\t' /proc/cpuinfo`
-echo "Number of requested ODE threads: $((threads))"
-echo "Max number of possilbe threads: $((system_threads))"
-if [ $((threads)) -gt $((system_threads)) ]; then
-	threads=$system_threads
-	echo "Requested ODE thread count too high, set to system max of $threads threads."
-elif [ $(( ${threads} )) -lt 1 ]; then
-	threads=1
-	echo "Requested ODE thread count too low, set to $threads thread."
-else
-	echo "Using $threads threads for ODE."
-fi
-
-if [ "$json_opts" == "mw" ] || [ "$json_opts" == "wm" ]; then
-	echo "JSON used for both world and model generation"
-	wjson="1"
-	mjson="1"
-elif [ "$json_opts" == "w" ]; then
-	echo "JSON used for world generation"
-	wjson="1"
-	mjson="0"
-elif [ "$json_opts" == "m" ]; then
-	echo "JSON used for model generation"
-	mjson="1"
-	wjson="0"
-else 
-	wjson="0"
-	mjson="0"
-fi
-
-if [ "$hitl" == "True" ] || [ "$hitl" == "1" ] || [ "$hitl" == "true" ]; then
-	hitl=true
-else
-	hitl=false
-fi
-
-export PX4_SIM_MODEL=${VEHICLE_MODEL:=iris}
-
-SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"
-src_path="$SCRIPT_DIR/.."
-echo ${SCRIPT}
-build_path=${src_path}/build/${target}
-source ${src_path}/Tools/setup_gazebo.bash ${src_path} ${src_path}/build/${target}
-sleep 1
-
-sitl_path=${SCRIPT_DIR}/sitl_gazebo
-world_name="--world_name ${world}"
-jinja_world_script=${sitl_path}/scripts/jinja_world_gen.py
-sitl_ode_threads="--ode_threads ${threads}"
-world_json="--json_gen ${wjson}"
-
-if [ $hitl == true ]; then
-	hitl_model_name="--model_name ${PX4_SIM_MODEL}"
-	echo "HITL mode is currently turned on, disabling multiple vehicle spawn and script spawn."
-	echo "RUNNING: python3 $jinja_world_script $world_name $hitl_model_name $world_json"
-	python3 $jinja_world_script $world_name $hitl_model_name $world_json
-	echo "Generated temp_${world}.world"
-	spawn_model ${PX4_SIM_MODEL} 0 ${world} ${mjson}
-
-else
-	echo "killing running instances"
-	pkill -x px4 || true
-	echo "HITL mode is currently turned off."
-	echo "RUNNING: python3 $jinja_world_script $world_name $sitl_ode_threads $world_json"
-	python3 $jinja_world_script $world_name $sitl_ode_threads $world_json
-	echo "Generated temp_${world}.world"
-	echo "Starting gazebo: gzserver ${sitl_path}/worlds/temp_${world}.world --verbose"
-	gzserver ${sitl_path}/worlds/temp_${world}.world --verbose &
-	sleep 5
-
-	n=0
-	if [ -z ${SCRIPT} ]; then
-		if [ $num_vehicles -gt 255 ]; then
-			echo "Tried spawning $num_vehicles vehicles. The maximum number of supported vehicles is 255"
-			exit 1
-		fi
-		while [ $n -lt $num_vehicles ]; do
-			spawn_model ${PX4_SIM_MODEL} $n ${world} ${mjson}
-			n=$(($n + 1))
-		done
-	else
-		for target in ${SCRIPT}; do
-			target="$(echo "$target" | tr -d ' ')" #Remove spaces
-			target_vehicle="${target%:*}"
-			target_number="${target#*:}"
-
-			if [ $n -gt 255 ]; then
-				echo "Tried spawning $n vehicles. The maximum number of supported vehicles is 255"
-				exit 1
-			fi
-
-			m=0
-			while [ $m -lt ${target_number} ]; do
-				spawn_model ${PX4_SIM_MODEL} $n ${world} ${mjson}
-				m=$(($m + 1))
-				n=$(($n + 1))
-			done
-		done
-	fi
-	echo "Starting gazebo client"
-	gzclient
-fi

Tools/px4_developer.mk.example

@@ -1,5 +0,0 @@
-$(info px4_developer.mk inclded)
-ifeq ($(UAVCAN_BL_OVERRIDE),y)
-$(info **************************  UAVCAN BOOT LOADERS built for In place application Debugging ***************************************)
-export EXTRAFLAGS +=-DDEBUG_APPLICATION_INPLACE
-endif

Tools/qgc_meta_sync.sh

@@ -1,6 +0,0 @@
-#!/usr/bin/env bash
-
-make parameters_metadata
-cp parameters.xml ../qgroundcontrol/src/FirmwarePlugin/PX4/PX4ParameterFactMetaData.xml
-make airframe_metadata
-cp airframes.xml ../qgroundcontrol/src/AutoPilotPlugins/PX4/AirframeFactMetaData.xml

Tools/setup_jsbsim.bash

@@ -1,39 +0,0 @@
-#!/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)
-            MODEL_NAME="Rascal110-JSBSim"
-            ;;
-        malolo)
-            MODEL_NAME="Malolo1"
-            ;;
-        quadrotor_x)
-            MODEL_NAME="quadrotor_x"
-            ;;
-        hexarotor_x)
-            MODEL_NAME="hexarotor_x"
-            ;;
-        *)
-            echo "Unknown Model"
-            exit 1
-
-esac
-
-export JSBSIM_AIRCRAFT_MODEL="$MODEL_NAME"

Tools/simulation/flightgear/sitl_run.sh

@@ -0,0 +1,47 @@
+#!/usr/bin/env bash
+
+set -e
+
+if [ "$#" -lt 4 ]; then
+	echo usage: sitl_run.sh sitl_bin model src_path build_path
+	exit 1
+fi
+
+sitl_bin="$1"
+model="$2"
+src_path="$3"
+build_path="$4"
+
+echo SITL ARGS
+
+echo sitl_bin: $sitl_bin
+echo model: $model
+echo src_path: $src_path
+echo build_path: $build_path
+
+rootfs="$build_path/rootfs" # this is the working directory
+mkdir -p "$rootfs"
+
+export PX4_SIM_MODEL=${model}
+
+echo "FG setup"
+cd "${src_path}/Tools/simulation/flightgear/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=$!
+
+pushd "$rootfs" >/dev/null
+
+# Do not exit on failure now from here on because we want the complete cleanup
+set +e
+
+sitl_command="\"$sitl_bin\" \"$build_path\"/etc"
+
+echo SITL COMMAND: $sitl_command
+
+eval $sitl_command
+
+popd >/dev/null
+
+kill $FG_BRIDGE_PID
+kill -9 `cat /tmp/px4fgfspid_0`

Tools/simulation/gazebo/setup_gazebo.bash

@@ -17,7 +17,7 @@ BUILD_DIR=$2
 
 # setup Gazebo env and update package path
 export GAZEBO_PLUGIN_PATH=$GAZEBO_PLUGIN_PATH:${BUILD_DIR}/build_gazebo
-export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:${SRC_DIR}/Tools/sitl_gazebo/models
+export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:${SRC_DIR}/Tools/simulation/gazebo/sitl_gazebo/models
 export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:${BUILD_DIR}/build_gazebo
 
 echo -e "GAZEBO_PLUGIN_PATH $GAZEBO_PLUGIN_PATH"

Tools/simulation/gazebo/sitl_multiple_run.sh

@@ -4,7 +4,7 @@
 
 # The simulator is expected to send to TCP port 4560+i for i in [0, N-1]
 # For example gazebo can be run like this:
-#./Tools/gazebo_sitl_multiple_run.sh -n 10 -m iris
+#./Tools/simulation/gazebo/sitl_multiple_run.sh -n 10 -m iris
 
 function cleanup() {
 	pkill -x px4
@@ -29,13 +29,13 @@ function spawn_model() {
 		exit 1
 	fi
 
-	working_dir="$build_path/instance_$n"
+	working_dir="$build_path/rootfs/$n"
 	[ ! -d "$working_dir" ] && mkdir -p "$working_dir"
 
 	pushd "$working_dir" &>/dev/null
 	echo "starting instance $N in $(pwd)"
-	../bin/px4 -i $N -d "$build_path/etc" -w sitl_${MODEL}_${N} -s etc/init.d-posix/rcS >out.log 2>err.log &
+	$build_path/bin/px4 -i $N -d "$build_path/etc" >out.log 2>err.log &
-	python3 ${src_path}/Tools/sitl_gazebo/scripts/jinja_gen.py ${src_path}/Tools/sitl_gazebo/models/${MODEL}/${MODEL}.sdf.jinja ${src_path}/Tools/sitl_gazebo --mavlink_tcp_port $((4560+${N})) --mavlink_udp_port $((14560+${N})) --mavlink_id $((1+${N})) --gst_udp_port $((5600+${N})) --video_uri $((5600+${N})) --mavlink_cam_udp_port $((14530+${N})) --output-file /tmp/${MODEL}_${N}.sdf
+	python3 ${src_path}/Tools/simulation/gazebo/sitl_gazebo/scripts/jinja_gen.py ${src_path}/Tools/simulation/gazebo/sitl_gazebo/models/${MODEL}/${MODEL}.sdf.jinja ${src_path}/Tools/simulation/gazebo/sitl_gazebo --mavlink_tcp_port $((4560+${N})) --mavlink_udp_port $((14560+${N})) --mavlink_id $((1+${N})) --gst_udp_port $((5600+${N})) --video_uri $((5600+${N})) --mavlink_cam_udp_port $((14530+${N})) --output-file /tmp/${MODEL}_${N}.sdf
 
 	echo "Spawning ${MODEL}_${N} at ${X} ${Y}"
 
@@ -73,7 +73,7 @@ export PX4_SIM_MODEL=${vehicle_model}
 
 echo ${SCRIPT}
 SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"
-src_path="$SCRIPT_DIR/.."
+src_path="$SCRIPT_DIR/../../.."
 
 build_path=${src_path}/build/${target}
 mavlink_udp_port=14560
@@ -84,7 +84,7 @@ pkill -x px4 || true
 
 sleep 1
 
-source ${src_path}/Tools/setup_gazebo.bash ${src_path} ${src_path}/build/${target}
+source ${src_path}/Tools/simulation/gazebo/setup_gazebo.bash ${src_path} ${src_path}/build/${target}
 
 # To use gazebo_ros ROS2 plugins
 if [[ -n "$ROS_VERSION" ]] && [ "$ROS_VERSION" == "2" ]; then
@@ -94,7 +94,7 @@ else
 fi
 
 echo "Starting gazebo"
-gzserver ${src_path}/Tools/sitl_gazebo/worlds/${world}.world --verbose $ros_args &
+gzserver ${src_path}/Tools/simulation/gazebo/sitl_gazebo/worlds/${world}.world --verbose $ros_args &
 sleep 5
 
 n=0

Tools/simulation/gazebo/sitl_run.sh

@@ -0,0 +1,180 @@
+#!/usr/bin/env bash
+set -e
+
+if [ "$#" -lt 6 ]; then
+	echo usage: sitl_run.sh sitl_bin debugger model world src_path build_path
+	exit 1
+fi
+
+if [[ -n "$DONT_RUN" ]]; then
+	echo "Not running simulation (DONT_RUN is set)."
+	exit 0
+fi
+
+sitl_bin="$1"
+debugger="$2"
+model="$3"
+world="$4"
+src_path="$5"
+build_path="$6"
+
+echo SITL ARGS
+
+echo sitl_bin: $sitl_bin
+echo debugger: $debugger
+echo model: $model
+echo world: $world
+echo src_path: $src_path
+echo build_path: $build_path
+
+rootfs="$build_path/rootfs" # this is the working directory
+mkdir -p "$rootfs"
+
+# To disable user input
+if [[ -n "$NO_PXH" ]]; then
+	no_pxh=-d
+else
+	no_pxh=""
+fi
+
+# To disable user input
+if [[ -n "$VERBOSE_SIM" ]]; then
+	verbose="--verbose"
+else
+	verbose=""
+fi
+
+# Disable follow mode
+if [[ "$PX4_NO_FOLLOW_MODE" != "1" ]]; then
+    follow_mode="--gui-client-plugin libgazebo_user_camera_plugin.so"
+else
+    follow_mode=""
+fi
+
+# To use gazebo_ros ROS2 plugins
+if [[ -n "$ROS_VERSION" ]] && [ "$ROS_VERSION" == "2" ]; then
+	ros_args="-s libgazebo_ros_init.so -s libgazebo_ros_factory.so"
+else
+	ros_args=""
+fi
+
+if [ "$model" == "" ] || [ "$model" == "none" ]; then
+	echo "empty model, setting iris as default"
+	model="iris"
+fi
+
+# kill process names that might stil
+# be running from last time
+pkill -x gazebo || true
+
+export PX4_SIM_MODEL=${model}
+export PX4_SIM_WORLD=${world}
+
+SIM_PID=0
+
+if [ -x "$(command -v gazebo)" ]; then
+	# Get the model name
+	model_name="${model}"
+	# Check if a 'modelname-gen.sdf' file exist for the models using jinja and generating the SDF files
+	if [ -f "${src_path}/Tools/simulation/gazebo/sitl_gazebo/models/${model}/${model}-gen.sdf" ]; then
+		model_name="${model}-gen"
+	fi
+
+	# Set the plugin path so Gazebo finds our model and sim
+	source "$src_path/Tools/simulation/gazebo/setup_gazebo.bash" "${src_path}" "${build_path}"
+	if [ -z $PX4_SITL_WORLD ]; then
+		#Spawn predefined world
+		if [ "$world" == "none" ]; then
+			if [ -f ${src_path}/Tools/simulation/gazebo/sitl_gazebo/worlds/${model}.world ]; then
+				echo "empty world, default world ${model}.world for model found"
+				world_path="${src_path}/Tools/simulation/gazebo/sitl_gazebo/worlds/${model}.world"
+			else
+				echo "empty world, setting empty.world as default"
+				world_path="${src_path}/Tools/simulation/gazebo/sitl_gazebo/worlds/empty.world"
+			fi
+		else
+			#Spawn empty world if world with model name doesn't exist
+			world_path="${src_path}/Tools/simulation/gazebo/sitl_gazebo/worlds/${world}.world"
+		fi
+	else
+		if [ -f ${src_path}/Tools/simulation/gazebo/sitl_gazebo/worlds/${PX4_SITL_WORLD}.world ]; then
+			# Spawn world by name if exists in the worlds directory from environment variable
+			world_path="${src_path}/Tools/simulation/gazebo/sitl_gazebo/worlds/${PX4_SITL_WORLD}.world"
+		else
+			# Spawn world from environment variable with absolute path
+			world_path="$PX4_SITL_WORLD"
+		fi
+	fi
+	gzserver $verbose $world_path $ros_args &
+	SIM_PID=$!
+
+	# Check all paths in ${GAZEBO_MODEL_PATH} for specified model
+	IFS_bak=$IFS
+	IFS=":"
+	for possible_model_path in ${GAZEBO_MODEL_PATH}; do
+		if [ -z $possible_model_path ]; then
+			continue
+		fi
+		# trim \r from path
+		possible_model_path=$(echo $possible_model_path | tr -d '\r')
+		if test -f "${possible_model_path}/${model}/${model}.sdf" ; then
+			modelpath=$possible_model_path
+			break
+		fi
+	done
+	IFS=$IFS_bak
+
+	if [ -z $modelpath ]; then
+		echo "Model ${model} not found in model path: ${GAZEBO_MODEL_PATH}"
+		exit 1
+	else
+		echo "Using: ${modelpath}/${model}/${model}.sdf"
+	fi
+
+	while gz model --verbose --spawn-file="${modelpath}/${model}/${model_name}.sdf" --model-name=${model} -x 1.01 -y 0.98 -z 0.83 2>&1 | grep -q "An instance of Gazebo is not running."; do
+		echo "gzserver not ready yet, trying again!"
+		sleep 1
+	done
+
+	if [[ -n "$HEADLESS" ]]; then
+		echo "not running gazebo gui"
+	else
+		# gzserver needs to be running to avoid a race. Since the launch
+		# is putting it into the background we need to avoid it by backing off
+		sleep 3
+		nice -n 20 gzclient --verbose $follow_mode &
+		GUI_PID=$!
+	fi
+else
+	echo "You need to have gazebo simulator installed!"
+	exit 1
+fi
+
+pushd "$rootfs" >/dev/null
+
+# Do not exit on failure now from here on because we want the complete cleanup
+set +e
+
+sitl_command="\"$sitl_bin\" $no_pxh \"$build_path\"/etc"
+
+echo SITL COMMAND: $sitl_command
+
+if [ "$debugger" == "lldb" ]; then
+	eval lldb -- $sitl_command
+elif [ "$debugger" == "gdb" ]; then
+	eval gdb --args $sitl_command
+elif [ "$debugger" == "valgrind" ]; then
+	eval valgrind --track-origins=yes --leak-check=full -v $sitl_command
+elif [ "$debugger" == "callgrind" ]; then
+	eval valgrind --tool=callgrind -v $sitl_command
+else
+	eval $sitl_command
+fi
+
+popd >/dev/null
+
+
+kill -9 $SIM_PID
+if [[ ! -n "$HEADLESS" ]]; then
+	kill -9 $GUI_PID
+fi