Run ignition gazebo as distributed lockstep

ign sim fix alt ground truth and baro
WIP: add X3
2026-06-15 16:31:30 +08:00 · 2022-08-15 11:15:03 +02:00 · 2022-08-14 15:22:24 -04:00 · 2022-08-14 15:15:24 -04:00 · 2022-08-14 13:04:08 -04:00 · 2022-08-14 17:14:25 +02:00
444 changed files with 40010 additions and 5522 deletions
--- a/.clang-tidy
+++ b/.clang-tidy
@ -1,11 +1,17 @@
 ---
 Checks:          '*,
+                  -*-avoid-c-arrays,
+                  -*-uppercase-literal-suffix,
+                  -*-magic-numbers,
+                  -altera-id-dependent-backward-branch,
+                  -altera-unroll-loops,
                  -android*,
                  -bugprone-integer-division,
                  -cert-dcl50-cpp,
                  -cert-env33-c,
                  -cert-err34-c,
                  -cert-err58-cpp,
+                  -cert-flp30-c,
                  -cert-msc30-c,
                  -cert-msc50-cpp,
                  -clang-analyzer-core.CallAndMessage,
@ -18,6 +24,7 @@ Checks:          '*,
                  -clang-analyzer-deadcode.DeadStores,
                  -clang-analyzer-optin.cplusplus.VirtualCall,
                  -clang-analyzer-optin.performance.Padding,
+                  -clang-analyzer-security.FloatLoopCounter,
                  -clang-analyzer-security.insecureAPI.strcpy,
                  -clang-analyzer-unix.API,
                  -clang-analyzer-unix.cstring.BadSizeArg,
@ -37,8 +44,7 @@ Checks:          '*,
                  -cppcoreguidelines-pro-type-union-access,
                  -cppcoreguidelines-pro-type-vararg,
                  -cppcoreguidelines-special-member-functions,
-                  -fuchsia-default-arguments,
-                  -fuchsia-overloaded-operator,
+                  -fuchsia-*,
                  -google-build-using-namespace,
                  -google-explicit-constructor,
                  -google-global-names-in-headers,
@ -62,6 +68,7 @@ Checks:          '*,
                  -hicpp-use-equals-delete,
                  -hicpp-use-override,
                  -hicpp-vararg,
+                  -llvmlibc-*,
                  -llvm-header-guard,
                  -llvm-include-order,
                  -llvm-namespace-comment,
@ -84,6 +91,7 @@ Checks:          '*,
                  -modernize-use-override,
                  -modernize-use-trailing-return-type,
                  -modernize-use-using,
+                  -modernize-use-trailing-return-type,
                  -performance-inefficient-string-concatenation,
                  -readability-avoid-const-params-in-decls,
                  -readability-container-size-empty,
--- a/.github/ISSUE_TEMPLATE/1_Bug_report.md
+++ b/.github/ISSUE_TEMPLATE/1_Bug_report.md
@ -20,7 +20,7 @@ A clear and concise description of what you expected to happen.

 ## Log Files and Screenshots
 *Always* provide a link to the flight log file:
- Download the flight log file from the vehicle ([tutorial](https://docs.px4.io/master/en/getting_started/flight_reporting.html)).
+- Download the flight log file from the vehicle ([tutorial](https://docs.px4.io/main/en/getting_started/flight_reporting.html)).
 - Upload the log to the [PX4 Flight Review](http://logs.px4.io/)
 - Share the link to the log (Copy and paste the URL of the log)

--- a/.gitmodules
+++ b/.gitmodules
@ -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
--- a/.vscode/cmake-variants.yaml
+++ b/.vscode/cmake-variants.yaml
@ -6,6 +6,11 @@ CONFIG:
      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
--- a/.vscode/tasks.json
+++ b/.vscode/tasks.json
@ -49,185 +49,18 @@
                "group": "test"
            }
        },
-        {
-            "label": "jmavsim build",
-            "type": "shell",
-            "command": "ant create_run_jar copy_res",
-            "options": {
-                "cwd": "${workspaceFolder}/Tools/jMAVSim"
-            },
-            "problemMatcher": [],
-            "presentation":{
-                "echo": true,
-                "reveal": "never",
-                "focus": false,
-                "panel": "shared",
-                "showReuseMessage": false,
-                "clear": false,
-            }
-        },
-        {
-            "label": "jmavsim",
-            "type": "shell",
-            "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",
-                "env": {
-                    "PX4_SIM_SPEED_FACTOR": "1"
-                }
-            },
-            "isBackground": true,
-            "presentation": {
-                "echo": true,
-                "reveal": "never",
-                "focus": false,
-                "panel": "shared",
-                "showReuseMessage": false,
-                "clear": false
-            },
-            "problemMatcher": [
-                {
-                    "pattern": [
-                        {
-                            "regexp": ".",
-                            "file": 1,
-                            "location": 2,
-                            "message": 3
-                        }
-                    ],
-                    "background": {
-                        "activeOnStart": true,
-                        "beginsPattern": ".",
-                        "endsPattern": ".",
-                    }
-                }
-            ]
-        },
-        {
-            "label": "jmavsim kill",
-            "type": "shell",
-            "command": "kill $(ps aux | grep jmavsim | grep -v 'grep' | awk '{print $2}')",
-            "presentation": {
-                "echo": false,
-                "reveal": "never",
-                "focus": false,
-                "panel": "shared",
-                "showReuseMessage": false,
-                "clear": false
-            },
-            "problemMatcher": []
-        },
-        {
-            "label": "gazebo build",
-            "type": "shell",
-            "command": "make px4_sitl_default sitl_gazebo",
-            "options": {
-                "cwd": "${workspaceFolder}"
-            },
-            "problemMatcher": [],
-            "presentation":{
-                "echo": true,
-                "reveal": "always",
-                "focus": false,
-                "panel": "shared",
-                "showReuseMessage": false,
-                "clear": false,
-            }
-        },
-        {
-            "label": "gazebo start",
-            "type": "shell",
-            "dependsOn": "gazebo build",
-            "options": {
-                "cwd": "${workspaceFolder}",
-                "env": {
-                    "GAZEBO_PLUGIN_PATH": "${workspaceFolder}/build/px4_sitl_default/build_gazebo",
-                    "GAZEBO_MODEL_PATH": "${workspaceFolder}/Tools/sitl_gazebo/models",
-                    "PX4_SIM_SPEED_FACTOR": "1"
-                }
-            },
-            "command": "gzserver --verbose ${workspaceFolder}/Tools/sitl_gazebo/worlds/${input:gazeboWorld}.world",
-            "isBackground": true,
-            "presentation": {
-                "echo": true,
-                "reveal": "never",
-                "focus": false,
-                "panel": "shared",
-                "showReuseMessage": false,
-                "clear": false
-            },
-            "problemMatcher": [
-                {
-                    "pattern": [
-                        {
-                            "regexp": ".",
-                            "file": 1,
-                            "location": 2,
-                            "message": 3
-                        }
-                    ],
-                    "background": {
-                        "activeOnStart": true,
-                        "beginsPattern": ".",
-                        "endsPattern": ".",
-                    }
-                }
-            ]
-        },
+
        {
            "label": "gazebo",
            "type": "shell",
-            "dependsOn": "gazebo start",
            "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/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",
-            "isBackground": false,
-            "presentation": {
-                "echo": true,
-                "reveal": "never",
-                "focus": false,
-                "panel": "shared",
-                "showReuseMessage": false,
-                "clear": false
-            },
-            "problemMatcher": [
-                {
-                    "pattern": [
-                        {
-                            "regexp": ".",
-                            "file": 1,
-                            "location": 2,
-                            "message": 3
-                        }
-                    ],
-                    "background": {
-                        "activeOnStart": true,
-                        "beginsPattern": ".",
-                        "endsPattern": ".",
-                    }
-                }
-            ]
-        },
-        {
-            "label": "gazebo client",
-            "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",
-                    "PX4_SIM_SPEED_FACTOR": "1"
-                }
-            },
-            "command": "gzclient --verbose",
+            "command": "ign gazebo -v 4 -r ${workspaceFolder}/Tools/simulation/gazebo/worlds/${input:gazeboWorld}.sdf",
            "isBackground": true,
            "presentation": {
                "echo": true,
@ -255,10 +88,11 @@
                }
            ]
        },
+
        {
            "label": "gazebo kill",
            "type": "shell",
-            "command": "killall gzserver",
+            "command": "killall ign",
            "presentation": {
                "echo": true,
                "reveal": "silent",
@ -270,6 +104,7 @@
            "problemMatcher": [],
            "dependsOn":["px4_sitl_cleanup"]
        },
+
        {
            "label": "px4_sitl_cleanup",
            "type": "shell",
@ -335,39 +170,14 @@
        }
    ],
    "inputs": [
-        {
-            "type": "pickString",
-            "id": "vehicleModel",
-            "description": "gazebo model",
-            "options": [
-              "iris",
-              "typhoon_h480",
-              "plane",
-              "plane_catapult",
-              "plane_lidar",
-              "standard_vtol",
-              "tailsitter",
-              "tiltrotor",
-              "r1_rover",
-              "boat"
-            ],
-            "default": "iris"
-        },
        {
            "type": "pickString",
            "id": "gazeboWorld",
            "description": "gazebo world",
            "options": [
-                "baylands",
-                "empty",
-                "ksql_airport",
-                "mcmillan_airfield",
-                "sonoma_raceway",
-                "warehouse",
-                "windy",
-                "yosemite"
+                "x4"
            ],
-            "default": "empty"
+            "default": "x4"
        }
    ]
 }
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@ -175,6 +175,12 @@ include(kconfig)
 message(STATUS "PX4 config: ${PX4_CONFIG}")
 message(STATUS "PX4 platform: ${PX4_PLATFORM}")

+if($ENV{CLION_IDE})
+	# CLion automatically executes some compiler commands after configuring the
+	# project. This would fail on NuttX, as visibility.h tries to (indirectly)
+	# include nuttx/config.h, which at that point does not exist yet
+	add_definitions(-DPX4_DISABLE_GCC_POISON)
+endif()

 if(${PX4_PLATFORM} STREQUAL "posix")
 	if(ENABLE_LOCKSTEP_SCHEDULER)
