Add airframe

- a growing number of samples come into the backend with the time
already delayed (sensor's interrupt setting timestamp sample)
 - if the incoming timestamp is already delayed then the new data checks
(relative to latest IMU) can be slightly wrong
 - handle almost all timestamps and checks on delayed time horizon,
except for explicit checks of new samples
 - isRecent() and isTimedOut() helpers use delayed time
 - add new isNewestSampleRecent() used for checking the incoming
timestamp of the incoming (adjusted) data

.ci/Jenkinsfile-hardware

@@ -794,7 +794,7 @@ void resetParameters() {
 void runTests() {
 
   // test loading a range of airframes
-  sh './Tools/HIL/test_airframes.sh `find /dev/serial -name *usb-*` 2100 3000 4001 6001 8001 10016'
+  sh './Tools/HIL/test_airframes.sh `find /dev/serial -name *usb-*` 2100 3000 4001 6001 8001'
 
   resetParameters()
 

.gitmodules

@@ -6,12 +6,12 @@
 	path = src/drivers/uavcan/libuavcan
 	url = https://github.com/dronecan/libuavcan.git
 	branch = main
-[submodule "Tools/jMAVSim"]
-	path = Tools/jMAVSim
+[submodule "Tools/simulation/jmavsim/jMAVSim"]
+	path = Tools/simulation/jmavsim/jMAVSim
 	url = https://github.com/PX4/jMAVSim.git
 	branch = master
-[submodule "Tools/sitl_gazebo"]
-	path = Tools/sitl_gazebo
+[submodule "Tools/simulation/gazebo/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"]
-	path = Tools/jsbsim_bridge
+[submodule "Tools/simulation/jsbsim/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
@@ -53,9 +53,6 @@
 [submodule "src/lib/events/libevents"]
 	path = src/lib/events/libevents
 	url = https://github.com/mavlink/libevents.git
-[submodule "Tools/simulation-ignition"]
-	path = Tools/simulation-ignition
-	url = https://github.com/PX4/px4-simulation-ignition.git
 [submodule "src/lib/crypto/libtomcrypt"]
 	path = src/lib/crypto/libtomcrypt
 	url = https://github.com/PX4/libtomcrypt.git

.vscode/cmake-variants.yaml

@@ -2,10 +2,15 @@ CONFIG:
   default: px4_sitl_default
   choices:
     px4_sitl_default:
-      short: px4_sitl
+      short: px4_sitl_default
       buildType: RelWithDebInfo
       settings:
         CONFIG: px4_sitl_default
+    px4_sitl_ign:
+      short: px4_sitl_ign
+      buildType: RelWithDebInfo
+      settings:
+        CONFIG: px4_sitl_ign
     px4_sitl_rtps:
       short: px4_sitl_rtps
       buildType: RelWithDebInfo

.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,23 +86,7 @@
                 "showReuseMessage": false,
                 "clear": false
             },
-            "problemMatcher": [
-                {
-                    "pattern": [
-                        {
-                            "regexp": ".",
-                            "file": 1,
-                            "location": 2,
-                            "message": 3
-                        }
-                    ],
-                    "background": {
-                        "activeOnStart": true,
-                        "beginsPattern": ".",
-                        "endsPattern": ".",
-                    }
-                }
-            ]
+            "problemMatcher": []
         },
         {
             "label": "jmavsim kill",
@@ -116,7 +100,8 @@
                 "showReuseMessage": false,
                 "clear": false
             },
-            "problemMatcher": []
+            "problemMatcher": [],
+            "dependsOn":["px4_sitl_cleanup"]
         },
         {
             "label": "gazebo build",
@@ -143,11 +128,11 @@
                 "cwd": "${workspaceFolder}",
                 "env": {
                     "GAZEBO_PLUGIN_PATH": "${workspaceFolder}/build/px4_sitl_default/build_gazebo",
-                    "GAZEBO_MODEL_PATH": "${workspaceFolder}/Tools/sitl_gazebo/models",
+                    "GAZEBO_MODEL_PATH": "${workspaceFolder}/Tools/simulation/gazebo/sitl_gazebo/models",
                     "PX4_SIM_SPEED_FACTOR": "1"
                 }
             },
-            "command": "gzserver --verbose ${workspaceFolder}/Tools/sitl_gazebo/worlds/${input:gazeboWorld}.world",
+            "command": "gzserver --verbose ${workspaceFolder}/Tools/simulation/gazebo/sitl_gazebo/worlds/${input:gazeboWorld}.world",
             "isBackground": true,
             "presentation": {
                 "echo": true,
@@ -183,11 +168,11 @@
                 "cwd": "${workspaceFolder}",
                 "env": {
                     "GAZEBO_PLUGIN_PATH": "${workspaceFolder}/build/px4_sitl_default/build_gazebo",
-                    "GAZEBO_MODEL_PATH": "${workspaceFolder}/Tools/sitl_gazebo/models",
+                    "GAZEBO_MODEL_PATH": "${workspaceFolder}/Tools/simulation/gazebo/sitl_gazebo/models",
                     "PX4_SIM_SPEED_FACTOR": "1"
                 }
             },
-            "command": "gz model --verbose --spawn-file=${workspaceFolder}/Tools/sitl_gazebo/models/${input:vehicleModel}/${input:vehicleModel}.sdf --model-name=${input:vehicleModel} -x 1.01 -y 0.98 -z 0.83",
+            "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": true,
@@ -216,26 +201,26 @@
             ]
         },
         {
-            "label": "gazebo client",
+            "label": "ign gazebo",
             "type": "shell",
-            "dependsOn": "gazebo build",
             "options": {
                 "cwd": "${workspaceFolder}",
                 "env": {
-                    "GAZEBO_PLUGIN_PATH": "${workspaceFolder}/build/px4_sitl_default/build_gazebo",
-                    "GAZEBO_MODEL_PATH": "${workspaceFolder}/Tools/sitl_gazebo/models",
+                    "IGN_GAZEBO_RESOURCE_PATH": "${workspaceFolder}/Tools/simulation/ignition/models",
                     "PX4_SIM_SPEED_FACTOR": "1"
                 }
             },
-            "command": "gzclient --verbose",
+            "command": "ign gazebo -v 4 -r ${workspaceFolder}/Tools/simulation/ignition/worlds/${input:ignWorld}.sdf",
             "isBackground": true,
             "presentation": {
                 "echo": true,
                 "reveal": "never",
+                "revealProblems": "onProblem",
                 "focus": false,
-                "panel": "shared",
+                "panel": "dedicated",
                 "showReuseMessage": false,
-                "clear": false
+                "clear": false,
+                "close": true
             },
             "problemMatcher": [
                 {
@@ -258,29 +243,51 @@
         {
             "label": "gazebo kill",
             "type": "shell",
-            "command": "killall gzserver",
+            "command": "pkill -9 -f gzserver || true",
             "presentation": {
                 "echo": true,
-                "reveal": "silent",
+                "reveal": "never",
+                "revealProblems": "onProblem",
                 "focus": false,
-                "panel": "shared",
+                "panel": "dedicated",
                 "showReuseMessage": false,
-                "clear": false
+                "clear": false,
+                "close": true
             },
             "problemMatcher": [],
             "dependsOn":["px4_sitl_cleanup"]
         },
         {
-            "label": "px4_sitl_cleanup",
+            "label": "ign gazebo kill",
             "type": "shell",
-            "command": "rm -rfv /tmp/px4*",
+            "command": "pkill -9 -f ign || true",
             "presentation": {
                 "echo": true,
-                "reveal": "silent",
+                "reveal": "never",
+                "revealProblems": "onProblem",
                 "focus": false,
-                "panel": "shared",
+                "panel": "dedicated",
                 "showReuseMessage": false,
-                "clear": false
+                "clear": false,
+                "close": true
+            },
+            "problemMatcher": [],
+            "dependsOn":["px4_sitl_cleanup"]
+        },
+
+        {
+            "label": "px4_sitl_cleanup",
+            "type": "shell",
+            "command": "rm -rfv /tmp/px4* || true",
+            "presentation": {
+                "echo": true,
+                "reveal": "never",
+                "revealProblems": "onProblem",
+                "focus": false,
+                "panel": "dedicated",
+                "showReuseMessage": false,
+                "clear": false,
+                "close": true
             },
             "problemMatcher": [],
             "dependsOn":["px4_kill"]
@@ -288,14 +295,16 @@
         {
             "label": "px4_kill",
             "type": "shell",
-            "command": "killall px4 || true",
+            "command": "pkill -9 px4 || true",
             "presentation": {
                 "echo": true,
-                "reveal": "silent",
+                "reveal": "never",
+                "revealProblems": "onProblem",
                 "focus": false,
-                "panel": "shared",
+                "panel": "dedicated",
                 "showReuseMessage": false,
-                "clear": false
+                "clear": false,
+                "close": true
             },
             "problemMatcher": []
         },
@@ -337,7 +346,16 @@
     "inputs": [
         {
             "type": "pickString",
-            "id": "vehicleModel",
+            "id": "ignWorld",
+            "description": "Ignition world",
+            "options": [
+                "default"
+            ],
+            "default": "default"
+        },
+        {
+            "type": "pickString",
+            "id": "gazeboModel",
             "description": "gazebo model",
             "options": [
               "iris",

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 clone --recursive ${WORKSPACE}/catkin_ws/src/Firmware/Tools/sitl_gazebo src/mavlink_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/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;

Makefile

@@ -314,7 +314,9 @@ uorb_graphs:
 	@$(MAKE) --no-print-directory px4_fmu-v5_default uorb_graph
 	@$(MAKE) --no-print-directory px4_sitl_default uorb_graph
 
-px4io_update: px4_io-v2_default cubepilot_io-v2_default
+px4io_update:
+	@$(MAKE) --no-print-directory px4_io-v2_default
+	@$(MAKE) --no-print-directory cubepilot_io-v2_default
 	# px4_io-v2_default
 	cp build/px4_io-v2_default/px4_io-v2_default.bin boards/holybro/durandal-v1/extras/px4_io-v2_default.bin
 	cp build/px4_io-v2_default/px4_io-v2_default.bin boards/holybro/pix32v5/extras/px4_io-v2_default.bin

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/1010_iris_opt_flow

@@ -9,6 +9,7 @@
 
 # EKF2
 param set-default EKF2_AID_MASK 2
+param set-default EKF2_GPS_CTRL 0
 param set-default EKF2_EVP_NOISE 0.05
 param set-default EKF2_EVA_NOISE 0.05
 

ROMFS/px4fmu_common/init.d-posix/airframes/1013_iris_vision

@@ -10,6 +10,7 @@
 # EKF2: Vision position and heading
 param set-default EKF2_AID_MASK 24
 param set-default EKF2_EV_DELAY 5
+param set-default EKF2_GPS_CTRL 0
 
 # LPE: Vision + baro
 param set-default LPE_FUSION 132

ROMFS/px4fmu_common/init.d-posix/airframes/1017_iris_opt_flow_mockup

@@ -9,6 +9,7 @@
 
 # EKF2
 param set-default EKF2_AID_MASK 2
+param set-default EKF2_GPS_CTRL 0
 
 # LPE: Flow-only mode
 param set-default LPE_FUSION 242

ROMFS/px4fmu_common/init.d-posix/airframes/1018_iris_vision_velocity

@@ -10,3 +10,4 @@
 # EKF2: Vision velocity and heading
 param set-default EKF2_AID_MASK 272
 param set-default EKF2_EV_DELAY 5
+param set-default EKF2_GPS_CTRL 0

ROMFS/px4fmu_common/init.d-posix/airframes/1039_advanced_plane

@@ -0,0 +1,75 @@
+#!/bin/sh
+#
+# @name Plane SITL
+#
+
+. ${R}etc/init.d/rc.fw_defaults
+
+param set-default EKF2_MAG_ACCLIM 0
+param set-default EKF2_MAG_YAWLIM 0
+
+param set-default FW_LND_AIRSPD_SC 1
+param set-default FW_LND_ANG 8
+
+param set-default FW_L1_PERIOD 12
+
+param set-default FW_PR_P 0.9
+param set-default FW_PR_FF 0.5
+param set-default FW_PR_I 0.5
+param set-default TRIM_PITCH -0.15
+
+param set-default FW_PSP_OFF 2
+param set-default FW_P_LIM_MIN -15
+
+param set-default FW_RR_FF 0.5
+param set-default FW_RR_P 0.3
+param set-default FW_RR_I 0.5
+
+param set-default FW_YR_FF 0.5
+param set-default FW_YR_P 0.6
+param set-default FW_YR_I 0.5
+
+param set-default FW_SPOILERS_LND 0.4
+
+param set-default FW_THR_MAX 0.6
+param set-default FW_THR_MIN 0.05
+param set-default FW_THR_TRIM 0.25
+
+param set-default FW_T_CLMB_MAX 8
+param set-default FW_T_SINK_MAX 2.7
+param set-default FW_T_SINK_MIN 2.2
+
+param set-default FW_W_EN 1
+
+param set-default MIS_LTRMIN_ALT 30
+param set-default MIS_TAKEOFF_ALT 30
+
+param set-default NAV_ACC_RAD 15
+param set-default NAV_DLL_ACT 2
+
+param set-default RWTO_TKOFF 1
+
+param set-default CA_AIRFRAME 1
+
+param set-default CA_ROTOR_COUNT 1
+param set-default CA_ROTOR0_PX 0.3
+
+param set-default CA_SV_CS_COUNT 6
+param set-default CA_SV_CS0_TRQ_R -0.5
+param set-default CA_SV_CS0_TYPE 1
+param set-default CA_SV_CS1_TRQ_R 0.5
+param set-default CA_SV_CS1_TYPE 2
+param set-default CA_SV_CS2_TRQ_P 1.0
+param set-default CA_SV_CS2_TYPE 3
+param set-default CA_SV_CS3_TRQ_Y 1.0
+param set-default CA_SV_CS3_TYPE 4
+param set-default CA_SV_CS4_TYPE 9
+param set-default CA_SV_CS5_TYPE 10
+param set-default PWM_MAIN_FUNC3 204
+param set-default PWM_MAIN_FUNC4 205
+param set-default PWM_MAIN_FUNC5 101
+param set-default PWM_MAIN_FUNC6 201
+param set-default PWM_MAIN_FUNC7 202
+param set-default PWM_MAIN_FUNC8 203
+param set-default PWM_MAIN_FUNC9 206
+param set-default PWM_MAIN_REV 256

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

@@ -0,0 +1,50 @@
+#!/bin/sh
+#
+# @name Ignition Gazebo X3
+#
+# @type Quadrotor
+#
+
+. ${R}etc/init.d/rc.mc_defaults
+
+PX4_SIMULATOR=${PX4_SIMULATOR:=ignition}
+PX4_SIM_MODEL=${PX4_SIM_MODEL:=x500}
+PX4_SIM_WORLD=${PX4_SIM_WORLD:=default}
+
+param set-default SYS_CTRL_ALLOC 1
+
+param set-default CA_AIRFRAME 0
+param set-default CA_ROTOR_COUNT 4
+
+param set-default CA_ROTOR0_PX 0.13
+param set-default CA_ROTOR0_PY 0.22
+param set-default CA_ROTOR0_KM  0.05
+
+param set-default CA_ROTOR1_PX -0.13
+param set-default CA_ROTOR1_PY -0.20
+param set-default CA_ROTOR1_KM  0.05
+
+param set-default CA_ROTOR2_PX 0.13
+param set-default CA_ROTOR2_PY -0.22
+param set-default CA_ROTOR2_KM -0.05
+
+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_IGN_FUNC1 101
+param set-default SIM_IGN_FUNC2 102
+param set-default SIM_IGN_FUNC3 103
+param set-default SIM_IGN_FUNC4 104
+
+param set-default SIM_IGN_MIN1 150
+param set-default SIM_IGN_MIN2 150
+param set-default SIM_IGN_MIN3 150
+param set-default SIM_IGN_MIN4 150
+
+param set-default SIM_IGN_MAX1 1000
+param set-default SIM_IGN_MAX2 1000
+param set-default SIM_IGN_MAX3 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

@@ -0,0 +1,80 @@
+#!/bin/sh
+#
+# @name Ignition Gazebo X4
+#
+# @type Quadrotor
+#
+
+. ${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
+param set-default CA_ROTOR_COUNT 6
+
+param set-default CA_ROTOR0_PX  0.25
+param set-default CA_ROTOR0_PY -0.15
+param set-default CA_ROTOR0_KM  0.05
+
+param set-default CA_ROTOR1_PX  0.00
+param set-default CA_ROTOR1_PY -0.29
+param set-default CA_ROTOR1_KM -0.05
+
+param set-default CA_ROTOR2_PX -0.25
+param set-default CA_ROTOR2_PY -0.15
+param set-default CA_ROTOR2_KM  0.05
+
+param set-default CA_ROTOR3_PX -0.25
+param set-default CA_ROTOR3_PY  0.15
+param set-default CA_ROTOR3_KM -0.05
+
+param set-default CA_ROTOR4_PX  0.00
+param set-default CA_ROTOR4_PY  0.29
+param set-default CA_ROTOR4_KM  0.05
+
+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_IGN_FUNC1 101
+param set-default SIM_IGN_FUNC2 102
+param set-default SIM_IGN_FUNC3 103
+param set-default SIM_IGN_FUNC4 104
+param set-default SIM_IGN_FUNC5 105
+param set-default SIM_IGN_FUNC6 106
+
+param set-default SIM_IGN_MIN1 150
+param set-default SIM_IGN_MIN2 150
+param set-default SIM_IGN_MIN3 150
+param set-default SIM_IGN_MIN4 150
+param set-default SIM_IGN_MIN5 150
+param set-default SIM_IGN_MIN6 150
+
+param set-default SIM_IGN_MAX1 1000
+param set-default SIM_IGN_MAX2 1000
+param set-default SIM_IGN_MAX3 1000
+param set-default SIM_IGN_MAX4 1000
+param set-default SIM_IGN_MAX5 1000
+param set-default SIM_IGN_MAX6 1000
+
+
+param set-default MC_PITCHRATE_D 0.0016
+param set-default MC_PITCHRATE_I 0.2500
+param set-default MC_PITCHRATE_P 0.1831
+param set-default MC_PITCH_P 5.216
+
+param set-default MC_ROLLRATE_D 0.0022
+param set-default MC_ROLLRATE_I 0.2095
+param set-default MC_ROLLRATE_P 0.1570
+param set-default MC_ROLL_P 6.081
+
+param set-default MC_YAWRATE_D 0.0009
+param set-default MC_YAWRATE_I 0.1800
+param set-default MC_YAWRATE_P 0.1773
+param set-default MC_YAW_P 5.386490
+
+param set-default MPC_THR_HOVER 0.61

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

@@ -61,6 +61,7 @@ px4_add_romfs_files(
 	1036_malolo
 	1037_believer
 	1038_glider
+	1039_advanced_plane
 	1040_standard_vtol
 	1041_tailsitter
 	1042_tiltrotor
@@ -72,9 +73,14 @@ px4_add_romfs_files(
 	1070_boat
 	3010_quadrotor_x
 	3011_hexarotor_x
+
+	4001_x500
+
 	17001_tf-g1
 	17002_tf-g2
 	2507_cloudship
+
+	6001_x4
 	6011_typhoon_h480
 	6011_typhoon_h480.post
 )

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.rtps

@@ -1,5 +1,4 @@
 #!/bin/sh
 # shellcheck disable=SC2154
 
-#micrortps_client start -t UDP -r $((2019+2*px4_instance)) -s $((2020+2*px4_instance))
-microdds_client start -t udp -p 15555
+micrortps_client start -t UDP -r $((2019+2*px4_instance)) -s $((2020+2*px4_instance))

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

@@ -1,20 +1,68 @@
 #!/bin/sh
 # shellcheck disable=SC2154
 
-simulator_tcp_port=$((4560+px4_instance))
+# Simulator IMU data provided at 250 Hz
+param set-default IMU_INTEG_RATE 250
+
+if [ "$PX4_SIMULATOR" = "sihsim" ] || [ "$(param show -q SYS_AUTOSTART)" -eq "0" ]; then
+
+	if ! simulator_sih start; then
+		echo "ERROR  [init] simulator_sih failed to start"
+		exit 1
+	fi
+
+elif [ "$PX4_SIMULATOR" = "ignition" ]; then
+
+	# source generated gazebo_env.sh for IGN_GAZEBO_RESOURCE_PATH
+	if [ -f gazebo_env.sh ]; then
+		. ./gazebo_env.sh
+	elif [ -f ../gazebo_env.sh ]; then
+		. ../gazebo_env.sh
+	fi
+
+	if ! ign service --info --service /world/${PX4_SIM_WORLD}/create | grep "ignition.msgs.EntityFactory"; then
+		# starting ign gazebo with ${PX4_SIM_WORLD} world
+		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
 
-# Check if PX4_SIM_HOSTNAME environment variable is empty
-# If empty check if PX4_SIM_HOST_ADDR environment variable is empty
-# If both are empty use localhost for simulator
-if [ -z "${PX4_SIM_HOSTNAME}" ]; then
-  if [ -z "${PX4_SIM_HOST_ADDR}" ]; then
-    echo "PX4 SIM HOST: localhost"
-    simulator start -c $simulator_tcp_port
-  else
-    echo "PX4 SIM HOST: $PX4_SIM_HOST_ADDR"
-    simulator 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
+	# otherwise start simulator (mavlink) module
+	simulator_tcp_port=$((4560+px4_instance))
+
+	# Check if PX4_SIM_HOSTNAME environment variable is empty
+	# If empty check if PX4_SIM_HOST_ADDR environment variable is empty
+	# If both are empty use localhost for simulator
+	if [ -z "${PX4_SIM_HOSTNAME}" ]; then
+
+		if [ -z "${PX4_SIM_HOST_ADDR}" ]; then
+			echo "PX4 SIM HOST: localhost"
+			simulator_mavlink start -c $simulator_tcp_port
+		else
+			echo "PX4 SIM HOST: $PX4_SIM_HOST_ADDR"
+			simulator_mavlink start -t $PX4_SIM_HOST_ADDR $simulator_tcp_port
+		fi
+
+	else
+		echo "PX4 SIM HOST: $PX4_SIM_HOSTNAME"
+		simulator_mavlink start -h $PX4_SIM_HOSTNAME $simulator_tcp_port
+	fi
+
 fi

ROMFS/px4fmu_common/init.d-posix/rcS

@@ -30,6 +30,7 @@ set MIXER_FILE                  none
 set OUTPUT_MODE                 sim
 set EXTRA_MIXER_MODE            none
 set PWM_OUT                     none
+set PWM_AUX_OUT                 none
 set SDCARD_MIXERS_PATH          etc/mixers
 set USE_IO                      no
 set VEHICLE_TYPE                none
@@ -38,10 +39,24 @@ set LOGGER_BUF                  1000
 
 set RUN_MINIMAL_SHELL           no
 
-# Use the variable set by sitl_run.sh to choose the model settings.
-if [ "$PX4_SIM_MODEL" = "shell" ]; then
+set SYS_AUTOSTART=0
+
+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 +65,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 +82,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.
-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
+elif [ "$SYS_AUTOSTART" -eq 0 ]
 then
 	set AUTOCNF no
 else
@@ -106,7 +112,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
@@ -143,6 +149,7 @@ param set-default EKF2_MULTI_MAG 2
 param set-default SENS_MAG_MODE 0
 
 param set-default IMU_GYRO_FFT_EN 1
+param set-default MAV_PROTO_VER 2 # Ensures QGC does not drop the first few packets after a SITL restart due to MAVLINK 1 packets
 
 param set-default -s MC_AT_EN 1
 
@@ -190,28 +197,28 @@ 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
 battery_simulator start
 tone_alarm start
@@ -228,12 +235,6 @@ commander start
 
 navigator start
 
-# Try to start the microdds_client with UDP transport if module exists
-if px4-microdds_client status > /dev/null 2>&1
-then
-	microdds_client start -t udp -p 15555
-fi
-
 # Try to start the micrortps_client with UDP transport if module exists
 if px4-micrortps_client status > /dev/null 2>&1
 then

ROMFS/px4fmu_common/init.d/airframes/4019_x500_v2

@@ -0,0 +1,35 @@
+#!/bin/sh
+#
+# @name Holybro X500 V2
+#
+# @type Quadrotor x
+# @class Copter
+#
+# @maintainer Farhang Naderi <farhang.nba@gmail.com>
+#
+# @board px4_fmu-v2 exclude
+# @board bitcraze_crazyflie exclude
+#
+
+. ${R}etc/init.d/rc.mc_defaults
+
+param set-default IMU_GYRO_CUTOFF 30
+
+param set-default MC_ROLLRATE_P 0.14
+param set-default MC_PITCHRATE_P 0.14
+param set-default MC_ROLLRATE_I 0.3
+param set-default MC_PITCHRATE_I 0.3
+param set-default MC_ROLLRATE_D 0.004
+param set-default MC_PITCHRATE_D 0.004
+
+param set-default CA_ROTOR_COUNT 4
+param set-default CA_ROTOR0_PX 0.25
+param set-default CA_ROTOR0_PY 0.25
+param set-default CA_ROTOR1_PX -0.25
+param set-default CA_ROTOR1_PY -0.25
+param set-default CA_ROTOR2_PX 0.25
+param set-default CA_ROTOR2_PY -0.25
+param set-default CA_ROTOR2_KM -0.05
+param set-default CA_ROTOR3_PX -0.25
+param set-default CA_ROTOR3_PY 0.25
+param set-default CA_ROTOR3_KM -0.05

ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu

@@ -73,7 +73,7 @@ param set-default EKF2_PCOEF_YN -0.4
 param set-default EKF2_PCOEF_YP -0.4
 
 param set-default EKF2_RNG_A_VMAX 1.0
-param set-default EKF2_RNG_AID 0
+param set-default EKF2_RNG_CTRL 0
 param set-default EKF2_RNG_DELAY 55
 param set-default EKF2_RNG_POS_X -0.035
 param set-default EKF2_RNG_POS_Y 0.0

ROMFS/px4fmu_common/init.d/airframes/4071_ifo

@@ -38,7 +38,7 @@ param set-default SENS_BOARD_ROT 10
 
 # EKF2
 param set-default EKF2_GND_EFF_DZ 6
-param set-default EKF2_HGT_MODE 1
+param set-default EKF2_HGT_REF 1
 
 # Position control
 param set-default MPC_Z_P 1

ROMFS/px4fmu_common/init.d/airframes/4073_ifo-s

@@ -42,7 +42,7 @@ param set-default SENS_EN_BATT 1
 # EKF2
 param set-default EKF2_AID_MASK 3
 param set-default EKF2_GND_EFF_DZ 6
-param set-default EKF2_HGT_MODE 1
+param set-default EKF2_HGT_REF 1
 param set-default EKF2_MIN_RNG 0.3
 
 # Flow

ROMFS/px4fmu_common/init.d/airframes/4900_crazyflie

@@ -26,7 +26,8 @@ param set-default COM_RC_IN_MODE 1
 
 param set-default EKF2_ABL_LIM 2
 param set-default EKF2_AID_MASK 3
-param set-default EKF2_HGT_MODE 2
+param set-default EKF2_HGT_REF 2
+param set-default EKF2_RNG_CTRL 2
 param set-default EKF2_MAG_TYPE 1
 param set-default EKF2_OF_DELAY 10
 

ROMFS/px4fmu_common/init.d/airframes/4901_crazyflie21

@@ -20,6 +20,7 @@
 param set-default SYS_MC_EST_GROUP 2
 param set-default SYS_HAS_MAG 0
 param set-default EKF2_AID_MASK 2
+param set-default EKF2_GPS_CTRL 0
 param set-default EKF2_MAG_TYPE 5
 
 param set-default BAT1_N_CELLS 1

ROMFS/px4fmu_common/init.d/airframes/6002_draco_r

@@ -71,7 +71,7 @@ param set-default MIS_TAKEOFF_ALT 1.1
 # EKF
 #####################################
 # Height mode as GPS
-param set-default EKF2_HGT_MODE 1
+param set-default EKF2_HGT_REF 1
 # Enable optical flow and GPS
 param set-default EKF2_MAG_TYPE 1
 param set-default EKF2_OF_QMIN 80

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

@@ -56,6 +56,7 @@ px4_add_romfs_files(
 	4015_holybro_s500
 	4016_holybro_px4vision
 	4017_nxp_hovergames
+	4019_x500_v2
 	4040_reaper
 	4041_beta75x
 	4050_generic_250

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

ROMFS/px4fmu_common/mixers/AAERTWF.main.mix

@@ -1,3 +1,4 @@
+# @board px4_fmu-v2 exclude
 Aileron/rudder/elevator/throttle/wheel/flaps mixer for PX4FMU
 =======================================================
 

ROMFS/px4fmu_common/mixers/AAVVTWFF.main.mix

@@ -1,3 +1,4 @@
+# @board px4_fmu-v2 exclude
 Aileron/v-tail/throttle/wheel/flaps mixer for PX4FMU
 =======================================================
 

ROMFS/px4fmu_common/mixers/AAVVTWFF_vtail.main.mix

@@ -1,3 +1,4 @@
+# @board px4_fmu-v2 exclude
 Aileron/v-tail/throttle/wheel/flaps mixer for PX4FMU
 =======================================================
 

ROMFS/px4fmu_common/mixers/AERT.main.mix

@@ -1,3 +1,4 @@
+# @board px4_fmu-v2 exclude
 Aileron/rudder/elevator/throttle mixer for PX4FMU
 ==================================================
 
@@ -73,4 +74,4 @@ Ch6: Landing gear mixer
 By default pass-through of gear switch
 
 M: 1
-S: 3 5 10000 10000 0 -10000 10000
+S: 3 5 10000 10000 0 -10000 10000

ROMFS/px4fmu_common/mixers/AETRFG.main.mix

@@ -1,3 +1,4 @@
+# @board px4_fmu-v2 exclude
 Aileron/Elevator/Throttle/Rudder/Gear/Flaps mixer
 ==================================================
 

ROMFS/px4fmu_common/mixers/CCPM.main.mix

@@ -1,3 +1,4 @@
+# @board px4_fmu-v2 exclude
 Helicopter 120 degree Cyclic-Collective-Pitch Mixing (CCPM) for PX4FMU
 ==================================================
 

ROMFS/px4fmu_common/mixers/IO_pass.main.mix

@@ -1,3 +1,4 @@
+# @board px4_fmu-v2 exclude
 Passthrough mixer for PX4IO
 ============================
 

ROMFS/px4fmu_common/mixers/coax.main.mix

@@ -1,3 +1,4 @@
+# @board px4_fmu-v2 exclude
 Coaxial helicopter mixer
 - Two servomotors act on the swashplate (90 degree angle on the swashplate, decoupled effect on roll and pitch).
 - No collective pitch.

ROMFS/px4fmu_common/mixers/fw_generic_wing.main.mix

@@ -1,3 +1,4 @@
+# @board px4_fmu-v2 exclude
 Generic wing mixer
 ===========================
 

ROMFS/px4fmu_common/mixers/generic_diff_rover.main.mix

@@ -1,3 +1,4 @@
+# @board px4_fmu-v2 exclude
 Generic differential-drive rover
 ===========================
 

ROMFS/px4fmu_common/mixers/hexa_+.main.mix

@@ -1,3 +1,4 @@
+# @board px4_fmu-v2 exclude
 # Hexa +
 
 R: 6+

ROMFS/px4fmu_common/mixers/hexa_cox.main.mix

@@ -1,3 +1,4 @@
+# @board px4_fmu-v2 exclude
 # Hexa coaxial
 
 R: 6c

ROMFS/px4fmu_common/mixers/hexa_x.main.mix

@@ -1,3 +1,4 @@
+# @board px4_fmu-v2 exclude
 # Hexa X
 
 R: 6x

ROMFS/px4fmu_common/mixers/mount.aux.mix

@@ -1,3 +1,4 @@
+# @board px4_fmu-v2 exclude
 # Mount Mixer (e.g. Gimbal, servo-controlled gimbal, etc...)
 
 

ROMFS/px4fmu_common/mixers/octo_+.main.mix

@@ -1,3 +1,4 @@
+# @board px4_fmu-v2 exclude
 # Octo +
 
 R: 8+

ROMFS/px4fmu_common/mixers/octo_cox.main.mix

@@ -1,3 +1,4 @@
+# @board px4_fmu-v2 exclude
 # Octo coaxial
 
 R: 8c

ROMFS/px4fmu_common/mixers/octo_x.main.mix

@@ -1,3 +1,4 @@
+# @board px4_fmu-v2 exclude
 # Octo X
 
 R: 8x

ROMFS/px4fmu_common/mixers/pass.aux.mix

@@ -1,3 +1,4 @@
+# @board px4_fmu-v2 exclude
 # Manual pass through mixer for servo outputs 1-4
 
 # AUX1 channel (select RC channel with RC_MAP_AUX1 param)

ROMFS/px4fmu_common/mixers/quad_+.main.mix

@@ -1,3 +1,4 @@
+# @board px4_fmu-v2 exclude
 Multirotor mixer for PX4FMU
 ===========================
 

ROMFS/px4fmu_common/mixers/quad_+_vtol.main.mix

@@ -1,3 +1,4 @@
+# @board px4_fmu-v2 exclude
 Mixer for Tailsitter with + motor configuration and elevons
 ===========================================================
 

ROMFS/px4fmu_common/mixers/quad_dc.main.mix

@@ -1,3 +1,4 @@
+# @board px4_fmu-v2 exclude
 Multirotor mixer
 ===========================
 

ROMFS/px4fmu_common/mixers/quad_h.main.mix

@@ -1,3 +1,4 @@
+# @board px4_fmu-v2 exclude
 Multirotor mixer for PX4FMU
 ===========================
 

ROMFS/px4fmu_common/mixers/quad_w.main.mix

@@ -1,3 +1,4 @@
+# @board px4_fmu-v2 exclude
 Multirotor mixer for PX4FMU
 ===========================
 

ROMFS/px4fmu_common/mixers/quad_x.main.mix

@@ -1,3 +1,4 @@
+# @board px4_fmu-v2 exclude
 R: 4x
 
 AUX1 Passthrough

ROMFS/px4fmu_common/mixers/quad_x_vtol_AAERT.main.mix

@@ -1,3 +1,4 @@
+# @board px4_fmu-v2 exclude
 Mixer for an AAERT VTOL
 =======================
 

ROMFS/px4fmu_common/mixers/rover_diff_and_servo.main.mix

@@ -1,3 +1,4 @@
+# @board px4_fmu-v2 exclude
 Generic car mixer (eg DF Robot GPX:Asurada RC Car)
 ===========================
 

ROMFS/px4fmu_common/mixers/rover_generic.main.mix

@@ -1,3 +1,4 @@
+# @board px4_fmu-v2 exclude
 Generic car mixer (eg Traxxas Stampede RC Car)
 ===========================
 

ROMFS/px4fmu_common/mixers/tri_y_yaw+.main.mix

@@ -1,3 +1,4 @@
+# @board px4_fmu-v2 exclude
 
 
 # Tricopter Y-Configuration Mixer

ROMFS/px4fmu_common/mixers/tri_y_yaw-.main.mix

@@ -1,3 +1,4 @@
+# @board px4_fmu-v2 exclude
 
 
 # Tricopter Y-Configuration Mixer

ROMFS/px4fmu_common/mixers/vtol_tailsitter_duo_sat.main.mix

@@ -1,3 +1,4 @@
+# @board px4_fmu-v2 exclude
 Tailsitter duo mixer
 ============================
 

Tools/fix_headers.sh

@@ -1,901 +0,0 @@
-#! /bin/bash
-
-# Written by Carlo Wood, September/October 2016.
-
-function fatal
-{
-	echo "$0: ERROR: $*"
-	exit 1
-}
-
-# Make sure we're not having a broken gawk.
-AWK_VERSION=$(awk -V | head -n 1)
-if [[ $AWK_VERSION =~ ^GNU\ Awk\ 4\.[0-9]+\.[0-9]+ ]]; then
-	AWK_VERSION=$(echo $AWK_VERSION | sed -e 's/GNU Awk \(4\.[0-9]*\.[0-9]*\).*/\1/')
-	if [[ $AWK_VERSION =~ ^4\.0*([2-9]+|1\.0*[2-9]+) ]]; then
-		fatal "Your version of awk ($AWK_VERSION) is broken. Please use version 4.1.1 or lower."
-	fi
-fi
-echo "AWK_VERSION=$AWK_VERSION"
-
-# Find out what the base directory is.
-BASEDIR="$(dirname $(dirname $(readlink -en "$0")))"
-echo "BASEDIR=\"$BASEDIR\""
-striplen=$((${#BASEDIR} + 2))
-
-# BASEDIR may not contain a space, that's just too hard to get to work.
-expr index "$BASEDIR" " " >/dev/null && fatal "it is not supported that BASEDIR contains a space."
-
-# Make sure that worked.
-test -f $BASEDIR/cmake/posix/px4_impl_posix.cmake || fatal "Failed to determine BASEDIR: '\$BASEDIR/cmake/posix/px4_impl_posix.cmake' is not a regular file."
-
-# Parse command line parameters.
-debug=0	# Set to non-zero to enable debugging.
-force=0	# Set to 1 to force running of script even when there are uncommitted changes.
-merge=0	# Set to 1 when merging a branch that didn't run this script into master that did already run this script.
-while [[ $# -gt 0 ]]
-do
-	case $1 in
-		--debug)
-			debug=1
-			;;
-		--force)
-			force=1
-			;;
-		--merge)
-			force=1
-			merge=1
-			fatal "--merge is not implemented yet."
-			;;
-		-*)
-			fatal "Unknown option $1"
-			;;
-		--|*)
-			break
-			;;
-	esac
-	shift
-done
-non_option_arguments=$#
-if [ $non_option_arguments -eq 0 -a $debug -ne 0 ]; then
-	fatal "--debug screws up the source files with debug output! You must provide a single filename to run on."
-fi
-
-# Better not run this script with changes that still need to be committed.
-cd "$BASEDIR" || fatal "Could not change directory to \"$BASEDIR\""
-if ! git diff-index --quiet HEAD --; then
-	if [ $non_option_arguments -ne 0 -o $force -eq 1 ]; then
-		if [ $force -eq 1 ]; then
-			echo "Uncommitted changes, but running anyway because --force is used."
-		else
-			echo -n "WARNING: You have uncommitted changes (use --force to remove this warning). Run anyway? [y/N] "
-			read answer
-			if [ "x$answer" != "xy" -a "x$answer" != "xY" ]; then exit 0; fi
-		fi
-	else
-		fatal "Your working directory has uncommitted changes (see 'git status')! Bailing out."
-	fi
-fi
-
-# Find a reasonable tmp directory.
-# First make a list of all build directories by looking for a CMakeCache.txt in them. Sort them so the most recent one is first.
-CMAKECACHE_FILES=$(find "$BASEDIR" -mindepth 2 -maxdepth 2 -type f -name CMakeCache.txt -wholename "$BASEDIR/build/*/CMakeCache.txt" | xargs /bin/ls -td)
-# Make a list of all candidate tmp directories.
-TMPDIRS=
-for f in $CMAKECACHE_FILES; do
-	if [ -d $(dirname $f)/tmp ]; then
-		TMPDIRS+=" $(dirname $f)/tmp"
-	fi
-done
-# Put BASEDIR first in case there are no build directories because /tmp is probably on a different file system.
-TMPDIRS+=" $BASEDIR /tmp ."
-# Pick the first one that is actually writable.
-for tmp in $TMPDIRS; do
-	TMPDIR="$tmp"
-	if [ -w "$TMPDIR" ]; then
-		break;
-	fi
-done
-test -n "$TMPDIR" || fatal "Can not find a writable tmp directory."
-echo "TMPDIR=\"$TMPDIR\""
-
-# Make a list of all source and header files that we need to fix.
-# List of directories that we don't want to touch.
-EXCLUDE_FOLDERS=".git Tools"
-EXCLUDE_PATTERNS="examples matlab/scripts tests test unit_test *_test *_tests test_* apps/test_* UnitTests"
-# A regular expression for the exclude patterns.
-EXCLUDE_PATTERNS_RE="($(echo $EXCLUDE_PATTERNS | sed -e 's/\*/[^\/]*/g;s/ /|/g'))"
-# Extensions of files that we do want to change (leaving out .y and .py for now).
-C_EXTENSIONS=".c .c_in .c_shipped"
-H_EXTENSIONS=".h .h.in .h_template"
-CXX_EXTENSIONS=".cc .cpp .cpp.in .cxx .cpp_template"
-HXX_EXTENSIONS=".hh .hpp .hxx"
-# The regular expression that we consider to be an #include.
-INCLUDE_RE='^[[:space:]]*#[[:space:]]*include[[:space:]]*[<"]'
-# Regular expression for empty lines.
-EMPTY_LINE_RE='^[[:space:]]*$'
-# Regular expression for one-line comments.
-COMMENT_LINE_RE='^[[:space:]]*(\/\/.*|\/\*([^*]|\*+[^\/*])*\*+\/[[:space:]]*)$'
-# Regular expression for a #define (on one line).
-DEFINE_RE='^[[:space:]]*#[[:space:]]*define[[:space:]].*[^\\]$'
-# Regular expression for an #if[[n]def].
-IF_RE='^[[:space:]]*#[[:space:]]*if(n?def)?[[:space:]]'
-# Regular expression for an #endif.
-ENDIF_RE='^[[:space:]]*#[[:space:]]*endif($|[^[:alnum:]])'
-# Regular expression for header file extension.
-HEADER_RE="($(echo $H_EXTENSIONS $HXX_EXTENSIONS | sed -e 's/\./\\./g;s/ /|/g'))"
-# Regular expression for C++ source and header files.
-CXXSRC_RE="($(echo $CXX_EXTENSIONS $HXX_EXTENSIONS | sed -e 's/\./\\./g;s/ /|/g'))"
-# List of standard C header files. Note that cfcntl, cshed and cunistd are NOT standard header files, even though they are in NuttX/nuttx/include/cxx.
-REAL_STDC_HEADERS_RE='(cassert|ccomplex|cctype|cerrno|cfenv|cfloat|cinttypes|ciso646|climits|clocale|cmath|csetjmp|csignal|cstdalign|cstdarg|cstdbool|cstddef|cstdint|cstdio|cstdlib|cstring|ctgmath|ctime|cuchar|cwchar|cwctype)'
-STDC_HEADERS=$(find "$BASEDIR/NuttX/nuttx/include/cxx" -mindepth 1 -maxdepth 1 -type f | xargs basename -a | grep -E "$REAL_STDC_HEADERS_RE" | xargs echo)
-# Regular expression of standard C header files, but with the leading 'c' stripped.
-STDC_HEADERS_RE="($(echo $STDC_HEADERS | sed -e 's/^c//;s/ c/|/g'))"
-# Actual list of standard C header files.
-# List of standard C++ header files.
-REAL_STDCXX_HEADERS_RE='(algorithm|any|array|atomic|bitset|cassert|ccomplex|cctype|cerrno|cfenv|cfloat|chrono|cinttypes|ciso646|climits|clocale|cmath|codecvt|complex|condition_variable|csetjmp|csignal|cstdalign|cstdarg|cstdbool|cstddef|cstdint|cstdio|cstdlib|cstring|ctgmath|ctime|cuchar|cwchar|cwctype|deque|exception|execution|filesystem|forward_list|fstream|functional|future|initializer_list|iomanip|ios|iosfwd|iostream|istream|iterator|limits|list|locale|map|memory|memory_resource|mutex|new|numeric|optional|ostream|queue|random|ratio|regex|scoped_allocator|set|shared_mutex|sstream|stack|stdexcept|streambuf|string|string_view|strstream|system_error|thread|tuple|typeindex|typeinfo|type_traits|unordered_map|unordered_set|utility|valarray|variant|vector)'
-STDCXX_HEADERS=$(find "$BASEDIR/NuttX/misc/uClibc++/include/uClibc++" -mindepth 1 -maxdepth 1 -type f | xargs basename -a | grep -E "$REAL_STDCXX_HEADERS_RE" | grep -E -v "$REAL_STDC_HEADERS_RE" | xargs echo)
-# Regular expression of C++ header files.
-STDCXX_HEADERS_RE="($(echo $STDCXX_HEADERS | sed -e 's/ /|/g'))"
-# Regular expression for #pragma once.
-PRAGMA_ONCE_RE='^#pragma once'
-# Regular expression to recognize the start of a C-comment block.
-COMMENT_BEGIN_RE='(^|[^\/])\/\*([^*]|\*+($|[^\/*]))*$'
-# Regular expression to recognize the end of a C-comment block.
-COMMENT_END_RE='\*\/'
-# Regular expression to match C++ unsafe headers. We currently don't have any C++ unsafe headers, do we?
-# v2.0/standard/mavlink.h is not unsafe, but this way the script will leave it alone and not
-# move it above function declarations that need to be declared before including it.
-UNSAFE_HEADERS_RE='(v2\.0\/standard\/mavlink\.h)'
-#UNSAFE_HEADERS_RE='(stm32\.h|arch\/board\/board\.h)'
-
-# Find all submodules.
-test -f $BASEDIR/.gitmodules || fatal "No such file: $BASEDIR/.gitmodules"
-SUBMODULES=$(grep -A 1 '^\[submodule' $BASEDIR/.gitmodules | grep '^[[:space:]]*path = ' | sed -r -e 's/^[[:space:]]*path = //' | xargs echo)
-echo "SUBMODULES=\"$SUBMODULES\""
-SUBMODULES_RE="($(echo $SUBMODULES | sed -e 's/ /|/g'))"
-
-# Disable path name expansion (otherwise the find patterns will be expanded against the files in the current working directory).
-set -f
-
-EXCLUDE_ARGS=
-for excl in $EXCLUDE_FOLDERS; do
-	if [ -z "$EXCLUDE_ARGS" ]; then
-		EXCLUDE_ARGS="-wholename $BASEDIR/$excl/*"
-	else
-		EXCLUDE_ARGS+=" -o -wholename $BASEDIR/$excl/*"
-	fi
-done
-for excl in $EXCLUDE_PATTERNS; do
-	EXCLUDE_ARGS+=" -o -wholename */$excl/*"
-done
-INCLUDE_H_ARGS=
-for ext in $H_EXTENSIONS $HXX_EXTENSIONS; do
-	if [ -z "$INCLUDE_H_ARGS" ]; then
-		INCLUDE_H_ARGS="-name *$ext"
-	else
-		INCLUDE_H_ARGS+=" -o -name *$ext"
-	fi
-done
-INCLUDE_C_ARGS=
-for ext in $C_EXTENSIONS $CXX_EXTENSIONS; do
-	if [ -z "$INCLUDE_C_ARGS" ]; then
-		INCLUDE_C_ARGS="-name *$ext"
-	else
-		INCLUDE_C_ARGS+=" -o -name *$ext"
-	fi
-done
-# Also exclude all submodules -- because we don't maintain those (are we?).
-for subm in $SUBMODULES; do
-	if [ -z "$SUBMODULES_ARGS" ]; then
-		SUBMODULES_ARGS="-wholename $BASEDIR/$subm/*"
-	else
-		SUBMODULES_ARGS+=" -o -wholename $BASEDIR/$subm/*"
-	fi
-done
-
-echo -n "Finding all source files with #include's (excluding submodules and build directory)... "
-find $BASEDIR -mindepth 2 -type f ! \( -wholename $BASEDIR/build/* -o $EXCLUDE_ARGS -o $SUBMODULES_ARGS \) \( $INCLUDE_C_ARGS -o $INCLUDE_H_ARGS \) > $TMPDIR/fix_headers_sources
-cat "$TMPDIR/fix_headers_sources" | xargs grep -l "$INCLUDE_RE" > $TMPDIR/fix_headers_sources_with_includes
-echo "done"
-number_of_files=$(sed -n '$=' "$TMPDIR/fix_headers_sources_with_includes")
-count=0
-
-echo -n "Finding all submodule header files (excluding stdc++ headers)... "
-find $BASEDIR -type f ! \( $EXCLUDE_ARGS \) \( $SUBMODULES_ARGS \) \( $INCLUDE_H_ARGS \) > $TMPDIR/fix_headers_SUBMODULE_HEADERS
-echo "done"
-
-echo -n "Finding all header files (excluding stdc++ headers)... "
-find $BASEDIR -type f ! \( $EXCLUDE_ARGS \) -wholename $BASEDIR/build/* \( $INCLUDE_H_ARGS \) > $TMPDIR/fix_headers_HEADERS
-grep -E "$HEADER_RE" $TMPDIR/fix_headers_sources >> $TMPDIR/fix_headers_HEADERS
-cat $TMPDIR/fix_headers_SUBMODULE_HEADERS >> $TMPDIR/fix_headers_HEADERS
-echo "done"
-
-echo -n "Finding all include paths... "
-for f in `cat $TMPDIR/fix_headers_sources_with_includes`; do grep -E "$INCLUDE_RE" $f | sed -r -e "s%$INCLUDE_RE%%"';s/[">].*//'; done | sort -u | grep -E -v "(/|^)$EXCLUDE_PATTERNS_RE/" > $TMPDIR/fix_headers_include_paths
-echo "done"
-
-function include_path()
-{
-	# If the include path starts with a '.', then it is a local header.
-	if [[ $1 =~ ^\. ]]; then return 1; fi
-	# If the include path starts with 'platforms/' then it is a local header;
-	# added this exception here because not everyone has all build_ directories for all targets installed.
-	if [[ $1 =~ platforms/ ]]; then return 1; fi
-	# apps.h is generated from apps.h.in.
-	if [ $1 = "apps.h" ]; then return 1; fi
-	# Treat the following headers from src/platforms/*/include as system header because they replace what is found in nuttx (for posix and qurt).
-	if [ $1 = "arch/board/board.h" -o $1 = "crc32.h" -o $1 = "i2c.h" -o $1 = "queue.h" -o $1 = "poll.h" -o $1 = "sys/ioctl.h" ]; then return 2; fi
-	# Escape the path for reg.exp. matching.
-	PATH_RE=$(echo $1 | sed -e 's/\([+.]\)/\\\1/')
-	issubmodule=0;
-	islocal=0;
-	foo=0
-	for includedir in $(grep "/$PATH_RE\$" $TMPDIR/fix_headers_HEADERS | cut -c $striplen-); do
-		# If the include directory is NuttX header that was copied to the build directory, then it's still a system file.
-		if [[ $includedir/ =~ ^build/.*/NuttX/ ]]; then
-			issubmodule=1
-		# If the include directory is a submodule, then treat it as a system file.
-		elif [[ $includedir/ =~ ^$SUBMODULES_RE/ ]]; then
-			issubmodule=1;
-		else
-			islocal=1
-		fi
-	done
-	if [ $islocal -eq 0 ]; then
-		if [ $issubmodule -eq 0 ]; then
-			# If an include path can't be found then usually it will be a real system header,
-			# however, there are a few (ros related?) files that start with px4... In that
-			# case just leave the quotes alone ("px4muorb.h" and several <px4/...>).
-			if [[ $1 =~ ^px4 ]]; then return 0; fi
-			# While if the include path starts with uORB/topics or topics, and it isn't found,
-			# then likely we just don't have a build directory. These should be local though.
-			# Same for the generated files mixer_multirotor.generated.h and build_git_version.h.
-			if [[ $1 =~ ((/|^)topics/|mixer_multirotor\.generated\.h|build_git_version\.h) ]]; then return 1; fi
-		fi
-		return 2;
-	fi	# Submodule or system header.
-	if [ $issubmodule -eq 0 ]; then return 1; fi	# Local.
-	# Files that are both local and submodule are simply left alone.
-	# These are (at this moment): "battery.h" "common.h" "Matrix.hpp" "mavlink.h" "protocol.h" "pwm.h" "spi.h" "Vector.hpp".
-	return 0;
-}
-
-# Run the include_path function for each of the files in $TMPDIR/fix_headers_include_paths
-echo -n "Determining which headers need to be included with double quotes... "
-echo -n > $TMPDIR/fix_headers_quotes
-for arg in $(cat "$TMPDIR/fix_headers_include_paths"); do
-	include_path $arg
-	localsystem=$?
-	if [ $localsystem -eq 1 ]; then
-		echo "$arg \"$arg\"" >> $TMPDIR/fix_headers_quotes
-	elif [ $localsystem -eq 2 ]; then
-		echo "$arg <$arg>" >> $TMPDIR/fix_headers_quotes
-	fi
-done
-echo "done"
-
-# Truncate the error log.
-echo -n > $TMPDIR/fix_headers_ERROR.log
-
-function print_error
-{
-	echo
-	echo -n "  ";
-	echo "*** $1" | tee -a "$TMPDIR/fix_headers_ERROR.log"
-	return 1
-}
-
-if [ $debug -ne 0 ]; then
-	# Debug Line.
-	DL='if (cdbl != NR) { printf "\n%u. \"%s\"", NR, $0; cdbl = NR }'
-	# Debug Begin.
-	DB='if (cdbl != NR) { printf "\n%u. \"%s\" ---> ", NR, $0; cdbl = NR } else printf "; "; printf'
-	# Debug End.
-	DE=''
-else
-	DL='#'
-	DB='#'
-	DE=''
-fi
-# Error Prefix.
-EP='###'
-
-# The main function that is called for each source file.
-function fixup_header
-{
-	count=$((count + 1))
-	echo -n "[$((100 * count / number_of_files))%] Fixing headers of $1... "
-
-	# Is this a header?
-	echo "$1" | sed -e 's/\.in$/;in/;s/.*\././;s/;in$/.in/' | grep -v -E "$HEADER_RE\$" >/dev/null; is_header=$?
-	if [ $debug -ne 0 ]; then echo "is_header = \"$is_header\""; fi
-
-	# Is this C++ source?
-	echo "$1" | sed -e 's/.*\././' | grep -v -E $CXXSRC_RE >/dev/null; is_cxxsrc=$?
-	if [ $debug -ne 0 ]; then echo "is_cxxsrc = \"$is_cxxsrc\""; fi
-	dont_make_cxxsrc=1
-	if [ $is_cxxsrc -eq 0 -a $is_header -ne 0 ]; then
-		grep -m 1 -q -E "^[[:space:]]*(#[[:space:]]*include[[:space:]]*<$STDCXX_HEADERS_RE>|(template|namespace|class)(\$|[^[:alnum:]_]))" "$1"
-		dont_make_cxxsrc=$?
-	fi
-	if [ $dont_make_cxxsrc -eq 0 ]; then
-		is_cxxsrc=1
-	fi
-
-	# Current directory.
-	curdir=$(dirname "$BASEDIR/$1")
-
-	# Parse the file.
-	#
-	# Returns an array of line[]'s. The first line is either the first #include, when it is outside
-	# any #if*...#endif constructs not counting the header guard if the file is a header. For example:
-	#
-	#	  // Anything here except #include lines.
-	#	  #include <first_include.h>	// <-- first line.
-	#
-	# Or, the first #if* that contains the first #include line. For example:
-	#	  // Anything here except #include lines.
-	#	  #ifndef FOO_H // header guard.
-	#	  #define FOO_H
-	#	  int global = 1; // Anything unknown.
-	#	  #ifdef SOMETHING		// <-- first line.
-	#	  #if maybe_more
-	#	    // anything except #include lines.
-	#	  #else
-	#	    // anything except #include lines.
-	#	    #include <first_include.h>
-	#
-	# Subsequent line[]'s mark the beginning of a new block, where we have the following blocks:
-	# type[]		Description
-	type_include=0		# An #include, outside #if*...#endif constructs except a possible header guard.
-	type_ifincludeendif=1	# #if*...#endif constructs with #include's.
-	type_ifendif=2		# #if*...#endif constructs without #include's.
-	type_decls=3		# __BEGIN_DECLS ... __END_DECLS block.
-	type_macro=4		# Contiguous #define block.
-	type_comment=5		#(Multi-line) comments.
-	type_emptyline=6	# Empty lines.
-	type_pragmaonce=7	# #pragma once (must be outside any #if*...#endif constructs).
-	type_end=8		# The first line of the remainder of the file.
-	#
-	# However, any block NOT containing one or more #include's (all types > 1) will
-	# cause subsequent blocks that do not contain #include's to be ignored,
-	# with as result that those blocks will be treated as contiguous code blocks.
-	# A comment that is followed by a type that is not to be ignored as such
-	# is given the type that follows (which itself is then ignored).
-	# Empty lines are ignored unless they appear directly in front of a type
-	# that is not to be ignored according to the above rules, where 'previous
-	# types' then are the types before the empty line.
-	#
-	# For example:
-	#
-	#   >	#include <first_header.h>	# type_include
-	#   >	#include <second_header.h>	# type_include
-	#   >					# type_emptyline
-	#   >	#include <third_header.h>	# type_include
-	#   ^	#ifdef FOO			# type_ifendif
-	#   |	#define BAR 1
-	#   |	#endif				# (detected here)
-	#   |								<-- ignored because:
-	#   v	#define BAZ(x) x // Do baz				<-- ignored (prev and this type in {type_ifendif, type_macro})
-	#   >					# type_emptyline
-	#   ^	// This include is important:	# type_comment, but then replaced by type_ifincludeendif
-	#   |								<-- ignored because:
-	#   |	// more here.						<-- ignored same type
-	#   |	#ifdef BAR						<-- "ignored": type_ifincludeendif, but put 3 lines higher.
-	#   |	#include <bar.h>
-	#   v	#endif				# (detected here)
-	#   >					# type_emptyline
-	#   >	#include <another.h>		# type_include
-	#
-	# This script stops parsing at the first not recognized line outside #if*...#endif constructs
-	# unless no first line was found yet. It does not attempt overly hard to decode rare constructs,
-	# most notably anything with a leading C comment is not recognized and will thus lead to an abort.
-	# For example the following lines are not recognized:
-	#
-	# /* Some comment */ #include <header.h>
-	# /* Some comment */ // Another comment.
-	#
-	# Lines that have a trailing comment are recognized (by ignoring the comment).
-
-	result=$(awk "\
-		function add(l, t) {
-			# First add always succeeds.
-			if (ptr > 0) {
-				# An empty line is always added, at first, unless the immediate preceding type is an empty line.
-				if (t == $type_emptyline && type[ptr - 1] == $type_emptyline) {
-					return;				$DB \"ignored because line %d is also empty.\", line[ptr - 1] $DE
-				}
-				# A comment is always, added at first, unless the preceding non-empty line type is a comment.
-				# Same for #include's.
-				if (t == $type_comment && last_none_type == t) {
-									$DB \"ignoring because same type as last_none_type (%s)\", type_name[t] $DE
-					# Gobble up preceding empty lines.
-					if (type[ptr - 1] == $type_emptyline) {
-						--ptr;			$DB \"ptr = %d; Removed type_emptyline @ line %d\", ptr, line[ptr] $DE
-					}
-					return;
-
-				}
-				# {ifendif, macro}'s are collapsed too.
-				if ((t == $type_ifendif || t == $type_macro) && (last_nonws_type == $type_ifendif || last_nonws_type == $type_macro)) {
-					# Gobble up preceding comments and empty lines.
-					while (ptr > 0 && (type[ptr - 1] == $type_emptyline || type[ptr - 1] == $type_comment)) {
-						--ptr;			$DB \"ptr = %d; Removed %s @ line %d\", ptr, type_name[type[ptr]], line[ptr] $DE
-					}
-					# ptr > 0 here because the first add is never for an empty line or comment.
-					last_none_type = type[ptr - 1];	$DB \"last_none_type = %s\", type_name[last_none_type] $DE
-					return;
-				}
-				# type_include and type_pragmaonce and higher are always added.
-			}
-			if (t == $type_end) {
-				# Remove drag.
-				while(ptr > 0 && line[ptr - 1] >= l) --ptr;
-			}
-			# If this type is not an empty line and it was preceded by a comment, then melt it together with that comment.
-			else if (t != $type_emptyline && last_none_type == $type_comment) {
-				# In this case t cannot be type_comment.
-				# Gobble up preceding empty lines.
-				if (type[ptr - 1] == $type_emptyline) {
-					--ptr;				$DB \"ptr = %d; Removed type_emptyline @ line %d\", ptr, line[ptr] $DE
-				}
-				# And replace the comment type.
-				--ptr;					$DB \"ptr = %d; replacing the %s @ line %d\", ptr, type_name[type[ptr]], line[ptr] $DE
-				l = line[ptr];
-
-			}
-			line[ptr] = l;					$DB \"ptr = %d; %s @ line %d\", ptr, type_name[t], l $DE;
-			type[ptr++] = t;
-			if (t != $type_emptyline) {
-				last_none_type = t;			$DB \"last_none_type = %s\", type_name[last_none_type] $DE
-				if (t != $type_comment)
-					last_nonws_type = t;
-			}
-		}
-
-		BEGIN {
-			debug = $debug;		# 0: no debug output; non-zero: print debug output.
-			header = $is_header;	# 0: do not look for a header guard; 1: treat first #ifndef as header guard.
-			in_if = 0;		# The number of nested levels inside #if, #ifdef or #ifndef ... #endif constructs.
-			in_if_base = 0;		# 0: no header guard was found (or #pragma once); 1: an #ifndef header guard was found.
-			in_decl = 0;		# 0: not inside a __BEGIN_DECLS ... __END_DECLS block; 1: inside such a block.
-			found_guard = 0;	# 0: no header guard was found; 1: a header guard was found (including #pragma once).
-			base_if = 0;		# The current base-level #if that we are scanning.
-			drag = 0;		# The number of lines since the last certainly relevant line (a base-level #include or a base-level #endif containing one of more #includes).
-			skipped = 0;		# 0: No #include was encountered in the current (base-level) #if block; 1: one or more #include's encountered in the current base-level #if block.
-			in_comment = 0;		# 0: not in a multi-line C comment; 1: in a multi-line C comment.
-			cdbl = 0;		# Current debug line.
-			error = 0;		# 0: no error occured; 1: an error occured.
-			ptr = 0;		# Current pointer into line[] and type[].
-			found_comment_end = 0;	# The last line (possibly the current line) that is/was a multi-line C comment termination.
-			last_none_type = -1;	# The last non-emptyline type that was added.
-			last_nonws_type = -1;	# The last non-whitespace type that was added.
-			# For debug purposes:
-			type_name[$type_ifendif] = \"type_ifendif\";
-			type_name[$type_ifincludeendif] = \"type_ifincludeendif\";
-			type_name[$type_decls] = \"type_decls\";
-			type_name[$type_comment] = \"type_comment\";
-			type_name[$type_emptyline] = \"type_emptyline\";
-			type_name[$type_include] = \"type_include\";
-			type_name[$type_macro] = \"type_macro\";
-			type_name[$type_pragmaonce] = \"type_pragmaonce\";
-			type_name[$type_end] = \"type_end\";
-		}
-
-		END {
-			last_line = NR - drag;
-			add(last_line + 1, $type_end);			$DB \"\n\" $DE;
-			# Print output.
-			if (error || ptr == 0 || last_line < line[0]) {
-				print \"error=1\";
-				exit
-			}
-			printf \"lines=\\\"\";
-			for (i = 0; i < ptr - 1; ++i)
-				printf \"%d \", line[i];
-			printf \"%d\\\"; \", line[ptr - 1];
-			printf \"types=\\\"\";
-			for (i = 0; i < ptr - 1; ++i)
-				printf \"%d \", type[i];
-			printf \"%d\\\"; \", type[ptr - 1];
-			print \"error=0; first_line=\" line[0] \"; last_line=\" last_line
-		}
-
-	#======================================================================================================
-	# Handle multi-line C comments.
-
-		/$COMMENT_END_RE/ {
-			if (in_comment) {
-				in_comment = 0;				$DB \"comment end\" $DE;
-				found_comment_end = NR;
-				sub(/^([^*]|\*+[^*\/])*\*+\//, \"\")	# Remove the tail of the comment.
-			}
-			# FALL-THROUGH
-		}
-
-		{
-			if (in_comment) {
-				++drag;					$DB \"in comment; drag = %d\", drag $DE;
-				next
-			}
-			found_comment_begin = 0;
-			# FALL-THROUGH
-		}
-
-		/$COMMENT_BEGIN_RE/ {
-			in_comment = 1;					$DB \"comment begin\" $DE;
-			found_comment_begin = 1;
-			sub(/\/\*([^*]|\*+($|[^*\/]))*$/, \"\")		# Remove the head of the comment so that we'll recognize this as an empty line if it is.
-			# FALL-THROUGH
-		}
-
-	#======================================================================================================
-	# Detect and handle header guard.
-
-		/$PRAGMA_ONCE_RE/ {
-			++drag;						$DB \"drag = %d\", drag $DE;
-			if (header && found_guard == 0 && in_if == 0) {
-				found_guard = NR;			$DB \"found_guard = %d\", found_guard $DE;
-				if (ptr > 0)
-					add(NR, $type_pragmaonce)
-				next
-			}
-			print \"\\n$EP $1:\" NR \": unexpected #pragma once\";
-			error = 1;
-			exit
-		}
-
-		/^#ifndef / {
-			if (ptr == 0 && header && found_guard == 0 && in_if == 0) {
-				found_guard = NR;			$DB \"found_guard = %d\", found_guard $DE;
-				in_if = 1;				$DB \"in_if = %d\", in_if $DE;
-				in_if_base = 1;				$DB \"in_if_base = %d\", in_if_base $DE;
-				next
-			}
-			# FALL-THROUGH
-		}
-
-	#======================================================================================================
-	# Detect and handle __BEGIN_DECLS ... __END_DECLS blocks.
-
-		/^[[:space:]]*__BEGIN_DECLS/ {
-			++drag;						$DB \"drag = %d\", drag $DE;
-			if (in_decl == 0) {
-				in_decl = 1;				$DB \"in_decl = 1\" $DE
-				add(NR, $type_decls);
-				next
-			}
-			print \"\\n$EP $1:\" NR \": Nested __BEGIN_DECLS!\";
-			error = 1;
-			exit
-		}
-
-		/^[[:space:]]*__END_DECLS/ {
-			++drag;						$DB \"drag = %d\", drag $DE;
-			if (in_decl == 1) {
-				in_decl = 0;				$DB \"in_decl = 0\" $DE
-				if (skipped) {
-					drag = 0;
-				} else if (ptr == 1) {
-					ptr = 0;			$DB \"erase DECLS block\" $DE
-					last_none_type = -1;
-				}
-				next
-			}
-			print \"\\n$EP $1:\" NR \": __END_DECLS without matching __BEGIN_DECLS!\";
-			error = 1;
-			exit
-		}
-
-	#======================================================================================================
-	# Detect and handle #if ... #endif blocks.
-
-		/$IF_RE/ {
-			++drag;						$DB \"drag = %d\", drag $DE;
-			if (in_if == in_if_base && in_decl == 0) {
-				skipped = 0;				$DB \"skipped = 0\" $DE;
-				base_if = NR;				$DB \"base_if = %d\", NR $DE;
-			}
-			++in_if;					$DB \"in_if = %d\", in_if $DE;
-			next
-		}
-
-		/$ENDIF_RE/ {
-			--in_if;					$DB \"in_if = %d\", in_if $DE;
-			if (in_if < 0) {
-				print \"\\n$EP $1:\" NR \": #endif without matching #if!\";
-				error = 1;
-				exit
-			}
-			++drag;
-			if (in_if == in_if_base && in_decl == 0) {
-				if (skipped) {
-					drag = 0;
-					add(base_if, $type_ifincludeendif);
-				} else if (ptr > 0)
-					add(base_if, $type_ifendif);
-			}						$DB \"drag = %d\", drag $DE;
-			# Left header guard?
-			if (in_if < in_if_base) {			$DB \"left header guard:\" $DE;
-				# assert(in_if == 0 && in_if_base == 1 && header && found_guard)
-				exit
-			}
-			next
-		}
-
-	#======================================================================================================
-	# Handle #include lines.
-
-		/$INCLUDE_RE/ {
-			if (!/\"(\.\/)?mavlink_msg/) {
-				# If we're inside a __BEGIN_DECLS ... __END_DECLS block then only certain headers may be included.
-				hname = gensub(/^[[:space:]]*#[[:space:]]*include[[:space:]]*[<\"]([^>\"]*)[>\"].*/, \"\\\1\", \"1\");
-				cpp_safe = !(hname ~ /$UNSAFE_HEADERS_RE/);
-										$DB \"hname = \\\"\" hname \"\\\"; cpp_safe = \" cpp_safe \"; in_decl = \" in_decl \"; is_cxxsrc = $is_cxxsrc\" $DE
-				if (in_decl && cpp_safe) {
-					print \"\\n$EP $1:\" NR \": including \" hname \" inside a __BEGIN_DECLS ... __END_DECLS block.\";
-					error = 1;
-					exit
-				} else if (!in_decl && !cpp_safe && $is_cxxsrc) {
-					print \"\\n$EP $1:\" NR \": including \" hname \" outside a __BEGIN_DECLS ... __END_DECLS block!\";
-					error = 1;
-					exit
-				}
-				if (in_if > in_if_base || in_decl) {
-					skipped = 1;				$DB \"skipped = 1\" $DE;
-				} else {
-					drag = 0;				$DB \"drag = 0\" $DE;
-					add(NR, $type_include);			$DB \"first_line = %d\", NR $DE;
-				}
-				next
-			}
-		}
-
-	#======================================================================================================
-	# Ignore #define's, empty lines and lines with just comments.
-
-		/$DEFINE_RE/ {
-			++drag;						$DB \"drag = %d\", drag $DE;
-			if (ptr > 0 && in_if == in_if_base && in_decl == 0) {
-				add(NR, $type_macro);
-			}
-			next
-		}
-
-		/$EMPTY_LINE_RE/ {
-			++drag;						$DB \"drag = %d\", drag $DE;
-			if (ptr > 0 && in_if == in_if_base && in_decl == 0) {
-				if (found_comment_begin)
-					add(NR, $type_comment);
-				else if (found_comment_end != NR)
-					add(NR, $type_emptyline);
-			}
-			next
-		}
-
-		/$COMMENT_LINE_RE/ {
-			++drag;						$DB \"drag = %d\", drag $DE;
-			if (ptr > 0 && in_if == in_if_base && in_decl == 0 && type[ptr - 1] != $type_comment) {
-				add(NR, $type_comment);
-			}
-			next
-		}
-
-	#======================================================================================================
-	# Handle everything else (unrecognized lines).
-
-		{
-			++drag;						$DB \"unknown; drag = %d\", drag $DE;
-			if (ptr > 0 && in_if <= in_if_base && in_decl == 0) {
-				exit
-			}
-		}
-
-		" "$BASEDIR/$1")
-
-	# Decode the result.
-	vars=$(echo "$result" | tail -n 1)
-	error_msg=$(echo "$result" | grep "^$EP " | sed -e 's/^....//')
-	if [ $debug -ne 0 ]; then
-		len=$(echo "$result" | wc --lines)
-		echo "$result" | head -n $((len - 1)) | grep -v "^$EP "	# Debug messages
-		echo "vars: $vars"
-	fi
-	# Evaluate the last line printed in END.
-	error=1; eval $vars
-	test -z "$error_msg" || print_error "$error_msg" || return
-	test $error -eq 0 -a $first_line -gt 0 || print_error "FAILED to find an #include in $1?!" || return
-	test $last_line -ge $first_line || print_error "FAILED to find a sensible last line in $1?!" || return
-
-	# Calculate the number of lines starting from the current line.
-	# Use sed to count lines, because wc --lines doesn't report the last line when that doesn't end on a new-line, contrary to the fact that tail treats such lines as lines.
-	total_lines=$(sed -n '$=' "$BASEDIR/$1")
-	if [ $debug -ne 0 ]; then echo "total_lines = \"$total_lines\""; fi
-
-	# Edit the first_line...last_line block.
-	# Header files are ordered as follows (lowest value first):
-	cat_winsock=0;		# Winsock2.h
-	cat_posix_sys=1;	# posix_sys.h or one of the px4_ headers that include it.
-	cat_px4=2;		# Other px4_*.h
-	cat_local=3;		# "*.h"
-	cat_cxx=4;		# <std c++ header>, ie <iostream>
-	cat_c=5;		# <c std c++ header>, ie <cstdio>
-	cat_system=6;		# <*.h>
-
-	head -n $last_line "$BASEDIR/$1" | tail -n $((last_line - first_line + 1))  | awk "
-
-		function sort_by_type_line_header_type_hname(blk2, v2, blk1, v1) {
-			# Return true if blk1 comes before blk2.
-			# Move type_include before the rest. Keep the same line order for the rest.
-			return (type[blk2] != $type_include && (type[blk1] == $type_include || line[blk1] < line[blk2])) ||
-			       (type[blk2] == $type_include && type[blk1] == $type_include &&
-				   # If both are include's then put include with a lower header_type first; sort alphabetically for the same header type.
-				   (header_type[blk1] < header_type[blk2] || (header_type[blk1] == header_type[blk2] && hname[blk1] < hname[blk2])));
-		}
-
-		BEGIN {
-			first_line = $first_line;
-			split(\"$lines\", line);
-			split(\"$types\", type);
-			i = 0;
-			do {
-				line[++i] -= first_line - 1;
-			} while(type[i] != $type_end)
-			for(b = 0; b < i; ++b) header_type[b] = 100;
-			blk = 1;
-			n = 0;
-			is_cxxsrc = $is_cxxsrc;
-			# px4_posix.h includes px4_defines.h includes px4_log.h includes posix_sys.h which must be the first header included.
-			sys_val[\"px4_posix.h\"] = 1;
-			sys_val[\"px4_defines.h\"] = 2;
-			sys_val[\"px4_log.h\"] = 3;
-			sys_val[\"posix_sys.h\"] = 4;
-			saw_sys_val = 5;		# Didn't see any of the above; otherwise the lowest value of the header seen.
-			for(b = 0; b < i; ++b) saw_sys[b] = saw_sys_val;
-		}
-
-		END {
-			l = asorti(txt, k, \"sort_by_type_line_header_type_hname\");
-			for (b = 1; b <= l; ++b) {
-				if (type[k[b]] == $type_include && header_type[k[b]] == $cat_posix_sys && saw_sys[k[b]] > saw_sys_val) continue;
-				len = length(txt[k[b]]);
-				for (n = 0; n < len; ++n) print txt[k[b]][n];
-				if (b < l && type[k[b]] == $type_include && type[k[b+1]] != $type_emptyline &&
-				    (type[k[b+1]] != $type_include || (header_type[k[b]] != header_type[k[b+1]] && header_type[k[b+1]] != $cat_px4))) {
-					printf \"\n\";
-				}
-			}
-		}
-
-		{
-			if (NR == line[blk + 1]) {
-				++blk;
-				n = 0;
-			}
-		}
-
-		/$INCLUDE_RE/ {
-			# Don't use double quotes around standard header names.
-			\$0 = gensub(/^([[:space:]]*#[[:space:]]*include[[:space:]]*)\\\"$STDC_HEADERS_RE\\.h\\\"/, \"\\\1<\\\2.h>\", \"1\");
-			if (is_cxxsrc) {
-				\$0 = gensub(/^([[:space:]]*#[[:space:]]*include[[:space:]]*)\\\"$STDCXX_HEADERS_RE\\\"/, \"\\\1<\\\2>\", \"1\");
-				# Change deprecated C header names to standard C++ header names in C++ source files.
-				\$0 = gensub(/^([[:space:]]*#[[:space:]]*include[[:space:]]*<)$STDC_HEADERS_RE\\.h>/, \"\\\1c\\\2>\", \"1\");
-			}
-			# Don't include \"./foo.h\", that is implied, so just include \"foo.h\".
-			\$0 = gensub(/^([[:space:]]*#[[:space:]]*include[[:space:]]*\\\")\.\//, \"\\\1\", \"1\");
-			# Extract the header filename.
-			hname[blk] = gensub(/^[[:space:]]*#[[:space:]]*include[[:space:]]*[<\"]([^>\"]*)[>\"].*/, \"\\\1\", \"1\");
-			# If the header exists in the directory of the including file, then it is a local header.
-			command = sprintf(\"test -e %s/%s\", \"$curdir\", hname[blk]);
-			if (system(command) == 0) {
-				\$0 = gensub(/^([[:space:]]*#[[:space:]]*include[[:space:]]*)[\"<]([^\">]*)[\">]/, \"\\\1\\\"\\\2\\\"\", \"1\");
-			} else {
-				# Do we know if this is a local file, or a submodule / system header?
-				# The grep reg.exp needs \\ (for backslah) and \1 for back reference, thus: \\\1.
-				# However we print the grep command using sprintf, so each backslash needs to be escaped once more: \\\\\\1.
-				# Finally, this is a bash string and we need to escape each backslash once more to pass it corrently to awk, hence we need twelve backslahes:
-				command = sprintf(\"grep '^%s ' '%s' 2>/dev/null\", gensub(/([.+])/, \"\\\\\\\\\\\\1\", \"g\", hname[blk]), \"$TMPDIR/fix_headers_quotes\");
-				ret = command | getline result;
-				if (ret != 0) {
-					result = substr(result, index(result, \" \") + 1);
-					\$0 = gensub(/^([[:space:]]*#[[:space:]]*include[[:space:]]*)[\"<][^\">]*[\">]/, \"\\\1\" result, \"1\");
-				}
-			}
-			# Categorise the header.
-			if (hname[blk] == \"Winsock2.h\") {
-				if (header_type[blk] > $cat_winsock)
-					header_type[blk] = $cat_winsock;
-			}
-			else if (hname[blk] in sys_val) {
-				if (header_type[blk] > $cat_posix_sys)
-					header_type[blk] = $cat_posix_sys;
-				# posix_sys.h is sometimes included within #ifdef __PX4_POSIX ... #endif. The other headers should not be conditional.
-				if ((hname[blk] == \"posix_sys.h\" || type[blk] == $type_include)) {
-					type[blk] = $type_include;		# Treat #ifdef __PX4_POSIX #include \"posix_sys.h\" #endif as an include for sorting purposes.
-					saw_sys[blk] = sys_val[hname[blk]];	# There will be only one include (header name) for this block.
-					if (sys_val[hname[blk]] < saw_sys_val)
-						saw_sys_val = sys_val[hname[blk]];
-				}
-				# Use double quotes for these headers.
-				\$0 = gensub(/<([[:alnum:]_\/.]*)>/, \"\\\"\\\1\\\"\", \"1\");
-			}
-			else if (hname[blk] ~ /^(platforms\/px4_|px4_)/) {
-				if (header_type[blk] > $cat_px4)
-					header_type[blk] = $cat_px4;
-				# Use double quotes for these headers.
-				\$0 = gensub(/<([[:alnum:]_\/.]*)>/, \"\\\"\\\1\\\"\", \"1\");
-			}
-			else if (\$0 ~ /^[[:space:]]*#[[:space:]]*include[[:space:]]*\"/) {
-				if (header_type[blk] > $cat_local)
-					header_type[blk] = $cat_local;
-			}
-			else if (hname[blk] ~ /^$STDCXX_HEADERS_RE\$/) {
-				if (header_type[blk] > $cat_cxx)
-					header_type[blk] = $cat_cxx;
-			}
-			else if (hname[blk] ~ /^c$STDC_HEADERS_RE\$/) {
-				if (header_type[blk] > $cat_c)
-					header_type[blk] = $cat_c;
-			}
-			else if (hname[blk] ~ /^$STDC_HEADERS_RE\.h\$/) {
-				if (header_type[blk] > $cat_system)
-					header_type[blk] = $cat_system;
-			}
-		}
-
-		{
-			# Remove empty lines before #include's.
-			if (type[blk] == $type_include) {
-				for (i = 1; blk > i && type[blk - i] == $type_emptyline; ++i)
-					delete txt[blk - i]
-			}
-			txt[blk][n++] = \$0;
-		}
-
-		" > $TMPDIR/fix_headers_current_block
-
-	# Construct a new file in TMPDIR.
-	head -n $((first_line - 1)) "$BASEDIR/$1" > $TMPDIR/fix_headers_current_file
-
-	# Append the editted block.
-	cat $TMPDIR/fix_headers_current_block >> $TMPDIR/fix_headers_current_file
-
-	# Append the rest.
-	tail -n $((total_lines - last_line)) "$BASEDIR/$1" >> $TMPDIR/fix_headers_current_file
-
-	# Compare original with result.
-	if cmp --quiet "$BASEDIR/$1" $TMPDIR/fix_headers_current_file; then
-		echo "No change"
-	else
-		echo "Fixed lines $first_line-$last_line"
-		mv $TMPDIR/fix_headers_current_file "$BASEDIR/$1" || fatal "Failed to move $TMPDIR/fix_headers_current_file to $BASEDIR/$1 !?!"
-	fi
-}
-
-if [ $non_option_arguments -ne 0 ]; then
-	fixup_header $1
-	exit
-fi
-
-# Run the fixup function for each of the files in $TMPDIR/fix_headers_sources_with_includes.
-# Strip BASEDIR because we don't know how long that is and it might too much for bash.
-for arg in $(cat "$TMPDIR/fix_headers_sources_with_includes" | cut -c $striplen-); do
-		fixup_header $arg
-done
-
-# Clean up.
-if [ $debug -eq 0 -o $# -eq 0 ]; then
-	rm "$TMPDIR/fix_headers_sources" "$TMPDIR/fix_headers_sources_with_includes" "$TMPDIR/fix_headers_SUBMODULE_HEADERS" "$TMPDIR/fix_headers_HEADERS" \
-	   "$TMPDIR/fix_headers_include_paths" "$TMPDIR/fix_headers_quotes" "$TMPDIR/fix_headers_current_block"
-fi
-
-# Print all error messages again at the end.
-if [ -s "$TMPDIR/fix_headers_ERROR.log" ]; then
-	echo "$0 finished with errors:"
-	cat "$TMPDIR/fix_headers_ERROR.log"
-else
-	echo "SUCCESS"
-	rm "$TMPDIR/fix_headers_ERROR.log"
-fi

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/px4events/jsonout.py

@@ -25,6 +25,8 @@ class JsonOutput():
                 event_obj = {}
                 event_obj['name'] = e.name
                 event_obj['message'] = e.message
+                if e.type is not None:
+                    event_obj['type'] = e.type
                 if e.description is not None:
                     event_obj['description'] = e.description
                 args = []

Tools/px4events/srcparser.py

@@ -23,6 +23,7 @@ class Event(object):
         self.message = None
         self.description = None
         self.group = "default"
+        self.type = None
         self._arguments = []
 
     @staticmethod
@@ -125,11 +126,13 @@ class SourceParser(object):
                         descr = descr[:i] + ' ' + descr[i+1:]
                 event.description = descr
             elif tag == "group":
-                known_groups = ["calibration", "health", "arming_check", "normal"]
+                known_groups = ["calibration", "health", "arming_check", "default"]
                 event.group = value.strip()
                 if not event.group in known_groups:
                     raise Exception("Unknown event group: '{}'\nKnown groups: {}\n" \
                         "If this is not a typo, add the new group to the script".format(event.group, known_groups))
+            elif tag == "type":
+                event.type = value.strip()
             elif tag.startswith("arg"):
                 arg_index = int(tag[3:])-1
                 arg_name = value.strip()
@@ -237,7 +240,7 @@ class SourceParser(object):
                             event.group = "health"
                         else:
                             event.group = "arming_check"
-                        event.prepend_arguments([('common::navigation_mode_category_t', 'modes'),
+                        event.prepend_arguments([('navigation_mode_group_t', 'modes'),
                                 ('uint8_t', 'health_component_index')])
                     else:
                         raise Exception("unknown event method call: {}, args: {}".format(call, args))

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_ignition.bash

@@ -1,23 +0,0 @@
-#!/bin/bash
-#
-# Setup environment to make PX4 visible to Gazebo.
-#
-# Note, this is not necessary if using a ROS catkin workspace with the px4
-# package as the paths are exported.
-#
-# License: according to LICENSE.md in the root directory of the PX4 Firmware repository
-
-if [ "$#" != 2 ]; then
-    echo -e "usage: source setup_gazebo.bash src_dir build_dir\n"
-    return 1
-fi
-
-SRC_DIR=$1
-BUILD_DIR=$2
-
-# setup Gazebo env and update package path
-export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:${SRC_DIR}/build/px4_sitl_default/build_ign_gazebo
-export IGN_GAZEBO_SYSTEM_PLUGIN_PATH=$IGN_GAZEBO_SYSTEM_PLUGIN_PATH:${SRC_DIR}/build/px4_sitl_default/build_ign_gazebo
-export IGN_GAZEBO_RESOURCE_PATH=$IGN_GAZEBO_RESOURCE_PATH:${SRC_DIR}/Tools/simulation-ignition/models
-
-echo -e "LD_LIBRARY_PATH $LD_LIBRARY_PATH"

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 &
-	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
+	$build_path/bin/px4 -i $N -d "$build_path/etc" >out.log 2>err.log &
+	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

Tools/simulation/ignition/models/x4/meshes/led.dae

@@ -0,0 +1,529 @@
+<?xml version="1.0" encoding="utf-8"?>
+<COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1">
+  <asset><contributor><author></author><authoring_tool>FBX COLLADA exporter</authoring_tool><comments></comments></contributor><created>2018-08-21T21:36:07Z</created><keywords></keywords><modified>2018-08-21T21:36:07Z</modified><revision></revision><subject></subject><title></title><unit meter="1.000000" name="centimeter"></unit><up_axis>Z_UP</up_axis></asset>
+  <library_images>
+    <image id="LEDIndicator_ncl1_2-image" name="LEDIndicator_ncl1_2"><init_from>../materials/textures/led.jpg</init_from></image>
+  </library_images>
+  <library_materials>
+    <material id="LEDIndicator_ncl1_1" name="LEDIndicator_ncl1_1">
+      <instance_effect url="#LEDIndicator_ncl1_1-fx"/>
+    </material>
+  </library_materials>
+  <library_effects>
+    <effect id="LEDIndicator_ncl1_1-fx" name="LEDIndicator_ncl1_1">
+      <profile_COMMON>
+        <technique sid="standard">
+          <phong>
+            <emission>
+              <color sid="emission">0.000000  0.000000 0.000000 1.000000</color>
+            </emission>
+            <ambient>
+              <color sid="ambient">0.588235  0.588235 0.588235 1.000000</color>
+            </ambient>
+            <diffuse>
+              <texture texture="LEDIndicator_ncl1_2-image" texcoord="CHANNEL0">
+                <extra>
+                  <technique profile="MAYA">
+                    <wrapU sid="wrapU0">TRUE</wrapU>
+                    <wrapV sid="wrapV0">TRUE</wrapV>
+                    <blend_mode>ADD</blend_mode>
+                  </technique>
+                </extra>
+              </texture>
+            </diffuse>
+            <specular>
+              <color sid="specular">0.000000  0.000000 0.000000 1.000000</color>
+            </specular>
+            <shininess>
+              <float sid="shininess">2.000000</float>
+            </shininess>
+            <reflective>
+              <color sid="reflective">0.000000  0.000000 0.000000 1.000000</color>
+            </reflective>
+            <reflectivity>
+              <float sid="reflectivity">1.000000</float>
+            </reflectivity>
+            <transparent opaque="RGB_ZERO">
+              <color sid="transparent">1.000000  1.000000 1.000000 1.000000</color>
+            </transparent>
+            <transparency>
+              <float sid="transparency">0.000000</float>
+            </transparency>
+          </phong>
+        </technique>
+      </profile_COMMON>
+    </effect>
+  </library_effects>
+  <library_geometries>
+    <geometry id="LEDIndicator-lib" name="LEDIndicatorMesh">
+      <mesh>
+        <source id="LEDIndicator-POSITION">
+          <float_array id="LEDIndicator-POSITION-array" count="147">
+-0.068518 0.000000 0.202589
+-0.059339 -0.034466 0.202589
+-0.118677 0.000000 0.152431
+-0.102777 -0.059696 0.152431
+-0.137036 0.000000 0.083912
+-0.118677 -0.068932 0.083912
+-0.179977 -0.104537 0.036913
+-0.179977 0.104537 0.036913
+-0.179977 -0.104537 0.000445
+-0.179977 0.104537 0.000445
+0.000000 0.068932 0.202589
+-0.034259 0.059697 0.202589
+-0.059338 0.034466 0.202589
+0.000000 0.119393 0.152431
+-0.059338 0.103397 0.152431
+-0.102777 0.059697 0.152431
+0.000000 0.137863 0.083912
+-0.068518 0.119393 0.083912
+-0.118677 0.068932 0.083912
+-0.034259 -0.059697 0.202589
+-0.059339 -0.103397 0.152431
+-0.000000 -0.119393 0.152431
+-0.068518 -0.119393 0.083912
+-0.000000 -0.068932 0.202589
+0.034259 -0.059697 0.202589
+0.059338 -0.034466 0.202589
+0.059338 -0.103397 0.152431
+-0.000000 -0.137863 0.083912
+0.068518 -0.119393 0.083912
+-0.000000 -0.209074 0.036913
+-0.000000 -0.209074 0.000445
+0.068518 -0.000000 0.202589
+0.102777 -0.059697 0.152431
+0.118677 -0.000000 0.152431
+0.118677 -0.068932 0.083912
+0.137036 -0.000000 0.083912
+0.179977 -0.104537 0.036913
+0.179977 -0.104537 0.000445
+0.000000 0.000000 0.220949
+0.059339 0.034466 0.202589
+0.034259 0.059697 0.202589
+0.102777 0.059696 0.152431
+0.059339 0.103397 0.152431
+0.118677 0.068932 0.083912
+0.068518 0.119393 0.083912
+0.000000 0.209074 0.036913
+0.179977 0.104537 0.036913
+0.000000 0.209074 0.000445
+0.179977 0.104537 0.000445
+</float_array>
+          <technique_common>
+            <accessor source="#LEDIndicator-POSITION-array" count="49" stride="3">
+              <param name="X" type="float"/>
+              <param name="Y" type="float"/>
+              <param name="Z" type="float"/>
+            </accessor>
+          </technique_common>
+        </source>
+        <source id="LEDIndicator-Normal0">
+          <float_array id="LEDIndicator-Normal0-array" count="846">
+0.000000 -0.000000 1.000000
+-0.459980 0.263947 0.847791
+-0.530731 0.000000 0.847540
+0.000000 -0.000000 1.000000
+-0.530731 0.000000 0.847540
+-0.459980 -0.263946 0.847791
+-0.875931 0.000000 0.482436
+-0.530731 0.000000 0.847540
+-0.459980 0.263947 0.847791
+-0.459980 0.263947 0.847791
+-0.759342 0.436180 0.482853
+-0.875931 0.000000 0.482436
+-0.759343 -0.436179 0.482853
+-0.459980 -0.263946 0.847791
+-0.530731 0.000000 0.847540
+-0.530731 0.000000 0.847540
+-0.875931 0.000000 0.482436
+-0.759343 -0.436179 0.482853
+-0.836841 0.000000 0.547447
+-0.875931 0.000000 0.482436
+-0.759342 0.436180 0.482853
+-0.759342 0.436180 0.482853
+-0.684937 0.393187 0.613404
+-0.836841 0.000000 0.547447
+-0.684938 -0.393186 0.613404
+-0.759343 -0.436179 0.482853
+-0.875931 0.000000 0.482436
+-0.875931 0.000000 0.482436
+-0.836841 0.000000 0.547447
+-0.684938 -0.393186 0.613404
+-0.836841 0.000000 0.547447
+-0.684937 0.393187 0.613404
+-0.797292 0.459082 0.391880
+-0.797292 -0.459081 0.391880
+-0.797292 0.459082 0.391880
+-0.866677 0.498870 0.000000
+-0.866677 0.498870 0.000000
+-0.866677 -0.498869 0.000000
+-0.797292 -0.459081 0.391880
+-0.797292 -0.459081 0.391880
+-0.684938 -0.393186 0.613404
+-0.836841 0.000000 0.547447
+-0.836841 0.000000 0.547447
+-0.797292 0.459082 0.391880
+-0.797292 -0.459081 0.391880
+0.000000 -0.000000 1.000000
+0.000000 0.529118 0.848548
+-0.265980 0.457875 0.848296
+0.000000 -0.000000 1.000000
+-0.265980 0.457875 0.848296
+-0.459980 0.263947 0.847791
+-0.439292 0.757014 0.483687
+-0.265980 0.457875 0.848296
+0.000000 0.529118 0.848548
+0.000000 0.529118 0.848548
+0.000001 0.875010 0.484104
+-0.439292 0.757014 0.483687
+-0.759342 0.436180 0.482853
+-0.459980 0.263947 0.847791
+-0.265980 0.457875 0.848296
+-0.265980 0.457875 0.848296
+-0.439292 0.757014 0.483687
+-0.759342 0.436180 0.482853
+-0.419462 0.722641 0.549401
+-0.439292 0.757014 0.483687
+0.000001 0.875010 0.484104
+0.000001 0.875010 0.484104
+0.000001 0.788497 0.615039
+-0.419462 0.722641 0.549401
+-0.684937 0.393187 0.613404
+-0.759342 0.436180 0.482853
+-0.439292 0.757014 0.483687
+-0.439292 0.757014 0.483687
+-0.419462 0.722641 0.549401
+-0.684937 0.393187 0.613404
+-0.419462 0.722641 0.549401
+0.000001 0.788497 0.615039
+0.000001 0.919830 0.392316
+-0.797292 0.459082 0.391880
+0.000001 0.919830 0.392316
+0.000001 1.000000 0.000000
+0.000001 1.000000 0.000000
+-0.866677 0.498870 0.000000
+-0.797292 0.459082 0.391880
+-0.797292 0.459082 0.391880
+-0.684937 0.393187 0.613404
+-0.419462 0.722641 0.549401
+-0.419462 0.722641 0.549401
+0.000001 0.919830 0.392316
+-0.797292 0.459082 0.391880
+0.000000 -0.000000 1.000000
+-0.459980 -0.263946 0.847791
+-0.265980 -0.457875 0.848296
+0.000000 -0.000000 1.000000
+-0.265980 -0.457875 0.848296
+-0.000000 -0.529118 0.848548
+-0.439293 -0.757013 0.483687
+-0.265980 -0.457875 0.848296
+-0.459980 -0.263946 0.847791
+-0.459980 -0.263946 0.847791
+-0.759343 -0.436179 0.482853
+-0.439293 -0.757013 0.483687
+-0.000000 -0.875010 0.484104
+-0.000000 -0.529118 0.848548
+-0.265980 -0.457875 0.848296
+-0.265980 -0.457875 0.848296
+-0.439293 -0.757013 0.483687
+-0.000000 -0.875010 0.484104
+-0.419463 -0.722640 0.549401
+-0.439293 -0.757013 0.483687
+-0.759343 -0.436179 0.482853
+-0.759343 -0.436179 0.482853
+-0.684938 -0.393186 0.613404
+-0.419463 -0.722640 0.549401
+-0.000000 -0.788497 0.615039
+-0.000000 -0.875010 0.484104
+-0.439293 -0.757013 0.483687
+-0.439293 -0.757013 0.483687
+-0.419463 -0.722640 0.549401
+-0.000000 -0.788497 0.615039
+-0.419463 -0.722640 0.549401
+-0.684938 -0.393186 0.613404
+-0.797292 -0.459081 0.391880
+-0.000001 -0.919830 0.392316
+-0.797292 -0.459081 0.391880
+-0.866677 -0.498869 0.000000
+-0.866677 -0.498869 0.000000
+-0.000001 -1.000000 0.000000
+-0.000001 -0.919830 0.392316
+-0.000001 -0.919830 0.392316
+-0.000000 -0.788497 0.615039
+-0.419463 -0.722640 0.549401
+-0.419463 -0.722640 0.549401
+-0.797292 -0.459081 0.391880
+-0.000001 -0.919830 0.392316
+0.000000 -0.000000 1.000000
+-0.000000 -0.529118 0.848548
+0.265980 -0.457875 0.848296
+0.000000 -0.000000 1.000000
+0.265980 -0.457875 0.848296
+0.459980 -0.263947 0.847791
+0.439292 -0.757014 0.483687
+0.265980 -0.457875 0.848296
+-0.000000 -0.529118 0.848548
+-0.000000 -0.529118 0.848548
+-0.000000 -0.875010 0.484104
+0.439292 -0.757014 0.483687
+0.759342 -0.436180 0.482853
+0.459980 -0.263947 0.847791
+0.265980 -0.457875 0.848296
+0.265980 -0.457875 0.848296
+0.439292 -0.757014 0.483687
+0.759342 -0.436180 0.482853
+0.419462 -0.722641 0.549401
+0.439292 -0.757014 0.483687
+-0.000000 -0.875010 0.484104
+-0.000000 -0.875010 0.484104
+-0.000000 -0.788497 0.615039
+0.419462 -0.722641 0.549401
+0.684937 -0.393187 0.613404
+0.759342 -0.436180 0.482853
+0.439292 -0.757014 0.483687
+0.439292 -0.757014 0.483687
+0.419462 -0.722641 0.549401
+0.684937 -0.393187 0.613404
+0.419462 -0.722641 0.549401
+-0.000000 -0.788497 0.615039
+-0.000001 -0.919830 0.392316
+0.797292 -0.459082 0.391880
+-0.000001 -0.919830 0.392316
+-0.000001 -1.000000 0.000000
+-0.000001 -1.000000 0.000000
+0.866677 -0.498871 0.000000
+0.797292 -0.459082 0.391880
+0.797292 -0.459082 0.391880
+0.684937 -0.393187 0.613404
+0.419462 -0.722641 0.549401
+0.419462 -0.722641 0.549401
+-0.000001 -0.919830 0.392316
+0.797292 -0.459082 0.391880
+0.000000 -0.000000 1.000000
+0.459980 -0.263947 0.847791
+0.530731 -0.000000 0.847540
+0.000000 -0.000000 1.000000
+0.530731 -0.000000 0.847540
+0.459981 0.263946 0.847791
+0.875931 -0.000001 0.482436
+0.530731 -0.000000 0.847540
+0.459980 -0.263947 0.847791
+0.459980 -0.263947 0.847791
+0.759342 -0.436180 0.482853
+0.875931 -0.000001 0.482436
+0.759343 0.436179 0.482853
+0.459981 0.263946 0.847791
+0.530731 -0.000000 0.847540
+0.530731 -0.000000 0.847540
+0.875931 -0.000001 0.482436
+0.759343 0.436179 0.482853
+0.836841 -0.000001 0.547447
+0.875931 -0.000001 0.482436
+0.759342 -0.436180 0.482853
+0.759342 -0.436180 0.482853
+0.684937 -0.393187 0.613404
+0.836841 -0.000001 0.547447
+0.684938 0.393186 0.613404
+0.759343 0.436179 0.482853
+0.875931 -0.000001 0.482436
+0.875931 -0.000001 0.482436
+0.836841 -0.000001 0.547447
+0.684938 0.393186 0.613404
+0.836841 -0.000001 0.547447
+0.684937 -0.393187 0.613404
+0.797292 -0.459082 0.391880
+0.797292 0.459081 0.391880
+0.797292 -0.459082 0.391880
+0.866677 -0.498871 0.000000
+0.866677 -0.498871 0.000000
+0.866677 0.498869 -0.000000
+0.797292 0.459081 0.391880
+0.797292 0.459081 0.391880
+0.684938 0.393186 0.613404
+0.836841 -0.000001 0.547447
+0.836841 -0.000001 0.547447
+0.797292 -0.459082 0.391880
+0.797292 0.459081 0.391880
+0.000000 -0.000000 1.000000
+0.459981 0.263946 0.847791
+0.265980 0.457875 0.848296
+0.000000 -0.000000 1.000000
+0.265980 0.457875 0.848296
+0.000000 0.529118 0.848548
+0.439293 0.757013 0.483687
+0.265980 0.457875 0.848296
+0.459981 0.263946 0.847791
+0.459981 0.263946 0.847791
+0.759343 0.436179 0.482853
+0.439293 0.757013 0.483687
+0.000001 0.875010 0.484104
+0.000000 0.529118 0.848548
+0.265980 0.457875 0.848296
+0.265980 0.457875 0.848296
+0.439293 0.757013 0.483687
+0.000001 0.875010 0.484104
+0.419463 0.722640 0.549401
+0.439293 0.757013 0.483687
+0.759343 0.436179 0.482853
+0.759343 0.436179 0.482853
+0.684938 0.393186 0.613404
+0.419463 0.722640 0.549401
+0.000001 0.788497 0.615039
+0.000001 0.875010 0.484104
+0.439293 0.757013 0.483687
+0.439293 0.757013 0.483687
+0.419463 0.722640 0.549401
+0.000001 0.788497 0.615039
+0.419463 0.722640 0.549401
+0.684938 0.393186 0.613404
+0.797292 0.459081 0.391880
+0.000001 0.919830 0.392316
+0.797292 0.459081 0.391880
+0.866677 0.498869 -0.000000
+0.866677 0.498869 -0.000000
+0.000001 1.000000 0.000000
+0.000001 0.919830 0.392316
+0.000001 0.919830 0.392316
+0.000001 0.788497 0.615039
+0.419463 0.722640 0.549401
+0.419463 0.722640 0.549401
+0.797292 0.459081 0.391880
+0.000001 0.919830 0.392316
+0.000000 0.000000 -1.000000
+0.000000 0.000000 -1.000000
+0.000000 -0.000000 -1.000000
+0.000000 -0.000000 -1.000000
+0.000000 -0.000000 -1.000000
+0.000000 -0.000000 -1.000000
+0.000000 0.000000 -1.000000
+0.000000 -0.000000 -1.000000
+0.000000 -0.000000 -1.000000
+0.000000 -0.000000 -1.000000
+0.000000 0.000000 -1.000000
+0.000000 -0.000000 -1.000000
+</float_array>
+          <technique_common>
+            <accessor source="#LEDIndicator-Normal0-array" count="282" stride="3">
+              <param name="X" type="float"/>
+              <param name="Y" type="float"/>
+              <param name="Z" type="float"/>
+            </accessor>
+          </technique_common>
+        </source>
+        <source id="LEDIndicator-UV0">
+          <float_array id="LEDIndicator-UV0-array" count="180">
+0.331040 0.999938
+0.220938 0.799324
+0.331041 0.775815
+0.441144 0.799325
+0.141050 0.582960
+0.331041 0.554785
+0.521033 0.582960
+0.102792 0.361034
+0.331041 0.328646
+0.559290 0.361035
+0.001390 0.116209
+0.660694 0.116211
+0.000033 0.000039
+0.662052 0.000041
+0.331040 0.999938
+0.220938 0.799324
+0.331041 0.775815
+0.441144 0.799325
+0.141050 0.582960
+0.331041 0.554785
+0.521033 0.582960
+0.102792 0.361034
+0.331041 0.328646
+0.559290 0.361035
+0.001390 0.116209
+0.660694 0.116211
+0.000033 0.000039
+0.662052 0.000041
+0.331040 0.999938
+0.220938 0.799324
+0.331041 0.775815
+0.441144 0.799325
+0.141050 0.582960
+0.331041 0.554785
+0.521033 0.582960
+0.102792 0.361034
+0.331041 0.328646
+0.559290 0.361035
+0.001390 0.116209
+0.660694 0.116211
+0.000033 0.000039
+0.662052 0.000041
+0.331040 0.999938
+0.220938 0.799324
+0.331041 0.775815
+0.441144 0.799325
+0.141050 0.582960
+0.331041 0.554785
+0.521033 0.582960
+0.102792 0.361034
+0.331041 0.328646
+0.559290 0.361035
+0.001390 0.116209
+0.660694 0.116211
+0.000033 0.000039
+0.662052 0.000041
+0.331040 0.999938
+0.220938 0.799324
+0.331041 0.775815
+0.441144 0.799325
+0.141050 0.582960
+0.331041 0.554785
+0.521033 0.582960
+0.102792 0.361034
+0.331041 0.328646
+0.559290 0.361035
+0.001390 0.116209
+0.660694 0.116211
+0.000033 0.000039
+0.662052 0.000041
+0.331040 0.999938
+0.220938 0.799324
+0.331041 0.775815
+0.441144 0.799325
+0.141050 0.582960
+0.331041 0.554785
+0.521033 0.582960
+0.102792 0.361034
+0.331041 0.328646
+0.559290 0.361035
+0.001390 0.116209
+0.660694 0.116211
+0.000033 0.000039
+0.662052 0.000041
+0.283335 0.017853
+0.278499 0.011567
+0.283335 0.005282
+0.293006 0.005282
+0.297841 0.011567
+0.293006 0.017853
+</float_array>
+          <technique_common>
+            <accessor source="#LEDIndicator-UV0-array" count="90" stride="2">
+              <param name="S" type="float"/>
+              <param name="T" type="float"/>
+            </accessor>
+          </technique_common>
+        </source>
+        <vertices id="LEDIndicator-VERTEX">
+          <input semantic="POSITION" source="#LEDIndicator-POSITION"/>
+        </vertices>
+        <triangles count="94" material="LEDIndicator_ncl1_1"><input semantic="VERTEX" offset="0" source="#LEDIndicator-VERTEX"/><input semantic="NORMAL" offset="1" source="#LEDIndicator-Normal0"/><input semantic="TEXCOORD" offset="2" set="0" source="#LEDIndicator-UV0"/><p> 38 0 0 12 1 1 0 2 2 38 3 0 0 4 2 1 5 3 2 6 5 0 7 2 12 8 1 12 9 1 15 10 4 2 11 5 3 12 6 1 13 3 0 14 2 0 15 2 2 16 5 3 17 6 4 18 8 2 19 5 15 20 4 15 21 4 18 22 7 4 23 8 5 24 9 3 25 6 2 26 5 2 27 5 4 28 8 5 29 9 4 30 8 18 31 7 7 32 10 6 33 11 7 34 10 9 35 12 9 36 12 8 37 13 6 38 11 6 39 11 5 40 9 4 41 8 4 42 8 7 43 10 6 44 11 38 45 14 10 46 15 11 47 16 38 48 14 11 49 16 12 50 17 14 51 19 11 52 16 10 53 15 10 54 15 13 55 18 14 56 19 15 57 20 12 58 17 11 59 16 11 60 16 14 61 19 15 62 20 17 63 22 14 64 19 13 65 18 13 66 18 16 67 21 17 68 22 18 69 23 15 70 20 14 71 19 14 72 19 17 73 22 18 74 23 17 75 22 16 76 21 45 77 24 7 78 25 45 79 24 47 80 26 47 81 26 9 82 27 7 83 25 7 84 25 18 85 23 17 86 22 17 87 22 45 88 24 7 89 25 38 90 28 1 91 29 19 92 30 38 93 28 19 94 30 23 95 31 20 96 33 19 97 30 1 98 29 1 99 29 3 100 32 20 101 33 21 102 34 23 103 31 19 104 30 19 105 30 20 106 33 21 107 34 22 108 36 20 109 33 3 110 32 3 111 32 5 112 35 22 113 36 27 114 37 21 115 34 20 116 33 20 117 33 22 118 36 27 119 37 22 120 36 5 121 35 6 122 38 29 123 39 6 124 38 8 125 40 8 126 40 30 127 41 29 128 39 29 129 39 27 130 37 22 131 36 22 132 36 6 133 38 29 134 39 38 135 42 23 136 43 24 137 44 38 138 42 24 139 44 25 140 45 26 141 47 24 142 44 23 143 43 23 144 43 21 145 46 26 146 47 32 147 48 25 148 45 24 149 44 24 150 44 26 151 47 32 152 48 28 153 50 26 154 47 21 155 46 21 156 46 27 157 49 28 158 50 34 159 51 32 160 48 26 161 47 26 162 47 28 163 50 34 164 51 28 165 50 27 166 49 29 167 52 36 168 53 29 169 52 30 170 54 30 171 54 37 172 55 36 173 53 36 174 53 34 175 51 28 176 50 28 177 50 29 178 52 36 179 53 38 180 56 25 181 57 31 182 58 38 183 56 31 184 58 39 185 59 33 186 61 31 187 58 25 188 57 25 189 57 32 190 60 33 191 61 41 192 62 39 193 59 31 194 58 31 195 58 33 196 61 41 197 62 35 198 64 33 199 61 32 200 60 32 201 60 34 202 63 35 203 64 43 204 65 41 205 62 33 206 61 33 207 61 35 208 64 43 209 65 35 210 64 34 211 63 36 212 66 46 213 67 36 214 66 37 215 68 37 216 68 48 217 69 46 218 67 46 219 67 43 220 65 35 221 64 35 222 64 36 223 66 46 224 67 38 225 70 39 226 71 40 227 72 38 228 70 40 229 72 10 230 73 42 231 75 40 232 72 39 233 71 39 234 71 41 235 74 42 236 75 13 237 76 10 238 73 40 239 72 40 240 72 42 241 75 13 242 76 44 243 78 42 244 75 41 245 74 41 246 74 43 247 77 44 248 78 16 249 79 13 250 76 42 251 75 42 252 75 44 253 78 16 254 79 44 255 78 43 256 77 46 257 80 45 258 81 46 259 80 48 260 82 48 261 82 47 262 83 45 263 81 45 264 81 16 265 79 44 266 78 44 267 78 46 268 80 45 269 81 47 270 85 48 271 86 37 272 87 37 273 87 30 274 88 8 275 89 47 276 85 37 277 87 8 278 89 9 279 84 47 280 85 8 281 89</p></triangles>
+      </mesh>
+    </geometry>
+  </library_geometries>
+  <library_visual_scenes>
+    <visual_scene id="LEDIndicator" name="LEDIndicator">
+      <node name="HuskyReplacement" id="HuskyReplacement" sid="HuskyReplacement"><matrix sid="matrix">0.100000 0.000000 0.000000 0.000000 0.000000 0.099400 0.000000 0.000000 0.000000 0.000000 0.100000 0.000000 0.000000 0.000000 0.000000 1.000000</matrix><extra><technique profile="FCOLLADA"><visibility>1.000000</visibility></technique></extra><node name="LEDIndicator" id="LEDIndicator" sid="LEDIndicator"><matrix sid="matrix">1.000000 -0.000000 -0.000000 0.000000 0.000000 1.000000 -0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 1.000000</matrix><instance_geometry url="#LEDIndicator-lib"><bind_material><technique_common><instance_material symbol="LEDIndicator_ncl1_1" target="#LEDIndicator_ncl1_1"/></technique_common></bind_material></instance_geometry><extra><technique profile="FCOLLADA"><visibility>1.000000</visibility></technique></extra></node></node>
+      <extra><technique profile="MAX3D"><frame_rate>30.000000</frame_rate></technique><technique profile="FCOLLADA"><start_time>0.000000</start_time><end_time>3.333333</end_time></technique></extra>
+    </visual_scene>
+  </library_visual_scenes>
+  <scene>
+    <instance_visual_scene url="#LEDIndicator"></instance_visual_scene>
+  </scene>
+</COLLADA>

Tools/simulation/ignition/models/x4/meshes/led_lamp.dae

@@ -0,0 +1,66 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1">
+    <asset>
+        <contributor>
+            <author>VCGLab</author>
+            <authoring_tool>VCGLib | MeshLab</authoring_tool>
+        </contributor>
+        <created>Wed Aug 22 18:54:08 2018 GMT</created>
+        <modified>Wed Aug 22 18:54:08 2018 GMT</modified>
+        <up_axis>Y_UP</up_axis>
+    </asset>
+    <library_geometries>
+        <geometry id="shape0-lib" name="shape0">
+            <mesh>
+                <source id="shape0-lib-positions" name="position">
+                    <float_array id="shape0-lib-positions-array" count="147">-0.0068518 0 0.0202589 -0.0059339 -0.00342592 0.0202589 -0.0118677 0 0.0152431 -0.0102777 -0.00593378 0.0152431 -0.0137036 0 0.0083912 -0.0118677 -0.00685184 0.0083912 -0.0179977 -0.010391 0.0036913 -0.0179977 0.010391 0.0036913 -0.0179977 -0.010391 4.45e-05 -0.0179977 0.010391 4.45e-05 0 0.00685184 0.0202589 -0.0034259 0.00593388 0.0202589 -0.0059338 0.00342592 0.0202589 0 0.0118677 0.0152431 -0.0059338 0.0102777 0.0152431 -0.0102777 0.00593388 0.0152431 0 0.0137036 0.0083912 -0.0068518 0.0118677 0.0083912 -0.0118677 0.00685184 0.0083912 -0.0034259 -0.00593388 0.0202589 -0.0059339 -0.0102777 0.0152431 0 -0.0118677 0.0152431 -0.0068518 -0.0118677 0.0083912 0 -0.00685184 0.0202589 0.0034259 -0.00593388 0.0202589 0.0059338 -0.00342592 0.0202589 0.0059338 -0.0102777 0.0152431 0 -0.0137036 0.0083912 0.0068518 -0.0118677 0.0083912 0 -0.020782 0.0036913 0 -0.020782 4.45e-05 0.0068518 0 0.0202589 0.0102777 -0.00593388 0.0152431 0.0118677 0 0.0152431 0.0118677 -0.00685184 0.0083912 0.0137036 0 0.0083912 0.0179977 -0.010391 0.0036913 0.0179977 -0.010391 4.45e-05 0 0 0.0220949 0.0059339 0.00342592 0.0202589 0.0034259 0.00593388 0.0202589 0.0102777 0.00593378 0.0152431 0.0059339 0.0102777 0.0152431 0.0118677 0.00685184 0.0083912 0.0068518 0.0118677 0.0083912 0 0.020782 0.0036913 0.0179977 0.010391 0.0036913 0 0.020782 4.45e-05 0.0179977 0.010391 4.45e-05</float_array>
+                    <technique_common>
+                        <accessor count="49" source="#shape0-lib-positions-array" stride="3">
+                            <param name="X" type="float"/>
+                            <param name="Y" type="float"/>
+                            <param name="Z" type="float"/>
+                        </accessor>
+                    </technique_common>
+                </source>
+                <source id="shape0-lib-normals" name="normal">
+                    <float_array id="shape0-lib-normals-array" count="180">-0.258207 0.0691885 0.963609 -0.258207 -0.069181 0.963609 -0.694739 0.18616 0.694753 -0.694739 0.186157 0.694754 -0.694754 -0.186144 0.694743 -0.694739 -0.18616 0.694753 -0.935114 0.250567 0.250555 -0.935115 0.250557 0.250563 -0.93511 -0.25057 0.250567 -0.935117 -0.250558 0.250555 -0.0691854 0.258206 0.963609 -0.189023 0.189018 0.963609 -0.186156 0.694749 0.694744 -0.186162 0.694744 0.694747 -0.50859 0.508578 0.694756 -0.508583 0.508597 0.694746 -0.250569 0.935111 0.250566 -0.250561 0.935115 0.250557 -0.684541 0.68456 0.250561 -0.684544 0.684555 0.250566 -0.189018 -0.189021 0.96361 -0.0691854 -0.258206 0.963609 -0.508583 -0.508591 0.694751 -0.508596 -0.508586 0.694745 -0.186155 -0.694745 0.694748 -0.186159 -0.694746 0.694746 -0.684558 -0.684545 0.250556 -0.684544 -0.684554 0.250571 -0.250566 -0.935114 0.250557 -0.250561 -0.935114 0.250562 0.0691854 -0.258206 0.963609 0.189023 -0.189018 0.963609 0.186156 -0.694749 0.694744 0.186162 -0.694744 0.694747 0.50859 -0.508578 0.694756 0.508583 -0.508597 0.694746 0.250569 -0.935111 0.250566 0.250561 -0.935115 0.250557 0.684541 -0.68456 0.250561 0.684544 -0.684555 0.250566 0.258207 -0.0691885 0.963609 0.258207 0.069181 0.963609 0.694739 -0.18616 0.694753 0.694739 -0.186157 0.694754 0.694754 0.186144 0.694743 0.694739 0.18616 0.694753 0.935114 -0.250567 0.250555 0.935115 -0.250557 0.250563 0.93511 0.25057 0.250567 0.935117 0.250558 0.250555 0.189018 0.189021 0.96361 0.0691854 0.258206 0.963609 0.508583 0.508591 0.694751 0.508596 0.508586 0.694745 0.186155 0.694745 0.694748 0.186159 0.694746 0.694746 0.684558 0.684545 0.250556 0.684544 0.684554 0.250571 0.250566 0.935114 0.250557 0.250561 0.935114 0.250562</float_array>
+                    <technique_common>
+                        <accessor count="60" source="#shape0-lib-normals-array" stride="3">
+                            <param name="X" type="float"/>
+                            <param name="Y" type="float"/>
+                            <param name="Z" type="float"/>
+                        </accessor>
+                    </technique_common>
+                </source>
+                <source id="shape0-lib-map" name="map">
+                    <float_array id="shape0-lib-map-array" count="360">0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0</float_array>
+                    <technique_common>
+                        <accessor count="180" source="#shape0-lib-map-array" stride="2">
+                            <param name="U" type="float"/>
+                            <param name="V" type="float"/>
+                        </accessor>
+                    </technique_common>
+                </source>
+                <vertices id="shape0-lib-vertices">
+                    <input semantic="POSITION" source="#shape0-lib-positions"/>
+                </vertices>
+                <triangles count="60">
+                    <input offset="0" semantic="VERTEX" source="#shape0-lib-vertices"/>
+                    <input offset="1" semantic="NORMAL" source="#shape0-lib-normals"/>
+                    <input offset="2" semantic="TEXCOORD" source="#shape0-lib-map"/>
+                    <p>38 0 0 12 0 1 0 0 2 38 1 3 0 1 4 1 1 5 2 2 6 0 2 7 12 2 8 12 3 9 15 3 10 2 3 11 3 4 12 1 4 13 0 4 14 0 5 15 2 5 16 3 5 17 4 6 18 2 6 19 15 6 20 15 7 21 18 7 22 4 7 23 5 8 24 3 8 25 2 8 26 2 9 27 4 9 28 5 9 29 38 10 30 10 10 31 11 10 32 38 11 33 11 11 34 12 11 35 14 12 36 11 12 37 10 12 38 10 13 39 13 13 40 14 13 41 15 14 42 12 14 43 11 14 44 11 15 45 14 15 46 15 15 47 17 16 48 14 16 49 13 16 50 13 17 51 16 17 52 17 17 53 18 18 54 15 18 55 14 18 56 14 19 57 17 19 58 18 19 59 38 20 60 1 20 61 19 20 62 38 21 63 19 21 64 23 21 65 20 22 66 19 22 67 1 22 68 1 23 69 3 23 70 20 23 71 21 24 72 23 24 73 19 24 74 19 25 75 20 25 76 21 25 77 22 26 78 20 26 79 3 26 80 3 27 81 5 27 82 22 27 83 27 28 84 21 28 85 20 28 86 20 29 87 22 29 88 27 29 89 38 30 90 23 30 91 24 30 92 38 31 93 24 31 94 25 31 95 26 32 96 24 32 97 23 32 98 23 33 99 21 33 100 26 33 101 32 34 102 25 34 103 24 34 104 24 35 105 26 35 106 32 35 107 28 36 108 26 36 109 21 36 110 21 37 111 27 37 112 28 37 113 34 38 114 32 38 115 26 38 116 26 39 117 28 39 118 34 39 119 38 40 120 25 40 121 31 40 122 38 41 123 31 41 124 39 41 125 33 42 126 31 42 127 25 42 128 25 43 129 32 43 130 33 43 131 41 44 132 39 44 133 31 44 134 31 45 135 33 45 136 41 45 137 35 46 138 33 46 139 32 46 140 32 47 141 34 47 142 35 47 143 43 48 144 41 48 145 33 48 146 33 49 147 35 49 148 43 49 149 38 50 150 39 50 151 40 50 152 38 51 153 40 51 154 10 51 155 42 52 156 40 52 157 39 52 158 39 53 159 41 53 160 42 53 161 13 54 162 10 54 163 40 54 164 40 55 165 42 55 166 13 55 167 44 56 168 42 56 169 41 56 170 41 57 171 43 57 172 44 57 173 16 58 174 13 58 175 42 58 176 42 59 177 44 59 178 16 59 179</p>
+                </triangles>
+            </mesh>
+        </geometry>
+    </library_geometries>
+    <library_visual_scenes>
+        <visual_scene id="VisualSceneNode" name="VisualScene">
+            <node id="node" name="node">
+                <instance_geometry url="#shape0-lib"/>
+            </node>
+        </visual_scene>
+    </library_visual_scenes>
+    <scene>
+        <instance_visual_scene url="#VisualSceneNode"/>
+    </scene>
+</COLLADA>

Tools/simulation/ignition/models/x4/model.config

@@ -0,0 +1,17 @@
+<?xml version="1.0"?>
+
+<model>
+  <name>x4</name>
+  <version>1.0</version>
+  <sdf version="1.6">model.sdf</sdf>
+
+  <author>
+    <name>Luis Pinto</name>
+    <email>luis.pinto@mov.ai</email>
+  </author>
+
+
+  <description>
+    X4 UAV with sensor configuration #1: IMU, pressure sensor, magnetometer + GPS , RGBD
+  </description>
+</model>

Tools/simulation/ignition/models/x4/model.sdf

@@ -0,0 +1,822 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<sdf version="1.6">
+   <model name="x4">
+      <pose>0 0 0.121078 0 0 0</pose>
+      <link name="base_link">
+         <pose frame="">0 0 0 0 -0 0</pose>
+         <inertial>
+            <pose frame="">0 0 0 0 -0 0</pose>
+            <mass>3.42</mass>
+            <inertia>
+               <ixx>0.075</ixx>
+               <ixy>0</ixy>
+               <ixz>0</ixz>
+               <iyy>0.075</iyy>
+               <iyz>0</iyz>
+               <izz>0.148916</izz>
+            </inertia>
+         </inertial>
+         <collision name="base_link_inertia_collision">
+            <pose frame="">0 0 0 0 -0 0</pose>
+            <geometry>
+               <box>
+                  <size>0.3 0.3 0.25</size>
+               </box>
+            </geometry>
+         </collision>
+         <visual name="base_link_inertia_visual">
+            <pose frame="">0 0 0 0 -0 0</pose>
+            <geometry>
+               <mesh>
+                  <scale>1 1 1</scale>
+                  <uri>meshes/x4.dae</uri>
+               </mesh>
+            </geometry>
+         </visual>
+         <visual name="marker_visual_1">
+            <pose frame="">-0.1 0 0.077 0 -0.1 0</pose>
+            <geometry>
+               <mesh>
+                  <scale>1 1 1</scale>
+                  <uri>meshes/led.dae</uri>
+               </mesh>
+            </geometry>
+         </visual>
+         <visual name="marker_visual_2">
+            <pose frame="">-0.09 0.059 0.059 -0.785397 -0 0.2</pose>
+            <geometry>
+               <mesh>
+                  <scale>1 1 1</scale>
+                  <uri>meshes/led.dae</uri>
+               </mesh>
+            </geometry>
+         </visual>
+         <visual name="marker_visual_3">
+            <pose frame="">-0.09 -0.059 0.059 0.785397 -0 -0.2</pose>
+            <geometry>
+               <mesh>
+                  <scale>1 1 1</scale>
+                  <uri>meshes/led.dae</uri>
+               </mesh>
+            </geometry>
+         </visual>
+         <visual name="downward_flashlight_visual">
+            <pose frame="">-0.043704 0 0.102914 -0.2 0.000158 -1.57002</pose>
+            <geometry>
+               <mesh>
+                  <scale>0.01 0.01 0.01</scale>
+                  <uri>meshes/spotlight.dae</uri>
+               </mesh>
+            </geometry>
+         </visual>
+         <visual name="left_flashlight_visual">
+            <pose frame="">-0.071985 0.090826 0.066102 0.06 -4.8e-05 -1.27</pose>
+            <geometry>
+               <mesh>
+                  <scale>0.01 0.01 0.01</scale>
+                  <uri>meshes/spotlight.dae</uri>
+               </mesh>
+            </geometry>
+         </visual>
+         <visual name="right_flashlight_visual">
+            <pose frame="">-0.071985 -0.090826 0.066102 0.06 -4.8e-05 -1.87</pose>
+            <geometry>
+               <mesh>
+                  <scale>0.01 0.01 0.01</scale>
+                  <uri>meshes/spotlight.dae</uri>
+               </mesh>
+            </geometry>
+         </visual>
+         <light name="right_light_source" type="spot">
+            <pose frame="">-0.0 -0.11 0.07 3.131592653589795 -1.5107899999999999 2.841592653589791</pose>
+            <attenuation>
+               <range>15</range>
+               <linear>0</linear>
+               <constant>0.1</constant>
+               <quadratic>0.01</quadratic>
+            </attenuation>
+            <diffuse>0.8 0.8 0.5 1</diffuse>
+            <specular>0.8 0.8 0.5 1</specular>
+            <spot>
+               <inner_angle>1</inner_angle>
+               <outer_angle>1.1</outer_angle>
+               <falloff>1</falloff>
+            </spot>
+            <direction>0 0 -1</direction>
+            <cast_shadows>1</cast_shadows>
+         </light>
+         <light name="downward_flashlight_source" type="spot">
+            <pose frame="">0.03 0 0.09 -0.01 -1.3708026535897933 0</pose>
+            <attenuation>
+               <range>15</range>
+               <linear>0</linear>
+               <constant>0.1</constant>
+               <quadratic>0.01</quadratic>
+            </attenuation>
+            <diffuse>0.8 0.8 0.5 1</diffuse>
+            <specular>0.8 0.8 0.5 1</specular>
+            <spot>
+               <inner_angle>1</inner_angle>
+               <outer_angle>1.1</outer_angle>
+               <falloff>1</falloff>
+            </spot>
+            <direction>0 0 -1</direction>
+            <cast_shadows>1</cast_shadows>
+         </light>
+         <light name="left_flashlight_source" type="spot">
+            <pose frame="">-0.0 0.11 0.07 3.131592653589795 -1.5107899999999999 -2.841592653589791</pose>
+            <attenuation>
+               <range>15</range>
+               <linear>0</linear>
+               <constant>0.1</constant>
+               <quadratic>0.01</quadratic>
+            </attenuation>
+            <diffuse>0.8 0.8 0.5 1</diffuse>
+            <specular>0.8 0.8 0.5 1</specular>
+            <spot>
+               <inner_angle>1</inner_angle>
+               <outer_angle>1.1</outer_angle>
+               <falloff>1</falloff>
+            </spot>
+            <direction>0 0 -1</direction>
+            <cast_shadows>1</cast_shadows>
+         </light>
+         <sensor name="imu_sensor" type="imu">
+            <always_on>1</always_on>
+            <update_rate>250</update_rate>
+            <imu>
+               <angular_velocity>
+                  <x>
+                     <noise type="gaussian">
+                        <mean>0</mean>
+                        <stddev>0.009</stddev>
+                        <bias_mean>0.00075</bias_mean>
+                        <bias_stddev>0.005</bias_stddev>
+                        <dynamic_bias_stddev>0.00002</dynamic_bias_stddev>
+                        <dynamic_bias_correlation_time>400.0</dynamic_bias_correlation_time>
+                        <precision>0.00025</precision>
+                     </noise>
+                  </x>
+                  <y>
+                     <noise type="gaussian">
+                        <mean>0</mean>
+                        <stddev>0.009</stddev>
+                        <bias_mean>0.00075</bias_mean>
+                        <bias_stddev>0.005</bias_stddev>
+                        <dynamic_bias_stddev>0.00002</dynamic_bias_stddev>
+                        <dynamic_bias_correlation_time>400.0</dynamic_bias_correlation_time>
+                        <precision>0.00025</precision>
+                     </noise>
+                  </y>
+                  <z>
+                     <noise type="gaussian">
+                        <mean>0</mean>
+                        <stddev>0.009</stddev>
+                        <bias_mean>0.00075</bias_mean>
+                        <bias_stddev>0.005</bias_stddev>
+                        <dynamic_bias_stddev>0.00002</dynamic_bias_stddev>
+                        <dynamic_bias_correlation_time>400.0</dynamic_bias_correlation_time>
+                        <precision>0.00025</precision>
+                     </noise>
+                  </z>
+               </angular_velocity>
+               <linear_acceleration>
+                  <x>
+                     <noise type="gaussian">
+                        <mean>0</mean>
+                        <stddev>0.021</stddev>
+                        <bias_mean>0.05</bias_mean>
+                        <bias_stddev>0.0075</bias_stddev>
+                        <dynamic_bias_stddev>0.000375</dynamic_bias_stddev>
+                        <dynamic_bias_correlation_time>175.0</dynamic_bias_correlation_time>
+                        <precision>0.005</precision>
+                     </noise>
+                  </x>
+                  <y>
+                     <noise type="gaussian">
+                        <mean>0</mean>
+                        <stddev>0.021</stddev>
+                        <bias_mean>0.05</bias_mean>
+                        <bias_stddev>0.0075</bias_stddev>
+                        <dynamic_bias_stddev>0.000375</dynamic_bias_stddev>
+                        <dynamic_bias_correlation_time>175.0</dynamic_bias_correlation_time>
+                        <precision>0.005</precision>
+                     </noise>
+                  </y>
+                  <z>
+                     <noise type="gaussian">
+                        <mean>0</mean>
+                        <stddev>0.021</stddev>
+                        <bias_mean>0.05</bias_mean>
+                        <bias_stddev>0.0075</bias_stddev>
+                        <dynamic_bias_stddev>0.000375</dynamic_bias_stddev>
+                        <dynamic_bias_correlation_time>175.0</dynamic_bias_correlation_time>
+                        <precision>0.005</precision>
+                     </noise>
+                  </z>
+               </linear_acceleration>
+            </imu>
+         </sensor>
+
+         <visual name="camera_mount_base_visual">
+            <pose>0.05 0 -0.030 0 0.0 0</pose>
+            <geometry>
+               <box>
+                  <size>0.06 0.06 0.006</size>
+               </box>
+            </geometry>
+         </visual>
+         <visual name="camera_mount_arm_visual">
+            <pose>0.08 0 -0.040 0 0.0 0</pose>
+            <geometry>
+               <cylinder>
+                  <radius>0.005</radius>
+                  <length>0.08</length>
+               </cylinder>
+            </geometry>
+         </visual>
+         <visual name="camera_mount_arm2_visual">
+            <pose>0.08 0 -0.08 0 1.57 0</pose>
+            <geometry>
+               <cylinder>
+                  <radius>0.01</radius>
+                  <length>0.025</length>
+               </cylinder>
+            </geometry>
+         </visual>
+         <visual name="camera_visual">
+            <pose>0.1 0 -0.08 0 0 0</pose>
+            <geometry>
+               <box>
+                  <size>0.02 0.025 0.025</size>
+               </box>
+            </geometry>
+         </visual>
+         <sensor name="camera_front" type="rgbd_camera">
+            <pose>0.2 0 0 0 +.785 0</pose>
+            <always_on>1</always_on>
+            <update_rate>20</update_rate>
+            <camera name="camera_front">
+               <horizontal_fov>1.0472</horizontal_fov>
+               <lens>
+                  <intrinsics>
+                     <!-- fx = fy = width / ( 2 * tan (hfov / 2 ) ) -->
+                     <fx>277.1</fx>
+                     <fy>277.1</fy>
+                     <!-- cx = ( width + 1 ) / 2 -->
+                     <cx>160.5</cx>
+                     <!-- cy = ( height + 1 ) / 2 -->
+                     <cy>120.5</cy>
+                     <s>0</s>
+                  </intrinsics>
+               </lens>
+               <distortion>
+                  <k1>0.0</k1>
+                  <k2>0.0</k2>
+                  <k3>0.0</k3>
+                  <p1>0.0</p1>
+                  <p2>0.0</p2>
+                  <center>0.5 0.5</center>
+               </distortion>
+               <image>
+                  <width>320</width>
+                  <height>240</height>
+                  <format>R8G8B8</format>
+               </image>
+               <clip>
+                  <near>0.01</near>
+                  <far>300</far>
+               </clip>
+               <depth_camera>
+                  <clip>
+                     <near>0.1</near>
+                     <far>10</far>
+                  </clip>
+               </depth_camera>
+               <noise>
+                  <type>gaussian</type>
+                  <mean>0</mean>
+                  <stddev>0.007</stddev>
+               </noise>
+            </camera>
+         </sensor>
+      </link>
+      <link name="rotor_0">
+         <pose frame="">0.247 0.1506 0.028 0.087267 0 0.523599</pose>
+         <inertial>
+            <pose frame="">0 0 0 0 -0 0</pose>
+            <mass>0.005</mass>
+            <inertia>
+               <ixx>9.75e-07</ixx>
+               <ixy>0</ixy>
+               <ixz>0</ixz>
+               <iyy>8.13545e-05</iyy>
+               <iyz>0</iyz>
+               <izz>8.22545e-05</izz>
+            </inertia>
+         </inertial>
+         <collision name="rotor_0_collision">
+            <pose frame="">0 0 0 0 -0 0</pose>
+            <geometry>
+               <cylinder>
+                  <length>0.005</length>
+                  <radius>0.1397</radius>
+               </cylinder>
+            </geometry>
+            <surface>
+               <contact>
+                  <ode />
+               </contact>
+               <friction>
+                  <ode />
+               </friction>
+            </surface>
+         </collision>
+         <visual name="rotor_0_visual">
+            <pose frame="">0 0 0 0 -0 0</pose>
+            <geometry>
+               <mesh>
+                  <scale>1 1 1</scale>
+                  <uri>meshes/neo11_propeller_ccw.dae</uri>
+               </mesh>
+            </geometry>
+            <material>
+               <diffuse>1 0 0 1</diffuse>
+               <script>
+                  <name>Gazebo/Red</name>
+                  <uri>file://media/materials/scripts/gazebo.material</uri>
+               </script>
+            </material>
+            <cast_shadows>0</cast_shadows>
+         </visual>
+         <gravity>1</gravity>
+         <velocity_decay />
+      </link>
+      <joint name="rotor_0_joint" type="revolute">
+         <child>rotor_0</child>
+         <parent>base_link</parent>
+         <axis>
+            <xyz>0.043578 -0.075479 0.996195</xyz>
+            <limit>
+               <lower>-1e+16</lower>
+               <upper>1e+16</upper>
+            </limit>
+            <dynamics>
+               <spring_reference>0</spring_reference>
+               <spring_stiffness>0</spring_stiffness>
+            </dynamics>
+            <use_parent_model_frame>1</use_parent_model_frame>
+         </axis>
+      </joint>
+      <link name="rotor_1">
+         <pose frame="">-0.00067 0.28929 0.028 0 -0.087267 0</pose>
+         <inertial>
+            <pose frame="">0 0 0 0 -0 0</pose>
+            <mass>0.005</mass>
+            <inertia>
+               <ixx>9.75e-07</ixx>
+               <ixy>0</ixy>
+               <ixz>0</ixz>
+               <iyy>8.13545e-05</iyy>
+               <iyz>0</iyz>
+               <izz>8.22545e-05</izz>
+            </inertia>
+         </inertial>
+         <collision name="rotor_1_collision">
+            <pose frame="">0 0 0 0 -0 0</pose>
+            <geometry>
+               <cylinder>
+                  <length>0.005</length>
+                  <radius>0.1397</radius>
+               </cylinder>
+            </geometry>
+            <surface>
+               <contact>
+                  <ode />
+               </contact>
+               <friction>
+                  <ode />
+               </friction>
+            </surface>
+         </collision>
+         <visual name="rotor_1_visual">
+            <pose frame="">0 0 0 0 -0 0</pose>
+            <geometry>
+               <mesh>
+                  <scale>1 1 1</scale>
+                  <uri>meshes/neo11_propeller_cw.dae</uri>
+               </mesh>
+            </geometry>
+            <material>
+               <diffuse>0 0 1 1</diffuse>
+               <script>
+                  <name>Gazebo/Blue</name>
+                  <uri>file://media/materials/scripts/gazebo.material</uri>
+               </script>
+            </material>
+            <cast_shadows>0</cast_shadows>
+         </visual>
+         <gravity>1</gravity>
+         <velocity_decay />
+      </link>
+      <joint name="rotor_1_joint" type="revolute">
+         <child>rotor_1</child>
+         <parent>base_link</parent>
+         <axis>
+            <xyz>-0.087156 0 0.996195</xyz>
+            <limit>
+               <lower>-1e+16</lower>
+               <upper>1e+16</upper>
+            </limit>
+            <dynamics>
+               <spring_reference>0</spring_reference>
+               <spring_stiffness>0</spring_stiffness>
+            </dynamics>
+            <use_parent_model_frame>1</use_parent_model_frame>
+         </axis>
+      </joint>
+      <link name="rotor_2">
+         <pose frame="">-0.2501 0.1454 0.028 0.087267 -0 2.61799</pose>
+         <inertial>
+            <pose frame="">0 0 0 0 -0 0</pose>
+            <mass>0.005</mass>
+            <inertia>
+               <ixx>9.75e-07</ixx>
+               <ixy>0</ixy>
+               <ixz>0</ixz>
+               <iyy>8.13545e-05</iyy>
+               <iyz>0</iyz>
+               <izz>8.22545e-05</izz>
+            </inertia>
+         </inertial>
+         <collision name="rotor_2_collision">
+            <pose frame="">0 0 0 0 -0 0</pose>
+            <geometry>
+               <cylinder>
+                  <length>0.005</length>
+                  <radius>0.1397</radius>
+               </cylinder>
+            </geometry>
+            <surface>
+               <contact>
+                  <ode />
+               </contact>
+               <friction>
+                  <ode />
+               </friction>
+            </surface>
+         </collision>
+         <visual name="rotor_2_visual">
+            <pose frame="">0 0 0 0 -0 0</pose>
+            <geometry>
+               <mesh>
+                  <scale>1 1 1</scale>
+                  <uri>meshes/neo11_propeller_ccw.dae</uri>
+               </mesh>
+            </geometry>
+            <material>
+               <diffuse>0 0 1 1</diffuse>
+               <script>
+                  <name>Gazebo/Blue</name>
+                  <uri>file://media/materials/scripts/gazebo.material</uri>
+               </script>
+            </material>
+            <cast_shadows>0</cast_shadows>
+         </visual>
+         <gravity>1</gravity>
+         <velocity_decay />
+      </link>
+      <joint name="rotor_2_joint" type="revolute">
+         <child>rotor_2</child>
+         <parent>base_link</parent>
+         <axis>
+            <xyz>0.043578 0.075479 0.996195</xyz>
+            <limit>
+               <lower>-1e+16</lower>
+               <upper>1e+16</upper>
+            </limit>
+            <dynamics>
+               <spring_reference>0</spring_reference>
+               <spring_stiffness>0</spring_stiffness>
+            </dynamics>
+            <use_parent_model_frame>1</use_parent_model_frame>
+         </axis>
+      </joint>
+      <link name="rotor_3">
+         <pose frame="">-0.2501 -0.1454 0.028 -0.087267 -0 -2.61799</pose>
+         <inertial>
+            <pose frame="">0 0 0 0 -0 0</pose>
+            <mass>0.005</mass>
+            <inertia>
+               <ixx>9.75e-07</ixx>
+               <ixy>0</ixy>
+               <ixz>0</ixz>
+               <iyy>8.13545e-05</iyy>
+               <iyz>0</iyz>
+               <izz>8.22545e-05</izz>
+            </inertia>
+         </inertial>
+         <collision name="rotor_3_collision">
+            <pose frame="">0 0 0 0 -0 0</pose>
+            <geometry>
+               <cylinder>
+                  <length>0.005</length>
+                  <radius>0.1397</radius>
+               </cylinder>
+            </geometry>
+            <surface>
+               <contact>
+                  <ode />
+               </contact>
+               <friction>
+                  <ode />
+               </friction>
+            </surface>
+         </collision>
+         <visual name="rotor_3_visual">
+            <pose frame="">0 0 0 0 -0 0</pose>
+            <geometry>
+               <mesh>
+                  <scale>1 1 1</scale>
+                  <uri>meshes/neo11_propeller_cw.dae</uri>
+               </mesh>
+            </geometry>
+            <material>
+               <diffuse>0 0 1 1</diffuse>
+               <script>
+                  <name>Gazebo/Blue</name>
+                  <uri>file://media/materials/scripts/gazebo.material</uri>
+               </script>
+            </material>
+            <cast_shadows>0</cast_shadows>
+         </visual>
+         <gravity>1</gravity>
+         <velocity_decay />
+      </link>
+      <joint name="rotor_3_joint" type="revolute">
+         <child>rotor_3</child>
+         <parent>base_link</parent>
+         <axis>
+            <xyz>0.043578 -0.075479 0.996195</xyz>
+            <limit>
+               <lower>-1e+16</lower>
+               <upper>1e+16</upper>
+            </limit>
+            <dynamics>
+               <spring_reference>0</spring_reference>
+               <spring_stiffness>0</spring_stiffness>
+            </dynamics>
+            <use_parent_model_frame>1</use_parent_model_frame>
+         </axis>
+      </joint>
+      <link name="rotor_4">
+         <pose frame="">-0.00067 -0.28929 0.028 -0 0.087267 -3.14159</pose>
+         <inertial>
+            <pose frame="">0 0 0 0 -0 0</pose>
+            <mass>0.005</mass>
+            <inertia>
+               <ixx>9.75e-07</ixx>
+               <ixy>0</ixy>
+               <ixz>0</ixz>
+               <iyy>8.13545e-05</iyy>
+               <iyz>0</iyz>
+               <izz>8.22545e-05</izz>
+            </inertia>
+         </inertial>
+         <collision name="rotor_4_collision">
+            <pose frame="">0 0 0 0 -0 0</pose>
+            <geometry>
+               <cylinder>
+                  <length>0.005</length>
+                  <radius>0.1397</radius>
+               </cylinder>
+            </geometry>
+            <surface>
+               <contact>
+                  <ode />
+               </contact>
+               <friction>
+                  <ode />
+               </friction>
+            </surface>
+         </collision>
+         <visual name="rotor_4_visual">
+            <pose frame="">0 0 0 0 -0 0</pose>
+            <geometry>
+               <mesh>
+                  <scale>1 1 1</scale>
+                  <uri>meshes/neo11_propeller_ccw.dae</uri>
+               </mesh>
+            </geometry>
+            <material>
+               <diffuse>0 0 1 1</diffuse>
+               <script>
+                  <name>Gazebo/Blue</name>
+                  <uri>file://media/materials/scripts/gazebo.material</uri>
+               </script>
+            </material>
+            <cast_shadows>0</cast_shadows>
+         </visual>
+         <gravity>1</gravity>
+         <velocity_decay />
+      </link>
+      <joint name="rotor_4_joint" type="revolute">
+         <child>rotor_4</child>
+         <parent>base_link</parent>
+         <axis>
+            <xyz>-0.087156 -0 0.996195</xyz>
+            <limit>
+               <lower>-1e+16</lower>
+               <upper>1e+16</upper>
+            </limit>
+            <dynamics>
+               <spring_reference>0</spring_reference>
+               <spring_stiffness>0</spring_stiffness>
+            </dynamics>
+            <use_parent_model_frame>1</use_parent_model_frame>
+         </axis>
+      </joint>
+      <link name="rotor_5">
+         <pose frame="">0.247 -0.1506 0.028 -0.087267 0 -0.523599</pose>
+         <inertial>
+            <pose frame="">0 0 0 0 -0 0</pose>
+            <mass>0.005</mass>
+            <inertia>
+               <ixx>9.75e-07</ixx>
+               <ixy>0</ixy>
+               <ixz>0</ixz>
+               <iyy>8.13545e-05</iyy>
+               <iyz>0</iyz>
+               <izz>8.22545e-05</izz>
+            </inertia>
+         </inertial>
+         <collision name="rotor_5_collision">
+            <pose frame="">0 0 0 0 -0 0</pose>
+            <geometry>
+               <cylinder>
+                  <length>0.005</length>
+                  <radius>0.1397</radius>
+               </cylinder>
+            </geometry>
+            <surface>
+               <contact>
+                  <ode />
+               </contact>
+               <friction>
+                  <ode />
+               </friction>
+            </surface>
+         </collision>
+         <visual name="rotor_5_visual">
+            <pose frame="">0 0 0 0 -0 0</pose>
+            <geometry>
+               <mesh>
+                  <scale>1 1 1</scale>
+                  <uri>meshes/neo11_propeller_cw.dae</uri>
+               </mesh>
+            </geometry>
+            <material>
+               <diffuse>1 0 0 1</diffuse>
+               <script>
+                  <name>Gazebo/Red</name>
+                  <uri>file://media/materials/scripts/gazebo.material</uri>
+               </script>
+            </material>
+            <cast_shadows>0</cast_shadows>
+         </visual>
+         <gravity>1</gravity>
+         <velocity_decay />
+      </link>
+      <joint name="rotor_5_joint" type="revolute">
+         <child>rotor_5</child>
+         <parent>base_link</parent>
+         <axis>
+            <xyz>0.043578 0.075479 0.996195</xyz>
+            <limit>
+               <lower>-1e+16</lower>
+               <upper>1e+16</upper>
+            </limit>
+            <dynamics>
+               <spring_reference>0</spring_reference>
+               <spring_stiffness>0</spring_stiffness>
+            </dynamics>
+            <use_parent_model_frame>1</use_parent_model_frame>
+         </axis>
+      </joint>
+      <plugin filename="libignition-gazebo-imu-system.so" name="ignition::gazebo::systems::Imu" />
+      <plugin filename="ignition-gazebo-multicopter-motor-model-system" name="ignition::gazebo::systems::MulticopterMotorModel">
+         <robotNamespace>x4</robotNamespace>
+         <jointName>rotor_0_joint</jointName>
+         <linkName>rotor_0</linkName>
+         <turningDirection>ccw</turningDirection>
+         <timeConstantUp>0.0182</timeConstantUp>
+         <timeConstantDown>0.0182</timeConstantDown>
+         <maxRotVelocity>1000.0</maxRotVelocity>
+         <motorConstant>1.269e-05</motorConstant>
+         <momentConstant>0.016754</momentConstant>
+         <commandSubTopic>command/motor_speed</commandSubTopic>
+         <motorNumber>0</motorNumber>
+         <rotorDragCoefficient>2.0673e-04</rotorDragCoefficient>
+         <rotorDragCoefficient>0</rotorDragCoefficient>
+         <rollingMomentCoefficient>0</rollingMomentCoefficient>
+         <rotorVelocitySlowdownSim>2</rotorVelocitySlowdownSim>
+         <motorType>velocity</motorType>
+      </plugin>
+      <plugin filename="ignition-gazebo-multicopter-motor-model-system" name="ignition::gazebo::systems::MulticopterMotorModel">
+         <robotNamespace>x4</robotNamespace>
+         <jointName>rotor_1_joint</jointName>
+         <linkName>rotor_1</linkName>
+         <turningDirection>cw</turningDirection>
+         <timeConstantUp>0.0182</timeConstantUp>
+         <timeConstantDown>0.0182</timeConstantDown>
+         <maxRotVelocity>1000.0</maxRotVelocity>
+         <motorConstant>1.269e-05</motorConstant>
+         <momentConstant>0.016754</momentConstant>
+         <commandSubTopic>command/motor_speed</commandSubTopic>
+         <motorNumber>1</motorNumber>
+         <rotorDragCoefficient>2.0673e-04</rotorDragCoefficient>
+         <rotorDragCoefficient>0</rotorDragCoefficient>
+         <rollingMomentCoefficient>0</rollingMomentCoefficient>
+         <rotorVelocitySlowdownSim>2</rotorVelocitySlowdownSim>
+         <motorType>velocity</motorType>
+      </plugin>
+      <plugin filename="ignition-gazebo-multicopter-motor-model-system" name="ignition::gazebo::systems::MulticopterMotorModel">
+         <robotNamespace>x4</robotNamespace>
+         <jointName>rotor_2_joint</jointName>
+         <linkName>rotor_2</linkName>
+         <turningDirection>ccw</turningDirection>
+         <timeConstantUp>0.0182</timeConstantUp>
+         <timeConstantDown>0.0182</timeConstantDown>
+         <maxRotVelocity>1000.0</maxRotVelocity>
+         <motorConstant>1.269e-05</motorConstant>
+         <momentConstant>0.016754</momentConstant>
+         <commandSubTopic>command/motor_speed</commandSubTopic>
+         <motorNumber>2</motorNumber>
+         <rotorDragCoefficient>2.0673e-04</rotorDragCoefficient>
+         <rotorDragCoefficient>0</rotorDragCoefficient>
+         <rollingMomentCoefficient>0</rollingMomentCoefficient>
+         <rotorVelocitySlowdownSim>2</rotorVelocitySlowdownSim>
+         <motorType>velocity</motorType>
+      </plugin>
+      <plugin filename="ignition-gazebo-multicopter-motor-model-system" name="ignition::gazebo::systems::MulticopterMotorModel">
+         <robotNamespace>x4</robotNamespace>
+         <jointName>rotor_3_joint</jointName>
+         <linkName>rotor_3</linkName>
+         <turningDirection>cw</turningDirection>
+         <timeConstantUp>0.0182</timeConstantUp>
+         <timeConstantDown>0.0182</timeConstantDown>
+         <maxRotVelocity>1000.0</maxRotVelocity>
+         <motorConstant>1.269e-05</motorConstant>
+         <momentConstant>0.016754</momentConstant>
+         <commandSubTopic>command/motor_speed</commandSubTopic>
+         <motorNumber>3</motorNumber>
+         <rotorDragCoefficient>2.0673e-04</rotorDragCoefficient>
+         <rotorDragCoefficient>0</rotorDragCoefficient>
+         <rollingMomentCoefficient>0</rollingMomentCoefficient>
+         <rotorVelocitySlowdownSim>2</rotorVelocitySlowdownSim>
+         <motorType>velocity</motorType>
+      </plugin>
+      <plugin filename="ignition-gazebo-multicopter-motor-model-system" name="ignition::gazebo::systems::MulticopterMotorModel">
+         <robotNamespace>x4</robotNamespace>
+         <jointName>rotor_4_joint</jointName>
+         <linkName>rotor_4</linkName>
+         <turningDirection>ccw</turningDirection>
+         <timeConstantUp>0.0182</timeConstantUp>
+         <timeConstantDown>0.0182</timeConstantDown>
+         <maxRotVelocity>1000.0</maxRotVelocity>
+         <motorConstant>1.269e-05</motorConstant>
+         <momentConstant>0.016754</momentConstant>
+         <commandSubTopic>command/motor_speed</commandSubTopic>
+         <motorNumber>4</motorNumber>
+         <rotorDragCoefficient>2.0673e-04</rotorDragCoefficient>
+         <rotorDragCoefficient>0</rotorDragCoefficient>
+         <rollingMomentCoefficient>0</rollingMomentCoefficient>
+         <rotorVelocitySlowdownSim>2</rotorVelocitySlowdownSim>
+         <motorType>velocity</motorType>
+      </plugin>
+      <plugin filename="ignition-gazebo-multicopter-motor-model-system" name="ignition::gazebo::systems::MulticopterMotorModel">
+         <robotNamespace>x4</robotNamespace>
+         <jointName>rotor_5_joint</jointName>
+         <linkName>rotor_5</linkName>
+         <turningDirection>cw</turningDirection>
+         <timeConstantUp>0.0182</timeConstantUp>
+         <timeConstantDown>0.0182</timeConstantDown>
+         <maxRotVelocity>1000.0</maxRotVelocity>
+         <motorConstant>1.269e-05</motorConstant>
+         <momentConstant>0.016754</momentConstant>
+         <commandSubTopic>command/motor_speed</commandSubTopic>
+         <motorNumber>5</motorNumber>
+         <rotorDragCoefficient>2.0673e-04</rotorDragCoefficient>
+         <rotorDragCoefficient>0</rotorDragCoefficient>
+         <rollingMomentCoefficient>0</rollingMomentCoefficient>
+         <rotorVelocitySlowdownSim>2</rotorVelocitySlowdownSim>
+         <motorType>velocity</motorType>
+      </plugin>
+
+      <plugin filename="ignition-gazebo-odometry-publisher-system" name="ignition::gazebo::systems::OdometryPublisher">
+         <dimensions>3</dimensions>
+         <odom_frame>x4/odom</odom_frame>
+         <robot_base_frame>x4/base_footprint</robot_base_frame>
+      </plugin>
+   </model>
+</sdf>