--- a/CONTRIBUTING.md
+++ b/CONTRIBUTING.md
@ -8,7 +8,7 @@ First [fork and clone](https://help.github.com/articles/fork-a-repo) the project

 ### Create a feature branch

-*Always* branch off master for new features.
+*Always* branch off main for new features.

 ```
 git checkout -b mydescriptivebranchname
@ -16,7 +16,7 @@ git checkout -b mydescriptivebranchname

 ### Edit and build the code

-The [developer guide](http://dev.px4.io/) explains how to set up the development environment on Mac OS, Linux or Windows. Please take note of our [coding style](https://dev.px4.io/master/en/contribute/code.html) when editing files.
+The [developer guide](https://docs.px4.io/main/en/development/development.html) explains how to set up the development environment on Mac OS, Linux or Windows. Please take note of our [coding style](https://docs.px4.io/main/en/contribute/code.html) when editing files.

 ### Commit your changes

--- a/39
+++ b/39
@ -7,7 +7,8 @@ pipeline {
    stage('Analysis') {
      when {
        anyOf {
-          branch 'master'
+          branch 'main'
+          branch 'master' // should be removed, but in case there is something going on...
          branch 'pr-jenkins' // for testing
        }
      }
@ -204,20 +205,21 @@ pipeline {
            unstash 'msg_documentation'
            unstash 'uorb_graph'
            withCredentials([usernamePassword(credentialsId: 'px4buildbot_github_personal_token', passwordVariable: 'GIT_PASS', usernameVariable: 'GIT_USER')]) {
-              sh('git clone https://${GIT_USER}:${GIT_PASS}@github.com/PX4/px4_user_guide.git')
-              sh('cp airframes.md px4_user_guide/en/airframes/airframe_reference.md')
-              sh('cp parameters.md px4_user_guide/en/advanced_config/parameter_reference.md')
-              sh('cp -R modules/*.md px4_user_guide/en/modules/')
-              sh('cp -R graph_*.json px4_user_guide/.vuepress/public/en/middleware/')
-              sh('cp -R msg_docs/*.md px4_user_guide/en/msg_docs/')
-              sh('cd px4_user_guide; git status; git add .; git commit -a -m "Update PX4 Firmware metadata `date`" || true')
-              sh('cd px4_user_guide; git push origin master || true')
-              sh('rm -rf px4_user_guide')
+              sh('git clone https://${GIT_USER}:${GIT_PASS}@github.com/PX4/PX4-user_guide.git')
+              sh('cp airframes.md PX4-user_guide/en/airframes/airframe_reference.md')
+              sh('cp parameters.md PX4-user_guide/en/advanced_config/parameter_reference.md')
+              sh('cp -R modules/*.md PX4-user_guide/en/modules/')
+              sh('cp -R graph_*.json PX4-user_guide/.vuepress/public/en/middleware/')
+              sh('cp -R msg_docs/*.md PX4-user_guide/en/msg_docs/')
+              sh('cd PX4-user_guide; git status; git add .; git commit -a -m "Update PX4 Firmware metadata `date`" || true')
+              sh('cd PX4-user_guide; git push origin main || true')
+              sh('rm -rf PX4-user_guide')
            }
          }
          when {
            anyOf {
-              branch 'master'
+              branch 'main'
+              branch 'master' // should be removed, but in case there is something going on...
              branch 'pr-jenkins' // for testing
            }
          }
@ -245,7 +247,8 @@ pipeline {
          }
          when {
            anyOf {
-              branch 'master'
+              branch 'main'
+              branch 'master' // should be removed, but in case there is something going on...
              branch 'pr-jenkins' // for testing
            }
          }
@ -278,7 +281,8 @@ pipeline {
          }
          when {
            anyOf {
-              branch 'master'
+              branch 'main'
+              branch 'master' // should be removed, but in case there is something going on...
              branch 'pr-jenkins' // for testing
            }
          }
@ -307,7 +311,8 @@ pipeline {
          }
          when {
            anyOf {
-              branch 'master'
+              branch 'main'
+              branch 'master' // should be removed, but in case there is something going on...
              branch 'pr-jenkins' // for testing
            }
          }
@ -350,7 +355,8 @@ pipeline {
          }
          when {
            anyOf {
-              branch 'master'
+              branch 'main'
+              branch 'master' // should be removed, but in case there is something going on...
              branch 'pr-jenkins' // for testing
            }
          }
@ -373,7 +379,8 @@ pipeline {
          }
          when {
            anyOf {
-              branch 'master'
+              branch 'main'
+              branch 'master' // should be removed, but in case there is something going on...
              branch 'pr-jenkins' // for testing
            }
          }
--- a/README.md
+++ b/README.md
@ -10,8 +10,8 @@ This repository holds the [PX4](http://px4.io) flight control solution for drone

 PX4 is highly portable, OS-independent and supports Linux, NuttX and MacOS out of the box.

-* Official Website: http://px4.io (License: BSD 3-clause, [LICENSE](https://github.com/PX4/PX4-Autopilot/blob/master/LICENSE))
-* [Supported airframes](https://docs.px4.io/main/en/airframes/airframe_reference.html) ([portfolio](http://px4.io/#airframes)):
+* Official Website: http://px4.io (License: BSD 3-clause, [LICENSE](https://github.com/PX4/PX4-Autopilot/blob/main/LICENSE))
+* [Supported airframes](https://docs.px4.io/main/en/airframes/airframe_reference.html) ([portfolio](https://px4.io/ecosystem/commercial-systems/)):
  * [Multicopters](https://docs.px4.io/main/en/frames_multicopter/)
  * [Fixed wing](https://docs.px4.io/main/en/frames_plane/)
  * [VTOL](https://docs.px4.io/main/en/frames_vtol/)
@ -120,3 +120,11 @@ Additional information about supported hardware can be found in [PX4 user Guide
 ## Project Roadmap

 A high level project roadmap is available [here](https://github.com/orgs/PX4/projects/25).
+
+## Project Governance
+
+The PX4 Autopilot project including all of its trademarks is hosted under [Dronecode](https://www.dronecode.org/), part of the Linux Foundation.
+
+<a href="https://www.dronecode.org/" style="padding:20px" ><img src="https://mavlink.io/assets/site/logo_dronecode.png" alt="Dronecode Logo" width="110px"/></a>
+<a href="https://www.linuxfoundation.org/projects" style="padding:20px;"><img src="https://mavlink.io/assets/site/logo_linux_foundation.png" alt="Linux Foundation Logo" width="80px" /></a>
+<div style="padding:10px">&nbsp;</div>
--- a/ROMFS/px4fmu_common/init.d-posix/airframes/10042_xvert
+++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10042_xvert
@ -8,10 +8,10 @@

 . ${R}etc/init.d/rc.vtol_defaults

-param set-default VT_MC_CS_LOCK 0
+param set-default VT_ELEV_MC_LOCK 0
 param set-default VT_TYPE 0
 param set-default VT_FW_DIFTHR_EN 1
-param set-default VT_FW_DIFTHR_S_Y 0.3
+param set-default VT_FW_DIFTHR_SC 0.3
 param set-default MPC_MAN_Y_MAX 60
 param set-default MC_PITCH_P 5

--- a/ROMFS/px4fmu_common/init.d-posix/airframes/1030_plane
+++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1030_plane
@ -10,16 +10,12 @@ 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_THR_LND_MAX 0

 param set-default FW_L1_PERIOD 12

-param set-default FW_MAN_P_MAX 30
-
 param set-default FW_PR_P 0.9
 param set-default FW_PR_FF 0.2
 param set-default FW_PSP_OFF 2
-param set-default FW_P_LIM_MAX 32
 param set-default FW_P_LIM_MIN -15

 param set-default FW_RR_FF 0.1
--- a/ROMFS/px4fmu_common/init.d-posix/airframes/1033_rascal
+++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1033_rascal
@ -7,12 +7,9 @@

 param set-default FW_LND_AIRSPD_SC 1.1
 param set-default FW_LND_ANG 5
-param set-default FW_THR_LND_MAX 0
-param set-default FW_LND_HHDIST 30
 param set-default FW_LND_FL_PMIN 9.5
 param set-default FW_LND_FL_PMAX 20
 param set-default FW_LND_FLALT 5
-param set-default FW_LND_TLALT 15

 param set-default FW_L1_PERIOD 25

@ -38,7 +35,6 @@ param set-default NAV_DLL_ACT 2
 param set-default RWTO_TKOFF 1

 param set-default RWTO_MAX_PITCH 20
-param set-default RWTO_MAX_ROLL 10

 param set-default RWTO_PSP 8
 param set-default RWTO_AIRSPD_SCL 1.8
--- a/ROMFS/px4fmu_common/init.d-posix/airframes/1034_rascal-electric
+++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1034_rascal-electric
@ -7,12 +7,9 @@

 param set-default FW_LND_AIRSPD_SC 1.1
 param set-default FW_LND_ANG 5
-param set-default FW_THR_LND_MAX 0
-param set-default FW_LND_HHDIST 30
 param set-default FW_LND_FL_PMIN 9.5
 param set-default FW_LND_FL_PMAX 20
 param set-default FW_LND_FLALT 5
-param set-default FW_LND_TLALT 15

 param set-default FW_L1_PERIOD 25

@ -38,7 +35,6 @@ param set-default NAV_DLL_ACT 2
 param set-default RWTO_TKOFF 1

 param set-default RWTO_MAX_PITCH 20
-param set-default RWTO_MAX_ROLL 10

 param set-default RWTO_PSP 8
 param set-default RWTO_AIRSPD_SCL 1.8
--- a/ROMFS/px4fmu_common/init.d-posix/airframes/1035_techpod
+++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1035_techpod
@ -10,7 +10,6 @@ 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_THR_LND_MAX 0

 param set-default FW_L1_PERIOD 15

--- a/ROMFS/px4fmu_common/init.d-posix/airframes/1036_malolo
+++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1036_malolo
@ -7,12 +7,9 @@

 param set-default FW_LND_AIRSPD_SC 1.1
 param set-default FW_LND_ANG 5
-param set-default FW_THR_LND_MAX 0
-param set-default FW_LND_HHDIST 30
 param set-default FW_LND_FL_PMIN 9.5
 param set-default FW_LND_FL_PMAX 20
 param set-default FW_LND_FLALT 5
-param set-default FW_LND_TLALT 15

 param set-default FW_L1_PERIOD 25

@ -37,7 +34,6 @@ param set-default NAV_DLL_ACT 2

 param set-default RWTO_TKOFF 1
 param set-default RWTO_MAX_PITCH 20
-param set-default RWTO_MAX_ROLL 10
 param set-default RWTO_PSP 8
 param set-default RWTO_AIRSPD_SCL 1.8

--- a/ROMFS/px4fmu_common/init.d-posix/airframes/1037_believer
+++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1037_believer
@ -10,16 +10,12 @@ 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_THR_LND_MAX 0

 param set-default FW_L1_PERIOD 12

-param set-default FW_MAN_P_MAX 30
-
 param set-default FW_PR_P 0.9
 param set-default FW_PR_FF 0.2
 param set-default FW_PSP_OFF 2
-param set-default FW_P_LIM_MAX 32
 param set-default FW_P_LIM_MIN -15

 param set-default FW_RR_FF 0.1
--- a/ROMFS/px4fmu_common/init.d-posix/airframes/1040_standard_vtol
+++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1040_standard_vtol
@ -49,11 +49,9 @@ param set-default PWM_MAIN_FUNC7 202
 param set-default PWM_MAIN_FUNC8 203

 param set-default FW_L1_PERIOD 12
-param set-default FW_MAN_P_MAX 30
 param set-default FW_PR_FF 0.2
 param set-default FW_PR_P 0.9
 param set-default FW_PSP_OFF 2
-param set-default FW_P_LIM_MAX 32
 param set-default FW_P_LIM_MIN -15
 param set-default FW_RR_FF 0.1
 param set-default FW_RR_P 0.3
--- a/ROMFS/px4fmu_common/init.d-posix/airframes/1041_tailsitter
+++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1041_tailsitter
@ -9,12 +9,12 @@

 param set-default MAV_TYPE 20

-param set-default SYS_CTRL_ALLOC 1
+# param set-default SYS_CTRL_ALLOC 1
 param set-default CA_AIRFRAME 4

 param set-default CA_ROTOR_COUNT 4
 param set-default CA_ROTOR0_PX 1
-param set-default CA_ROTOR0_PY 1
+param set-default CA_ROTOR0_PY 2
 param set-default CA_ROTOR0_KM 0.05
 param set-default CA_ROTOR1_PX -1
 param set-default CA_ROTOR1_PY -1
@ -26,17 +26,27 @@ param set-default CA_ROTOR3_PX -1
 param set-default CA_ROTOR3_PY 1
 param set-default CA_ROTOR3_KM -0.05

+param set-default CA_SV_CS_COUNT 2
+param set-default CA_SV_CS0_TYPE 5
+param set-default CA_SV_CS0_TRQ_P 0.5
+param set-default CA_SV_CS0_TRQ_Y -0.5
+param set-default CA_SV_CS1_TYPE 6
+param set-default CA_SV_CS1_TRQ_P 0.5
+param set-default CA_SV_CS1_TRQ_Y 0.5
+
 param set-default PWM_MAIN_FUNC1 101
 param set-default PWM_MAIN_FUNC2 102
 param set-default PWM_MAIN_FUNC3 103
 param set-default PWM_MAIN_FUNC4 104
+param set-default PWM_MAIN_FUNC5 0
+param set-default PWM_MAIN_FUNC6 201
+param set-default PWM_MAIN_FUNC7 202
+param set-default PWM_MAIN_REV 96 # invert both elevons

 param set-default FW_L1_PERIOD 12
-param set-default FW_MAN_P_MAX 30
 param set-default FW_PR_I 0.2
 param set-default FW_PR_P 0.2
 param set-default FW_PSP_OFF 2
-param set-default FW_P_LIM_MAX 32
 param set-default FW_P_LIM_MIN -15
 param set-default FW_RR_P 0.2
 param set-default FW_THR_TRIM 0.33
@ -57,29 +67,15 @@ param set-default MPC_XY_VEL_P_ACC 3
 param set-default MPC_XY_VEL_I_ACC 4
 param set-default MPC_XY_VEL_D_ACC 0.1

-param set-default VT_FW_DIFTHR_EN 7
-param set-default VT_FW_DIFTHR_S_P 1
-param set-default VT_FW_DIFTHR_S_R 1
-param set-default VT_FW_DIFTHR_S_Y 1
+param set-default NAV_ACC_RAD 5
+
+param set-default VT_FW_DIFTHR_EN 1
+param set-default VT_FW_DIFTHR_SC 0.5
 param set-default VT_F_TRANS_DUR 1.5
 param set-default VT_F_TRANS_THR 0.7
 param set-default VT_TYPE 0

 param set-default WV_EN 0

-#uncomment the following block to enable elevons (otherwise only motors are used for attitdue control)
-# param set-default CA_SV_CS_COUNT 2
-# param set-default CA_SV_CS0_TYPE 5
-# param set-default CA_SV_CS0_TRQ_P 0.5
-# param set-default CA_SV_CS0_TRQ_Y -0.5
-# param set-default CA_SV_CS1_TYPE 6
-# param set-default CA_SV_CS1_TRQ_P 0.5
-# param set-default CA_SV_CS1_TRQ_Y 0.5
-# param set-default PWM_MAIN_FUNC6 201
-# param set-default PWM_MAIN_FUNC7 202
-# param set-default PWM_MAIN_REV 96 # invert both elevons
-# param set-default VT_FW_DIFTHR_EN 1
-
-
 set MIXER_FILE etc/mixers-sitl/quad_x_vtol.main.mix
 set MIXER custom
--- a/ROMFS/px4fmu_common/init.d-posix/airframes/1042_tiltrotor
+++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1042_tiltrotor
@ -27,9 +27,7 @@ param set-default CA_ROTOR3_PY 0.1875
 param set-default CA_ROTOR3_KM -0.05

 param set-default CA_ROTOR0_TILT 1
-param set-default CA_ROTOR1_TILT 2
 param set-default CA_ROTOR2_TILT 3
-param set-default CA_ROTOR3_TILT 4
 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
@ -56,11 +54,9 @@ param set-default PWM_MAIN_FUNC10 202
 param set-default PWM_MAIN_FUNC11 203

 param set-default FW_L1_PERIOD 12
-param set-default FW_MAN_P_MAX 30
 param set-default FW_PR_FF 0.2
 param set-default FW_PR_P 0.9
 param set-default FW_PSP_OFF 2
-param set-default FW_P_LIM_MAX 32
 param set-default FW_P_LIM_MIN -15
 param set-default FW_RR_FF 0.1
 param set-default FW_RR_P 0.3
--- a/ROMFS/px4fmu_common/init.d-posix/airframes/1043_standard_vtol_drop
+++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1043_standard_vtol_drop
@ -8,11 +8,9 @@
 . ${R}etc/init.d/rc.vtol_defaults

 param set-default FW_L1_PERIOD 12
-param set-default FW_MAN_P_MAX 30
 param set-default FW_PR_FF 0.2
 param set-default FW_PR_P 0.9
 param set-default FW_PSP_OFF 2
-param set-default FW_P_LIM_MAX 32
 param set-default FW_P_LIM_MIN -15
 param set-default FW_RR_FF 0.1
 param set-default FW_RR_P 0.3
--- a/ROMFS/px4fmu_common/init.d-posix/airframes/17001_tf-g1
+++ b/ROMFS/px4fmu_common/init.d-posix/airframes/17001_tf-g1
@ -38,7 +38,6 @@ param set-default FW_P_LIM_MAX 25
 param set-default FW_P_LIM_MIN -5
 param set-default FW_R_LIM 30

-param set-default FW_MAN_P_MAX 30.0
 param set-default FW_MAN_R_MAX 30.0

 param set-default FW_THR_TRIM 0.8
--- a/ROMFS/px4fmu_common/init.d-posix/airframes/17002_tf-g2
+++ b/ROMFS/px4fmu_common/init.d-posix/airframes/17002_tf-g2
@ -0,0 +1,54 @@
+#!/bin/sh
+#
+# @name ThunderFly TF-G2
+# ThunderFly TF-G2 autogyro airframe. Only for FlightGear simulator
+#
+# @type Autogyro
+# @class Autogyro
+#
+# @url https://github.com/ThunderFly-aerospace/TF-G2/
+#
+#
+
+. ${R}etc/init.d/rc.fw_defaults
+
+param set-default FW_AIRSPD_STALL 5
+
+param set-default FW_P_RMAX_NEG 20.0
+param set-default FW_W_RMAX 10
+param set-default FW_W_EN 1
+
+param set-default FW_RR_P 0.08
+
+param set-default MIS_LTRMIN_ALT 50
+param set-default MIS_TAKEOFF_ALT 7
+
+param set-default NAV_ACC_RAD 20
+param set-default NAV_DLL_ACT 2
+param set-default NAV_LOITER_RAD 50
+
+param set-default RWTO_TKOFF 0
+# Parameters related to autogyro takeoff PR
+#param set-default AG_TKOFF 1
+#param set-default AG_PROT_TYPE 1
+#param set-default AG_PROT_MIN_RPM 50.0
+#param set-default AG_PROT_TRG_RPM 900.0
+#param set-defoult AG_ROTOR_RPM 900.0
+
+param set-default FW_ARSP_SCALE_EN 0
+
+param set-default FW_AIRSPD_MAX 35
+param set-default FW_AIRSPD_MIN 7
+
+param set-default FW_P_LIM_MAX 25
+param set-default FW_P_LIM_MIN -5
+param set-default FW_R_LIM 30
+
+param set-default FW_MAN_R_MAX 30.0
+
+param set-default FW_THR_CRUISE 0.8
+param set-default FW_THR_IDLE 0
+param set-default COM_DISARM_PRFLT 0
+
+set MIXER_FILE etc/mixers-sitl/autogyro_sitl.main.mix
+set MIXER custom
--- a/ROMFS/px4fmu_common/init.d-posix/airframes/4001_x3
+++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4001_x3
@ -0,0 +1,10 @@
+#!/bin/sh
+#
+# @name Ignition Gazebo X3
+#
+# @type Quadrotor
+#
+
+. ${R}etc/init.d/rc.mc_defaults
+
+set MIXER quad_w
--- a/ROMFS/px4fmu_common/init.d-posix/airframes/6001_x4
+++ b/ROMFS/px4fmu_common/init.d-posix/airframes/6001_x4
@ -0,0 +1,13 @@
+#!/bin/sh
+#
+# @name Ignition Gazebo X4
+#
+# @type Quadrotor
+#
+
+. ${R}etc/init.d/rc.mc_defaults
+
+param set-default MC_PITCHRATE_P 0.1
+param set-default MC_ROLLRATE_P 0.05
+
+set MIXER hexa_x
--- a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt
+++ b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt
@ -77,8 +77,14 @@ px4_add_romfs_files(
 	1070_boat
 	3010_quadrotor_x
 	3011_hexarotor_x
+
+	4001_x3
+
 	17001_tf-g1
+	17002_tf-g2
 	2507_cloudship
+
+	6001_x4
 	6011_typhoon_h480
 	6011_typhoon_h480.post
 	6012_typhoon_h480_ctrlalloc
--- a/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator
+++ b/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator
@ -1,20 +1,33 @@
 #!/bin/sh
 # shellcheck disable=SC2154

-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 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
+if ignition_simulator start
+then
+
+	sensor_baro_sim start
+	sensor_gps_sim start
+	sensor_mag_sim start
+
 else
-  echo "PX4 SIM HOST: $PX4_SIM_HOSTNAME"
-  simulator start -h $PX4_SIM_HOSTNAME $simulator_tcp_port
+	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 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
+	fi
+
 fi
--- a/ROMFS/px4fmu_common/init.d-posix/rcS
+++ b/ROMFS/px4fmu_common/init.d-posix/rcS
@ -1,6 +1,6 @@
 #!/bin/sh

-set -e
+#set -e

 # PX4 commands need the 'px4-' prefix in bash.
 # (px4-alias.sh is expected to be in the PATH)
@ -212,6 +212,7 @@ elif ! replay tryapplyparams
 then
  . px4-rc.simulator
 fi
+
 load_mon start
 battery_simulator start
 tone_alarm start
--- a/ROMFS/px4fmu_common/init.d/airframes/1000_rc_fw_easystar.hil
+++ b/ROMFS/px4fmu_common/init.d/airframes/1000_rc_fw_easystar.hil
@ -29,9 +29,6 @@ param set-default FW_L1_DAMPING 0.74
 param set-default FW_L1_PERIOD 16
 param set-default FW_LND_ANG 15
 param set-default FW_LND_FLALT 5
-param set-default FW_LND_HVIRT 13
-param set-default FW_LND_TLALT 5
-param set-default FW_THR_LND_MAX 0
 param set-default FW_PR_FF 0.35
 param set-default FW_PR_P 0.2
 param set-default FW_RR_FF 0.6
--- a/ROMFS/px4fmu_common/init.d/airframes/1002_standard_vtol.hil
+++ b/ROMFS/px4fmu_common/init.d/airframes/1002_standard_vtol.hil
@ -45,7 +45,6 @@ param set-default MPC_XY_VEL_I_ACC 4
 param set-default MPC_XY_VEL_P_ACC 3
 param set-default MPC_Z_VEL_P_ACC 12
 param set-default MPC_Z_VEL_I_ACC 3
-param set-default MPC_Z_VEL_MAX_DN 1.5

 param set-default NAV_ACC_RAD 5
 param set-default NAV_DLL_ACT 2
--- a/ROMFS/px4fmu_common/init.d/airframes/1102_tailsitter_duo_sih.hil
+++ b/ROMFS/px4fmu_common/init.d/airframes/1102_tailsitter_duo_sih.hil
@ -18,11 +18,11 @@
 . ${R}etc/init.d/rc.vtol_defaults

 param set UAVCAN_ENABLE 0
-param set-default VT_MC_CS_LOCK 0
+param set-default VT_ELEV_MC_LOCK 0
 param set-default VT_MOT_COUNT 2
 param set-default VT_TYPE 0
 param set-default VT_FW_DIFTHR_EN 1
-param set-default VT_FW_DIFTHR_S_Y 0.3
+param set-default VT_FW_DIFTHR_SC 0.3
 param set-default MPC_MAN_Y_MAX 60
 param set-default MC_PITCH_P 5

--- a/ROMFS/px4fmu_common/init.d/airframes/13000_generic_vtol_standard
+++ b/ROMFS/px4fmu_common/init.d/airframes/13000_generic_vtol_standard
@ -1,22 +1,10 @@
 #!/bin/sh
 #
-# @name Generic Quadplane VTOL
+# @name Generic Standard VTOL
 #
 # @type Standard VTOL
 # @class VTOL
-#
-# @maintainer
-#
-# @output MAIN1 motor 1
-# @output MAIN2 motor 2
-# @output MAIN3 motor 3
-# @output MAIN4 motor 4
-# @output AUX1 Aileron 1
-# @output AUX2 Aileron 2
-# @output AUX3 Elevator
-# @output AUX4 Rudder
-# @output AUX5 Throttle
-#
+
 # @board px4_fmu-v2 exclude
 # @board bitcraze_crazyflie exclude
 # @board holybro_kakutef7 exclude
@ -24,21 +12,21 @@

 . ${R}etc/init.d/rc.vtol_defaults

+param set-default SYS_CTRL_ALLOC 1
 param set-default CA_AIRFRAME 2
 param set-default CA_ROTOR_COUNT 5
-param set-default CA_ROTOR0_PX 0.15
-param set-default CA_ROTOR0_PY 0.15
-param set-default CA_ROTOR1_PX -0.15
-param set-default CA_ROTOR1_PY -0.15
-param set-default CA_ROTOR2_PX 0.15
-param set-default CA_ROTOR2_PY -0.15
+param set-default CA_ROTOR0_PX 1
+param set-default CA_ROTOR0_PY 1
+param set-default CA_ROTOR1_PX -1
+param set-default CA_ROTOR1_PY -1
+param set-default CA_ROTOR2_PX 1
+param set-default CA_ROTOR2_PY -1
 param set-default CA_ROTOR2_KM -0.05
-param set-default CA_ROTOR3_PX -0.15
-param set-default CA_ROTOR3_PY 0.15
+param set-default CA_ROTOR3_PX -1
+param set-default CA_ROTOR3_PY 1
 param set-default CA_ROTOR3_KM -0.05
 param set-default CA_ROTOR4_AX 1.0
 param set-default CA_ROTOR4_AZ 0.0
-param set-default CA_ROTOR4_PX 0.2
 param set-default CA_SV_CS_COUNT 4
 param set-default CA_SV_CS0_TYPE 1
 param set-default CA_SV_CS0_TRQ_R -0.5
@ -48,16 +36,5 @@ param set-default CA_SV_CS2_TYPE 3
 param set-default CA_SV_CS2_TRQ_P 1.0
 param set-default CA_SV_CS3_TRQ_Y 1.0
 param set-default CA_SV_CS3_TYPE 4
-
-param set-default PWM_AUX_DIS5 950
-
 param set-default VT_TYPE 2
-param set-default VT_MOT_ID 1234
-param set-default VT_FW_MOT_OFFID 1234
-
 param set-default MAV_TYPE 22
-
-set MIXER quad_x
-set MIXER_AUX vtol_AAERT
-
-set PWM_OUT 1234
--- a/ROMFS/px4fmu_common/init.d/airframes/13001_caipirinha_vtol
+++ b/ROMFS/px4fmu_common/init.d/airframes/13001_caipirinha_vtol
@ -34,7 +34,7 @@ param set-default MC_YAWRATE_P 0.22
 param set-default MC_YAWRATE_I 0.02

 param set-default VT_IDLE_PWM_MC  1080
-param set-default VT_MC_CS_LOCK 0
+param set-default VT_ELEV_MC_LOCK 0
 param set-default VT_MOT_ID 12
 param set-default VT_TYPE 0

--- a/ROMFS/px4fmu_common/init.d/airframes/13002_firefly6
+++ b/ROMFS/px4fmu_common/init.d/airframes/13002_firefly6
@ -44,7 +44,7 @@ param set-default VT_FW_MOT_OFFID 56
 param set-default VT_TILT_MC 0.08
 param set-default VT_TILT_TRANS 0.5
 param set-default VT_TILT_FW 0.9
-param set-default VT_MC_CS_LOCK 0
+param set-default VT_ELEV_MC_LOCK 0
 param set-default VT_TYPE 1

 set MIXER firefly6
--- a/ROMFS/px4fmu_common/init.d/airframes/13008_QuadRanger
+++ b/ROMFS/px4fmu_common/init.d/airframes/13008_QuadRanger
@ -31,7 +31,6 @@ param set-default MC_YAWRATE_I 0.04
 param set-default MC_YAWRATE_MAX 40

 param set-default MPC_ACC_HOR_MAX 2
-param set-default MPC_Z_VEL_MAX_DN 1.5
 param set-default MPC_LAND_SPEED 0.8
 param set-default MPC_YAWRAUTO_MAX 40

--- a/ROMFS/px4fmu_common/init.d/airframes/13012_convergence
+++ b/ROMFS/px4fmu_common/init.d/airframes/13012_convergence
@ -34,8 +34,6 @@ param set-default FW_ACRO_X_MAX   270
 param set-default FW_ACRO_Y_MAX   270
 param set-default FW_ACRO_Z_MAX   180
 param set-default FW_PSP_OFF  5
-param set-default FW_P_LIM_MAX    30
-param set-default FW_P_LIM_MIN    -30
 param set-default FW_RR_FF    0.33
 param set-default FW_RR_P 0.11

@ -57,14 +55,14 @@ param set-default VT_B_TRANS_DUR  1
 param set-default VT_F_TRANS_DUR  1.2
 param set-default VT_F_TR_OL_TM   4
 param set-default VT_FW_DIFTHR_EN 1
-param set-default VT_FW_DIFTHR_S_Y 0.17
+param set-default VT_FW_DIFTHR_SC 0.17
 param set-default VT_FW_MOT_OFFID 3
 param set-default VT_IDLE_PWM_MC 1200
 param set-default VT_MOT_ID 123
 param set-default VT_TILT_TRANS   0.45
 param set-default VT_TRANS_MIN_TM 1.2
 param set-default VT_TRANS_P2_DUR 1.3
-param set-default VT_MC_CS_LOCK 0
+param set-default VT_ELEV_MC_LOCK 0
 param set-default VT_TYPE 1

 set MIXER vtol_convergence
--- a/ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad
+++ b/ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad
@ -55,7 +55,6 @@ param set-default FW_T_SINK_MIN 1
 param set-default FW_T_VERT_ACC 6
 param set-default FW_THR_TRIM 0.70
 param set-default FW_THR_SLEW_MAX 1
-param set-default FW_MAN_P_MAX 30
 param set-default FW_P_LIM_MAX 15
 param set-default FW_P_LIM_MIN -25
 param set-default FW_P_RMAX_NEG 45
@ -90,7 +89,6 @@ param set-default MPC_ACC_HOR_MAX 2
 param set-default MPC_LAND_SPEED 1.2
 param set-default MPC_TILTMAX_LND 35
 param set-default MPC_Z_VEL_MAX_UP 1.5
-param set-default MPC_Z_VEL_MAX_DN 1.5
 param set-default MPC_HOLD_MAX_XY 0.5
 param set-default MPC_HOLD_MAX_Z 0.5
 param set-default MPC_TKO_RAMP_T 0.8
--- a/ROMFS/px4fmu_common/init.d/airframes/13014_vtol_babyshark
+++ b/ROMFS/px4fmu_common/init.d/airframes/13014_vtol_babyshark
@ -31,7 +31,6 @@ param set-default FW_AIRSPD_MAX 30
 param set-default FW_AIRSPD_MIN 19
 param set-default FW_AIRSPD_TRIM 23
 param set-default FW_L1_R_SLEW_MAX 40
-param set-default FW_MAN_P_MAX 30
 param set-default FW_PSP_OFF 3
 param set-default FW_P_LIM_MAX 18
 param set-default FW_P_LIM_MIN -25
@ -71,7 +70,6 @@ param set-default MPC_VEL_MANUAL 3
 param set-default MPC_XY_CRUISE 3
 param set-default MPC_XY_VEL_MAX 3.5
 param set-default MPC_YAWRAUTO_MAX 40
-param set-default MPC_Z_VEL_MAX_DN 1.5
 param set-default MPC_Z_VEL_MAX_UP 2

 param set-default NAV_ACC_RAD 3
@ -86,7 +84,7 @@ param set-default VT_ARSP_BLEND 10
 param set-default VT_ARSP_TRANS 21
 param set-default VT_B_DEC_MSS 1.5
 param set-default VT_B_TRANS_DUR 12
-param set-default VT_MC_CS_LOCK 0
+param set-default VT_ELEV_MC_LOCK 0
 param set-default VT_FWD_THRUST_SC 1.2
 param set-default VT_FW_MOT_OFFID 5678
 param set-default VT_F_TR_OL_TM 8
--- a/ROMFS/px4fmu_common/init.d/airframes/13100_generic_vtol_tiltrotor
+++ b/ROMFS/px4fmu_common/init.d/airframes/13100_generic_vtol_tiltrotor
@ -0,0 +1,44 @@
+#!/bin/sh
+#
+# @name Generic Tiltrotor VTOL
+#
+# @type VTOL Tiltrotor
+# @class VTOL
+#
+# @board px4_fmu-v2 exclude
+# @board bitcraze_crazyflie exclude
+#
+
+. ${R}etc/init.d/rc.vtol_defaults
+
+param set-default SYS_CTRL_ALLOC 1
+
+param set-default CA_AIRFRAME 3
+param set-default CA_ROTOR_COUNT 4
+param set-default CA_ROTOR0_PX 1
+param set-default CA_ROTOR0_PY 1
+param set-default CA_ROTOR0_TILT 2
+param set-default CA_ROTOR1_PX -1
+param set-default CA_ROTOR1_PY -1
+param set-default CA_ROTOR2_PX 1
+param set-default CA_ROTOR2_PY -1
+param set-default CA_ROTOR2_TILT 1
+param set-default CA_ROTOR2_KM -0.05
+param set-default CA_ROTOR3_PX -1
+param set-default CA_ROTOR3_PY 1
+param set-default CA_ROTOR3_KM -0.05
+param set-default CA_SV_CS_COUNT 4
+param set-default CA_SV_CS0_TYPE 1
+param set-default CA_SV_CS0_TRQ_R -0.5
+param set-default CA_SV_CS1_TYPE 2
+param set-default CA_SV_CS1_TRQ_R 0.5
+param set-default CA_SV_CS2_TYPE 7
+param set-default CA_SV_CS2_TRQ_P 0.5
+param set-default CA_SV_CS2_TRQ_Y 0.5
+param set-default CA_SV_CS3_TYPE 8
+param set-default CA_SV_CS3_TRQ_P 0.5
+param set-default CA_SV_CS3_TRQ_Y -0.5
+param set-default CA_SV_TL_COUNT 2
+
+param set-default MAV_TYPE 21
+param set-default VT_TYPE 1
--- a/ROMFS/px4fmu_common/init.d/airframes/13200_generic_vtol_tailsitter
+++ b/ROMFS/px4fmu_common/init.d/airframes/13200_generic_vtol_tailsitter
@ -1,17 +1,10 @@
 #!/bin/sh
 #
-# @name Generic Tailsitter
+# @name Generic VTOL Tailsitter
 #
-# @type VTOL Duo Tailsitter
+# @type VTOL Tailsitter
 # @class VTOL
 #
-# @output MAIN1 motor right
-# @output MAIN2 motor left
-# @output MAIN5 elevon right
-# @output MAIN6 elevon left
-#
-# @maintainer Roman Bapst <roman@px4.io>
-#
 # @board px4_fmu-v2 exclude
 # @board bitcraze_crazyflie exclude
 # @board holybro_kakutef7 exclude
@ -19,18 +12,13 @@

 . ${R}etc/init.d/rc.vtol_defaults

-param set-default MAV_TYPE 19
-
-param set-default VT_MC_CS_LOCK 0
-param set-default VT_MOT_COUNT 2
-param set-default VT_TYPE 0
-
+param set-default SYS_CTRL_ALLOC 1
 param set-default CA_AIRFRAME 4
 param set-default CA_ROTOR_COUNT 2
 param set-default CA_ROTOR0_KM -0.05
-param set-default CA_ROTOR0_PY 0.2
-param set-default CA_ROTOR1_KM -0.05
-param set-default CA_ROTOR1_PY -0.2
+param set-default CA_ROTOR0_PY -0.2
+param set-default CA_ROTOR1_KM 0.05
+param set-default CA_ROTOR1_PY 0.2
 param set-default CA_SV_CS_COUNT 2
 param set-default CA_SV_CS0_TRQ_P 0.5
 param set-default CA_SV_CS0_TRQ_Y 0.5
@ -39,6 +27,6 @@ param set-default CA_SV_CS1_TRQ_P 0.5
 param set-default CA_SV_CS1_TRQ_Y -0.5
 param set-default CA_SV_CS1_TYPE 6

-set MIXER vtol_tailsitter_duo
-
-set PWM_OUT 1234
+param set-default MAV_TYPE 19
+param set-default VT_TYPE 0
+param set-default VT_ELEV_MC_LOCK 0
--- a/ROMFS/px4fmu_common/init.d/airframes/13201_swan_quad_tailsitter
+++ b/ROMFS/px4fmu_common/init.d/airframes/13201_swan_quad_tailsitter
@ -1,39 +0,0 @@
-#!/bin/sh
-#
-# @name Swan K1 Quad Tailsitter VTOL
-#
-# @type VTOL Quad Tailsitter
-# @class VTOL
-#
-# @board px4_fmu-v2 exclude
-# @board bitcraze_crazyflie exclude
-#
-
-. ${R}etc/init.d/rc.vtol_defaults
-
-param set-default MAV_TYPE 20
-
-param set-default SYS_CTRL_ALLOC 1
-
-param set-default CA_AIRFRAME 4
-param set-default CA_ROTOR_COUNT 4
-param set-default CA_ROTOR0_PX 0.15
-param set-default CA_ROTOR0_PY 0.23
-param set-default CA_ROTOR1_PX -0.15
-param set-default CA_ROTOR1_PY -0.23
-param set-default CA_ROTOR2_PX 0.15
-param set-default CA_ROTOR2_PY -0.23
-param set-default CA_ROTOR2_KM -0.05
-param set-default CA_ROTOR3_PX -0.15
-param set-default CA_ROTOR3_PY 0.23
-param set-default CA_ROTOR3_KM -0.05
-
-#output allocation
-#min pwm (1100)
-#controller tuning
-
-param set-default VT_MC_CS_LOCK 0
-
-param set-default VT_TYPE 0
-
-param set-default SENS_EN_MS4525DO 1
--- a/ROMFS/px4fmu_common/init.d/airframes/2100_standard_plane
+++ b/ROMFS/px4fmu_common/init.d/airframes/2100_standard_plane
@ -1,28 +1,16 @@
 #!/bin/sh
 #
-# @name Standard Plane
+# @name Generic Standard Plane
 #
 # @type Standard Plane
 # @class Plane
 #
-# @output MAIN1 aileron
-# @output MAIN2 elevator
-# @output MAIN3 throttle
-# @output MAIN4 rudder
-# @output MAIN5 flaps
-# @output MAIN6 gear
-#
-# @output AUX1 feed-through of RC AUX1 channel
-# @output AUX2 feed-through of RC AUX2 channel
-# @output AUX3 feed-through of RC AUX3 channel
-#
-# @maintainer Lorenz Meier <lorenz@px4.io>
-#
 # @board bitcraze_crazyflie exclude
 #

 . ${R}etc/init.d/rc.fw_defaults

+param set-default SYS_CTRL_ALLOC 1
 param set-default CA_AIRFRAME 1
 param set-default CA_ROTOR_COUNT 1
 param set-default CA_ROTOR0_PX 0.3
@ -34,11 +22,3 @@ 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 PWM_AUX_RATE 50
-param set-default PWM_MAIN_RATE 50
-
-set MIXER AETRFG
-
-# Rate must be set by group (see pwm info).
-# Throttle is in the same group as servos.
--- a/ROMFS/px4fmu_common/init.d/airframes/3000_generic_wing
+++ b/ROMFS/px4fmu_common/init.d/airframes/3000_generic_wing
@ -5,19 +5,20 @@
 # @type Flying Wing
 # @class Plane
 #
-# @output MAIN1 left aileron
-# @output MAIN2 right aileron
-# @output MAIN4 throttle
-#
-# @output AUX1 feed-through of RC AUX1 channel
-# @output AUX2 feed-through of RC AUX2 channel
-# @output AUX3 feed-through of RC AUX3 channel
-#
-# @maintainer
-#
 # @board bitcraze_crazyflie exclude
 #

 . ${R}etc/init.d/rc.fw_defaults

-set MIXER fw_generic_wing
+param set-default SYS_CTRL_ALLOC 1
+param set-default CA_AIRFRAME 1
+
+param set-default CA_ROTOR_COUNT 1
+param set-default CA_ROTOR0_PX 0.15
+param set-default CA_SV_CS_COUNT 2
+param set-default CA_SV_CS0_TYPE 5
+param set-default CA_SV_CS0_TRQ_P 0.5
+param set-default CA_SV_CS0_TRQ_R -0.5
+param set-default CA_SV_CS1_TYPE 6
+param set-default CA_SV_CS1_TRQ_P 0.5
+param set-default CA_SV_CS1_TRQ_R 0.5
--- a/ROMFS/px4fmu_common/init.d/airframes/3031_phantom
+++ b/ROMFS/px4fmu_common/init.d/airframes/3031_phantom
@ -2,7 +2,7 @@
 #
 # @name Phantom FPV Flying Wing
 #
-# @url https://docs.px4.io/master/en/frames_plane/wing_wing_z84.html
+# @url https://docs.px4.io/main/en/frames_plane/wing_wing_z84.html
 #
 # @type Flying Wing
 # @class Plane
--- a/ROMFS/px4fmu_common/init.d/airframes/3032_skywalker_x5
+++ b/ROMFS/px4fmu_common/init.d/airframes/3032_skywalker_x5
@ -30,9 +30,6 @@ param set-default FW_L1_DAMPING 0.74
 param set-default FW_L1_PERIOD 16
 param set-default FW_LND_ANG 15
 param set-default FW_LND_FLALT 5
-param set-default FW_LND_HVIRT 13
-param set-default FW_LND_TLALT 5
-param set-default FW_THR_LND_MAX 0
 param set-default FW_PR_FF 0.35
 param set-default FW_RR_FF 0.6
 param set-default FW_RR_P 0.04
--- a/ROMFS/px4fmu_common/init.d/airframes/3033_wingwing
+++ b/ROMFS/px4fmu_common/init.d/airframes/3033_wingwing
@ -2,7 +2,7 @@
 #
 # @name Wing Wing (aka Z-84) Flying Wing
 #
-# @url https://docs.px4.io/master/en/frames_plane/wing_wing_z84.html
+# @url https://docs.px4.io/main/en/frames_plane/wing_wing_z84.html
 #
 # @type Flying Wing
 # @class Plane
@ -32,9 +32,6 @@ param set-default FW_L1_DAMPING 0.74
 param set-default FW_L1_PERIOD 16
 param set-default FW_LND_ANG 15
 param set-default FW_LND_FLALT 5
-param set-default FW_LND_HVIRT 13
-param set-default FW_LND_TLALT 5
-param set-default FW_THR_LND_MAX 0
 param set-default FW_PR_FF 0.35
 param set-default FW_RR_FF 0.6
 param set-default FW_RR_P 0.04
--- a/ROMFS/px4fmu_common/init.d/airframes/3100_tbs_caipirinha
+++ b/ROMFS/px4fmu_common/init.d/airframes/3100_tbs_caipirinha
@ -27,11 +27,7 @@ param set-default FW_AIRSPD_TRIM 16.5
 param set-default FW_L1_PERIOD 15
 param set-default FW_LND_ANG 15
 param set-default FW_LND_FLALT 8
-param set-default FW_LND_HVIRT 13
-param set-default FW_LND_TLALT 10
-param set-default FW_THR_LND_MAX 0
 param set-default FW_P_LIM_MAX 20
-param set-default FW_P_LIM_MIN -30
 param set-default FW_R_LIM 45
 param set-default FW_PR_FF 0.45
 param set-default FW_PR_P 0.005
--- a/ROMFS/px4fmu_common/init.d/airframes/4051_s250aq
+++ b/ROMFS/px4fmu_common/init.d/airframes/4051_s250aq
@ -1,7 +1,7 @@
 #!/bin/sh
 #
 # @name Spedix S250AQ
-# @url https://docs.px4.io/master/en/frames_multicopter/spedix_s250_pixracer.html
+# @url https://docs.px4.io/main/en/frames_multicopter/spedix_s250_pixracer.html
 #
 # @type Quadrotor asymmetric
 # @class Copter
--- a/ROMFS/px4fmu_common/init.d/airframes/4052_holybro_qav250
+++ b/ROMFS/px4fmu_common/init.d/airframes/4052_holybro_qav250
@ -2,7 +2,7 @@
 #
 # @name HolyBro QAV250
 #
-# @url https://docs.px4.io/master/en/frames_multicopter/holybro_qav250_pixhawk4_mini.html
+# @url https://docs.px4.io/main/en/frames_multicopter/holybro_qav250_pixhawk4_mini.html
 #
 # @type Quadrotor x
 # @class Copter
--- a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt
+++ b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt
@ -124,13 +124,13 @@ px4_add_romfs_files(
 	13007_vtol_AAVVT_quad
 	13008_QuadRanger
 	13009_vtol_spt_ranger
+	13100_generic_vtol_tiltrotor
 	13012_convergence
 	13013_deltaquad
 	13014_vtol_babyshark
 	13030_generic_vtol_quad_tiltrotor
 	13050_generic_vtol_octo
 	13200_generic_vtol_tailsitter
-	13201_swan_quad_tailsitter

 	# [14000, 14999] Tri Y
 	14001_tri_y_yaw+
--- a/ROMFS/px4fmu_common/init.d/rc.vtol_defaults
+++ b/ROMFS/px4fmu_common/init.d/rc.vtol_defaults
@ -21,16 +21,20 @@ param set-default HTE_VXY_THR 2.0
 param set-default MIS_DIST_WPS 5000

 param set-default MPC_ACC_HOR_MAX 2
-param set-default MPC_TKO_SPEED 1
-param set-default MPC_VEL_MANUAL 3
-param set-default MPC_XY_CRUISE 3
+param set-default MPC_VEL_MANUAL 5
+param set-default MPC_XY_CRUISE 5
 param set-default MPC_XY_ERR_MAX 5
-param set-default MPC_XY_VEL_MAX 4
-param set-default MPC_Z_VEL_MAX_DN 1.5
+param set-default MPC_XY_VEL_MAX 8
 param set-default MPC_JERK_MAX 4.5
 param set-default MPC_YAW_MODE 4

-param set-default NAV_ACC_RAD 3
+# reduce aggressiveness around roll and yaw axis,
+# as VTOLs usually have high intertia and lot af drag due to wings
+param set-default MC_ROLL_P 5
+param set-default MC_ROLLRATE_MAX 120
+param set-default MC_YAW_P 2
+param set-default MC_YAWRATE_MAX 120
+param set-default MPC_MAN_Y_MAX 90

 param set-default PWM_AUX_RATE 50
 param set-default PWM_MAIN_RATE 400
--- a/ROMFS/px4fmu_common/mixers/vtol_tailsitter_duo.main.mix
+++ b/ROMFS/px4fmu_common/mixers/vtol_tailsitter_duo.main.mix
@ -5,7 +5,7 @@ Tailsitter duo mixer
 This file defines a mixer for a generic duo tailsitter VTOL (eg TBS Caipirinha tailsitter edition). This vehicle
 has two motors in total, one attached to each wing. It also has two elevons which
 are located in the slipstream of the propellers. This mixer generates 4 PWM outputs
-on the main PWM ouput port, two at 400Hz for the motors, and two at 50Hz for the
+on the main PWM output port, two at 400Hz for the motors, and two at 50Hz for the
 elevon servos. Channels 1-4 are configured to run at 400Hz, while channels 5-8 run
 at the default rate of 50Hz. Note that channels 3 and 4 are assigned but not used.

--- a/ROMFS/px4fmu_common/mixers/vtol_tailsitter_duo_sat.main.mix
+++ b/ROMFS/px4fmu_common/mixers/vtol_tailsitter_duo_sat.main.mix
@ -4,7 +4,7 @@ Tailsitter duo mixer
 This file defines a mixer for a generic duo tailsitter VTOL (eg TBS Caipirinha tailsitter edition). This vehicle
 has two motors in total, one attached to each wing. It also has two elevons which
 are located in the slipstream of the propellers. This mixer generates 4 PWM outputs
-on the main PWM ouput port, two at 400Hz for the motors, and two at 50Hz for the
+on the main PWM output port, two at 400Hz for the motors, and two at 50Hz for the
 elevon servos. Channels 1-4 are configured to run at 400Hz, while channels 5-8 run
 at the default rate of 50Hz. Note that channels 3 and 4 are assigned but not used.

--- a/Tools/astyle/files_to_check_code_style.sh
+++ b/Tools/astyle/files_to_check_code_style.sh
@ -17,6 +17,7 @@ exec find boards msg src platforms test \
    -path src/lib/crypto/monocypher -prune -o \
    -path src/lib/events/libevents -prune -o \
    -path src/lib/parameters/uthash -prune -o \
+    -path src/lib/wind_estimator/python/generated -prune -o \
    -path src/modules/ekf2/EKF -prune -o \
    -path src/modules/gyro_fft/CMSIS_5 -prune -o \
    -path src/modules/mavlink/mavlink -prune -o \
--- a/Tools/mavlink_px4.py
+++ b/Tools/mavlink_px4.py
@ -3019,7 +3019,7 @@ class MAVLink(object):
                time_boot_ms              : Timestamp (milliseconds since system boot) (uint32_t)
                press_abs                 : Absolute pressure (hectopascal) (float)
                press_diff                : Differential pressure 1 (hectopascal) (float)
-                temperature               : Temperature measurement (0.01 degrees celsius) (int16_t)
+                temperature               : Temperature measurement (0.01 degrees Celsius) (int16_t)

                '''
                msg = MAVLink_scaled_pressure_message(time_boot_ms, press_abs, press_diff, temperature)
@ -3035,7 +3035,7 @@ class MAVLink(object):
                time_boot_ms              : Timestamp (milliseconds since system boot) (uint32_t)
                press_abs                 : Absolute pressure (hectopascal) (float)
                press_diff                : Differential pressure 1 (hectopascal) (float)
-                temperature               : Temperature measurement (0.01 degrees celsius) (int16_t)
+                temperature               : Temperature measurement (0.01 degrees Celsius) (int16_t)

                '''
                return self.send(self.scaled_pressure_encode(time_boot_ms, press_abs, press_diff, temperature))
@ -4879,7 +4879,7 @@ class MAVLink(object):
                abs_pressure              : Absolute pressure in millibar (float)
                diff_pressure             : Differential pressure in millibar (float)
                pressure_alt              : Altitude calculated from pressure (float)
-                temperature               : Temperature in degrees celsius (float)
+                temperature               : Temperature in degrees Celsius (float)
                fields_updated            : Bitmask for fields that have updated since last message, bit 0 = xacc, bit 12: temperature (uint16_t)

                '''
@ -4904,7 +4904,7 @@ class MAVLink(object):
                abs_pressure              : Absolute pressure in millibar (float)
                diff_pressure             : Differential pressure in millibar (float)
                pressure_alt              : Altitude calculated from pressure (float)
-                temperature               : Temperature in degrees celsius (float)
+                temperature               : Temperature in degrees Celsius (float)
                fields_updated            : Bitmask for fields that have updated since last message, bit 0 = xacc, bit 12: temperature (uint16_t)

                '''
--- a/Tools/module_config/generate_actuators_metadata.py
+++ b/Tools/module_config/generate_actuators_metadata.py
@ -330,7 +330,7 @@ def get_mixers(yaml_config, output_functions, verbose):
        option = select_param + '==' + str(type_index)
        mixer_config = {
                'option': option,
-                'help-url': 'https://docs.px4.io/master/en/config/actuators.html',
+                'help-url': 'https://docs.px4.io/main/en/config/actuators.html',
            }
        for optional in ['type', 'title']:
            if optional in current_type:
--- a/Tools/px4airframes/markdownout.py
+++ b/Tools/px4airframes/markdownout.py
@ -8,7 +8,7 @@ class MarkdownTablesOutput():
        result = """# Airframes Reference

 :::note
-**This list is [auto-generated](https://github.com/PX4/PX4-Autopilot/blob/master/Tools/px4airframes/markdownout.py) from the source code** using the build command: `make airframe_metadata`.
+**This list is [auto-generated](https://github.com/PX4/PX4-Autopilot/blob/main/Tools/px4airframes/markdownout.py) from the source code** using the build command: `make airframe_metadata`.
 :::

 This page lists all supported airframes and types including the motor assignment and numbering.
--- a/Tools/px4moduledoc/markdownout.py
+++ b/Tools/px4moduledoc/markdownout.py
@ -14,12 +14,14 @@ class MarkdownOutput():

        result = """
 # Modules & Commands Reference
-The following pages document the PX4 modules, drivers and commands. They
-describe the provided functionality, high-level implementation overview and how
+
+The following pages document the PX4 modules, drivers and commands.
+They describe the provided functionality, high-level implementation overview and how
 to use the command-line interface.

-> **Note** **This is auto-generated from the source code** and contains the
-> most recent modules documentation.
+:::note
+**This is auto-generated from the source code** and contains the most recent modules documentation.
+:::

 It is not a complete list and NuttX provides some additional commands
 as well (such as `free`). Use `help` on the console to get a list of all
@ -29,6 +31,7 @@ Since this is generated from source, errors must be reported/fixed
 in the [PX4-Autopilot](https://github.com/PX4/PX4-Autopilot) repository.
 The documentation pages can be generated by running the following command from
 the root of the PX4-Autopilot directory:
+
 ```
 make module_documentation
 ```
@ -66,7 +69,7 @@ The generated files will be written to the `modules` directory.
        result = ''
        for module in module_list:
            result += "## %s\n" % module.name()
-            result += "Source: [%s](https://github.com/PX4/PX4-Autopilot/tree/master/src/%s)\n\n" % (module.scope(), module.scope())
+            result += "Source: [%s](https://github.com/PX4/PX4-Autopilot/tree/main/src/%s)\n\n" % (module.scope(), module.scope())
            doc = module.documentation()
            if len(doc) > 0:
                result += "%s\n" % doc
--- a/Tools/px4moduledoc/srcparser.py
+++ b/Tools/px4moduledoc/srcparser.py
@ -12,7 +12,7 @@ class ModuleDocumentation(object):
    """

    # If you add categories or subcategories, they also need to be added to the
-    # TOC in https://github.com/PX4/Devguide/blob/master/en/SUMMARY.md
+    # TOC in https://github.com/PX4/PX4-user_guide/blob/main/en/SUMMARY.md
    valid_categories = ['driver', 'estimator', 'controller', 'system',
                        'communication', 'command', 'template', 'simulation', 'autotune']
    valid_subcategories = ['', 'distance_sensor', 'imu', 'airspeed_sensor',
--- a/Tools/px_uploader.py
+++ b/Tools/px_uploader.py
@ -331,7 +331,7 @@ class uploader(object):

        except NotImplementedError:
            raise RuntimeError("Programing not supported for this version of silicon!\n"
-                               "See https://docs.px4.io/master/en/flight_controller/silicon_errata.html")
+                               "See https://docs.px4.io/main/en/flight_controller/silicon_errata.html")
        except RuntimeError:
            # timeout, no response yet
            return False
--- a/Tools/setup/requirements.txt
+++ b/Tools/setup/requirements.txt
@ -26,3 +26,5 @@ requests
 setuptools>=39.2.0
 six>=1.12.0
 toml>=0.9
+symforce>=0.5.0
+sympy>=1.10.1
--- a/Tools/setup/ubuntu.sh
+++ b/Tools/setup/ubuntu.sh
@ -104,7 +104,7 @@ sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends i
 echo
 echo "Installing PX4 Python3 dependencies"
 if [ -n "$VIRTUAL_ENV" ]; then
-	# virtual envrionments don't allow --user option
+	# virtual environments don't allow --user option
 	python -m pip install -r ${DIR}/requirements.txt
 else
 	# older versions of Ubuntu require --user option
--- a/Tools/setup_ignition.bash
+++ b/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"
--- a/Tools/simulation-ignition
+++ b/Tools/simulation-ignition
@ -1 +0,0 @@
-Subproject commit 483193d9b8b89211c3b970c735b4fbb5f724b63a
--- a/Tools/simulation/gazebo/models/X3/meshes/propeller_ccw.dae
+++ b/Tools/simulation/gazebo/models/X3/meshes/propeller_ccw.dae
--- a/Tools/simulation/gazebo/models/X3/meshes/propeller_cw.dae
+++ b/Tools/simulation/gazebo/models/X3/meshes/propeller_cw.dae
--- a/Tools/simulation/gazebo/models/X3/meshes/x3.dae
+++ b/Tools/simulation/gazebo/models/X3/meshes/x3.dae
--- a/Tools/simulation/gazebo/models/X3/meshes/x3.jpg
+++ b/Tools/simulation/gazebo/models/X3/meshes/x3.jpg
--- a/Tools/simulation/gazebo/models/X3/model.config
+++ b/Tools/simulation/gazebo/models/X3/model.config
@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+
+<model>
+  <name>x3</name>
+  <version>1.0</version>
+  <sdf version="1.6">model.sdf</sdf>
+
+  <author>
+    <name>Carlos Aguero</name>
+    <email>caguero@openrobotics.org</email>
+  </author>
+
+  <author>
+    <name>Cole Biesemeyer</name>
+    <email>cole@openrobotics.org</email>
+  </author>
+
+  <description>
+  </description>
+</model>
--- a/Tools/simulation/gazebo/models/X3/model.sdf
+++ b/Tools/simulation/gazebo/models/X3/model.sdf
@ -0,0 +1,374 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<sdf version="1.6">
+   <model name="X3">
+      <pose>0 0 0.053302 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>1.5</mass>
+            <inertia>
+               <ixx>0.0347563</ixx>
+               <ixy>0</ixy>
+               <ixz>0</ixz>
+               <iyy>0.07</iyy>
+               <iyz>0</iyz>
+               <izz>0.0977</izz>
+            </inertia>
+         </inertial>
+         <collision name="base_link_inertia_collision">
+            <pose frame="">0 0 0 0 -0 0</pose>
+            <geometry>
+               <box>
+                  <size>0.47 0.47 0.11</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/x3.dae</uri>
+               </mesh>
+            </geometry>
+         </visual>
+         <sensor name="imu_sensor" type="imu">
+            <always_on>1</always_on>
+            <update_rate>250</update_rate>
+         </sensor>
+      </link>
+      <link name="rotor_0">
+         <pose frame="">0.13 -0.22 0.023 0 -0 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>4.17041e-05</iyy>
+               <iyz>0</iyz>
+               <izz>4.26041e-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.1</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>0.1 0.1 0.1</scale>
+                  <uri>meshes/propeller_ccw.dae</uri>
+               </mesh>
+            </geometry>
+            <material>
+               <script>
+                  <name>Gazebo/Blue</name>
+                  <uri>file://media/materials/scripts/gazebo.material</uri>
+               </script>
+            </material>
+         </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 0 1</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.13 0.2 0.023 0 -0 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>4.17041e-05</iyy>
+               <iyz>0</iyz>
+               <izz>4.26041e-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.1</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>0.1 0.1 0.1</scale>
+                  <uri>meshes/propeller_ccw.dae</uri>
+               </mesh>
+            </geometry>
+            <material>
+               <script>
+                  <name>Gazebo/Red</name>
+                  <uri>file://media/materials/scripts/gazebo.material</uri>
+               </script>
+            </material>
+         </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 0 1</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.13 0.22 0.023 0 -0 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>4.17041e-05</iyy>
+               <iyz>0</iyz>
+               <izz>4.26041e-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.1</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>0.1 0.1 0.1</scale>
+                  <uri>meshes/propeller_cw.dae</uri>
+               </mesh>
+            </geometry>
+            <material>
+               <script>
+                  <name>Gazebo/Blue</name>
+                  <uri>file://media/materials/scripts/gazebo.material</uri>
+               </script>
+            </material>
+         </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 0 1</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.13 -0.2 0.023 0 -0 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>4.17041e-05</iyy>
+               <iyz>0</iyz>
+               <izz>4.26041e-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.1</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>0.1 0.1 0.1</scale>
+                  <uri>meshes/propeller_cw.dae</uri>
+               </mesh>
+            </geometry>
+            <material>
+               <script>
+                  <name>Gazebo/Red</name>
+                  <uri>file://media/materials/scripts/gazebo.material</uri>
+               </script>
+            </material>
+         </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 0 1</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="ignition-gazebo-multicopter-motor-model-system" name="ignition::gazebo::systems::MulticopterMotorModel">
+         <robotNamespace>X3</robotNamespace>
+         <jointName>rotor_0_joint</jointName>
+         <linkName>rotor_0</linkName>
+         <turningDirection>ccw</turningDirection>
+         <timeConstantUp>0.0125</timeConstantUp>
+         <timeConstantDown>0.025</timeConstantDown>
+         <maxRotVelocity>800.0</maxRotVelocity>
+         <motorConstant>8.54858e-06</motorConstant>
+         <momentConstant>0.016</momentConstant>
+         <commandSubTopic>command/motor_speed</commandSubTopic>
+         <motorNumber>0</motorNumber>
+         <rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
+         <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
+         <motorSpeedPubTopic>motor_speed/0</motorSpeedPubTopic>
+         <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
+         <motorType>velocity</motorType>
+      </plugin>
+      <plugin filename="ignition-gazebo-multicopter-motor-model-system" name="ignition::gazebo::systems::MulticopterMotorModel">
+         <robotNamespace>X3</robotNamespace>
+         <jointName>rotor_1_joint</jointName>
+         <linkName>rotor_1</linkName>
+         <turningDirection>ccw</turningDirection>
+         <timeConstantUp>0.0125</timeConstantUp>
+         <timeConstantDown>0.025</timeConstantDown>
+         <maxRotVelocity>800.0</maxRotVelocity>
+         <motorConstant>8.54858e-06</motorConstant>
+         <momentConstant>0.016</momentConstant>
+         <commandSubTopic>command/motor_speed</commandSubTopic>
+         <motorNumber>1</motorNumber>
+         <rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
+         <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
+         <motorSpeedPubTopic>motor_speed/1</motorSpeedPubTopic>
+         <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
+         <motorType>velocity</motorType>
+      </plugin>
+      <plugin filename="ignition-gazebo-multicopter-motor-model-system" name="ignition::gazebo::systems::MulticopterMotorModel">
+         <robotNamespace>X3</robotNamespace>
+         <jointName>rotor_2_joint</jointName>
+         <linkName>rotor_2</linkName>
+         <turningDirection>cw</turningDirection>
+         <timeConstantUp>0.0125</timeConstantUp>
+         <timeConstantDown>0.025</timeConstantDown>
+         <maxRotVelocity>800.0</maxRotVelocity>
+         <motorConstant>8.54858e-06</motorConstant>
+         <momentConstant>0.016</momentConstant>
+         <commandSubTopic>command/motor_speed</commandSubTopic>
+         <motorNumber>2</motorNumber>
+         <rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
+         <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
+         <motorSpeedPubTopic>motor_speed/2</motorSpeedPubTopic>
+         <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
+         <motorType>velocity</motorType>
+      </plugin>
+      <plugin filename="ignition-gazebo-multicopter-motor-model-system" name="ignition::gazebo::systems::MulticopterMotorModel">
+         <robotNamespace>X3</robotNamespace>
+         <jointName>rotor_3_joint</jointName>
+         <linkName>rotor_3</linkName>
+         <turningDirection>cw</turningDirection>
+         <timeConstantUp>0.0125</timeConstantUp>
+         <timeConstantDown>0.025</timeConstantDown>
+         <maxRotVelocity>800.0</maxRotVelocity>
+         <motorConstant>8.54858e-06</motorConstant>
+         <momentConstant>0.016</momentConstant>
+         <commandSubTopic>command/motor_speed</commandSubTopic>
+         <motorNumber>3</motorNumber>
+         <rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
+         <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
+         <motorSpeedPubTopic>motor_speed/3</motorSpeedPubTopic>
+         <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
+         <motorType>velocity</motorType>
+      </plugin>
+   </model>
+</sdf>
--- a/Tools/simulation/gazebo/models/X4/materials/textures/led.jpg
+++ b/Tools/simulation/gazebo/models/X4/materials/textures/led.jpg
--- a/Tools/simulation/gazebo/models/X4/materials/textures/spotlight.jpg
+++ b/Tools/simulation/gazebo/models/X4/materials/textures/spotlight.jpg
--- a/Tools/simulation/gazebo/models/X4/meshes/led.dae
+++ b/Tools/simulation/gazebo/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>
--- a/Tools/simulation/gazebo/models/X4/meshes/led_lamp.dae
+++ b/Tools/simulation/gazebo/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>
--- a/Tools/simulation/gazebo/models/X4/meshes/neo11_propeller_ccw.dae
+++ b/Tools/simulation/gazebo/models/X4/meshes/neo11_propeller_ccw.dae
--- a/Tools/simulation/gazebo/models/X4/meshes/neo11_propeller_cw.dae
+++ b/Tools/simulation/gazebo/models/X4/meshes/neo11_propeller_cw.dae
--- a/Tools/simulation/gazebo/models/X4/meshes/pelican.dae
+++ b/Tools/simulation/gazebo/models/X4/meshes/pelican.dae
--- a/Tools/simulation/gazebo/models/X4/meshes/propeller_ccw.dae
+++ b/Tools/simulation/gazebo/models/X4/meshes/propeller_ccw.dae
--- a/Tools/simulation/gazebo/models/X4/meshes/propeller_cw.dae
+++ b/Tools/simulation/gazebo/models/X4/meshes/propeller_cw.dae
--- a/Tools/simulation/gazebo/models/X4/meshes/spotlight.dae
+++ b/Tools/simulation/gazebo/models/X4/meshes/spotlight.dae
--- a/Tools/simulation/gazebo/models/X4/meshes/vi_camera.dae
+++ b/Tools/simulation/gazebo/models/X4/meshes/vi_camera.dae
--- a/Tools/simulation/gazebo/models/X4/meshes/vi_camera.jpg
+++ b/Tools/simulation/gazebo/models/X4/meshes/vi_camera.jpg
--- a/Tools/simulation/gazebo/models/X4/meshes/vi_sensor.dae
+++ b/Tools/simulation/gazebo/models/X4/meshes/vi_sensor.dae
--- a/Tools/simulation/gazebo/models/X4/meshes/vi_sensor.jpg
+++ b/Tools/simulation/gazebo/models/X4/meshes/vi_sensor.jpg
--- a/Tools/simulation/gazebo/models/X4/meshes/x4.dae
+++ b/Tools/simulation/gazebo/models/X4/meshes/x4.dae
--- a/Tools/simulation/gazebo/models/X4/meshes/x4.jpg
+++ b/Tools/simulation/gazebo/models/X4/meshes/x4.jpg
--- a/Tools/simulation/gazebo/models/X4/model.config
+++ b/Tools/simulation/gazebo/models/X4/model.config
@ -0,0 +1,17 @@
+<?xml version="1.0"?>
+
+<model>
+  <name>X4_GPS_RGBD</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>
--- a/Tools/simulation/gazebo/models/X4/model.sdf
+++ b/Tools/simulation/gazebo/models/X4/model.sdf
@ -0,0 +1,827 @@
+<?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="ignition-gazebo-multicopter-motor-model-system" name="ignition::gazebo::systems::MulticopterMotorModel">
+         <robotNamespace>X4</robotNamespace>
+         <jointName>rotor_0_joint</jointName>
+         <linkName>rotor_0</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>2</motorNumber>
+         <rotorDragCoefficient>2.0673e-04</rotorDragCoefficient>
+         <rotorDragCoefficient>0</rotorDragCoefficient>
+         <rollingMomentCoefficient>0</rollingMomentCoefficient>
+         <motorSpeedPubTopic>motor_speed/2</motorSpeedPubTopic>
+         <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>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>1</motorNumber>
+         <rotorDragCoefficient>2.0673e-04</rotorDragCoefficient>
+         <rotorDragCoefficient>0</rotorDragCoefficient>
+         <rollingMomentCoefficient>0</rollingMomentCoefficient>
+         <motorSpeedPubTopic>motor_speed/1</motorSpeedPubTopic>
+         <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>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>
+         <motorSpeedPubTopic>motor_speed/5</motorSpeedPubTopic>
+         <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>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>3</motorNumber>
+         <rotorDragCoefficient>2.0673e-04</rotorDragCoefficient>
+         <rotorDragCoefficient>0</rotorDragCoefficient>
+         <rollingMomentCoefficient>0</rollingMomentCoefficient>
+         <motorSpeedPubTopic>motor_speed/3</motorSpeedPubTopic>
+         <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>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>0</motorNumber>
+         <rotorDragCoefficient>2.0673e-04</rotorDragCoefficient>
+         <rotorDragCoefficient>0</rotorDragCoefficient>
+         <rollingMomentCoefficient>0</rollingMomentCoefficient>
+         <motorSpeedPubTopic>motor_speed/0</motorSpeedPubTopic>
+         <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>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>
+         <motorSpeedPubTopic>motor_speed/4</motorSpeedPubTopic>
+         <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>
--- a/Tools/simulation/gazebo/models/X4/thumbnails/1.jpg
+++ b/Tools/simulation/gazebo/models/X4/thumbnails/1.jpg
--- a/Tools/simulation/gazebo/models/X4/thumbnails/2.jpg
+++ b/Tools/simulation/gazebo/models/X4/thumbnails/2.jpg
--- a/Tools/simulation/gazebo/models/X4/thumbnails/3.jpg
+++ b/Tools/simulation/gazebo/models/X4/thumbnails/3.jpg
--- a/Tools/simulation/gazebo/models/X4/thumbnails/4.jpg
+++ b/Tools/simulation/gazebo/models/X4/thumbnails/4.jpg
--- a/Tools/simulation/gazebo/models/X4/thumbnails/5.jpg
+++ b/Tools/simulation/gazebo/models/X4/thumbnails/5.jpg
--- a/Tools/simulation/gazebo/worlds/x3.sdf
+++ b/Tools/simulation/gazebo/worlds/x3.sdf
@ -0,0 +1,114 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<sdf version="1.6">
+   <world name="quadcopter">
+      <physics name="4ms" type="ode">
+         <max_step_size>0.004</max_step_size>
+         <real_time_factor>1.0</real_time_factor>
+      </physics>
+      <plugin filename="libignition-gazebo-physics-system.so" name="ignition::gazebo::systems::Physics" />
+      <plugin filename="libignition-gazebo-scene-broadcaster-system.so" name="ignition::gazebo::systems::SceneBroadcaster" />
+      <plugin filename="libignition-gazebo-user-commands-system.so" name="ignition::gazebo::systems::UserCommands" />
+      <plugin filename="libignition-gazebo-sensors-system.so" name="ignition::gazebo::systems::Sensors">
+         <render_engine>ogre2</render_engine>
+      </plugin>
+      <plugin filename="libignition-gazebo-imu-system.so" name="ignition::gazebo::systems::Imu" />
+      <gui fullscreen="0">
+         <!-- 3D scene -->
+         <plugin filename="GzScene3D" name="3D View">
+            <ignition-gui>
+               <title>3D View</title>
+               <property type="bool" key="showTitleBar">false</property>
+               <property type="string" key="state">docked</property>
+            </ignition-gui>
+            <engine>ogre2</engine>
+            <scene>scene</scene>
+            <ambient_light>0.4 0.4 0.4</ambient_light>
+            <background_color>0.8 0.8 0.8</background_color>
+            <camera_pose>-6 0 6 0 0.5 0</camera_pose>
+         </plugin>
+         <!-- World control -->
+         <plugin filename="WorldControl" name="World control">
+            <ignition-gui>
+               <title>World control</title>
+               <property type="bool" key="showTitleBar">false</property>
+               <property type="bool" key="resizable">false</property>
+               <property type="double" key="height">72</property>
+               <property type="double" key="width">121</property>
+               <property type="double" key="z">1</property>
+               <property type="string" key="state">floating</property>
+               <anchors target="3D View">
+                  <line own="left" target="left" />
+                  <line own="bottom" target="bottom" />
+               </anchors>
+            </ignition-gui>
+            <play_pause>true</play_pause>
+            <step>true</step>
+            <start_paused>true</start_paused>
+            <service>/world/quadcopter/control</service>
+            <stats_topic>/world/quadcopter/stats</stats_topic>
+         </plugin>
+         <!-- World statistics -->
+         <plugin filename="WorldStats" name="World stats">
+            <ignition-gui>
+               <title>World stats</title>
+               <property type="bool" key="showTitleBar">false</property>
+               <property type="bool" key="resizable">false</property>
+               <property type="double" key="height">110</property>
+               <property type="double" key="width">290</property>
+               <property type="double" key="z">1</property>
+               <property type="string" key="state">floating</property>
+               <anchors target="3D View">
+                  <line own="right" target="right" />
+                  <line own="bottom" target="bottom" />
+               </anchors>
+            </ignition-gui>
+            <sim_time>true</sim_time>
+            <real_time>true</real_time>
+            <real_time_factor>true</real_time_factor>
+            <iterations>true</iterations>
+            <topic>/world/quadcopter/stats</topic>
+         </plugin>
+      </gui>
+      <light type="directional" name="sun">
+         <cast_shadows>true</cast_shadows>
+         <pose>0 0 10 0 0 0</pose>
+         <diffuse>0.8 0.8 0.8 1</diffuse>
+         <specular>0.2 0.2 0.2 1</specular>
+         <attenuation>
+            <range>1000</range>
+            <constant>0.9</constant>
+            <linear>0.01</linear>
+            <quadratic>0.001</quadratic>
+         </attenuation>
+         <direction>-0.5 0.1 -0.9</direction>
+      </light>
+      <model name="ground_plane">
+         <static>true</static>
+         <link name="link">
+            <collision name="collision">
+               <geometry>
+                  <plane>
+                     <normal>0 0 1</normal>
+                  </plane>
+               </geometry>
+            </collision>
+            <visual name="visual">
+               <geometry>
+                  <plane>
+                     <normal>0 0 1</normal>
+                     <size>100 100</size>
+                  </plane>
+               </geometry>
+               <material>
+                  <ambient>0.8 0.8 0.8 1</ambient>
+                  <diffuse>0.8 0.8 0.8 1</diffuse>
+                  <specular>0.8 0.8 0.8 1</specular>
+               </material>
+            </visual>
+         </link>
+      </model>
+      <include>
+         <uri>model://X3</uri>
+      </include>
+   </world>
+</sdf>
--- a/Tools/simulation/gazebo/worlds/x4.sdf
+++ b/Tools/simulation/gazebo/worlds/x4.sdf
@ -0,0 +1,114 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<sdf version="1.6">
+   <world name="quadcopter">
+      <physics name="4ms" type="ode">
+         <max_step_size>0.004</max_step_size>
+         <real_time_factor>1.0</real_time_factor>
+      </physics>
+      <plugin filename="libignition-gazebo-physics-system.so" name="ignition::gazebo::systems::Physics" />
+      <plugin filename="libignition-gazebo-scene-broadcaster-system.so" name="ignition::gazebo::systems::SceneBroadcaster" />
+      <plugin filename="libignition-gazebo-user-commands-system.so" name="ignition::gazebo::systems::UserCommands" />
+      <plugin filename="libignition-gazebo-sensors-system.so" name="ignition::gazebo::systems::Sensors">
+         <render_engine>ogre2</render_engine>
+      </plugin>
+      <plugin filename="libignition-gazebo-imu-system.so" name="ignition::gazebo::systems::Imu" />
+      <gui fullscreen="0">
+         <!-- 3D scene -->
+         <plugin filename="GzScene3D" name="3D View">
+            <ignition-gui>
+               <title>3D View</title>
+               <property type="bool" key="showTitleBar">false</property>
+               <property type="string" key="state">docked</property>
+            </ignition-gui>
+            <engine>ogre2</engine>
+            <scene>scene</scene>
+            <ambient_light>0.4 0.4 0.4</ambient_light>
+            <background_color>0.8 0.8 0.8</background_color>
+            <camera_pose>-6 0 6 0 0.5 0</camera_pose>
+         </plugin>
+         <!-- World control -->
+         <plugin filename="WorldControl" name="World control">
+            <ignition-gui>
+               <title>World control</title>
+               <property type="bool" key="showTitleBar">false</property>
+               <property type="bool" key="resizable">false</property>
+               <property type="double" key="height">72</property>
+               <property type="double" key="width">121</property>
+               <property type="double" key="z">1</property>
+               <property type="string" key="state">floating</property>
+               <anchors target="3D View">
+                  <line own="left" target="left" />
+                  <line own="bottom" target="bottom" />
+               </anchors>
+            </ignition-gui>
+            <play_pause>true</play_pause>
+            <step>true</step>
+            <start_paused>true</start_paused>
+            <service>/world/quadcopter/control</service>
+            <stats_topic>/world/quadcopter/stats</stats_topic>
+         </plugin>
+         <!-- World statistics -->
+         <plugin filename="WorldStats" name="World stats">
+            <ignition-gui>
+               <title>World stats</title>
+               <property type="bool" key="showTitleBar">false</property>
+               <property type="bool" key="resizable">false</property>
+               <property type="double" key="height">110</property>
+               <property type="double" key="width">290</property>
+               <property type="double" key="z">1</property>
+               <property type="string" key="state">floating</property>
+               <anchors target="3D View">
+                  <line own="right" target="right" />
+                  <line own="bottom" target="bottom" />
+               </anchors>
+            </ignition-gui>
+            <sim_time>true</sim_time>
+            <real_time>true</real_time>
+            <real_time_factor>true</real_time_factor>
+            <iterations>true</iterations>
+            <topic>/world/quadcopter/stats</topic>
+         </plugin>
+      </gui>
+      <light type="directional" name="sun">
+         <cast_shadows>true</cast_shadows>
+         <pose>0 0 10 0 0 0</pose>
+         <diffuse>0.8 0.8 0.8 1</diffuse>
+         <specular>0.2 0.2 0.2 1</specular>
+         <attenuation>
+            <range>1000</range>
+            <constant>0.9</constant>
+            <linear>0.01</linear>
+            <quadratic>0.001</quadratic>
+         </attenuation>
+         <direction>-0.5 0.1 -0.9</direction>
+      </light>
+      <model name="ground_plane">
+         <static>true</static>
+         <link name="link">
+            <collision name="collision">
+               <geometry>
+                  <plane>
+                     <normal>0 0 1</normal>
+                  </plane>
+               </geometry>
+            </collision>
+            <visual name="visual">
+               <geometry>
+                  <plane>
+                     <normal>0 0 1</normal>
+                     <size>100 100</size>
+                  </plane>
+               </geometry>
+               <material>
+                  <ambient>0.8 0.8 0.8 1</ambient>
+                  <diffuse>0.8 0.8 0.8 1</diffuse>
+                  <specular>0.8 0.8 0.8 1</specular>
+               </material>
+            </visual>
+         </link>
+      </model>
+      <include>
+         <uri>model://X4</uri>
+      </include>
+   </world>
+</sdf>
--- a/Tools/sitl_gazebo
+++ b/Tools/sitl_gazebo
@ -1 +1 @@
-Subproject commit 5610c3fb441a2f3babc8ad7a63c8c4ce3e40abfa
+Subproject commit 1262a58dfdd0df2075d816b1cb4ced4f8f8c6cf6
--- a/Tools/sitl_run.sh
+++ b/Tools/sitl_run.sh
@ -194,8 +194,12 @@ elif [ "$program" == "ignition" ] && [ -z "$no_sim" ]; then
 	else
 		ignition_headless=""
 	fi
-	source "$src_path/Tools/setup_ignition.bash" "${src_path}" "${build_path}"
-	ign gazebo --force-version 5 ${verbose} ${ignition_headless} -r "${src_path}/Tools/simulation-ignition/worlds/${model}.world"&
+
+	export IGN_GAZEBO_RESOURCE_PATH=$IGN_GAZEBO_RESOURCE_PATH:${src_path}/Tools/simulation/gazebo/models
+	#ign gazebo --force-version 5 ${verbose} ${ignition_headless} -r "${src_path}/Tools/simulation/gazebo/worlds/${model}.world"&
+
+	ign gazebo --distributed --network-role=primary --network-secondaries=1 -r "${src_path}/Tools/simulation/gazebo/worlds/${model}.sdf" &
+
 elif [ "$program" == "flightgear" ] && [ -z "$no_sim" ]; then
 	echo "FG setup"
 	cd "${src_path}/Tools/flightgear_bridge/"
@ -231,9 +235,9 @@ pushd "$rootfs" >/dev/null
 set +e

 if [[ ${model} == test_* ]] || [[ ${model} == *_generated ]]; then
-	sitl_command="\"$sitl_bin\" $no_pxh \"$src_path\"/ROMFS/px4fmu_test -s \"${src_path}\"/posix-configs/SITL/init/test/${model} -t \"$src_path\"/test_data"
+	sitl_command="\"$sitl_bin\" $no_pxh \"$src_path\"/ROMFS/px4fmu_test -s \"${src_path}\"/posix-configs/SITL/init/test/${model}"
 else
-	sitl_command="\"$sitl_bin\" $no_pxh \"$build_path\"/etc -s etc/init.d-posix/rcS -t \"$src_path\"/test_data"
+	sitl_command="\"$sitl_bin\" $no_pxh \"$build_path\"/etc -s etc/init.d-posix/rcS"
 fi

 echo SITL COMMAND: $sitl_command
--- a/Tools/uorb_graph/create.py
+++ b/Tools/uorb_graph/create.py
@ -694,7 +694,7 @@ class OutputJSON(object):
            node['type'] = 'topic'
            node['color'] = topic_colors[topic]
            # url is opened when double-clicking on the node
-            node['url'] = 'https://github.com/PX4/PX4-Autopilot/blob/master/msg/'+topic_filename(topic)+'.msg'
+            node['url'] = 'https://github.com/PX4/PX4-Autopilot/blob/main/msg/'+topic_filename(topic)+'.msg'
            nodes.append(node)

        data['nodes'] = nodes
--- a/boards/av/x-v1/src/CMakeLists.txt
+++ b/boards/av/x-v1/src/CMakeLists.txt
@ -41,6 +41,7 @@ add_library(drivers_board

 target_link_libraries(drivers_board
 	PRIVATE
+		arch_io_pins
 		arch_spi
 		drivers__led # drv_led_start
 		nuttx_arch # sdio
--- a/boards/cuav/nora/src/CMakeLists.txt
+++ b/boards/cuav/nora/src/CMakeLists.txt
@ -56,6 +56,7 @@ else()

 	target_link_libraries(drivers_board
 		PRIVATE
+			arch_io_pins
 			arch_spi
 			arch_board_hw_info
 			drivers__led
--- a/boards/cuav/x7pro/src/CMakeLists.txt
+++ b/boards/cuav/x7pro/src/CMakeLists.txt
@ -56,6 +56,7 @@ else()

 	target_link_libraries(drivers_board
 		PRIVATE
+			arch_io_pins
 			arch_spi
 			arch_board_hw_info
 			drivers__led
--- a/Show More
+++ b/Show More
				`@ -1 +0,0 @@`
				`Subproject commit 483193d9b8b89211c3b970c735b4fbb5f724b63a`