Fix heightrate feedforard for fixdwings

cubeorangeplus: add check for SMPS support
If NuttX is built without support for SMPS it can brick the hardware. Therefore, I suggest that we add this additional compile-time check. Signed-off-by: Julian Oes <julian@oes.ch>
2026-05-27 03:10:06 +08:00 · 2023-03-27 13:13:24 +02:00 · 2023-03-13 22:54:41 -04:00 · 2023-03-13 22:53:56 -04:00 · 2023-03-13 22:53:56 -04:00 · 2023-03-13 22:53:56 -04:00
722 changed files with 65220 additions and 14563 deletions
@@ -9,10 +9,10 @@ pipeline {
        script {
          def build_nodes = [:]
          def docker_images = [
-            armhf: "px4io/px4-dev-armhf:2021-09-08",
-            arm64: "px4io/px4-dev-aarch64:2021-09-08",
-            base: "px4io/px4-dev-base-bionic:2021-09-08",
-            nuttx: "px4io/px4-dev-nuttx-focal:2021-09-08",
+            armhf: "px4io/px4-dev-armhf:2022-08-12",
+            arm64: "px4io/px4-dev-aarch64:2022-08-12",
+            base: "px4io/px4-dev-ros2-foxy:2022-08-12",
+            nuttx: "px4io/px4-dev-nuttx-focal:2022-08-12",
          ]

          def armhf_builds = [
@@ -62,6 +62,8 @@ pipeline {
                     "holybro_durandal-v1_default",
                     "holybro_kakutef7_default",
                     "holybro_kakuteh7_default",
+                     "holybro_kakuteh7v2_default",
+                     "holybro_kakuteh7mini_default",
                     "holybro_pix32v5_default",
                     "matek_gnss-m9n-f4_canbootloader",
                     "matek_gnss-m9n-f4_default",
@@ -108,6 +110,8 @@ pipeline {
                     "raspberrypi_pico_default",
                     "sky-drones_smartap-airlink_default",
                     "spracing_h7extreme_default",
+                     "thepeach_k1_default",
+                     "thepeach_r1_default",
                     "uvify_core_default"
            ],
            image: docker_images.nuttx,
@@ -30,6 +30,7 @@ jobs:
          cuav_nora,
          cuav_x7pro,
          cubepilot_cubeorange,
+          cubepilot_cubeorangeplus,
          cubepilot_cubeyellow,
          diatone_mamba-f405-mk2,
          freefly_can-rtk-gps,
@@ -180,7 +180,7 @@ CONFIG:
      short: cubepilot_cubeorange
      buildType: MinSizeRel
      settings:
-        CONFIG: cubepilot_orange_test
+        CONFIG: cubepilot_cubeorange_test
    emlid_navio2_default:
      short: emlid_navio2
      buildType: MinSizeRel
@@ -2,7 +2,6 @@
        "astyle.astylerc": "${workspaceFolder}/Tools/astyle/astylerc",
        "astyle.c.enable": true,
        "astyle.cpp.enable": true,
-        "breadcrumbs.enabled": true,
        "C_Cpp.autoAddFileAssociations": false,
        "C_Cpp.clang_format_fallbackStyle": "none",
        "C_Cpp.default.browse.limitSymbolsToIncludedHeaders": true,
@@ -20,7 +19,6 @@
        "cmakeExplorer.buildDir": "${workspaceFolder}/build/px4_sitl_test",
        "cmakeExplorer.parallelJobs": 1,
        "cmakeExplorer.suiteDelimiter": "-",
-        "cortex-debug.enableTelemetry": false,
        "cSpell.allowCompoundWords": true,
        "cSpell.diagnosticLevel": "Hint",
        "cSpell.showStatus": false,
@@ -31,7 +29,6 @@
        ],
        "debug.toolBarLocation": "docked",
        "editor.defaultFormatter": "chiehyu.vscode-astyle",
-        "editor.dragAndDrop": false,
        "editor.insertSpaces": false,
        "editor.minimap.maxColumn": 120,
        "editor.minimap.renderCharacters": false,
@@ -127,12 +124,7 @@
                "${workspaceFolder}/build": true
        },
        "search.showLineNumbers": true,
-        "telemetry.enableTelemetry": false,
-        "terminal.integrated.scrollback": 5000,
-        "window.title": "${dirty} ${activeEditorMedium}${separator}${rootName}",
-        "workbench.editor.highlightModifiedTabs": true,
-        "workbench.enableExperiments": false,
-        "workbench.settings.enableNaturalLanguageSearch": false,
+        "terminal.integrated.scrollback": 15000,
        "yaml.schemas": {
                "${workspaceFolder}/validation/module_schema.yaml": "${workspaceFolder}/src/modules/*/module.yaml"
        }
@@ -170,7 +170,7 @@
            ]
        },
        {
-            "label": "ign gazebo",
+            "label": "gazebo",
            "type": "shell",
            "options": {
                "cwd": "${workspaceFolder}",
@@ -178,7 +178,7 @@
                    "IGN_GAZEBO_RESOURCE_PATH": "${workspaceFolder}/Tools/simulation/gz/models",
                }
            },
-            "command": "ign gazebo -v 4 -r ${workspaceFolder}/Tools/simulation/gz/worlds/${input:gzWorld}.sdf",
+            "command": "gz sim -v 4 -r ${workspaceFolder}/Tools/simulation/gz/worlds/${input:gzWorld}.sdf",
            "isBackground": true,
            "presentation": {
                "echo": true,
@@ -191,7 +191,7 @@
                "close": false
            },
            "problemMatcher": [],
-            "dependsOn":["ign gazebo kill"]
+            "dependsOn":["gazebo kill"]
        },
        {
            "label": "gazebo-classic kill",
@@ -211,9 +211,9 @@
            "dependsOn":["px4_sitl_cleanup"]
        },
        {
-            "label": "ign gazebo kill",
+            "label": "gazebo kill",
            "type": "shell",
-            "command": "pkill -9 -f 'ign gazebo' || true",
+            "command": "pkill -9 -f 'gz sim' || true",
            "presentation": {
                "echo": true,
                "reveal": "never",
@@ -118,7 +118,21 @@ execute_process(
 	OUTPUT_STRIP_TRAILING_WHITESPACE
 	WORKING_DIRECTORY ${PX4_SOURCE_DIR}
 	)
-message(STATUS "PX4 version: ${PX4_GIT_TAG}")
+
+# git describe to X.Y.Z version
+string(REPLACE "." ";" VERSION_LIST ${PX4_GIT_TAG})
+
+# major version
+list(GET VERSION_LIST 0 PX4_VERSION_MAJOR)
+string(REPLACE "v" "" PX4_VERSION_MAJOR ${PX4_VERSION_MAJOR})
+# minor version
+list(GET VERSION_LIST 1 PX4_VERSION_MINOR)
+# patch version
+list(GET VERSION_LIST 2 PX4_VERSION_PATCH)
+string(REPLACE "-" ";" PX4_VERSION_PATCH ${PX4_VERSION_PATCH})
+list(GET PX4_VERSION_PATCH 0 PX4_VERSION_PATCH)
+
+message(STATUS "PX4 version: ${PX4_GIT_TAG} (${PX4_VERSION_MAJOR}.${PX4_VERSION_MINOR}.${PX4_VERSION_PATCH})")

 define_property(GLOBAL PROPERTY PX4_MODULE_LIBRARIES
                 BRIEF_DOCS "PX4 module libs"
@@ -456,6 +470,9 @@ add_custom_command(OUTPUT ${uorb_graph_config}
 add_custom_target(uorb_graph DEPENDS ${uorb_graph_config})


+include(bloaty)
+
+
 include(doxygen)
 include(metadata)
 include(package)
@@ -73,6 +73,13 @@ menu "Toolchain"
        help
            relative path to the ROMFS root directory

+    config BOARD_ROOTFSDIR
+        string "Root directory"
+        depends on PLATFORM_POSIX
+        default "."
+        help
+            Configure the root directory in the file system for PX4 files
+
    config BOARD_IO
        string "IO board name"
        default "px4_io-v2_default"
@@ -1,6 +1,6 @@
 BSD 3-Clause License

-Copyright (c) 2012 - 2022, PX4 Development Team
+Copyright (c) 2012 - 2023, PX4 Development Team
 All rights reserved.

 Redistribution and use in source and binary forms, with or without
@@ -44,81 +44,88 @@ The PX4 Dev Team syncs up on a [weekly dev call](https://docs.px4.io/main/en/con

 ## Maintenance Team

-  * Project: Founder
-    * [Lorenz Meier](https://github.com/LorenzMeier)
-  * Architecture
-    * [Daniel Agar](https://github.com/dagar)
-  * [Dev Call](https://github.com/PX4/PX4-Autopilot/labels/devcall)
-    * [Ramon Roche](https://github.com/mrpollo)
-  * Communication Architecture
-    * [Beat Kueng](https://github.com/bkueng)
-    * [Julian Oes](https://github.com/JulianOes)
-  * UI in QGroundControl
-    * [Gus Grubba](https://github.com/dogmaphobic)
-  * [Multicopter Flight Control](https://github.com/PX4/PX4-Autopilot/labels/multicopter)
-    * [Mathieu Bresciani](https://github.com/bresch)
-  * [Multicopter Software Architecture](https://github.com/PX4/PX4-Autopilot/labels/multicopter)
-    * [Matthias Grob](https://github.com/MaEtUgR)
-  * [VTOL Flight Control](https://github.com/PX4/PX4-Autopilot/labels/vtol)
-    * [Roman Bapst](https://github.com/RomanBapst)
-  * [Fixed Wing Flight Control](https://github.com/PX4/PX4-Autopilot/labels/fixedwing)
-    * [Roman Bapst](https://github.com/RomanBapst)
-  * OS / NuttX
-    * [David Sidrane](https://github.com/davids5)
-  * Driver Architecture
-    * [Daniel Agar](https://github.com/dagar)
-  * Commander Architecture
-    * [Julian Oes](https://github.com/julianoes)
-  * [UAVCAN](https://github.com/PX4/PX4-Autopilot/labels/uavcan)
-    * [Daniel Agar](https://github.com/dagar)
-  * [State Estimation](https://github.com/PX4/PX4-Autopilot/issues?q=is%3Aopen+is%3Aissue+label%3A%22state+estimation%22)
-    * [Paul Riseborough](https://github.com/priseborough)
-  * Vision based navigation and Obstacle Avoidance
-    * [Markus Achtelik](https://github.com/markusachtelik)
-  * DDS/ROS2 Interface
-    * [Nuno Marques](https://github.com/TSC21)
+Note: This is the source of truth for the active maintainers of PX4 ecosystem.

-See also [maintainers list](https://px4.io/community/maintainers/) (px4.io) and the [contributors list](https://github.com/PX4/PX4-Autopilot/graphs/contributors) (Github).
+| Sector | Maintainer |
+|---|---|
+| Founder | [Lorenz Meier](https://github.com/LorenzMeier) |
+| Architecture | [Daniel Agar](https://github.com/dagar) / [Beat Küng](https://github.com/bkueng)|
+| State Estimation | [Mathieu Bresciani](https://github.com/bresch) / [Paul Riseborough](https://github.com/priseborough) |
+| OS/NuttX | [David Sidrane](https://github.com/davids5) |
+| Drivers | [Daniel Agar](https://github.com/dagar) |
+| Simulation | [Jaeyoung Lim](https://github.com/Jaeyoung-Lim) |
+| ROS2 | [Beniamino Pozzan](https://github.com/beniaminopozzan) |
+| Community QnA Call | [Ramon Roche](https://github.com/mrpollo) |
+| [Documentation](https://docs.px4.io/main/en/) | [Hamish Willee](https://github.com/hamishwillee) |
+
+| Vehicle Type | Maintainer |
+|---|---|
+| Multirotor | [Matthias Grob](https://github.com/MaEtUgR) |
+| Fixed Wing | [Thomas Stastny](https://github.com/tstastny) |
+| Hybrid VTOL | [Silvan Fuhrer](https://github.com/sfuhrer) |
+| Boat | x |
+| Rover | x |
+
+See also [maintainers list](https://px4.io/community/maintainers/) (px4.io) and the [contributors list](https://github.com/PX4/PX4-Autopilot/graphs/contributors) (Github). However it may be not up to date.

 ## Supported Hardware

-This repository contains code supporting Pixhawk standard boards (best supported, best tested, recommended choice) and proprietary boards.
+Pixhawk standard boards and proprietary boards are shown below (discontinued boards aren't listed).
+
+For the most up to date information, please visit [PX4 user Guide > Autopilot Hardware](https://docs.px4.io/main/en/flight_controller/).

 ### Pixhawk Standard Boards
-  * FMUv6X and FMUv6U (STM32H7, 2021)
-    * Various vendors will provide FMUv6X and FMUv6U based designs Q3/2021
-  * FMUv5 and FMUv5X (STM32F7, 2019/20)
-    * [Pixhawk 4 (FMUv5)](https://docs.px4.io/main/en/flight_controller/pixhawk4.html)
-    * [Pixhawk 4 mini (FMUv5)](https://docs.px4.io/main/en/flight_controller/pixhawk4_mini.html)
-    * [CUAV V5+ (FMUv5)](https://docs.px4.io/main/en/flight_controller/cuav_v5_plus.html)
-    * [CUAV V5 nano (FMUv5)](https://docs.px4.io/main/en/flight_controller/cuav_v5_nano.html)
-    * [Auterion Skynode (FMUv5X)](https://docs.auterion.com/skynode)
-  * FMUv4 (STM32F4, 2015)
-    * [Pixracer](https://docs.px4.io/main/en/flight_controller/pixracer.html)
-    * [Pixhawk 3 Pro](https://docs.px4.io/main/en/flight_controller/pixhawk3_pro.html)
-  * FMUv3 (STM32F4, 2014)
-    * [Pixhawk 2](https://docs.px4.io/main/en/flight_controller/pixhawk-2.html)
-    * [Pixhawk Mini](https://docs.px4.io/main/en/flight_controller/pixhawk_mini.html)
-    * [CUAV Pixhack v3](https://docs.px4.io/main/en/flight_controller/pixhack_v3.html)
-  * FMUv2 (STM32F4, 2013)
-    * [Pixhawk](https://docs.px4.io/main/en/flight_controller/pixhawk.html)
-    * [Pixfalcon](https://docs.px4.io/main/en/flight_controller/pixfalcon.html)

-### Manufacturer and Community supported
-  * [Holybro Durandal](https://docs.px4.io/main/en/flight_controller/durandal.html)
-  * [Hex Cube Orange](https://docs.px4.io/main/en/flight_controller/cubepilot_cube_orange.html)
-  * [Hex Cube Yellow](https://docs.px4.io/main/en/flight_controller/cubepilot_cube_yellow.html)
-  * [Airmind MindPX V2.8](http://www.mindpx.net/assets/accessories/UserGuide_MindPX.pdf)
-  * [Airmind MindRacer V1.2](http://mindpx.net/assets/accessories/mindracer_user_guide_v1.2.pdf)
-  * [Bitcraze Crazyflie 2.0](https://docs.px4.io/main/en/complete_vehicles/crazyflie2.html)
-  * [Omnibus F4 SD](https://docs.px4.io/main/en/flight_controller/omnibus_f4_sd.html)
-  * [Holybro Kakute F7](https://docs.px4.io/main/en/flight_controller/kakutef7.html)
-  * [Raspberry PI with Navio 2](https://docs.px4.io/main/en/flight_controller/raspberry_pi_navio2.html)
+These boards fully comply with Pixhawk Standard, and are maintained by the PX4-Autopilot maintainers and Dronecode team

-Additional information about supported hardware can be found in [PX4 user Guide > Autopilot Hardware](https://docs.px4.io/main/en/flight_controller/).
+* FMUv6X and FMUv6C
+  * [CUAV Pixahwk V6X (FMUv6X)](https://docs.px4.io/main/en/flight_controller/cuav_pixhawk_v6x.html)
+  * [Holybro Pixhawk 6X (FMUv6X)](https://docs.px4.io/main/en/flight_controller/pixhawk6x.html)
+  * [Holybro Pixhawk 6C (FMUv6C)](https://docs.px4.io/main/en/flight_controller/pixhawk6c.html)
+  * [Holybro Pix32 v6 (FMUv6C)](https://docs.px4.io/main/en/flight_controller/holybro_pix32_v6.html)
+* FMUv5 and FMUv5X (STM32F7, 2019/20)
+  * [Pixhawk 4 (FMUv5)](https://docs.px4.io/main/en/flight_controller/pixhawk4.html)
+  * [Pixhawk 4 mini (FMUv5)](https://docs.px4.io/main/en/flight_controller/pixhawk4_mini.html)
+  * [CUAV V5+ (FMUv5)](https://docs.px4.io/main/en/flight_controller/cuav_v5_plus.html)
+  * [CUAV V5 nano (FMUv5)](https://docs.px4.io/main/en/flight_controller/cuav_v5_nano.html)
+  * [Auterion Skynode (FMUv5X)](https://docs.auterion.com/avionics/skynode)
+* FMUv4 (STM32F4, 2015)
+  * [Pixracer](https://docs.px4.io/main/en/flight_controller/pixracer.html)
+  * [Pixhawk 3 Pro](https://docs.px4.io/main/en/flight_controller/pixhawk3_pro.html)
+* FMUv3 (STM32F4, 2014)
+  * [Pixhawk 2](https://docs.px4.io/main/en/flight_controller/pixhawk-2.html)
+  * [Pixhawk Mini](https://docs.px4.io/main/en/flight_controller/pixhawk_mini.html)
+  * [CUAV Pixhack v3](https://docs.px4.io/main/en/flight_controller/pixhack_v3.html)
+* FMUv2 (STM32F4, 2013)
+  * [Pixhawk](https://docs.px4.io/main/en/flight_controller/pixhawk.html)
+
+### Manufacturer supported
+
+These boards are maintained to be compatible with PX4-Autopilot by the Manufacturers.
+
+* [ARK Electronics ARKV6X](https://docs.px4.io/main/en/flight_controller/arkv6x.html)
+* [Hex Cube Orange](https://docs.px4.io/main/en/flight_controller/cubepilot_cube_orange.html)
+* [Hex Cube Yellow](https://docs.px4.io/main/en/flight_controller/cubepilot_cube_yellow.html)
+* [Holybro Durandal](https://docs.px4.io/main/en/flight_controller/durandal.html)
+* [Airmind MindPX V2.8](http://www.mindpx.net/assets/accessories/UserGuide_MindPX.pdf)
+* [Airmind MindRacer V1.2](http://mindpx.net/assets/accessories/mindracer_user_guide_v1.2.pdf)
+* [Holybro Kakute F7](https://docs.px4.io/main/en/flight_controller/kakutef7.html)
+
+### Community supported
+
+These boards don't fully comply industry standards, and thus is solely maintained by the PX4 publc community members.
+
+### Experimental
+
+These boards are nor maintained by PX4 team nor Manufacturer, and is not guaranteed to be compatible with up to date PX4 releases.
+
+* [Raspberry PI with Navio 2](https://docs.px4.io/main/en/flight_controller/raspberry_pi_navio2.html)
+* [Bitcraze Crazyflie 2.0](https://docs.px4.io/main/en/complete_vehicles/crazyflie2.html)

 ## Project Roadmap

+**Note: Outdated**
+
 A high level project roadmap is available [here](https://github.com/orgs/PX4/projects/25).

 ## Project Governance
@@ -12,6 +12,10 @@
 PX4_SIMULATOR=${PX4_SIMULATOR:=sihsim}
 PX4_SIM_MODEL=${PX4_SIM_MODEL:=quadx}

+param set-default SENS_EN_GPSSIM 1
+param set-default SENS_EN_BAROSIM 1
+param set-default SENS_EN_MAGSIM 1
+
 # disable some checks to allow to fly:
 # - with usb
 param set-default CBRK_USB_CHK 197848
@@ -11,6 +11,11 @@
 PX4_SIMULATOR=${PX4_SIMULATOR:=sihsim}
 PX4_SIM_MODEL=${PX4_SIM_MODEL:=airplane}

+param set-default SENS_EN_GPSSIM 1
+param set-default SENS_EN_BAROSIM 1
+param set-default SENS_EN_MAGSIM 1
+param set-default SENS_EN_ARSPDSIM 1
+
 # disable some checks to allow to fly:
 # - with usb
 param set-default CBRK_USB_CHK 197848
@@ -11,6 +11,10 @@
 PX4_SIMULATOR=${PX4_SIMULATOR:=sihsim}
 PX4_SIM_MODEL=${PX4_SIM_MODEL:=xvert}

+param set-default SENS_EN_GPSSIM 1
+param set-default SENS_EN_BAROSIM 1
+param set-default SENS_EN_MAGSIM 1
+
 param set-default VT_ELEV_MC_LOCK 0
 param set-default VT_TYPE 0
 param set-default VT_FW_DIFTHR_EN 1
@@ -49,7 +53,7 @@ param set-default CA_AIRFRAME 4

 param set-default CA_ROTOR_COUNT 2
 param set-default CA_ROTOR0_PX 0
-param set-default CA_ROTOR0_PY 2
+param set-default CA_ROTOR0_PY 1
 param set-default CA_ROTOR0_KM 0.05
 param set-default CA_ROTOR1_PX 0
 param set-default CA_ROTOR1_PY -1
@@ -5,12 +5,12 @@
 # @type Quadrotor Wide
 #

-. ${R}etc/init.d-posix/airframes/10016_iris
+. ${R}etc/init.d-posix/airframes/10016_gazebo-classic_iris

-# EKF2: Vision position and heading
-param set-default EKF2_AID_MASK 24
+# EKF2: Vision position and heading, no GPS
 param set-default EKF2_EV_DELAY 5
 param set-default EKF2_EV_CTRL 15
+param set-default EKF2_HGT_REF 3
 param set-default EKF2_GPS_CTRL 0

 # LPE: Vision + baro
@@ -10,7 +10,7 @@ param set-default EKF2_MAG_YAWLIM 0

 param set-default FW_LND_ANG 8

-param set-default FW_L1_PERIOD 12
+param set-default NPFG_PERIOD 12

 param set-default FW_PR_P 0.9
 param set-default FW_PR_FF 0.5
@@ -11,7 +11,7 @@ param set-default EKF2_MAG_YAWLIM 0
 param set-default FW_LND_AIRSPD_SC 1
 param set-default FW_LND_ANG 8

-param set-default FW_L1_PERIOD 12
+param set-default NPFG_PERIOD 12

 param set-default FW_PR_P 0.9
 param set-default FW_PR_FF 0.5
@@ -11,7 +11,7 @@ param set-default EKF2_MAG_YAWLIM 0
 param set-default FW_LND_AIRSPD_SC 1
 param set-default FW_LND_ANG 8

-param set-default FW_L1_PERIOD 12
+param set-default NPFG_PERIOD 12

 param set-default FW_PR_P 0.9
 param set-default FW_PR_FF 0.5
@@ -10,7 +10,7 @@ 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_L1_PERIOD 25
+param set-default NPFG_PERIOD 25

 param set-default FW_PR_FF 0.40
 param set-default FW_PR_I 0.05
@@ -10,7 +10,7 @@ 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_L1_PERIOD 25
+param set-default NPFG_PERIOD 25

 param set-default FW_PR_FF 0.40
 param set-default FW_PR_I 0.05
@@ -10,8 +10,6 @@ param set-default EKF2_MAG_YAWLIM 0

 param set-default FW_LND_ANG 8

-param set-default FW_L1_PERIOD 15
-
 param set-default FW_P_TC 0.5
 param set-default FW_PR_FF 0.40
 param set-default FW_PR_I 0.05
@@ -22,7 +20,7 @@ param set-default FW_RR_FF 0.20
 param set-default FW_RR_I 0.02
 param set-default FW_RR_P 0.22

-param set-default FW_L1_PERIOD 12
+param set-default NPFG_PERIOD 12

 param set-default FW_W_EN 1

@@ -10,7 +10,7 @@ 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_L1_PERIOD 25
+param set-default NPFG_PERIOD 25

 param set-default FW_PR_FF 0.40
 param set-default FW_PR_I 0.05
@@ -10,7 +10,7 @@ param set-default EKF2_MAG_YAWLIM 0

 param set-default FW_LND_ANG 8

-param set-default FW_L1_PERIOD 12
+param set-default NPFG_PERIOD 12

 param set-default FW_PR_P 0.9
 param set-default FW_PR_FF 0.2
@@ -11,7 +11,7 @@ param set-default EKF2_MAG_YAWLIM 0
 param set-default FW_LND_AIRSPD_SC 1
 param set-default FW_LND_ANG 8

-param set-default FW_L1_PERIOD 12
+param set-default NPFG_PERIOD 12

 param set-default FW_PR_P 0.9
 param set-default FW_PR_FF 0.5
@@ -11,7 +11,7 @@ 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_L1_PERIOD 25
+param set-default NPFG_PERIOD 25

 param set-default FW_PR_FF 0.40
 param set-default FW_PR_I 0.05
@@ -11,7 +11,7 @@ param set-default EKF2_MAG_YAWLIM 0
 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 NPFG_PERIOD 12

 param set-default FW_MAN_P_MAX 30

@@ -47,7 +47,7 @@ param set-default PWM_MAIN_FUNC6 201
 param set-default PWM_MAIN_FUNC7 202
 param set-default PWM_MAIN_FUNC8 203

-param set-default FW_L1_PERIOD 12
+param set-default NPFG_PERIOD 12
 param set-default FW_PR_FF 0.2
 param set-default FW_PR_P 0.9
 param set-default FW_PSP_OFF 2
@@ -13,7 +13,7 @@ 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 2
+param set-default CA_ROTOR0_PY 1
 param set-default CA_ROTOR0_KM 0.05
 param set-default CA_ROTOR1_PX -1
 param set-default CA_ROTOR1_PY -1
@@ -42,7 +42,7 @@ 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 NPFG_PERIOD 12
 param set-default FW_PR_I 0.2
 param set-default FW_PR_P 0.2
 param set-default FW_PSP_OFF 2
@@ -46,7 +46,7 @@ param set-default PWM_MAIN_FUNC7 201
 param set-default PWM_MAIN_FUNC8 202
 param set-default PWM_MAIN_FUNC9 203

-param set-default FW_L1_PERIOD 12
+param set-default NPFG_PERIOD 12
 param set-default FW_PR_FF 0.2
 param set-default FW_PR_P 0.9
 param set-default FW_PSP_OFF 2
@@ -47,7 +47,7 @@ param set-default PWM_MAIN_FUNC6 201
 param set-default PWM_MAIN_FUNC7 202
 param set-default PWM_MAIN_FUNC8 203

-param set-default FW_L1_PERIOD 12
+param set-default NPFG_PERIOD 12
 param set-default FW_PR_FF 0.2
 param set-default FW_PR_P 0.9
 param set-default FW_PSP_OFF 2
@@ -11,7 +11,7 @@ param set-default EKF2_MAG_YAWLIM 0
 param set-default FW_LND_AIRSPD_SC 1
 param set-default FW_LND_ANG 8

-param set-default FW_L1_PERIOD 12
+param set-default NPFG_PERIOD 12

 param set-default FW_PR_P 0.9
 param set-default FW_PR_FF 0.5
@@ -11,6 +11,12 @@ PX4_SIMULATOR=${PX4_SIMULATOR:=gz}
 PX4_GZ_WORLD=${PX4_GZ_WORLD:=default}
 PX4_SIM_MODEL=${PX4_SIM_MODEL:=x500}

+param set-default SIM_GZ_EN 1
+
+param set-default SENS_EN_GPSSIM 1
+param set-default SENS_EN_BAROSIM 0
+param set-default SENS_EN_MAGSIM 1
+
 param set-default CA_AIRFRAME 0
 param set-default CA_ROTOR_COUNT 4

@@ -30,19 +36,19 @@ param set-default CA_ROTOR3_PX -0.13
 param set-default CA_ROTOR3_PY 0.20
 param set-default CA_ROTOR3_KM -0.05

-param set-default SIM_GZ_FUNC1 101
-param set-default SIM_GZ_FUNC2 102
-param set-default SIM_GZ_FUNC3 103
-param set-default SIM_GZ_FUNC4 104
+param set-default SIM_GZ_EC_FUNC1 101
+param set-default SIM_GZ_EC_FUNC2 102
+param set-default SIM_GZ_EC_FUNC3 103
+param set-default SIM_GZ_EC_FUNC4 104

-param set-default SIM_GZ_MIN1 150
-param set-default SIM_GZ_MIN2 150
-param set-default SIM_GZ_MIN3 150
-param set-default SIM_GZ_MIN4 150
+param set-default SIM_GZ_EC_MIN1 150
+param set-default SIM_GZ_EC_MIN2 150
+param set-default SIM_GZ_EC_MIN3 150
+param set-default SIM_GZ_EC_MIN4 150

-param set-default SIM_GZ_MAX1 1000
-param set-default SIM_GZ_MAX2 1000
-param set-default SIM_GZ_MAX3 1000
-param set-default SIM_GZ_MAX4 1000
+param set-default SIM_GZ_EC_MAX1 1000
+param set-default SIM_GZ_EC_MAX2 1000
+param set-default SIM_GZ_EC_MAX3 1000
+param set-default SIM_GZ_EC_MAX4 1000

 param set-default MPC_THR_HOVER 0.60
@@ -11,6 +11,12 @@ PX4_SIMULATOR=${PX4_SIMULATOR:=gz}
 PX4_GZ_WORLD=${PX4_GZ_WORLD:=default}
 PX4_SIM_MODEL=${PX4_SIM_MODEL:=x500_depth}

+param set-default SIM_GZ_EN 1
+
+param set-default SENS_EN_GPSSIM 1
+param set-default SENS_EN_BAROSIM 0
+param set-default SENS_EN_MAGSIM 1
+
 param set-default CA_AIRFRAME 0
 param set-default CA_ROTOR_COUNT 4

@@ -30,19 +36,19 @@ param set-default CA_ROTOR3_PX -0.13
 param set-default CA_ROTOR3_PY 0.20
 param set-default CA_ROTOR3_KM -0.05

-param set-default SIM_GZ_FUNC1 101
-param set-default SIM_GZ_FUNC2 102
-param set-default SIM_GZ_FUNC3 103
-param set-default SIM_GZ_FUNC4 104
+param set-default SIM_GZ_EC_FUNC1 101
+param set-default SIM_GZ_EC_FUNC2 102
+param set-default SIM_GZ_EC_FUNC3 103
+param set-default SIM_GZ_EC_FUNC4 104

-param set-default SIM_GZ_MIN1 150
-param set-default SIM_GZ_MIN2 150
-param set-default SIM_GZ_MIN3 150
-param set-default SIM_GZ_MIN4 150
+param set-default SIM_GZ_EC_MIN1 150
+param set-default SIM_GZ_EC_MIN2 150
+param set-default SIM_GZ_EC_MIN3 150
+param set-default SIM_GZ_EC_MIN4 150

-param set-default SIM_GZ_MAX1 1000
-param set-default SIM_GZ_MAX2 1000
-param set-default SIM_GZ_MAX3 1000
-param set-default SIM_GZ_MAX4 1000
+param set-default SIM_GZ_EC_MAX1 1000
+param set-default SIM_GZ_EC_MAX2 1000
+param set-default SIM_GZ_EC_MAX3 1000
+param set-default SIM_GZ_EC_MAX4 1000

 param set-default MPC_THR_HOVER 0.60
@@ -0,0 +1,88 @@
+#!/bin/sh
+#
+# @name Gazebo rc_cessna
+# @type Fixedwing
+#
+
+. ${R}etc/init.d/rc.fw_defaults
+
+PX4_SIMULATOR=${PX4_SIMULATOR:=gz}
+PX4_GZ_WORLD=${PX4_GZ_WORLD:=default}
+PX4_SIM_MODEL=${PX4_SIM_MODEL:=rc_cessna}
+
+param set-default SENS_EN_GPSSIM 1
+param set-default SENS_EN_BAROSIM 0
+param set-default SENS_EN_MAGSIM 1
+param set-default SENS_EN_ARSPDSIM 1
+
+param set-default EKF2_MAG_ACCLIM 0
+param set-default EKF2_MAG_YAWLIM 0
+
+param set-default FW_LND_ANG 8
+
+param set-default NPFG_PERIOD 12
+
+param set-default FW_PR_P 0.9
+param set-default FW_PR_FF 0.5
+param set-default FW_PR_I 0.5
+param set-default TRIM_PITCH -0.15
+
+param set-default FW_PSP_OFF 2
+param set-default FW_P_LIM_MIN -15
+
+param set-default FW_RR_FF 0.5
+param set-default FW_RR_P 0.3
+param set-default FW_RR_I 0.5
+
+param set-default FW_YR_FF 0.5
+param set-default FW_YR_P 0.6
+param set-default FW_YR_I 0.5
+
+param set-default FW_SPOILERS_LND 0.4
+
+param set-default FW_THR_MAX 0.6
+param set-default FW_THR_MIN 0.05
+param set-default FW_THR_TRIM 0.25
+
+param set-default FW_T_CLMB_MAX 8
+param set-default FW_T_SINK_MAX 2.7
+param set-default FW_T_SINK_MIN 2.2
+
+param set-default FW_W_EN 1
+
+param set-default MIS_TAKEOFF_ALT 30
+
+param set-default NAV_ACC_RAD 15
+param set-default NAV_DLL_ACT 2
+
+param set-default RWTO_TKOFF 1
+
+param set-default CA_AIRFRAME 1
+
+param set-default COM_PREARM_MODE 2
+
+param set-default CA_ROTOR_COUNT 1
+param set-default CA_ROTOR0_PX 0.3
+
+param set-default CA_SV_CS_COUNT 6
+param set-default CA_SV_CS0_TRQ_R -0.5
+param set-default CA_SV_CS0_TYPE 1
+param set-default CA_SV_CS1_TRQ_R 0.5
+param set-default CA_SV_CS1_TYPE 2
+param set-default CA_SV_CS2_TRQ_P 1.0
+param set-default CA_SV_CS2_TYPE 3
+param set-default CA_SV_CS3_TRQ_Y 1.0
+param set-default CA_SV_CS3_TYPE 4
+param set-default CA_SV_CS4_TYPE 9
+param set-default CA_SV_CS5_TYPE 10
+
+param set-default SIM_GZ_EC_FUNC1 101
+param set-default SIM_GZ_EC_MIN1 0
+param set-default SIM_GZ_EC_MAX1 1000
+
+param set-default SIM_GZ_SV_FUNC1 201
+param set-default SIM_GZ_SV_FUNC2 202
+param set-default SIM_GZ_SV_FUNC3 203
+param set-default SIM_GZ_SV_FUNC4 204
+param set-default SIM_GZ_SV_FUNC5 205
+param set-default SIM_GZ_SV_FUNC6 206
@@ -0,0 +1,109 @@
+#!/bin/sh
+#
+# @name Standard VTOL
+#
+# @type Standard VTOL
+#
+
+. ${R}etc/init.d/rc.vtol_defaults
+
+PX4_SIMULATOR=${PX4_SIMULATOR:=gz}
+PX4_GZ_WORLD=${PX4_GZ_WORLD:=default}
+PX4_SIM_MODEL=${PX4_SIM_MODEL:=standard_vtol}
+
+param set-default SENS_EN_GPSSIM 1
+param set-default SENS_EN_BAROSIM 0
+param set-default SENS_EN_MAGSIM 1
+param set-default SENS_EN_ARSPDSIM 1
+
+# TODO: Enable motor failure detection when the
+# VTOL no longer reports 0A for all ESCs in SITL
+param set-default FD_ACT_EN 0
+param set-default FD_ACT_MOT_TOUT 500
+
+param set-default CA_AIRFRAME 2
+
+param set-default COM_PREARM_MODE 2
+
+param set-default CA_ROTOR_COUNT 5
+param set-default CA_ROTOR0_PX 0.1515
+param set-default CA_ROTOR0_PY 0.245
+param set-default CA_ROTOR0_KM 0.05
+param set-default CA_ROTOR1_PX -0.1515
+param set-default CA_ROTOR1_PY -0.1875
+param set-default CA_ROTOR1_KM 0.05
+param set-default CA_ROTOR2_PX 0.1515
+param set-default CA_ROTOR2_PY -0.245
+param set-default CA_ROTOR2_KM -0.05
+param set-default CA_ROTOR3_PX -0.1515
+param set-default CA_ROTOR3_PY 0.1875
+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 SIM_GZ_EC_FUNC1 101
+param set-default SIM_GZ_EC_MIN1 10
+param set-default SIM_GZ_EC_MAX1 1500
+param set-default SIM_GZ_EC_FUNC2 102
+param set-default SIM_GZ_EC_MIN2 10
+param set-default SIM_GZ_EC_MAX2 1500
+param set-default SIM_GZ_EC_FUNC3 103
+param set-default SIM_GZ_EC_MIN3 10
+param set-default SIM_GZ_EC_MAX3 1500
+param set-default SIM_GZ_EC_FUNC4 104
+param set-default SIM_GZ_EC_MIN4 10
+param set-default SIM_GZ_EC_MAX4 1500
+
+param set-default SIM_GZ_EC_FUNC5 105
+param set-default SIM_GZ_EC_MIN5 0
+param set-default SIM_GZ_EC_MAX5 3500
+
+param set-default SIM_GZ_SV_FUNC1 201
+param set-default SIM_GZ_SV_FUNC2 202
+param set-default SIM_GZ_SV_FUNC3 203
+
+param set-default COM_RC_IN_MODE 1
+param set-default ASPD_PRIMARY 1
+
+param set-default CA_SV_CS_COUNT 3
+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 3
+param set-default CA_SV_CS2_TRQ_P 1.0
+
+param set-default FW_L1_PERIOD 12
+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_MIN -15
+param set-default FW_RR_FF 0.1
+param set-default FW_RR_P 0.3
+param set-default FW_THR_TRIM 0.25
+param set-default FW_THR_MAX 0.6
+param set-default FW_THR_MIN 0.05
+param set-default FW_T_CLMB_MAX 8
+param set-default FW_T_SINK_MAX 2.7
+param set-default FW_T_SINK_MIN 2.2
+
+param set-default MC_AIRMODE 1
+param set-default MC_ROLLRATE_P 0.3
+param set-default MC_YAW_P 1.6
+
+param set-default MIS_TAKEOFF_ALT 10
+
+param set-default MPC_ACC_HOR_MAX 2
+param set-default MPC_XY_P 0.8
+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 NAV_ACC_RAD 5
+
+param set-default VT_FWD_THRUST_EN 4
+param set-default VT_F_TRANS_THR 0.75
+param set-default VT_B_TRANS_DUR 8
+param set-default VT_TYPE 2
+param set-default FD_ESCS_EN 0
@@ -0,0 +1,12 @@
+#!/bin/sh
+#
+# @name Gazebo x500 vision
+#
+# @type Quadrotor
+#
+
+. ${R}etc/init.d-posix/airframes/4001_gz_x500
+
+PX4_SIMULATOR=${PX4_SIMULATOR:=gz}
+PX4_GZ_WORLD=${PX4_GZ_WORLD:=default}
+PX4_SIM_MODEL=${PX4_SIM_MODEL:=x500_vision}
@@ -1,8 +1,6 @@

 mavlink start -x -u 14558 -r 4000 -f -m onboard -o 14530 -p

-# shellcheck disable=SC2154
-mavlink stream -r 10 -s MOUNT_ORIENTATION -u $udp_gcs_port_local
 # shellcheck disable=SC2154
 mavlink stream -r 50 -s ATTITUDE_QUATERNION -u $udp_offboard_port_local
 mavlink stream -r 10 -s MOUNT_ORIENTATION -u $udp_offboard_port_local
@@ -72,6 +72,9 @@ px4_add_romfs_files(

 	4001_gz_x500
 	4002_gz_x500_depth
+	4003_gz_rc_cessna
+	4004_gz_standard_vtol
+	4005_gz_x500_vision

 	6011_gazebo-classic_typhoon_h480
 	6011_gazebo-classic_typhoon_h480.post
@@ -8,20 +8,37 @@ if [ "$PX4_SIMULATOR" = "sihsim" ] || [ "$(param show -q SYS_AUTOSTART)" -eq "0"

 	echo "INFO  [init] SIH simulator"

+	if [ -n "${PX4_HOME_LAT}" ]; then
+		param set SIH_LOC_LAT0 ${PX4_HOME_LAT}
+	fi
+
+	if [ -n "${PX4_HOME_LON}" ]; then
+		param set SIH_LOC_LON0 ${PX4_HOME_LON}
+	fi
+
 	if simulator_sih start; then

-		sensor_baro_sim start
-		sensor_gps_sim start
-		sensor_mag_sim start
+		if param compare -s SENS_EN_BAROSIM 1
+		then
+			sensor_baro_sim start
+		fi
+		if param compare -s SENS_EN_GPSSIM 1
+		then
+			sensor_gps_sim start
+		fi
+		if param compare -s SENS_EN_MAGSIM 1
+		then
+			sensor_mag_sim start
+		fi

 	else
 		echo "ERROR [init] simulator_sih failed to start"
 		exit 1
 	fi

-elif [ "$PX4_SIMULATOR" = "gz" ]; then
+elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" -eq "1" ]; then

-	# source generated gz_env.sh for IGN_GAZEBO_RESOURCE_PATH
+	# source generated gz_env.sh for GZ_SIM_RESOURCE_PATH
 	if [ -f ./gz_env.sh ]; then
 		. ./gz_env.sh

@@ -37,16 +54,8 @@ elif [ "$PX4_SIMULATOR" = "gz" ]; then
 		gz_command="gz"
 		gz_sub_command="sim"
 	else
-		IGN_GAZEBO_VERSIONS=$(ign gazebo --versions 2>&1)
-		if [ $? -eq 0 ] && [ "${IGN_GAZEBO_VERSIONS}" != "" ]
-		then
-			# "ign gazebo" for Fortress and earlier
-			gz_command="ign"
-			gz_sub_command="gazebo"
-		else
-			echo "ERROR [init] Gazebo gz and ign commands unavailable"
-			exit 1
-		fi
+		echo "ERROR [init] Gazebo gz please install gz-garden"
+		exit 1
 	fi

 	# look for running ${gz_command} gazebo world
@@ -85,9 +94,22 @@ elif [ "$PX4_SIMULATOR" = "gz" ]; then

 		# start gz bridge with pose arg.
 		if gz_bridge start -p "${model_pose}" -m "${PX4_GZ_MODEL}" -w "${PX4_GZ_WORLD}" -i "${px4_instance}"; then
-			sensor_baro_sim start
-			sensor_gps_sim start
-			sensor_mag_sim start
+			if param compare -s SENS_EN_BAROSIM 1
+			then
+				sensor_baro_sim start
+			fi
+			if param compare -s SENS_EN_GPSSIM 1
+			then
+				sensor_gps_sim start
+			fi
+			if param compare -s SENS_EN_MAGSIM 1
+			then
+				sensor_mag_sim start
+			fi
+			if param compare -s SENS_EN_ARSPDSIM 1
+			then
+				sensor_airspeed_sim start
+			fi

 		else
 			echo "ERROR [init] gz_bridge failed to start"
@@ -98,9 +120,22 @@ elif [ "$PX4_SIMULATOR" = "gz" ]; then
 		# model name specificed, gz_bridge will attach to existing model

 		if gz_bridge start -n "${PX4_GZ_MODEL_NAME}" -w "${PX4_GZ_WORLD}"; then
-			sensor_baro_sim start
-			sensor_gps_sim start
-			sensor_mag_sim start
+			if param compare -s SENS_EN_BAROSIM 1
+			then
+				sensor_baro_sim start
+			fi
+			if param compare -s SENS_EN_GPSSIM 1
+			then
+				sensor_gps_sim start
+			fi
+			if param compare -s SENS_EN_MAGSIM 1
+			then
+				sensor_mag_sim start
+			fi
+			if param compare -s SENS_EN_ARSPDSIM 1
+			then
+				sensor_airspeed_sim start
+			fi

 		else
 			echo "ERROR [init] gz_bridge failed to start"
@@ -112,9 +147,22 @@ elif [ "$PX4_SIMULATOR" = "gz" ]; then
 		echo "WARN  [init] PX4_GZ_MODEL_NAME or PX4_GZ_MODEL not set using PX4_SIM_MODEL."

 		if gz_bridge start -m "${PX4_SIM_MODEL#*gz_}" -w "${PX4_GZ_WORLD}" -i "${px4_instance}"; then
-			sensor_baro_sim start
-			sensor_gps_sim start
-			sensor_mag_sim start
+			if param compare -s SENS_EN_BAROSIM 1
+			then
+				sensor_baro_sim start
+			fi
+			if param compare -s SENS_EN_GPSSIM 1
+			then
+				sensor_gps_sim start
+			fi
+			if param compare -s SENS_EN_MAGSIM 1
+			then
+				sensor_mag_sim start
+			fi
+			if param compare -s SENS_EN_ARSPDSIM 1
+			then
+				sensor_airspeed_sim start
+			fi

 		else
 			echo "ERROR [init] gz_bridge failed to start"
@@ -184,6 +184,9 @@ param set-default SDLOG_DIRS_MAX 7
 param set-default TRIG_INTERFACE 3

 param set-default SYS_FAILURE_EN 1
+# Enable low-battery actions by default for (automated) testing. Battery sim
+# does not go below 50% by default, but failure injection can trigger failsafes.
+param set-default COM_LOW_BAT_ACT 2

 # Adapt timeout parameters if simulation runs faster or slower than realtime.
 if [ -n "$PX4_SIM_SPEED_FACTOR" ]; then
@@ -36,6 +36,7 @@ add_subdirectory(airframes)
 px4_add_romfs_files(
 	rc.airship_apps
 	rc.airship_defaults
+	rc.autostart_ext
 	rc.balloon_apps
 	rc.balloon_defaults
 	rc.boat_defaults
@@ -38,7 +38,7 @@ param set-default EKF2_GPS_P_GATE 10
 param set-default EKF2_GPS_V_GATE 10

 param set-default FW_ARSP_MODE 1
-param set-default FW_L1_PERIOD 25
+param set-default NPFG_PERIOD 25
 param set-default FW_PR_FF 0.7
 param set-default FW_PR_I 0.18
 param set-default FW_PR_P 0.15
@@ -122,16 +122,12 @@ param set-default VT_TRANS_MIN_TM 15
 param set-default VT_B_TRANS_DUR 8
 param set-default VT_FWD_THRUST_SC 4
 param set-default VT_F_TRANS_DUR 1
-param set-default VT_B_REV_OUT 0.5
 param set-default VT_B_TRANS_THR 0.7
 param set-default VT_TRANS_TIMEOUT 22
 param set-default VT_F_TRANS_RAMP 4

 param set-default COM_RC_OVERRIDE 0

-
-
-
 param set-default CA_AIRFRAME 2

 param set-default CA_ROTOR_COUNT 5
@@ -30,7 +30,7 @@ param set-default BAT1_N_CELLS 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_PN_R_SLEW_MAX 40
 param set-default FW_PSP_OFF 3
 param set-default FW_P_LIM_MAX 18
 param set-default FW_P_LIM_MIN -25
@@ -54,14 +54,6 @@ param set-default CBRK_AIRSPD_CHK 162128
 # Differential drive acts like ackermann steering with a maximum turn angle of 180 degrees, or pi radians
 param set-default GND_MAX_ANG 3.1415

-param set-default RBCLW_BAUD 8
-param set-default RBCLW_COUNTS_REV 1200
-param set-default RBCLW_ADDRESS 128
-# 104 corresponds to Telem 4
-param set-default RBCLW_SER_CFG 104
-# Start this driver after setting parameters, because the driver uses some of those parameters.
-# roboclaw start /dev/ttyS3
-
 # Set geometry & output configration
 param set-default CA_AIRFRAME 6
 param set-default CA_R_REV 3
@@ -20,7 +20,7 @@ control_allocator start
 #
 fw_rate_control start
 fw_att_control start
-fw_pos_control_l1 start
+fw_path_navigation start
 airspeed_selector start

 #
@@ -14,9 +14,13 @@ param set-default MAV_TYPE 1
 # Default parameters for fixed wing UAVs.
 #
 param set-default COM_POS_FS_DELAY 5
-param set-default COM_POS_FS_EPH 15
-param set-default COM_POS_FS_EPV 30
+
+# there is a 2.5 factor applied on the _FS thresholds if for invalidation
+param set-default COM_POS_FS_EPH 50
 param set-default COM_VEL_FS_EVH 5
+
+param set-default COM_POS_LOW_EPH 50
+
 # Disable preflight disarm to not interfere with external launching
 param set-default COM_DISARM_PRFLT -1

@@ -23,6 +23,12 @@ then
 	set LOGGER_ARGS "${LOGGER_ARGS} -x"
 fi

+if param compare SDLOG_MODE 4
+then
+	set LOGGER_ARGS "${LOGGER_ARGS} -a"
+fi
+
+
 if ! param compare SDLOG_MODE -1
 then
 	logger start -b ${LOGGER_BUF} -t ${LOGGER_ARGS}
@@ -37,7 +37,7 @@ fi

 fw_rate_control start vtol
 fw_att_control start vtol
-fw_pos_control_l1 start vtol
+fw_path_navigation start vtol
 fw_autotune_attitude_control start vtol

 # Start Land Detector
@@ -10,6 +10,11 @@ set VEHICLE_TYPE vtol
 # MAV_TYPE_VTOL_FIXEDROTOR 22
 param set-default MAV_TYPE 22

+# there is a 2.5 factor applied on COM_POS_FS_EPH if for invalidation
+param set-default COM_POS_FS_EPH 50
+
+param set-default COM_POS_LOW_EPH 50
+
 param set-default MIS_TAKEOFF_ALT 20
 param set-default MIS_YAW_TMT 10

@@ -54,7 +54,10 @@ do
 	${DIR}/run_nsh_cmd.py --device ${SERIAL_DEVICE} --cmd 'ps'
 	${DIR}/run_nsh_cmd.py --device ${SERIAL_DEVICE} --cmd 'work_queue status'

+	${DIR}/run_nsh_cmd.py --device ${SERIAL_DEVICE} --cmd 'mavlink status' || true
 	${DIR}/run_nsh_cmd.py --device ${SERIAL_DEVICE} --cmd 'mavlink stop-all'
+
+	${DIR}/run_nsh_cmd.py --device ${SERIAL_DEVICE} --cmd 'gps status' || true
 	${DIR}/run_nsh_cmd.py --device ${SERIAL_DEVICE} --cmd 'gps stop'

 done
@@ -10,6 +10,7 @@ fi
 exec find boards msg src platforms test \
    -path platforms/nuttx/NuttX -prune -o \
    -path platforms/qurt/dspal -prune -o \
+    -path src/drivers/ins/vectornav/libvnc -prune -o \
    -path src/drivers/uavcan/libuavcan -prune -o \
    -path src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis -prune -o \
    -path src/drivers/cyphal/libcanard -prune -o \
@@ -25,7 +25,7 @@ if __name__ == "__main__":
    if not os.path.isdir(output_dir):
        os.mkdir(output_dir)

-    msg_path = os.path.join(os.path.dirname(os.path.realpath(__file__)),"..")
+    msg_path = os.path.join(os.path.dirname(os.path.realpath(__file__)),"../../msg")
    msg_files = get_msgs_list(msg_path)
    msg_files.sort()

@@ -164,7 +164,8 @@ for field_type, field_name, field_size, padding in fields:
 	print('\tmemcpy(&topic.{0}, buf.iterator, sizeof(topic.{0}));'.format(field_name))

 	if field_type == 'uint64' and (field_name == 'timestamp' or field_name == 'timestamp_sample'):
-		print('\ttopic.{0} -= time_offset;'.format(field_name))
+		print('\tif (topic.{0} == 0) topic.{0} = hrt_absolute_time();'.format(field_name, field_name))
+		print('\telse topic.{0} = math::min(topic.{0} - time_offset, hrt_absolute_time());'.format(field_name, field_name))

 	print('\tbuf.iterator += sizeof(topic.{:});'.format(field_name))
 	print('\tbuf.offset += sizeof(topic.{:});'.format(field_name))
@@ -15,7 +15,7 @@ class ModuleDocumentation(object):
    # 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',
+    valid_subcategories = ['', 'distance_sensor', 'imu', 'ins', 'airspeed_sensor',
                           'magnetometer', 'baro', 'optical_flow', 'rpm_sensor', 'transponder']

    max_line_length = 80 # wrap lines that are longer than this
@@ -102,7 +102,7 @@ class ModuleDocumentation(object):
    def _handle_usage_param_int(self, args):
        assert(len(args) == 6) # option_char, default_val, min_val, max_val, description, is_optional
        option_char = self._get_option_char(args[0])
-        default_val = int(args[1], 0)
+        default_val = self._get_int(args[1])
        description = self._get_string(args[4])
        if self._is_bool_true(args[5]):
            self._usage_string += "     [-%s <val>]  %s\n" % (option_char, description)
@@ -214,6 +214,9 @@ class ModuleDocumentation(object):
            f = f[:-1]
        return float(f)

+    def _get_int(self, argument):
+        return int(eval(argument))
+
    def _is_string(self, argument):
        return len(argument) > 0 and argument[0] == '"'

@@ -307,6 +310,8 @@ class SourceParser(object):
            r'//.*?$|/\*.*?\*/|\'(?:\\.|[^\\\'])*\'|"(?:\\.|[^\\"])*"',
            re.DOTALL | re.MULTILINE)

+        self._define_pattern = re.compile(r'#define\s+(\w+?)[^\S\r\n]+(.+?)\s*?\n')
+
    def Parse(self, scope, contents):
        """
        Incrementally parse program contents and append all found documentations
@@ -316,6 +321,9 @@ class SourceParser(object):
        # remove comments from source
        contents = self._comment_remover(contents)

+        # replace preprocessor defines defined in file directly
+        contents = self._define_replacer(contents)
+
        extracted_function_calls = [] # list of tuples: (FUNC_NAME, list(ARGS))

        start_index = 0
@@ -379,6 +387,15 @@ class SourceParser(object):
                return s
        return re.sub(self._comment_remove_pattern, replacer, text)

+    def _define_replacer(self, text):
+        """ check for C preprocesor #define in text and replace with argument"""
+        text = re.sub(r"\\\s*?\n"," ",text)
+        define_iter = self._define_pattern.finditer(text)
+        for define_pattern in define_iter:
+            text = re.sub(r"\b" +re.escape(str(define_pattern.groups()[0])) + r"\b", re.escape(str(define_pattern.groups()[1])), text)
+        return text
+
+
    def _do_consistency_check(self, contents, scope, module_doc):
        """
        check the documentation for consistency with the code (arguments to
@@ -84,6 +84,7 @@ sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends i
 	gdb \
 	git \
 	lcov \
+	libfuse2 \
 	libxml2-dev \
 	libxml2-utils \
 	make \
@@ -182,6 +183,7 @@ if [[ $INSTALL_NUTTX == "true" ]]; then
 			echo "${NUTTX_GCC_VERSION} path already set.";
 		else
 			echo $exportline >> $HOME/.profile;
+			source $HOME/.profile; # Allows to directly build NuttX targets in the same terminal
 		fi
 	fi
 fi
@@ -217,22 +219,34 @@ if [[ $INSTALL_SIM == "true" ]]; then
 	# Set Java 11 as default
 	sudo update-alternatives --set java $(update-alternatives --list java | grep "java-$java_version")

-	# Install Gazebo classic
-	if [[ "${UBUNTU_RELEASE}" == "18.04" ]]; then
-		gazebo_version=9
-		gazebo_packages="gazebo$gazebo_version libgazebo$gazebo_version-dev"
-	elif [[ "${UBUNTU_RELEASE}" == "22.04" ]]; then
-		gazebo_packages="gazebo libgazebo-dev"
+	# Gazebo / Gazebo classic installation
+	if [[ "${UBUNTU_RELEASE}" == "22.04" ]]; then
+		echo "Gazebo (Garden) will be installed"
+		echo "Earlier versions will be removed"
+		# Add Gazebo binary repository
+		sudo wget https://packages.osrfoundation.org/gazebo.gpg -O /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg
+		echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] http://packages.osrfoundation.org/gazebo/ubuntu-stable $(lsb_release -cs) main" | sudo tee /etc/apt/sources.list.d/gazebo-stable.list > /dev/null
+		sudo apt-get update -y --quiet
+
+		# Install Gazebo
+		gazebo_packages="gz-garden"
 	else
-		# default and Ubuntu 20.04
-		gazebo_version=11
-		gazebo_packages="gazebo$gazebo_version libgazebo$gazebo_version-dev"
+		sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list'
+		wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add -
+		# Update list, since new gazebo-stable.list has been added
+		sudo apt-get update -y --quiet
+
+		# Install Gazebo classic
+		if [[ "${UBUNTU_RELEASE}" == "18.04" ]]; then
+			gazebo_classic_version=9
+			gazebo_packages="gazebo$gazebo_classic_version libgazebo$gazebo_classic_version-dev"
+		else
+			# default and Ubuntu 20.04
+			gazebo_classic_version=11
+			gazebo_packages="gazebo$gazebo_classic_version libgazebo$gazebo_classic_version-dev"
+		fi
 	fi

-	sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list'
-	wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add -
-	# Update list, since new gazebo-stable.list has been added
-	sudo apt-get update -y --quiet
 	sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \
 		dmidecode \
 		$gazebo_packages \
@@ -255,16 +269,6 @@ if [[ $INSTALL_SIM == "true" ]]; then
 		echo "export SVGA_VGPU10=0" >> ~/.profile
 	fi

-	# Install Gazebo
-	if [[ "${UBUNTU_RELEASE}" == "22.04" ]]; then
-		sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list'
-		wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add -
-		# Update list, since new gazebo-stable.list has been added
-		sudo apt-get update -y --quiet
-		sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \
-			ignition-fortress \
-			;
-	fi
 fi

 if [[ $INSTALL_NUTTX == "true" ]]; then
@@ -0,0 +1,165 @@
+<?xml version="1.0" encoding="UTF-8" standalone="no" ?>
+<COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1">
+    <asset>
+        <contributor>
+            <authoring_tool>SketchUp 15.3.331</authoring_tool>
+        </contributor>
+        <created>2015-05-26T23:41:25Z</created>
+        <modified>2015-05-26T23:41:25Z</modified>
+        <unit meter="0.0254" name="inch" />
+        <up_axis>Z_UP</up_axis>
+    </asset>
+    <library_visual_scenes>
+        <visual_scene id="ID1">
+            <node name="SketchUp">
+                <node id="ID2" name="instance_0">
+                    <matrix>0.001745328 -0.9999985 0 70.84882 0.9999985 0.001745328 0 -0.03157813 -0 0 1 -19.50896 0 0 0 1</matrix>
+                    <instance_node url="#ID3" />
+                </node>
+            </node>
+        </visual_scene>
+    </library_visual_scenes>
+    <library_nodes>
+        <node id="ID3" name="ske2A3">
+            <node id="ID4" name="group_0">
+                <matrix>1.963935 -3.469447e-018 0 1.237124 3.469447e-018 1.963935 0 258.791 0 0 1.963935 40.4385 0 0 0 1</matrix>
+                <instance_geometry url="#ID5">
+                    <bind_material>
+                        <technique_common>
+                            <instance_material symbol="Material2" target="#ID6">
+                                <bind_vertex_input semantic="UVSET0" input_semantic="TEXCOORD" input_set="0" />
+                            </instance_material>
+                            <instance_material symbol="Material3" target="#ID11">
+                                <bind_vertex_input semantic="UVSET0" input_semantic="TEXCOORD" input_set="0" />
+                            </instance_material>
+                        </technique_common>
+                    </bind_material>
+                </instance_geometry>
+            </node>
+            <node id="ID15" name="group_1">
+                <matrix>1.963935 -4.336809e-018 0 -68.35112 4.336809e-018 1.963935 0 258.791 0 0 1.963935 40.4385 0 0 0 1</matrix>
+                <instance_geometry url="#ID16">
+                    <bind_material>
+                        <technique_common>
+                            <instance_material symbol="Material2" target="#ID6">
+                                <bind_vertex_input semantic="UVSET0" input_semantic="TEXCOORD" input_set="0" />
+                            </instance_material>
+                            <instance_material symbol="Material3" target="#ID11">
+                                <bind_vertex_input semantic="UVSET0" input_semantic="TEXCOORD" input_set="0" />
+                            </instance_material>
+                        </technique_common>
+                    </bind_material>
+                </instance_geometry>
+            </node>
+        </node>
+    </library_nodes>
+    <library_geometries>
+        <geometry id="ID5">
+            <mesh>
+                <source id="ID8">
+                    <float_array id="ID13" count="126">29.29134 15.47244 1.496063 -2.220446e-016 15.47244 0.2362205 29.29134 15.47244 0.1574803 29.29134 15.47244 0.1574803 -2.220446e-016 15.47244 0.2362205 29.29134 15.47244 1.496063 0.07874016 15.55118 1.574803 -2.220446e-016 15.47244 0.2362205 29.29134 15.47244 1.496063 29.29134 15.47244 1.496063 -2.220446e-016 15.47244 0.2362205 0.07874016 15.55118 1.574803 29.29134 15.47244 0.1574803 -2.220446e-016 15.47244 0.2362205 0.2362205 17.71654 0.2362205 0.2362205 17.71654 0.2362205 -2.220446e-016 15.47244 0.2362205 29.29134 15.47244 0.1574803 0.2362205 17.71654 0.2362205 -2.220446e-016 15.47244 0.2362205 0.07874016 15.55118 1.574803 0.07874016 15.55118 1.574803 -2.220446e-016 15.47244 0.2362205 0.2362205 17.71654 0.2362205 29.29134 15.47244 1.496063 0.2362205 17.71654 1.574803 0.07874016 15.55118 1.574803 0.07874016 15.55118 1.574803 0.2362205 17.71654 1.574803 29.29134 15.47244 1.496063 29.33071 23.14961 1.220472 29.33071 23.14961 1.220472 0.2362205 17.71654 1.574803 0.2362205 17.71654 1.574803 3.779528 26.81102 1.220472 3.779528 26.81102 1.220472 3.779528 26.81102 1.220472 3.779528 26.81102 1.220472 3.779528 26.81102 1.220472 3.779528 26.81102 1.220472 29.33071 23.14961 1.220472 29.33071 23.14961 1.220472</float_array>
+                    <technique_common>
+                        <accessor count="42" source="#ID13" stride="3">
+                            <param name="X" type="float" />
+                            <param name="Y" type="float" />
+                            <param name="Z" type="float" />
+                        </accessor>
+                    </technique_common>
+                </source>
+                <source id="ID9">
+                    <float_array id="ID14" count="126">8.832897e-019 -1 0 8.832897e-019 -1 0 8.832897e-019 -1 0 -8.832897e-019 1 -0 -8.832897e-019 1 -0 -8.832897e-019 1 -0 -0.002532054 -0.9982624 0.05887026 -0.002532054 -0.9982624 0.05887026 -0.002532054 -0.9982624 0.05887026 0.002532054 0.9982624 -0.05887026 0.002532054 0.9982624 -0.05887026 0.002532054 0.9982624 -0.05887026 0.007390569 0.1304752 -0.991424 -0.002688162 0.0002829644 -0.9999963 0.004389455 0.04876336 -0.9988007 -0.004389455 -0.04876336 0.9988007 0.002688162 -0.0002829644 0.9999963 -0.007390569 -0.1304752 0.991424 -0.9752032 0.2211027 0.009609949 -0.9931459 0.1045417 0.05227084 -0.9949927 0.093762 0.03461586 0.9949927 -0.093762 -0.03461586 0.9931459 -0.1045417 -0.05227084 0.9752032 -0.2211027 -0.009609949 0.005110827 0.03434514 0.999397 0.003976796 0.01640632 0.9998575 0.00269488 -0.0001959912 0.9999963 -0.00269488 0.0001959912 -0.9999963 -0.003976796 -0.01640632 -0.9998575 -0.005110827 -0.03434514 -0.999397 0.009196932 0.1303557 -0.9914246 -0.009196932 -0.1303557 0.9914246 -0.9758321 0.2185214 -5.056631e-018 0.9758321 -0.2185214 5.056631e-018 0.00548519 0.03663055 0.9993138 -0.00548519 -0.03663055 -0.9993138 0.0146102 0.1019572 -0.9946815 -0.0146102 -0.1019572 0.9946815 -0.9317774 0.3630302 0 0.9317774 -0.3630302 -0 0.00513686 0.03584755 0.9993441 -0.00513686 -0.03584755 -0.9993441</float_array>
+                    <technique_common>
+                        <accessor count="42" source="#ID14" stride="3">
+                            <param name="X" type="float" />
+                            <param name="Y" type="float" />
+                            <param name="Z" type="float" />
+                        </accessor>
+                    </technique_common>
+                </source>
+                <vertices id="ID10">
+                    <input semantic="POSITION" source="#ID8" />
+                    <input semantic="NORMAL" source="#ID9" />
+                </vertices>
+                <triangles count="13" material="Material2">
+                    <input offset="0" semantic="VERTEX" source="#ID10" />
+                    <p>0 1 2 6 7 8 6 7 8 12 13 14 18 19 20 0 1 2 24 25 26 30 12 14 18 20 32 24 34 25 30 14 36 38 18 32 40 34 24</p>
+                </triangles>
+                <triangles count="13" material="Material3">
+                    <input offset="0" semantic="VERTEX" source="#ID10" />
+                    <p>3 4 5 9 10 11 9 10 11 15 16 17 21 22 23 3 4 5 27 28 29 15 17 31 33 21 23 28 35 29 37 15 31 33 23 39 29 35 41</p>
+                </triangles>
+            </mesh>
+        </geometry>
+        <geometry id="ID16">
+            <mesh>
+                <source id="ID17">
+                    <float_array id="ID20" count="144">34.13386 15.47244 0.2362205 4.80315 15.47244 0.1574803 33.89764 17.6378 0.2362205 33.89764 17.6378 0.2362205 4.80315 15.47244 0.1574803 34.13386 15.47244 0.2362205 30.31496 26.73228 1.220472 30.31496 26.73228 1.220472 33.89764 17.6378 1.574803 34.13386 15.47244 0.2362205 33.89764 17.6378 0.2362205 33.89764 17.6378 0.2362205 34.13386 15.47244 0.2362205 33.89764 17.6378 1.574803 34.05512 15.55118 1.574803 4.80315 15.47244 0.1574803 34.13386 15.47244 0.2362205 34.13386 15.47244 0.2362205 4.80315 15.47244 0.1574803 34.05512 15.55118 1.574803 34.05512 15.55118 1.574803 4.80315 15.47244 0.1574803 34.13386 15.47244 0.2362205 34.13386 15.47244 0.2362205 4.80315 15.47244 0.1574803 34.05512 15.55118 1.574803 30.31496 26.73228 1.220472 30.31496 26.73228 1.220472 4.88189 23.22835 1.220472 4.88189 23.22835 1.220472 34.05512 15.55118 1.574803 34.05512 15.55118 1.574803 34.05512 15.55118 1.574803 4.80315 15.47244 1.496063 4.80315 15.47244 0.1574803 4.80315 15.47244 0.1574803 4.80315 15.47244 1.496063 34.05512 15.55118 1.574803 33.89764 17.6378 1.574803 30.31496 26.73228 1.220472 4.88189 23.22835 1.220472 4.88189 23.22835 1.220472 30.31496 26.73228 1.220472 33.89764 17.6378 1.574803 34.05512 15.55118 1.574803 4.80315 15.47244 1.496063 4.80315 15.47244 1.496063 34.05512 15.55118 1.574803</float_array>
+                    <technique_common>
+                        <accessor count="48" source="#ID20" stride="3">
+                            <param name="X" type="float" />
+                            <param name="Y" type="float" />
+                            <param name="Z" type="float" />
+                        </accessor>
+                    </technique_common>
+                </source>
+                <source id="ID18">
+                    <float_array id="ID21" count="144">0.002684554 0.0002928604 -0.9999964 -0.01473734 0.12288 -0.9923121 -0.000787119 0.04689963 -0.9988993 0.000787119 -0.04689963 0.9988993 0.01473734 -0.12288 0.9923121 -0.002684554 -0.0002928604 0.9999964 -0.007277174 0.110315 -0.99387 0.007277174 -0.110315 0.99387 0.9725871 0.2323356 0.009718386 0.9955834 0.08736469 0.03436596 0.9719196 0.2353134 -1.202976e-016 -0.9719196 -0.2353134 1.202976e-016 -0.9955834 -0.08736469 -0.03436596 -0.9725871 -0.2323356 -0.009718386 -0.0001576182 -0.9982749 0.05871278 -0.0001576182 -0.9982749 0.05871278 -0.0001576182 -0.9982749 0.05871278 0.0001576182 0.9982749 -0.05871278 0.0001576182 0.9982749 -0.05871278 0.0001576182 0.9982749 -0.05871278 -0.0001576182 -0.9982749 0.05871278 -0.0001576182 -0.9982749 0.05871278 -0.0001576182 -0.9982749 0.05871278 0.0001576182 0.9982749 -0.05871278 0.0001576182 0.9982749 -0.05871278 0.0001576182 0.9982749 -0.05871278 0.9304084 0.3665245 -2.676506e-016 -0.9304084 -0.3665245 2.676506e-016 -0.01872985 0.1359493 -0.9905387 0.01872985 -0.1359493 0.9905387 0.9957011 0.07514725 0.05415023 -0.9957011 -0.07514725 -0.05415023 0.00269178 -0.9999964 0 0.00269178 -0.9999964 0 0.00269178 -0.9999964 0 -0.00269178 0.9999964 -0 -0.00269178 0.9999964 -0 -0.00269178 0.9999964 -0 -0.003787425 0.01626622 0.9998605 -0.005087832 0.03692966 0.9993049 -0.005301222 0.03582462 0.999344 0.005301222 -0.03582462 -0.999344 0.005087832 -0.03692966 -0.9993049 0.003787425 -0.01626622 -0.9998605 -0.002691234 -0.000203112 0.9999964 -0.005229389 0.03392175 0.9994108 0.005229389 -0.03392175 -0.9994108 0.002691234 0.000203112 -0.9999964</float_array>
+                    <technique_common>
+                        <accessor count="48" source="#ID21" stride="3">
+                            <param name="X" type="float" />
+                            <param name="Y" type="float" />
+                            <param name="Z" type="float" />
+                        </accessor>
+                    </technique_common>
+                </source>
+                <vertices id="ID19">
+                    <input semantic="POSITION" source="#ID17" />
+                    <input semantic="NORMAL" source="#ID18" />
+                </vertices>
+                <triangles count="13" material="Material2">
+                    <input offset="0" semantic="VERTEX" source="#ID19" />
+                    <p>0 1 2 2 1 6 8 9 10 14 15 16 20 21 22 26 8 10 6 1 28 8 30 9 32 33 34 32 33 34 38 39 40 44 38 45 38 40 45</p>
+                </triangles>
+                <triangles count="13" material="Material3">
+                    <input offset="0" semantic="VERTEX" source="#ID19" />
+                    <p>3 4 5 7 4 3 11 12 13 17 18 19 23 24 25 11 13 27 29 4 7 12 31 13 35 36 37 35 36 37 41 42 43 46 43 47 46 41 43</p>
+                </triangles>
+            </mesh>
+        </geometry>
+    </library_geometries>
+    <library_materials>
+        <material id="ID6" name="Color_003">
+            <instance_effect url="#ID7" />
+        </material>
+        <material id="ID11" name="__White_">
+            <instance_effect url="#ID12" />
+        </material>
+    </library_materials>
+    <library_effects>
+        <effect id="ID7">
+            <profile_COMMON>
+                <technique sid="COMMON">
+                    <lambert>
+                        <diffuse>
+                            <color>0.6666667 0.6666667 0.6666667 1</color>
+                        </diffuse>
+                    </lambert>
+                </technique>
+            </profile_COMMON>
+        </effect>
+        <effect id="ID12">
+            <profile_COMMON>
+                <technique sid="COMMON">
+                    <lambert>
+                        <diffuse>
+                            <color>1 1 1 1</color>
+                        </diffuse>
+                    </lambert>
+                </technique>
+            </profile_COMMON>
+        </effect>
+    </library_effects>
+    <scene>
+        <instance_visual_scene url="#ID1" />
+    </scene>
+</COLLADA>
@@ -0,0 +1,114 @@
+<?xml version="1.0" encoding="UTF-8" standalone="no" ?>
+<COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1">
+    <asset>
+        <contributor>
+            <authoring_tool>SketchUp 15.3.331</authoring_tool>
+        </contributor>
+        <created>2015-05-26T23:42:29Z</created>
+        <modified>2015-05-26T23:42:29Z</modified>
+        <unit meter="0.0254" name="inch" />
+        <up_axis>Z_UP</up_axis>
+    </asset>
+    <library_visual_scenes>
+        <visual_scene id="ID1">
+            <node name="SketchUp">
+                <node id="ID2" name="instance_0">
+                    <matrix>0.001745328 -0.9999985 0 70.84882 0.9999985 0.001745328 0 -0.03157813 -0 0 1 -19.50896 0 0 0 1</matrix>
+                    <instance_node url="#ID3" />
+                </node>
+            </node>
+        </visual_scene>
+    </library_visual_scenes>
+    <library_nodes>
+        <node id="ID3" name="ske2A3">
+            <node id="ID4" name="group_0">
+                <matrix>1.963935 -1.149254e-016 0 -217.1153 1.149254e-016 1.963935 0 16.85582 0 0 1.963935 19.40739 0 0 0 1</matrix>
+                <instance_geometry url="#ID5">
+                    <bind_material>
+                        <technique_common>
+                            <instance_material symbol="Material2" target="#ID6">
+                                <bind_vertex_input semantic="UVSET0" input_semantic="TEXCOORD" input_set="0" />
+                            </instance_material>
+                            <instance_material symbol="Material3" target="#ID11">
+                                <bind_vertex_input semantic="UVSET0" input_semantic="TEXCOORD" input_set="0" />
+                            </instance_material>
+                        </technique_common>
+                    </bind_material>
+                </instance_geometry>
+            </node>
+        </node>
+    </library_nodes>
+    <library_geometries>
+        <geometry id="ID5">
+            <mesh>
+                <source id="ID8">
+                    <float_array id="ID13" count="120">173.5827 65.35433 28.34646 173.5433 59.72441 29.01575 162.197 60.69059 28.48352 162.197 60.69059 28.48352 173.5433 59.72441 29.01575 173.5827 65.35433 28.34646 162.1654 66.5748 27.75591 162.1654 66.5748 27.75591 182.126 64.29134 28.77953 182.126 64.29134 28.77953 173.5827 65.35433 28.34646 162.1654 66.5748 27.75591 173.5433 59.76378 30.43307 173.5433 59.76378 30.43307 162.1654 66.5748 27.75591 173.5827 65.35433 28.34646 182.1654 58.62205 31.06299 182.126 64.29134 28.77953 182.126 64.29134 28.77953 182.1654 58.62205 31.06299 182.1654 58.62205 29.44882 182.1654 58.62205 29.44882 162.1654 60.7874 29.93396 162.1654 60.7874 29.93396 191.0236 57.48031 31.22047 191.0236 57.48031 31.22047 191.063 63.07087 29.29134 191.063 63.07087 29.29134 191.063 63.07087 29.29134 191.063 63.07087 29.29134 191.0236 57.48031 29.96063 191.0236 57.48031 29.96063 207.0079 55.62992 31.77165 207.0079 55.62992 31.77165 207.0472 60.90551 30.11811 207.0472 60.90551 30.11811 207.0472 60.90551 30.11811 207.0472 60.90551 30.11811 207.0079 55.62992 30.55118 207.0079 55.62992 30.55118</float_array>
+                    <technique_common>
+                        <accessor count="40" source="#ID13" stride="3">
+                            <param name="X" type="float" />
+                            <param name="Y" type="float" />
+                            <param name="Z" type="float" />
+                        </accessor>
+                    </technique_common>
+                </source>
+                <source id="ID9">
+                    <float_array id="ID14" count="120">0.03634531 -0.1188937 -0.9922416 0.03587327 -0.1179703 -0.9923689 0.03771441 -0.121225 -0.9919083 -0.03771441 0.121225 0.9919083 -0.03587327 0.1179703 0.9923689 -0.03634531 0.1188937 0.9922416 0.0382097 -0.1224293 -0.9917414 -0.0382097 0.1224293 0.9917414 0.03775177 -0.1171551 -0.9923958 -0.03775177 0.1171551 0.9923958 -0.01224891 0.3540955 0.9351291 -0.00989876 0.3514344 0.9361602 -0.01674714 0.3501002 0.9365625 0.01674714 -0.3501002 -0.9365625 0.00989876 -0.3514344 -0.9361602 0.01224891 -0.3540955 -0.9351291 0.01322657 0.3694758 0.9291462 0.006999627 0.365608 0.9307426 -0.006999627 -0.365608 -0.9307426 -0.01322657 -0.3694758 -0.9291462 0.03836032 -0.1173106 -0.9923541 -0.03836032 0.1173106 0.9923541 -0.009366748 0.3522106 0.9358739 0.009366748 -0.3522106 -0.9358739 0.005662451 0.3347006 0.9423075 -0.005662451 -0.3347006 -0.9423075 0.03866059 -0.1186285 -0.9921858 -0.03866059 0.1186285 0.9921858 -0.004253652 0.3235305 0.9462082 0.004253652 -0.3235305 -0.9462082 0.03750441 -0.1151827 -0.9926361 -0.03750441 0.1151827 0.9926361 -0.005596535 0.305409 0.9522048 0.005596535 -0.305409 -0.9522048 0.02892951 -0.08956398 -0.9955608 -0.02892951 0.08956398 0.9955608 -0.008830529 0.2991342 0.9541702 0.008830529 -0.2991342 -0.9541702 0.02731657 -0.08198631 -0.996259 -0.02731657 0.08198631 0.996259</float_array>
+                    <technique_common>
+                        <accessor count="40" source="#ID14" stride="3">
+                            <param name="X" type="float" />
+                            <param name="Y" type="float" />
+                            <param name="Z" type="float" />
+                        </accessor>
+                    </technique_common>
+                </source>
+                <vertices id="ID10">
+                    <input semantic="POSITION" source="#ID8" />
+                    <input semantic="NORMAL" source="#ID9" />
+                </vertices>
+                <triangles count="16" material="Material2">
+                    <input offset="0" semantic="VERTEX" source="#ID10" />
+                    <p>0 1 2 0 2 6 8 1 0 10 11 12 16 17 10 20 1 8 12 11 22 16 10 12 24 17 16 26 20 8 28 17 24 26 30 20 32 28 24 34 30 26 36 28 32 34 38 30</p>
+                </triangles>
+                <triangles count="16" material="Material3">
+                    <input offset="0" semantic="VERTEX" source="#ID10" />
+                    <p>3 4 5 7 3 5 5 4 9 13 14 15 15 18 19 9 4 21 23 14 13 13 15 19 19 18 25 9 21 27 25 18 29 21 31 27 25 29 33 27 31 35 33 29 37 31 39 35</p>
+                </triangles>
+            </mesh>
+        </geometry>
+    </library_geometries>
+    <library_materials>
+        <material id="ID6" name="__White_">
+            <instance_effect url="#ID7" />
+        </material>
+        <material id="ID11" name="__Gray_">
+            <instance_effect url="#ID12" />
+        </material>
+    </library_materials>
+    <library_effects>
+        <effect id="ID7">
+            <profile_COMMON>
+                <technique sid="COMMON">
+                    <lambert>
+                        <diffuse>
+                            <color>1 1 1 1</color>
+                        </diffuse>
+                    </lambert>
+                </technique>
+            </profile_COMMON>
+        </effect>
+        <effect id="ID12">
+            <profile_COMMON>
+                <technique sid="COMMON">
+                    <lambert>
+                        <diffuse>
+                            <color>0.5019608 0.5019608 0.5019608 1</color>
+                        </diffuse>
+                    </lambert>
+                </technique>
+            </profile_COMMON>
+        </effect>
+    </library_effects>
+    <scene>
+        <instance_visual_scene url="#ID1" />
+    </scene>
+</COLLADA>
@@ -0,0 +1,114 @@
+<?xml version="1.0" encoding="UTF-8" standalone="no" ?>
+<COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1">
+    <asset>
+        <contributor>
+            <authoring_tool>SketchUp 15.3.331</authoring_tool>
+        </contributor>
+        <created>2015-05-26T23:42:19Z</created>
+        <modified>2015-05-26T23:42:19Z</modified>
+        <unit meter="0.0254" name="inch" />
+        <up_axis>Z_UP</up_axis>
+    </asset>
+    <library_visual_scenes>
+        <visual_scene id="ID1">
+            <node name="SketchUp">
+                <node id="ID2" name="instance_0">
+                    <matrix>0.001745328 -0.9999985 0 70.84882 0.9999985 0.001745328 0 -0.03157813 -0 0 1 -19.50896 0 0 0 1</matrix>
+                    <instance_node url="#ID3" />
+                </node>
+            </node>
+        </visual_scene>
+    </library_visual_scenes>
+    <library_nodes>
+        <node id="ID3" name="ske2A3">
+            <node id="ID4" name="group_0">
+                <matrix>1.963935 -1.144917e-016 0 -217.1153 1.144917e-016 1.963935 0 16.85582 0 0 1.963935 19.40739 0 0 0 1</matrix>
+                <instance_geometry url="#ID5">
+                    <bind_material>
+                        <technique_common>
+                            <instance_material symbol="Material2" target="#ID6">
+                                <bind_vertex_input semantic="UVSET0" input_semantic="TEXCOORD" input_set="0" />
+                            </instance_material>
+                            <instance_material symbol="Material3" target="#ID11">
+                                <bind_vertex_input semantic="UVSET0" input_semantic="TEXCOORD" input_set="0" />
+                            </instance_material>
+                        </technique_common>
+                    </bind_material>
+                </instance_geometry>
+            </node>
+        </node>
+    </library_nodes>
+    <library_geometries>
+        <geometry id="ID5">
+            <mesh>
+                <source id="ID8">
+                    <float_array id="ID13" count="96">137.2835 66.92913 26.77165 121.9291 59.25197 27.79528 121.9291 66.92913 26.25984 121.9291 66.92913 26.25984 121.9291 59.25197 27.79528 137.2835 66.92913 26.77165 137.2835 59.25197 29.96063 137.2835 66.92913 26.77165 121.9291 66.92913 26.25984 121.9291 66.92913 26.25984 137.2835 66.92913 26.77165 137.2835 59.25197 29.96063 137.2835 59.25197 27.79528 137.2835 59.25197 27.79528 121.9291 59.25197 29.33071 121.9291 59.25197 29.33071 152.3228 59.25197 30.23622 152.3228 59.25197 30.23622 152.3622 66.92913 27.44094 152.3622 66.92913 27.44094 152.3622 66.92913 27.44094 152.3622 66.92913 27.44094 152.3228 59.25197 28.4252 152.3228 59.25197 28.4252 162.1654 59.25197 30.51181 162.1654 59.25197 30.51181 162.1654 66.5748 27.75591 162.1654 66.5748 27.75591 162.1654 66.5748 27.75591 162.1654 66.5748 27.75591 162.2047 59.25197 28.66142 162.2047 59.25197 28.66142</float_array>
+                    <technique_common>
+                        <accessor count="32" source="#ID13" stride="3">
+                            <param name="X" type="float" />
+                            <param name="Y" type="float" />
+                            <param name="Z" type="float" />
+                        </accessor>
+                    </technique_common>
+                </source>
+                <source id="ID9">
+                    <float_array id="ID14" count="96">0.02682621 -0.1417739 -0.9895355 0.02297159 -0.1771324 -0.9839189 0.03266858 -0.1960115 -0.9800573 -0.03266858 0.1960115 0.9800573 -0.02297159 0.1771324 0.9839189 -0.02682621 0.1417739 0.9895355 -0.02493687 0.3815814 0.9239988 -0.02780814 0.3770153 0.9257895 -0.03575652 0.3750234 0.9263255 0.03575652 -0.3750234 -0.9263255 0.02780814 -0.3770153 -0.9257895 0.02493687 -0.3815814 -0.9239988 0.02165175 -0.1314105 -0.9910916 -0.02165175 0.1314105 0.9910916 -0.03806375 0.3711215 0.9278038 0.03806375 -0.3711215 -0.9278038 -0.03001613 0.3487806 0.9367236 0.03001613 -0.3487806 -0.9367236 0.03483823 -0.1279932 -0.991163 -0.03483823 0.1279932 0.991163 -0.03222275 0.344211 0.9383392 0.03222275 -0.344211 -0.9383392 0.03366092 -0.1263005 -0.9914208 -0.03366092 0.1263005 0.9914208 -0.02130276 0.3477379 0.9373497 0.02130276 -0.3477379 -0.9373497 0.02520877 -0.1245398 -0.9918943 -0.02520877 0.1245398 0.9918943 -0.01733589 0.3521731 0.9357743 0.01733589 -0.3521731 -0.9357743 0.02371739 -0.1225611 -0.9921775 -0.02371739 0.1225611 0.9921775</float_array>
+                    <technique_common>
+                        <accessor count="32" source="#ID14" stride="3">
+                            <param name="X" type="float" />
+                            <param name="Y" type="float" />
+                            <param name="Z" type="float" />
+                        </accessor>
+                    </technique_common>
+                </source>
+                <vertices id="ID10">
+                    <input semantic="POSITION" source="#ID8" />
+                    <input semantic="NORMAL" source="#ID9" />
+                </vertices>
+                <triangles count="12" material="Material2">
+                    <input offset="0" semantic="VERTEX" source="#ID10" />
+                    <p>0 1 2 6 7 8 12 1 0 6 8 14 16 7 6 18 12 0 20 7 16 18 22 12 24 20 16 26 22 18 24 28 20 30 22 26</p>
+                </triangles>
+                <triangles count="12" material="Material3">
+                    <input offset="0" semantic="VERTEX" source="#ID10" />
+                    <p>3 4 5 9 10 11 5 4 13 15 9 11 11 10 17 5 13 19 17 10 21 13 23 19 17 21 25 19 23 27 21 29 25 27 23 31</p>
+                </triangles>
+            </mesh>
+        </geometry>
+    </library_geometries>
+    <library_materials>
+        <material id="ID6" name="__White_">
+            <instance_effect url="#ID7" />
+        </material>
+        <material id="ID11" name="__Gray_">
+            <instance_effect url="#ID12" />
+        </material>
+    </library_materials>
+    <library_effects>
+        <effect id="ID7">
+            <profile_COMMON>
+                <technique sid="COMMON">
+                    <lambert>
+                        <diffuse>
+                            <color>1 1 1 1</color>
+                        </diffuse>
+                    </lambert>
+                </technique>
+            </profile_COMMON>
+        </effect>
+        <effect id="ID12">
+            <profile_COMMON>
+                <technique sid="COMMON">
+                    <lambert>
+                        <diffuse>
+                            <color>0.5019608 0.5019608 0.5019608 1</color>
+                        </diffuse>
+                    </lambert>
+                </technique>
+            </profile_COMMON>
+        </effect>
+    </library_effects>
+    <scene>
+        <instance_visual_scene url="#ID1" />
+    </scene>
+</COLLADA>
@@ -0,0 +1,114 @@
+<?xml version="1.0" encoding="UTF-8" standalone="no" ?>
+<COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1">
+    <asset>
+        <contributor>
+            <authoring_tool>SketchUp 15.3.331</authoring_tool>
+        </contributor>
+        <created>2015-05-26T23:41:47Z</created>
+        <modified>2015-05-26T23:41:47Z</modified>
+        <unit meter="0.0254" name="inch" />
+        <up_axis>Z_UP</up_axis>
+    </asset>
+    <library_visual_scenes>
+        <visual_scene id="ID1">
+            <node name="SketchUp">
+                <node id="ID2" name="instance_0">
+                    <matrix>0.001745328 -0.9999985 0 70.84882 0.9999985 0.001745328 0 -0.03157813 -0 0 1 -19.50896 0 0 0 1</matrix>
+                    <instance_node url="#ID3" />
+                </node>
+            </node>
+        </visual_scene>
+    </library_visual_scenes>
+    <library_nodes>
+        <node id="ID3" name="ske2A3">
+            <node id="ID4" name="group_0">
+                <matrix>1.963935 -1.140581e-016 0 -217.1153 1.140581e-016 1.963935 0 16.85582 0 0 1.963935 19.40739 0 0 0 1</matrix>
+                <instance_geometry url="#ID5">
+                    <bind_material>
+                        <technique_common>
+                            <instance_material symbol="Material2" target="#ID6">
+                                <bind_vertex_input semantic="UVSET0" input_semantic="TEXCOORD" input_set="0" />
+                            </instance_material>
+                            <instance_material symbol="Material3" target="#ID11">
+                                <bind_vertex_input semantic="UVSET0" input_semantic="TEXCOORD" input_set="0" />
+                            </instance_material>
+                        </technique_common>
+                    </bind_material>
+                </instance_geometry>
+            </node>
+        </node>
+    </library_nodes>
+    <library_geometries>
+        <geometry id="ID5">
+            <mesh>
+                <source id="ID8">
+                    <float_array id="ID13" count="120">58.93701 66.5748 27.75591 47.55906 59.6063 29.13386 47.59843 65.15748 28.30709 47.59843 65.15748 28.30709 47.55906 59.6063 29.13386 58.93701 66.5748 27.75591 39.33071 58.62205 29.56693 39.33071 58.62205 29.56693 58.97638 60.95635 29.96063 58.93701 66.5748 27.75591 47.59843 65.15748 28.30709 47.59843 65.15748 28.30709 58.93701 66.5748 27.75591 58.97638 60.95635 29.96063 58.97638 60.7874 28.50244 58.97638 60.7874 28.50244 39.29134 64.13386 28.74016 39.29134 64.13386 28.74016 47.59843 59.64567 30.43307 47.59843 59.64567 30.43307 30.11811 57.59843 29.96063 30.11811 57.59843 29.96063 39.29134 64.13386 28.74016 39.29134 64.13386 28.74016 30.07874 63.0315 29.25197 30.07874 63.0315 29.25197 39.33071 58.62205 30.7874 39.33071 58.62205 30.7874 14.13386 55.62992 30.55118 14.13386 55.62992 30.55118 30.07874 63.0315 29.25197 30.07874 63.0315 29.25197 14.05512 60.94488 30.07874 14.05512 60.94488 30.07874 30.11811 57.59843 31.1811 30.11811 57.59843 31.1811 14.05512 60.94488 30.07874 14.05512 60.94488 30.07874 14.13386 55.62992 31.77165 14.13386 55.62992 31.77165</float_array>
+                    <technique_common>
+                        <accessor count="40" source="#ID13" stride="3">
+                            <param name="X" type="float" />
+                            <param name="Y" type="float" />
+                            <param name="Z" type="float" />
+                        </accessor>
+                    </technique_common>
+                </source>
+                <source id="ID9">
+                    <float_array id="ID14" count="120">-0.03804767 -0.1336973 -0.9902916 -0.03393509 -0.1443297 -0.9889476 -0.03164446 -0.1472782 -0.9885888 0.03164446 0.1472782 0.9885888 0.03393509 0.1443297 0.9889476 0.03804767 0.1336973 0.9902916 -0.0294332 -0.1482161 -0.9885169 0.0294332 0.1482161 0.9885169 -0.001092279 0.3636841 0.9315217 -0.000409118 0.3652875 0.9308947 0.0008682838 0.3607536 0.9326607 -0.0008682838 -0.3607536 -0.9326607 0.000409118 -0.3652875 -0.9308947 0.001092279 -0.3636841 -0.9315217 -0.04154783 -0.1281008 -0.9908905 0.04154783 0.1281008 0.9908905 -0.03197393 -0.1454677 -0.9888462 0.03197393 0.1454677 0.9888462 -0.0004293308 0.3574471 0.9339333 0.0004293308 -0.3574471 -0.9339333 -0.0272016 -0.1325668 -0.9908007 0.0272016 0.1325668 0.9908007 0.004716089 0.3506517 0.9364941 -0.004716089 -0.3506517 -0.9364941 -0.03271636 -0.125567 -0.9915455 0.03271636 0.125567 0.9915455 0.002870077 0.3456667 0.938353 -0.002870077 -0.3456667 -0.938353 -0.03566361 -0.09775055 -0.9945717 0.03566361 0.09775055 0.9945717 0.00538455 0.3372325 0.941406 -0.00538455 -0.3372325 -0.941406 -0.03975437 -0.08905412 -0.9952331 0.03975437 0.08905412 0.9952331 0.003353341 0.3312469 0.9435382 -0.003353341 -0.3312469 -0.9435382 -0.0006590497 0.3100183 0.9507304 0.0006590497 -0.3100183 -0.9507304 -0.002169095 0.3034651 0.9528401 0.002169095 -0.3034651 -0.9528401</float_array>
+                    <technique_common>
+                        <accessor count="40" source="#ID14" stride="3">
+                            <param name="X" type="float" />
+                            <param name="Y" type="float" />
+                            <param name="Z" type="float" />
+                        </accessor>
+                    </technique_common>
+                </source>
+                <vertices id="ID10">
+                    <input semantic="POSITION" source="#ID8" />
+                    <input semantic="NORMAL" source="#ID9" />
+                </vertices>
+                <triangles count="16" material="Material2">
+                    <input offset="0" semantic="VERTEX" source="#ID10" />
+                    <p>0 1 2 2 1 6 8 9 10 14 1 0 2 6 16 8 10 18 6 20 16 18 10 22 16 20 24 18 22 26 20 28 24 26 22 30 24 28 32 26 30 34 34 30 36 34 36 38</p>
+                </triangles>
+                <triangles count="16" material="Material3">
+                    <input offset="0" semantic="VERTEX" source="#ID10" />
+                    <p>3 4 5 7 4 3 11 12 13 5 4 15 17 7 3 19 11 13 17 21 7 23 11 19 25 21 17 27 23 19 25 29 21 31 23 27 33 29 25 35 31 27 37 31 35 39 37 35</p>
+                </triangles>
+            </mesh>
+        </geometry>
+    </library_geometries>
+    <library_materials>
+        <material id="ID6" name="__White_">
+            <instance_effect url="#ID7" />
+        </material>
+        <material id="ID11" name="__Gray_">
+            <instance_effect url="#ID12" />
+        </material>
+    </library_materials>
+    <library_effects>
+        <effect id="ID7">
+            <profile_COMMON>
+                <technique sid="COMMON">
+                    <lambert>
+                        <diffuse>
+                            <color>1 1 1 1</color>
+                        </diffuse>
+                    </lambert>
+                </technique>
+            </profile_COMMON>
+        </effect>
+        <effect id="ID12">
+            <profile_COMMON>
+                <technique sid="COMMON">
+                    <lambert>
+                        <diffuse>
+                            <color>0.5019608 0.5019608 0.5019608 1</color>
+                        </diffuse>
+                    </lambert>
+                </technique>
+            </profile_COMMON>
+        </effect>
+    </library_effects>
+    <scene>
+        <instance_visual_scene url="#ID1" />
+    </scene>
+</COLLADA>
@@ -0,0 +1,161 @@
+<?xml version="1.0" encoding="UTF-8" standalone="no" ?>
+<COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1">
+    <asset>
+        <contributor>
+            <authoring_tool>SketchUp 15.3.331</authoring_tool>
+        </contributor>
+        <created>2015-05-26T23:42:08Z</created>
+        <modified>2015-05-26T23:42:08Z</modified>
+        <unit meter="0.0254" name="inch" />
+        <up_axis>Z_UP</up_axis>
+    </asset>
+    <library_visual_scenes>
+        <visual_scene id="ID1">
+            <node name="SketchUp">
+                <node id="ID2" name="instance_0">
+                    <matrix>0.001745328 -0.9999985 0 70.84882 0.9999985 0.001745328 0 -0.03157813 -0 0 1 -19.50896 0 0 0 1</matrix>
+                    <instance_node url="#ID3" />
+                </node>
+            </node>
+        </visual_scene>
+    </library_visual_scenes>
+    <library_nodes>
+        <node id="ID3" name="ske2A3">
+            <node id="ID4" name="group_0">
+                <matrix>1.963935 -1.140581e-016 0 -217.1153 1.140581e-016 1.963935 0 16.85582 0 0 1.963935 19.40739 0 0 0 1</matrix>
+                <instance_geometry url="#ID5">
+                    <bind_material>
+                        <technique_common>
+                            <instance_material symbol="Material2" target="#ID6">
+                                <bind_vertex_input semantic="UVSET0" input_semantic="TEXCOORD" input_set="0" />
+                            </instance_material>
+                            <instance_material symbol="Material3" target="#ID11">
+                                <bind_vertex_input semantic="UVSET0" input_semantic="TEXCOORD" input_set="0" />
+                            </instance_material>
+                        </technique_common>
+                    </bind_material>
+                </instance_geometry>
+                <instance_geometry url="#ID15">
+                    <bind_material>
+                        <technique_common>
+                            <instance_material symbol="Material2" target="#ID16">
+                                <bind_vertex_input semantic="UVSET0" input_semantic="TEXCOORD" input_set="0" />
+                            </instance_material>
+                        </technique_common>
+                    </bind_material>
+                </instance_geometry>
+            </node>
+        </node>
+    </library_nodes>
+    <library_geometries>
+        <geometry id="ID5">
+            <mesh>
+                <source id="ID8">
+                    <float_array id="ID13" count="102">99.2126 66.9685 26.29921 83.85827 66.92913 26.81102 99.2126 59.25197 29.33071 99.2126 59.25197 29.33071 83.85827 66.92913 26.81102 99.2126 66.9685 26.29921 83.85827 59.25197 29.96063 83.85827 59.25197 29.96063 99.2126 66.9685 26.29921 83.85827 59.25197 27.79528 83.85827 66.92913 26.81102 83.85827 66.92913 26.81102 83.85827 59.25197 27.79528 99.2126 66.9685 26.29921 68.77953 66.92913 27.44094 68.77953 66.92913 27.44094 68.8189 59.25197 28.4252 68.77953 66.92913 27.44094 76.33858 59.25197 28.11024 76.33858 59.25197 28.11024 68.8189 59.25197 28.4252 68.77953 66.92913 27.44094 99.2126 59.25197 27.79528 99.2126 59.25197 27.79528 68.8189 59.25197 30.23622 68.8189 59.25197 30.23622 58.97638 59.25197 28.70079 58.97638 59.25197 28.70079 58.93701 66.5748 27.75591 58.93701 66.5748 27.75591 58.93701 66.5748 27.75591 58.93701 66.5748 27.75591 58.97638 59.25197 30.62992 58.97638 59.25197 30.62992</float_array>
+                    <technique_common>
+                        <accessor count="34" source="#ID13" stride="3">
+                            <param name="X" type="float" />
+                            <param name="Y" type="float" />
+                            <param name="Z" type="float" />
+                        </accessor>
+                    </technique_common>
+                </source>
+                <source id="ID9">
+                    <float_array id="ID14" count="102">0.03007387 0.365487 0.9303305 0.03702604 0.3770976 0.9254331 0.03254441 0.3698361 0.9285269 -0.03254441 -0.3698361 -0.9285269 -0.03702604 -0.3770976 -0.9254331 -0.03007387 -0.365487 -0.9303305 0.0348804 0.3734313 0.9270018 -0.0348804 -0.3734313 -0.9270018 -0.009746117 -0.1715996 -0.9851186 -0.03218648 -0.1366818 -0.990092 -0.03707021 -0.127129 -0.9911932 0.03707021 0.127129 0.9911932 0.03218648 0.1366818 0.990092 0.009746117 0.1715996 0.9851186 0.02096268 0.3482456 0.9371689 -0.02096268 -0.3482456 -0.9371689 -0.03461144 -0.1272104 -0.9912717 -0.03455417 -0.1273749 -0.9912526 -0.04145418 -0.1271596 -0.9910156 0.04145418 0.1271596 0.9910156 0.03461144 0.1272104 0.9912717 0.03455417 0.1273749 0.9912526 6.679878e-018 -0.1903334 -0.9817195 -6.679878e-018 0.1903334 0.9817195 0.02167083 0.3471584 0.9375561 -0.02167083 -0.3471584 -0.9375561 -0.02738866 -0.1277233 -0.9914316 0.02738866 0.1277233 0.9914316 0.02865303 0.3551468 0.9343713 -0.02865303 -0.3551468 -0.9343713 -0.02711419 -0.1280677 -0.9913947 0.02711419 0.1280677 0.9913947 0.03720642 0.365263 0.9301605 -0.03720642 -0.365263 -0.9301605</float_array>
+                    <technique_common>
+                        <accessor count="34" source="#ID14" stride="3">
+                            <param name="X" type="float" />
+                            <param name="Y" type="float" />
+                            <param name="Z" type="float" />
+                        </accessor>
+                    </technique_common>
+                </source>
+                <vertices id="ID10">
+                    <input semantic="POSITION" source="#ID8" />
+                    <input semantic="NORMAL" source="#ID9" />
+                </vertices>
+                <triangles count="13" material="Material2">
+                    <input offset="0" semantic="VERTEX" source="#ID10" />
+                    <p>0 1 2 2 1 6 8 9 10 1 14 6 10 16 17 16 10 18 18 10 9 8 22 9 6 14 24 16 26 17 24 14 28 17 26 30 24 28 32</p>
+                </triangles>
+                <triangles count="13" material="Material3">
+                    <input offset="0" semantic="VERTEX" source="#ID10" />
+                    <p>3 4 5 7 4 3 11 12 13 7 15 4 12 11 19 19 11 20 21 20 11 12 23 13 25 15 7 21 27 20 29 15 25 31 27 21 33 29 25</p>
+                </triangles>
+            </mesh>
+        </geometry>
+        <geometry id="ID15">
+            <mesh>
+                <source id="ID18">
+                    <float_array id="ID20" count="9">58.97638 60.7874 28.50244 58.93701 66.5748 27.75591 58.97638 60.95635 29.96063</float_array>
+                    <technique_common>
+                        <accessor count="3" source="#ID20" stride="3">
+                            <param name="X" type="float" />
+                            <param name="Y" type="float" />
+                            <param name="Z" type="float" />
+                        </accessor>
+                    </technique_common>
+                </source>
+                <vertices id="ID19">
+                    <input semantic="POSITION" source="#ID18" />
+                </vertices>
+                <lines count="2" material="Material2">
+                    <input offset="0" semantic="VERTEX" source="#ID19" />
+                    <p>1 0 2 1</p>
+                </lines>
+            </mesh>
+        </geometry>
+    </library_geometries>
+    <library_materials>
+        <material id="ID6" name="__White_">
+            <instance_effect url="#ID7" />
+        </material>
+        <material id="ID11" name="__Gray_">
+            <instance_effect url="#ID12" />
+        </material>
+        <material id="ID16" name="edge_color477979255">
+            <instance_effect url="#ID17" />
+        </material>
+    </library_materials>
+    <library_effects>
+        <effect id="ID7">
+            <profile_COMMON>
+                <technique sid="COMMON">
+                    <lambert>
+                        <diffuse>
+                            <color>1 1 1 1</color>
+                        </diffuse>
+                    </lambert>
+                </technique>
+            </profile_COMMON>
+        </effect>
+        <effect id="ID12">
+            <profile_COMMON>
+                <technique sid="COMMON">
+                    <lambert>
+                        <diffuse>
+                            <color>0.5019608 0.5019608 0.5019608 1</color>
+                        </diffuse>
+                    </lambert>
+                </technique>
+            </profile_COMMON>
+        </effect>
+        <effect id="ID17">
+            <profile_COMMON>
+                <technique sid="COMMON">
+                    <constant>
+                        <transparent opaque="A_ONE">
+                            <color>0.1843137 0.3098039 0.3098039 1</color>
+                        </transparent>
+                        <transparency>
+                            <float>1</float>
+                        </transparency>
+                    </constant>
+                </technique>
+            </profile_COMMON>
+        </effect>
+    </library_effects>
+    <scene>
+        <instance_visual_scene url="#ID1" />
+    </scene>
+</COLLADA>
@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<model>
+  <name>rc_cessna</name>
+  <version>1.0</version>
+  <sdf version='1.9'>model.sdf</sdf>
+
+  <author>
+   <name>Benjamin Perseghetti</name>
+   <email>bperseghetti@rudislabs.com</email>
+  </author>
+
+  <description>
+    This is a model of an RC Cessna 182.
+  </description>
+</model>
@@ -0,0 +1,824 @@
+<?xml version="1.0"?>
+<sdf version='1.9'>
+  <model name='rc_cessna'>
+    <pose>0 0 0.246 0 0 0</pose>
+    <link name='base_link'>
+      <pose>0 0 0 0 0 0</pose>
+      <inertial>
+        <pose>0 0 0 0 0 0</pose>
+        <mass>1.5</mass>
+        <inertia>
+          <ixx>0.197563</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>0.1458929</iyy>
+          <iyz>0</iyz>
+          <izz>0.1477</izz>
+        </inertia>
+      </inertial>
+      <collision name='fuselodge_collision'>
+        <pose>-.14 0 0 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.65 .08 0.10</size>
+          </box>
+        </geometry>
+        <surface>
+          <contact>
+            <ode>
+              <max_vel>10</max_vel>
+              <min_depth>0.01</min_depth>
+            </ode>
+          </contact>
+        </surface>
+      </collision>
+      <collision name='wings_collision'>
+        <pose>-0.01 0 0.07 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.1 1.0 0.01</size>
+          </box>
+        </geometry>
+        <surface>
+          <contact>
+            <ode>
+              <max_vel>10</max_vel>
+              <min_depth>0.01</min_depth>
+            </ode>
+          </contact>
+        </surface>
+      </collision>
+      <!-- <visual name='fuselodge_collision_visual'>
+        <pose>-.14 0 0 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.65 .08 0.1</size>
+          </box>
+        </geometry>
+      </visual>
+      <visual name='wings_collision_visual'>
+        <pose>-0.01 0 0.07 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.1 1.0 0.01</size>
+          </box>
+        </geometry>
+      </visual> -->
+      <visual name='base_link_visual'>
+        <pose>0.07 0 -0.08 0 0 0</pose>
+        <geometry>
+          <mesh>
+            <scale>0.1 0.1 0.1</scale>
+            <uri>model://rc_cessna/meshes/body.dae</uri>
+          </mesh>
+        </geometry>
+        <material>
+          <ambient>.175 .175 .175 1.0</ambient>
+          <diffuse>.175 .175 .175 1.0</diffuse>
+        </material>
+      </visual>
+      <gravity>1</gravity>
+      <velocity_decay/>
+      <self_collide>0</self_collide>
+      <sensor name="imu_sensor" type="imu">
+        <always_on>1</always_on>
+        <update_rate>250</update_rate>
+      </sensor>
+      <sensor name="air_pressure_sensor" type="air_pressure">
+        <always_on>1</always_on>
+        <update_rate>50</update_rate>
+        <air_pressure>
+          <pressure>
+            <noise type="gaussian">
+              <mean>0</mean>
+              <stddev>0.01</stddev>
+            </noise>
+          </pressure>
+        </air_pressure>
+      </sensor>
+    </link>
+    <link name="airspeed">
+      <pose>0 0 0 0 0 0</pose>
+      <inertial>
+        <pose>0 0 0 0 0 0</pose>
+        <mass>0.015</mass>
+        <inertia>
+          <ixx>1e-05</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>1e-05</iyy>
+          <iyz>0</iyz>
+          <izz>1e-05</izz>
+        </inertia>
+      </inertial>
+      <visual name="airspeed_visual">
+        <geometry>
+          <cylinder>
+            <radius>0.01</radius>
+            <length>0.1</length>
+          </cylinder>
+        </geometry>
+        <material>
+          <ambient>0 0 0 1.0</ambient>
+          <diffuse>0 0 0 1.0</diffuse>
+        </material>
+      </visual>
+      <!-- <sensor name="air_speed" type="air_speed">
+        <always_on>1</always_on>
+        <update_rate>5.0</update_rate>
+        <enable_metrics>false</enable_metrics>
+        <air_speed>
+          <airspeed>
+            <noise type="gaussian">
+              <mean>0</mean>
+              <stddev>0.01</stddev>
+            </noise>
+          </airspeed>
+        </air_speed>
+      </sensor> -->
+    </link>
+    <joint name='airspeed_joint' type='fixed'>
+      <child>airspeed</child>
+      <parent>base_link</parent>
+    </joint>
+    <link name='rotor_puller'>
+      <pose>0.22 0 0.0 0 1.57079632679 0</pose>
+      <inertial>
+        <pose>0 0 0 0 0 0</pose>
+        <mass>0.005</mass>
+        <inertia>
+          <ixx>9.75e-07</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>0.000166704</iyy>
+          <iyz>0</iyz>
+          <izz>0.000167604</izz>
+        </inertia>
+      </inertial>
+      <collision name='rotor_puller_collision'>
+        <pose>0 0 0 0 -1.57079632679 0</pose>
+        <geometry>
+          <box>
+            <size>0.005 0.22 0.02</size>
+          </box>
+        </geometry>
+        <surface>
+          <contact>
+            <ode/>
+          </contact>
+          <friction>
+            <ode/>
+          </friction>
+        </surface>
+      </collision>
+      <!-- <visual name='rotor_puller_collision_visual'>
+        <pose>0 0 0 0 -1.57079632679 0</pose>
+        <geometry>
+          <box>
+            <size>0.005 0.22 0.02</size>
+          </box>
+        </geometry>
+      </visual> -->
+      <visual name='rotor_puller_visual'>
+        <pose>0 0 0 0 0 -1.57079632679</pose>
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://rc_cessna/meshes/iris_prop_ccw.dae</uri>
+          </mesh>
+        </geometry>
+        <material>
+          <ambient>0 0 1 1.0</ambient>
+          <diffuse>0 0 1 1.0</diffuse>
+        </material>
+      </visual>
+      <gravity>1</gravity>
+      <velocity_decay/>
+      <self_collide>0</self_collide>
+    </link>
+    <joint name='rotor_puller_joint' type='revolute'>
+      <child>rotor_puller</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>
+      </axis>
+    </joint>
+    <link name="left_elevon">
+      <inertial>
+        <mass>0.00000001</mass>
+        <inertia>
+          <ixx>0.000001</ixx>
+          <ixy>0.0</ixy>
+          <iyy>0.000001</iyy>
+          <ixz>0.0</ixz>
+          <iyz>0.0</iyz>
+          <izz>0.000001</izz>
+        </inertia>
+        <pose>0 0.3 0 0.00 0 0.0</pose>
+      </inertial>
+      <visual name='left_elevon_visual'>
+        <pose>0.07 0.0 -0.08 0.00 0 0.0</pose>
+        <geometry>
+          <mesh>
+            <scale>0.1 0.1 0.1</scale>
+            <uri>model://rc_cessna/meshes/left_aileron.dae</uri>
+          </mesh>
+        </geometry>
+        <material>
+          <ambient>0 0 1 1.0</ambient>
+          <diffuse>0 0 1 1.0</diffuse>
+        </material>
+      </visual>
+    </link>
+    <link name="LeftWheel">
+      <pose relative_to="LeftWheelJoint">0 0 0 0 0 0</pose>
+      <inertial>
+        <mass>.05</mass>
+        <inertia>
+          <ixx>0.00003331</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>0.0000204</iyy>
+          <iyz>0</iyz>
+          <izz>0.0000204</izz>
+        </inertia>
+      </inertial>
+      <gravity>true</gravity>
+      <velocity_decay/>
+      <visual name="LeftWheelVisual">
+        <pose>0 0 0 -1.57079632679 0 0</pose>
+        <geometry>
+          <cylinder>
+            <length>0.01</length>
+            <radius>0.03</radius>
+          </cylinder>
+        </geometry>
+      </visual>
+      <collision name="LeftWheelCollision">
+        <pose>0 0 0 -1.57079632679 0 0</pose>
+        <geometry>
+          <cylinder>
+            <length>0.01</length>
+            <radius>0.03</radius>
+          </cylinder>
+        </geometry>
+        <surface>
+          <friction>
+            <ode>
+              <mu>1.0</mu>
+              <mu2>0.5</mu2>
+              <fdir1>0 0 1</fdir1>
+            </ode>
+          </friction>
+        </surface>
+      </collision>
+    </link>
+    <link name="RightWheel">
+      <pose relative_to="RightWheelJoint">0 0 0 0 0 0</pose>
+      <inertial>
+        <mass>.05</mass>
+        <inertia>
+          <ixx>0.00003331</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>0.0000204</iyy>
+          <iyz>0</iyz>
+          <izz>0.0000204</izz>
+        </inertia>
+      </inertial>
+      <gravity>true</gravity>
+      <velocity_decay/>
+      <visual name="RightWheelVisual">
+        <pose>0 0 0 -1.57079632679 0 0</pose>
+        <geometry>
+          <cylinder>
+            <length>0.01</length>
+            <radius>0.03</radius>
+          </cylinder>
+        </geometry>
+      </visual>
+      <collision name="RightWheelCollision">
+        <pose>0 0 0 -1.57079632679 0 0</pose>
+        <geometry>
+          <cylinder>
+            <length>0.01</length>
+            <radius>0.03</radius>
+          </cylinder>
+        </geometry>
+        <surface>
+          <friction>
+            <ode>
+              <mu>1.0</mu>
+              <mu2>0.5</mu2>
+              <fdir1>0 0 1</fdir1>
+            </ode>
+          </friction>
+        </surface>
+      </collision>
+    </link>
+    <link name="CenterWheel">
+      <pose relative_to="CenterWheelJoint">0 0 0 0 0 0</pose>
+      <inertial>
+        <mass>.05</mass>
+        <inertia>
+          <ixx>0.00003331</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>0.0000204</iyy>
+          <iyz>0</iyz>
+          <izz>0.0000204</izz>
+        </inertia>
+      </inertial>
+      <gravity>true</gravity>
+      <velocity_decay/>
+      <visual name="CenterWheelVisual">
+        <pose>0 0 0 -1.57079632679 0 0</pose>
+        <geometry>
+          <cylinder>
+            <length>0.01</length>
+            <radius>0.025</radius>
+          </cylinder>
+        </geometry>
+      </visual>
+      <collision name="CenterWheelCollision">
+        <pose>0 0 0 -1.57079632679 0 0</pose>
+        <geometry>
+          <cylinder>
+            <length>0.01</length>
+            <radius>0.025</radius>
+          </cylinder>
+        </geometry>
+        <surface>
+          <friction>
+            <ode>
+              <mu>1.0</mu>
+              <mu2>0.5</mu2>
+              <fdir1>0 0 1</fdir1>
+            </ode>
+          </friction>
+        </surface>
+      </collision>
+    </link>
+    <link name="right_elevon">
+      <inertial>
+        <mass>0.00000001</mass>
+        <inertia>
+          <ixx>0.000001</ixx>
+          <ixy>0.0</ixy>
+          <iyy>0.000001</iyy>
+          <ixz>0.0</ixz>
+          <iyz>0.0</iyz>
+          <izz>0.000001</izz>
+        </inertia>
+        <pose>0 -0.3 0 0.00 0 0.0</pose>
+      </inertial>
+      <visual name='right_elevon_visual'>
+        <pose>0.07 0.0 -0.08 0.00 0 0.0</pose>
+        <geometry>
+          <mesh>
+            <scale>0.1 0.1 0.1</scale>
+            <uri>model://rc_cessna/meshes/right_aileron.dae</uri>
+          </mesh>
+        </geometry>
+        <material>
+          <ambient>0 0 1 1.0</ambient>
+          <diffuse>0 0 1 1.0</diffuse>
+        </material>
+      </visual>
+    </link>
+    <link name="left_flap">
+      <inertial>
+        <mass>0.00000001</mass>
+        <inertia>
+          <ixx>0.000001</ixx>
+          <ixy>0.0</ixy>
+          <iyy>0.000001</iyy>
+          <ixz>0.0</ixz>
+          <iyz>0.0</iyz>
+          <izz>0.000001</izz>
+        </inertia>
+        <pose>0 0.15 0 0.00 0 0.0</pose>
+      </inertial>
+      <visual name='left_flap_visual'>
+        <pose>0.07 0.0 -0.08 0.00 0 0.0</pose>
+        <geometry>
+          <mesh>
+            <scale>0.1 0.1 0.1</scale>
+            <uri>model://rc_cessna/meshes/left_flap.dae</uri>
+          </mesh>
+        </geometry>
+        <material>
+          <ambient>0 0 1 1.0</ambient>
+          <diffuse>0 0 1 1.0</diffuse>
+        </material>
+      </visual>
+    </link>
+    <link name="right_flap">
+      <inertial>
+        <mass>0.00000001</mass>
+        <inertia>
+          <ixx>0.000001</ixx>
+          <ixy>0.0</ixy>
+          <iyy>0.000001</iyy>
+          <ixz>0.0</ixz>
+          <iyz>0.0</iyz>
+          <izz>0.000001</izz>
+        </inertia>
+        <pose>0 -0.15 0 0.00 0 0.0</pose>
+      </inertial>
+      <visual name='right_flap_visual'>
+        <pose>0.07 0.0 -0.08 0.00 0 0.0</pose>
+        <geometry>
+          <mesh>
+            <scale>0.1 0.1 0.1</scale>
+            <uri>model://rc_cessna/meshes/right_flap.dae</uri>
+          </mesh>
+        </geometry>
+        <material>
+          <ambient>0 0 1 1.0</ambient>
+          <diffuse>0 0 1 1.0</diffuse>
+        </material>
+      </visual>
+    </link>
+    <link name="elevator">
+      <inertial>
+        <mass>0.00000001</mass>
+        <inertia>
+          <ixx>0.000001</ixx>
+          <ixy>0.0</ixy>
+          <iyy>0.000001</iyy>
+          <ixz>0.0</ixz>
+          <iyz>0.0</iyz>
+          <izz>0.000001</izz>
+        </inertia>
+        <pose> -0.5 0 0 0.00 0 0.0</pose>
+      </inertial>
+      <visual name='elevator_visual'>
+        <pose>0.07 0.0 -0.08 0.00 0 0.0</pose>
+        <geometry>
+          <mesh>
+            <scale>0.1 0.1 0.1</scale>
+            <uri>model://rc_cessna/meshes/elevators.dae</uri>
+          </mesh>
+        </geometry>
+        <material>
+          <ambient>0 0 1 1.0</ambient>
+          <diffuse>0 0 1 1.0</diffuse>
+        </material>
+      </visual>
+    </link>
+    <link name="rudder">
+      <inertial>
+        <mass>0.00000001</mass>
+        <inertia>
+          <ixx>0.000001</ixx>
+          <ixy>0.0</ixy>
+          <iyy>0.000001</iyy>
+          <ixz>0.0</ixz>
+          <iyz>0.0</iyz>
+          <izz>0.000001</izz>
+        </inertia>
+        <pose>-0.5 0 0.05 0 0 0 </pose>
+      </inertial>
+      <visual name='rudder_visual'>
+        <pose>0.07 0.0 -0.08 0.00 0 0.0</pose>
+        <geometry>
+          <mesh>
+            <scale>0.1 0.1 0.1</scale>
+            <uri>model://rc_cessna/meshes/rudder.dae</uri>
+          </mesh>
+        </geometry>
+        <material>
+          <ambient>0 0 1 1.0</ambient>
+          <diffuse>0 0 1 1.0</diffuse>
+        </material>
+      </visual>
+    </link>
+    <joint name='servo_0' type='revolute'>
+      <parent>base_link</parent>
+      <child>left_elevon</child>
+      <pose>-0.07 0.4 0.08 0.00 0 0.0</pose>
+      <axis>
+        <xyz>0 1 0</xyz>
+        <limit>
+          <lower>-0.53</lower>
+          <upper>0.53</upper>
+        </limit>
+        <dynamics>
+          <damping>1.000</damping>
+        </dynamics>
+      </axis>
+      <physics>
+        <ode>
+          <implicit_spring_damper>1</implicit_spring_damper>
+        </ode>
+      </physics>
+    </joint>
+    <joint name='servo_1' type='revolute'>
+      <parent>base_link</parent>
+      <child>right_elevon</child>
+      <pose>-0.07 -0.4 0.08 0.00 0 0.0</pose>
+      <axis>
+        <xyz>0 1 0</xyz>
+        <limit>
+          <lower>-0.53</lower>
+          <upper>0.53</upper>
+        </limit>
+        <dynamics>
+          <damping>1.000</damping>
+        </dynamics>
+      </axis>
+      <physics>
+        <ode>
+          <implicit_spring_damper>1</implicit_spring_damper>
+        </ode>
+      </physics>
+    </joint>
+    <joint name='left_flap_joint' type='revolute'>
+      <parent>base_link</parent>
+      <child>left_flap</child>
+      <pose>-0.07 0.2 0.08 0.00 0 0.0</pose>
+      <axis>
+        <xyz>0 1 0</xyz>
+        <limit>
+          <lower>-0.53</lower>
+          <upper>0.53</upper>
+        </limit>
+        <dynamics>
+          <damping>1.000</damping>
+        </dynamics>
+      </axis>
+      <physics>
+        <ode>
+          <implicit_spring_damper>1</implicit_spring_damper>
+        </ode>
+      </physics>
+    </joint>
+    <joint name='right_flap_joint' type='revolute'>
+      <parent>base_link</parent>
+      <child>right_flap</child>
+      <pose>-0.07 -0.2 0.08 0.00 0 0.0</pose>
+      <axis>
+        <xyz>0 1 0</xyz>
+        <limit>
+          <lower>-0.53</lower>
+          <upper>0.53</upper>
+        </limit>
+        <dynamics>
+          <damping>1.000</damping>
+        </dynamics>
+      </axis>
+      <physics>
+        <ode>
+          <implicit_spring_damper>1</implicit_spring_damper>
+        </ode>
+      </physics>
+    </joint>
+    <joint name='servo_2' type='revolute'>
+      <parent>base_link</parent>
+      <child>elevator</child>
+      <pose> -0.5 0 0 0 0 0</pose>
+      <axis>
+        <xyz>0 1 0</xyz>
+        <limit>
+          <!-- -30/+30 deg. -->
+          <lower>-0.53</lower>
+          <upper>0.53</upper>
+        </limit>
+        <dynamics>
+          <damping>1.000</damping>
+        </dynamics>
+      </axis>
+      <physics>
+        <ode>
+          <implicit_spring_damper>1</implicit_spring_damper>
+        </ode>
+      </physics>
+    </joint>
+    <joint name='rudder_joint' type='revolute'>
+      <parent>base_link</parent>
+      <child>rudder</child>
+      <pose>-0.5 0 0.05 0.00 0 0.0</pose>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <limit>
+          <lower>-0.53</lower>
+          <upper>0.53</upper>
+        </limit>
+        <dynamics>
+          <damping>1.000</damping>
+        </dynamics>
+      </axis>
+      <physics>
+        <ode>
+          <implicit_spring_damper>1</implicit_spring_damper>
+        </ode>
+      </physics>
+    </joint>
+    <joint name="LeftWheelJoint" type="revolute">
+      <parent>base_link</parent>
+      <child>LeftWheel</child>
+      <pose relative_to="base_link">-.035 .13 -0.12 0 0 0</pose>
+      <axis>
+        <xyz>0 1 0</xyz>
+        <limit>
+          <lower>-1.79769e+308</lower>
+          <upper>1.79769e+308</upper>
+        </limit>
+        <dynamics>
+          <spring_reference>0</spring_reference>
+          <spring_stiffness>0</spring_stiffness>
+        </dynamics>
+      </axis>
+    </joint>
+    <joint name="RightWheelJoint" type="revolute">
+      <parent>base_link</parent>
+      <child>RightWheel</child>
+      <pose relative_to="base_link">-.035 -.13 -0.12 0 0 0</pose>
+      <axis>
+        <xyz>0 1 0</xyz>
+        <limit>
+          <lower>-1.79769e+308</lower>
+          <upper>1.79769e+308</upper>
+        </limit>
+        <dynamics>
+          <spring_reference>0</spring_reference>
+          <spring_stiffness>0</spring_stiffness>
+        </dynamics>
+      </axis>
+    </joint>
+    <joint name="CenterWheelJoint" type="revolute">
+      <parent>base_link</parent>
+      <child>CenterWheel</child>
+      <pose relative_to="base_link">.135 0 -0.12 0 0 0</pose>
+      <axis>
+        <xyz>0 1 0</xyz>
+        <limit>
+          <lower>-1.79769e+308</lower>
+          <upper>1.79769e+308</upper>
+        </limit>
+        <dynamics>
+          <spring_reference>0</spring_reference>
+          <spring_stiffness>0</spring_stiffness>
+        </dynamics>
+      </axis>
+    </joint>
+    <plugin filename="gz-sim-lift-drag-system" name="gz::sim::systems::LiftDrag">
+      <a0>0.05984281113</a0>
+      <cla>4.752798721</cla>
+      <cda>0.6417112299</cda>
+      <cma>0.0</cma>
+      <alpha_stall>0.3391428111</alpha_stall>
+      <cla_stall>-3.85</cla_stall>
+      <cda_stall>-0.9233984055</cda_stall>
+      <cma_stall>0</cma_stall>
+      <cp>-0.05 0.45 0.05</cp>
+      <area>0.6</area>
+      <air_density>1.2041</air_density>
+      <forward>1 0 0</forward>
+      <upward>0 0 1</upward>
+      <link_name>base_link</link_name>
+      <control_joint_name>servo_0</control_joint_name>
+      <control_joint_rad_to_cl>-0.3</control_joint_rad_to_cl>
+    </plugin>
+    <plugin
+      filename="gz-sim-joint-position-controller-system" name="gz::sim::systems::JointPositionController">
+      <joint_name>servo_0</joint_name>
+      <sub_topic>servo_0</sub_topic>
+      <p_gain>10.0</p_gain>
+    </plugin>
+    <plugin filename="gz-sim-lift-drag-system" name="gz::sim::systems::LiftDrag">
+      <a0>0.05984281113</a0>
+      <cla>4.752798721</cla>
+      <cda>0.6417112299</cda>
+      <cma>0.0</cma>
+      <alpha_stall>0.3391428111</alpha_stall>
+      <cla_stall>-3.85</cla_stall>
+      <cda_stall>-0.9233984055</cda_stall>
+      <cma_stall>0</cma_stall>
+      <cp>-0.05 -0.45 0.05</cp>
+      <area>0.6</area>
+      <air_density>1.2041</air_density>
+      <forward>1 0 0</forward>
+      <upward>0 0 1</upward>
+      <link_name>base_link</link_name>
+      <control_joint_name>servo_1</control_joint_name>
+      <control_joint_rad_to_cl>-0.3</control_joint_rad_to_cl>
+    </plugin>
+    <plugin
+      filename="gz-sim-joint-position-controller-system" name="gz::sim::systems::JointPositionController">
+      <joint_name>servo_1</joint_name>
+      <sub_topic>servo_1</sub_topic>
+      <p_gain>10.0</p_gain>
+    </plugin>
+    <plugin filename="gz-sim-lift-drag-system" name="gz::sim::systems::LiftDrag">
+      <a0>0.05984281113</a0>
+      <cla>4.752798721</cla>
+      <cda>0.6417112299</cda>
+      <cma>0.0</cma>
+      <alpha_stall>0.3391428111</alpha_stall>
+      <cla_stall>-3.85</cla_stall>
+      <cda_stall>-0.9233984055</cda_stall>
+      <cma_stall>0</cma_stall>
+      <cp>-0.05 0.15 0.05</cp>
+      <area>0.6</area>
+      <air_density>1.2041</air_density>
+      <forward>1 0 0</forward>
+      <upward>0 0 1</upward>
+      <link_name>base_link</link_name>
+      <control_joint_name>left_flap_joint</control_joint_name>
+      <control_joint_rad_to_cl>-0.1</control_joint_rad_to_cl>
+    </plugin>
+    <plugin filename="gz-sim-lift-drag-system" name="gz::sim::systems::LiftDrag">
+      <a0>0.05984281113</a0>
+      <cla>4.752798721</cla>
+      <cda>0.6417112299</cda>
+      <cma>0.0</cma>
+      <alpha_stall>0.3391428111</alpha_stall>
+      <cla_stall>-3.85</cla_stall>
+      <cda_stall>-0.9233984055</cda_stall>
+      <cma_stall>0</cma_stall>
+      <cp>-0.05 -0.15 0.05</cp>
+      <area>0.6</area>
+      <air_density>1.2041</air_density>
+      <forward>1 0 0</forward>
+      <upward>0 0 1</upward>
+      <link_name>base_link</link_name>
+      <control_joint_name>right_flap_joint</control_joint_name>
+      <control_joint_rad_to_cl>-0.1</control_joint_rad_to_cl>
+    </plugin>
+    <plugin filename="gz-sim-lift-drag-system" name="gz::sim::systems::LiftDrag">
+      <a0>-0.2</a0>
+      <cla>4.752798721</cla>
+      <cda>0.6417112299</cda>
+      <cma>0.0</cma>
+      <alpha_stall>0.3391428111</alpha_stall>
+      <cla_stall>-3.85</cla_stall>
+      <cda_stall>-0.9233984055</cda_stall>
+      <cma_stall>0</cma_stall>
+      <cp>-0.5 0 0</cp>
+      <area>0.01</area>
+      <air_density>1.2041</air_density>
+      <forward>1 0 0</forward>
+      <upward>0 0 1</upward>
+      <link_name>base_link</link_name>
+      <control_joint_name>servo_2</control_joint_name>
+      <control_joint_rad_to_cl>-4.0</control_joint_rad_to_cl>
+    </plugin>
+    <plugin
+      filename="gz-sim-joint-position-controller-system" name="gz::sim::systems::JointPositionController">
+      <joint_name>servo_2</joint_name>
+      <sub_topic>servo_2</sub_topic>
+      <p_gain>10.0</p_gain>
+    </plugin>
+    <plugin filename="gz-sim-lift-drag-system" name="gz::sim::systems::LiftDrag">
+      <a0>0.0</a0>
+      <cla>4.752798721</cla>
+      <cda>0.6417112299</cda>
+      <cma>0.0</cma>
+      <alpha_stall>0.3391428111</alpha_stall>
+      <cla_stall>-3.85</cla_stall>
+      <cda_stall>-0.9233984055</cda_stall>
+      <cma_stall>0</cma_stall>
+      <cp>-0.5 0 0.05</cp>
+      <area>0.02</area>
+      <air_density>1.2041</air_density>
+      <forward>1 0 0</forward>
+      <upward>0 1 0</upward>
+      <link_name>base_link</link_name>
+      <control_joint_name>rudder_joint</control_joint_name>
+      <control_joint_rad_to_cl>0.8</control_joint_rad_to_cl>
+    </plugin>
+    <plugin
+      filename="gz-sim-joint-position-controller-system" name="gz::sim::systems::JointPositionController">
+      <joint_name>rudder_joint</joint_name>
+      <sub_topic>servo_3</sub_topic>
+      <p_gain>10.0</p_gain>
+    </plugin>
+    <plugin filename="gz-sim-multicopter-motor-model-system" name="gz::sim::systems::MulticopterMotorModel">
+      <jointName>rotor_puller_joint</jointName>
+      <linkName>rotor_puller</linkName>
+      <turningDirection>cw</turningDirection>
+      <timeConstantUp>0.0125</timeConstantUp>
+      <timeConstantDown>0.025</timeConstantDown>
+      <maxRotVelocity>1000</maxRotVelocity>
+      <motorConstant>2.44858e-05</motorConstant>
+      <momentConstant>0.016</momentConstant>
+      <commandSubTopic>command/motor_speed</commandSubTopic>
+      <motorNumber>0</motorNumber>
+      <rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
+      <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
+      <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
+      <motorType>velocity</motorType>
+    </plugin>
+    <static>0</static>
+  </model>
+</sdf>
@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<model>
+  <name>Standard VTOL</name>
+  <version>1.0</version>
+  <sdf version='1.10'>model.sdf</sdf>
+
+  <author>
+   <name>Roman Bapst</name>
+   <email>roman@px4.io</email>
+  </author>
+
+  <description>
+    This is a model of a standard VTOL quad plane.
+  </description>
+</model>
@@ -0,0 +1,755 @@
+<?xml version="1.0"?>
+<!-- DO NOT EDIT: Generated from standard_vtol.sdf.jinja -->
+<sdf version='1.10'>
+  <model name='standard_vtol'>
+    <pose>0 0 0.246 0 0 0</pose>
+    <link name='base_link'>
+      <pose>0 0 0 0 0 0</pose>
+      <inertial>
+        <pose>0 0 0 0 0 0</pose>
+        <mass>5</mass>
+        <inertia>
+          <ixx>0.477708333333</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>0.341666666667</iyy>
+          <iyz>0</iyz>
+          <izz>0.811041666667</izz>
+        </inertia>
+      </inertial>
+      <collision name='base_link_collision'>
+        <pose>0 0 -0.07 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.55 2.144 0.05</size>
+          </box>
+        </geometry>
+        <surface>
+          <contact>
+            <ode>
+              <kp>100000</kp>
+              <kd>1.0</kd>
+              <max_vel>0.1</max_vel>
+              <min_depth>0.001</min_depth>
+            </ode>
+          </contact>
+          <friction>
+            <ode/>
+          </friction>
+        </surface>
+      </collision>
+      <visual name='base_link_visual'>
+        <pose>0.53 -1.072 -0.1 1.5707963268 0 3.1415926536</pose>
+        <geometry>
+          <mesh>
+            <scale>0.001 0.001 0.001</scale>
+            <uri>model://standard_vtol/meshes/x8_wing.dae</uri>
+          </mesh>
+        </geometry>
+        <material>
+          <ambient>.175 .175 .175 1.0</ambient>
+          <diffuse>.175 .175 .175 1.0</diffuse>
+        </material>
+      </visual>
+      <visual name='left_motor_column'>
+        <pose>0 0.35 0.01 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.74 0.03 0.03</size>
+          </box>
+        </geometry>
+        <material>
+          <ambient>.175 .175 .175 1.0</ambient>
+          <diffuse>.175 .175 .175 1.0</diffuse>
+        </material>
+      </visual>
+      <visual name='right_motor_column'>
+        <pose>0 -0.35 0.01 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.74 0.03 0.03</size>
+          </box>
+        </geometry>
+        <material>
+          <ambient>.175 .175 .175 1.0</ambient>
+          <diffuse>.175 .175 .175 1.0</diffuse>
+        </material>
+      </visual>
+      <visual name='m0'>
+        <pose>-0.35 0.35 0.045 0 0 0</pose>
+        <geometry>
+          <cylinder>
+            <length>0.035</length>
+            <radius>0.02</radius>
+          </cylinder>
+        </geometry>
+        <material>
+          <ambient>.175 .175 .175 1.0</ambient>
+          <diffuse>.175 .175 .175 1.0</diffuse>
+        </material>
+      </visual>
+      <visual name='m1'>
+        <pose>-0.35 -0.35 0.045 0 0 0</pose>
+        <geometry>
+          <cylinder>
+            <length>0.035</length>
+            <radius>0.02</radius>
+          </cylinder>
+        </geometry>
+        <material>
+          <ambient>.175 .175 .175 1.0</ambient>
+          <diffuse>.175 .175 .175 1.0</diffuse>
+        </material>
+      </visual>
+      <visual name='m2'>
+        <pose>0.35 -0.35 0.045 0 0 0</pose>
+        <geometry>
+          <cylinder>
+            <length>0.035</length>
+            <radius>0.02</radius>
+          </cylinder>
+        </geometry>
+        <material>
+          <ambient>.175 .175 .175 1.0</ambient>
+          <diffuse>.175 .175 .175 1.0</diffuse>
+        </material>
+      </visual>
+      <visual name='m3'>
+        <pose>0.35 0.35 0.045 0 0 0</pose>
+        <geometry>
+          <cylinder>
+            <length>0.035</length>
+            <radius>0.02</radius>
+          </cylinder>
+        </geometry>
+        <material>
+          <ambient>.175 .175 .175 1.0</ambient>
+          <diffuse>.175 .175 .175 1.0</diffuse>
+        </material>
+      </visual>
+      <gravity>1</gravity>
+      <velocity_decay/>
+      <self_collide>0</self_collide>
+      <sensor name="imu_sensor" type="imu">
+        <always_on>1</always_on>
+        <update_rate>250</update_rate>
+      </sensor>
+      <sensor name="air_pressure_sensor" type="air_pressure">
+        <always_on>1</always_on>
+        <update_rate>50</update_rate>
+        <air_pressure>
+          <pressure>
+            <noise type="gaussian">
+              <mean>0</mean>
+              <stddev>0.01</stddev>
+            </noise>
+          </pressure>
+        </air_pressure>
+      </sensor>
+    </link>
+    <link name='rotor_0'>
+      <pose>0.35 -0.35 0.07 0 0 0</pose>
+      <inertial>
+        <pose>0 0 0 0 0 0</pose>
+        <mass>0.005</mass>
+        <inertia>
+          <ixx>9.75e-07</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>0.000166704</iyy>
+          <iyz>0</iyz>
+          <izz>0.000167604</izz>
+        </inertia>
+      </inertial>
+      <collision name='rotor_0_collision'>
+        <pose>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>0 0 0 0 0 0</pose>
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://standard_vtol/meshes/iris_prop_ccw.dae</uri>
+          </mesh>
+        </geometry>
+        <material>
+          <ambient>0 0 1 1.0</ambient>
+          <diffuse>0 0 1 1.0</diffuse>
+        </material>
+      </visual>
+      <gravity>1</gravity>
+      <velocity_decay/>
+      <self_collide>0</self_collide>
+    </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>
+      </axis>
+    </joint>
+    <link name='rotor_1'>
+      <pose>-0.35 0.35 0.07 0 0 0</pose>
+      <inertial>
+        <pose>0 0 0 0 0 0</pose>
+        <mass>0.005</mass>
+        <inertia>
+          <ixx>9.75e-07</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>0.000166704</iyy>
+          <iyz>0</iyz>
+          <izz>0.000167604</izz>
+        </inertia>
+      </inertial>
+      <collision name='rotor_1_collision'>
+        <pose>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>0 0 0 0 0 0</pose>
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://standard_vtol/meshes/iris_prop_ccw.dae</uri>
+          </mesh>
+        </geometry>
+        <material>
+          <ambient>0 0 1 1.0</ambient>
+          <diffuse>0 0 1 1.0</diffuse>
+        </material>
+      </visual>
+      <gravity>1</gravity>
+      <velocity_decay/>
+      <self_collide>0</self_collide>
+    </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>
+      </axis>
+    </joint>
+    <link name='rotor_2'>
+      <pose>0.35 0.35 0.07 0 0 0</pose>
+      <inertial>
+        <pose>0 0 0 0 0 0</pose>
+        <mass>0.005</mass>
+        <inertia>
+          <ixx>9.75e-07</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>0.000166704</iyy>
+          <iyz>0</iyz>
+          <izz>0.000167604</izz>
+        </inertia>
+      </inertial>
+      <collision name='rotor_2_collision'>
+        <pose>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>0 0 0 0 0 0</pose>
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://standard_vtol/meshes/iris_prop_ccw.dae</uri>
+          </mesh>
+        </geometry>
+        <material>
+          <ambient>0 0 1 1.0</ambient>
+          <diffuse>0 0 1 1.0</diffuse>
+        </material>
+      </visual>
+      <gravity>1</gravity>
+      <velocity_decay/>
+      <self_collide>0</self_collide>
+    </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>
+      </axis>
+    </joint>
+    <link name='rotor_3'>
+      <pose>-0.35 -0.35 0.07 0 0 0</pose>
+      <inertial>
+        <pose>0 0 0 0 0 0</pose>
+        <mass>0.005</mass>
+        <inertia>
+          <ixx>9.75e-07</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>0.000166704</iyy>
+          <iyz>0</iyz>
+          <izz>0.000167604</izz>
+        </inertia>
+      </inertial>
+      <collision name='rotor_3_collision'>
+        <pose>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>0 0 0 0 0 0</pose>
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://standard_vtol/meshes/iris_prop_ccw.dae</uri>
+          </mesh>
+        </geometry>
+        <material>
+          <ambient>0 0 1 1.0</ambient>
+          <diffuse>0 0 1 1.0</diffuse>
+        </material>
+      </visual>
+      <gravity>1</gravity>
+      <velocity_decay/>
+      <self_collide>0</self_collide>
+    </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>
+      </axis>
+    </joint>
+
+    <link name='rotor_puller'>
+      <pose>-0.22 0 0.0 0 1.57 0</pose>
+      <inertial>
+        <pose>0 0 0 0 0 0</pose>
+        <mass>0.005</mass>
+        <inertia>
+          <ixx>9.75e-07</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>0.000166704</iyy>
+          <iyz>0</iyz>
+          <izz>0.000167604</izz>
+        </inertia>
+      </inertial>
+      <collision name='rotor_puller_collision'>
+        <pose>0.0 0 -0.04 0 0 0</pose>
+        <geometry>
+          <cylinder>
+            <length>0.005</length>
+            <radius>0.06</radius>
+          </cylinder>
+        </geometry>
+        <surface>
+          <contact>
+            <ode/>
+          </contact>
+          <friction>
+            <ode/>
+          </friction>
+        </surface>
+      </collision>
+      <visual name='rotor_puller_visual'>
+        <pose>0 0 -0.04 0 0 0</pose>
+        <geometry>
+          <mesh>
+            <scale>0.8 0.8 0.8</scale>
+            <uri>model://standard_vtol/meshes/iris_prop_ccw.dae</uri>
+          </mesh>
+        </geometry>
+        <material>
+          <ambient>0 0 1 1.0</ambient>
+          <diffuse>0 0 1 1.0</diffuse>
+        </material>
+      </visual>
+      <gravity>1</gravity>
+      <velocity_decay/>
+      <self_collide>0</self_collide>
+    </link>
+    <joint name='rotor_puller_joint' type='revolute'>
+      <pose>0.0 0 0.0 0 -1.57 0</pose>
+      <child>rotor_puller</child>
+      <parent>base_link</parent>
+      <axis>
+        <xyz>1 0 0</xyz>
+        <limit>
+          <lower>-1e+16</lower>
+          <upper>1e+16</upper>
+        </limit>
+        <dynamics>
+          <spring_reference>0</spring_reference>
+          <spring_stiffness>0</spring_stiffness>
+        </dynamics>
+      </axis>
+    </joint>
+
+    <link name="left_elevon">
+      <inertial>
+        <mass>0.00000001</mass>
+        <inertia>
+          <ixx>0.000001</ixx>
+          <ixy>0.0</ixy>
+          <iyy>0.000001</iyy>
+          <ixz>0.0</ixz>
+          <iyz>0.0</iyz>
+          <izz>0.000001</izz>
+        </inertia>
+        <pose>0 0.3 0 0.00 0 0.0</pose>
+      </inertial>
+      <visual name='left_elevon_visual'>
+        <pose>-0.105 0.004 -0.034 1.5707963268 0 3.1415926536</pose>
+        <geometry>
+          <mesh>
+            <scale>0.001 0.001 0.001</scale>
+            <uri>model://standard_vtol/meshes/x8_elevon_left.dae</uri>
+          </mesh>
+        </geometry>
+        <material>
+          <ambient>0 0 1 1.0</ambient>
+          <diffuse>0 0 1 1.0</diffuse>
+        </material>
+      </visual>
+    </link>
+    <link name="right_elevon">
+      <inertial>
+        <mass>0.00000001</mass>
+        <inertia>
+          <ixx>0.000001</ixx>
+          <ixy>0.0</ixy>
+          <iyy>0.000001</iyy>
+          <ixz>0.0</ixz>
+          <iyz>0.0</iyz>
+          <izz>0.000001</izz>
+        </inertia>
+        <pose>0 -0.6 0 0.00 0 0.0</pose>
+      </inertial>
+      <visual name='right_elevon_visual'>
+        <pose>0.281 -1.032 -0.034 1.5707963268 0 3.1415926536</pose>
+        <geometry>
+          <mesh>
+            <scale>0.001 0.001 0.001</scale>
+            <uri>model://standard_vtol/meshes/x8_elevon_right.dae</uri>
+          </mesh>
+        </geometry>
+        <material>
+          <ambient>0 0 1 1.0</ambient>
+          <diffuse>0 0 1 1.0</diffuse>
+        </material>
+      </visual>
+    </link>
+    <link name="elevator">
+      <inertial>
+        <mass>0.00000001</mass>
+        <inertia>
+          <ixx>0.000001</ixx>
+          <ixy>0.0</ixy>
+          <iyy>0.000001</iyy>
+          <ixz>0.0</ixz>
+          <iyz>0.0</iyz>
+          <izz>0.000001</izz>
+        </inertia>
+        <pose> -0.5 0 0 0.00 0 0.0</pose>
+      </inertial>
+    </link>
+    <joint name='servo_0' type='revolute'>
+      <parent>base_link</parent>
+      <child>left_elevon</child>
+      <pose>-0.18 0.6 -0.005 0 0 0.265</pose>
+      <axis>
+        <xyz>0 1 0</xyz>
+        <limit>
+          <lower>-0.53</lower>
+          <upper>0.53</upper>
+        </limit>
+        <dynamics>
+          <damping>1.000</damping>
+        </dynamics>
+      </axis>
+      <physics>
+        <ode>
+          <implicit_spring_damper>1</implicit_spring_damper>
+        </ode>
+      </physics>
+    </joint>
+    <joint name='servo_1' type='revolute'>
+      <parent>base_link</parent>
+      <child>right_elevon</child>
+      <pose>-0.18 -0.6 -0.005 0 0 -0.265</pose>
+      <axis>
+        <xyz>0 1 0</xyz>
+        <limit>
+          <lower>-0.53</lower>
+          <upper>0.53</upper>
+        </limit>
+        <dynamics>
+          <damping>1.000</damping>
+        </dynamics>
+      </axis>
+      <physics>
+        <ode>
+          <implicit_spring_damper>1</implicit_spring_damper>
+        </ode>
+      </physics>
+    </joint>
+    <joint name='servo_2' type='revolute'>
+      <parent>base_link</parent>
+      <child>elevator</child>
+      <pose> -0.5 0 0 0 0 0</pose>
+      <axis>
+        <xyz>0 1 0</xyz>
+        <limit>
+          <!-- -30/+30 deg. -->
+          <lower>-0.53</lower>
+          <upper>0.53</upper>
+        </limit>
+        <dynamics>
+          <damping>1.000</damping>
+        </dynamics>
+      </axis>
+      <physics>
+        <ode>
+          <implicit_spring_damper>1</implicit_spring_damper>
+        </ode>
+      </physics>
+    </joint>
+    <plugin filename="gz-sim-lift-drag-system" name="gz::sim::systems::LiftDrag">
+      <a0>0.05984281113</a0>
+      <cla>4.752798721</cla>
+      <cda>0.6417112299</cda>
+      <cma>0.0</cma>
+      <alpha_stall>0.3391428111</alpha_stall>
+      <cla_stall>-3.85</cla_stall>
+      <cda_stall>-0.9233984055</cda_stall>
+      <cma_stall>0</cma_stall>
+      <cp>-0.05 0.3 0.05</cp>
+      <area>0.50</area>
+      <air_density>1.2041</air_density>
+      <forward>1 0 0</forward>
+      <upward>0 0 1</upward>
+      <link_name>base_link</link_name>
+      <control_joint_name>servo_0</control_joint_name>
+      <control_joint_rad_to_cl>-1.0</control_joint_rad_to_cl>
+    </plugin>
+    <plugin
+      filename="gz-sim-joint-position-controller-system" name="gz::sim::systems::JointPositionController">
+      <joint_name>servo_0</joint_name>
+      <sub_topic>servo_0</sub_topic>
+    </plugin>
+    <plugin filename="gz-sim-lift-drag-system" name="gz::sim::systems::LiftDrag">
+      <a0>0.05984281113</a0>
+      <cla>4.752798721</cla>
+      <cda>0.6417112299</cda>
+      <cma>0.0</cma>
+      <alpha_stall>0.3391428111</alpha_stall>
+      <cla_stall>-3.85</cla_stall>
+      <cda_stall>-0.9233984055</cda_stall>
+      <cma_stall>0</cma_stall>
+      <cp>-0.05 -0.3 0.05</cp>
+      <area>0.50</area>
+      <air_density>1.2041</air_density>
+      <forward>1 0 0</forward>
+      <upward>0 0 1</upward>
+      <link_name>base_link</link_name>
+      <control_joint_name>servo_1</control_joint_name>
+      <control_joint_rad_to_cl>-1.0</control_joint_rad_to_cl>
+    </plugin>
+    <plugin
+      filename="gz-sim-joint-position-controller-system" name="gz::sim::systems::JointPositionController">
+      <joint_name>servo_1</joint_name>
+      <sub_topic>servo_1</sub_topic>
+    </plugin>
+    <plugin filename="gz-sim-lift-drag-system" name="gz::sim::systems::LiftDrag">
+      <a0>-0.2</a0>
+      <cla>4.752798721</cla>
+      <cda>0.6417112299</cda>
+      <cma>0.0</cma>
+      <alpha_stall>0.3391428111</alpha_stall>
+      <cla_stall>-3.85</cla_stall>
+      <cda_stall>-0.9233984055</cda_stall>
+      <cma_stall>0</cma_stall>
+      <cp>-0.5 0 0</cp>
+      <area>0.01</area>
+      <air_density>1.2041</air_density>
+      <forward>1 0 0</forward>
+      <upward>0 0 1</upward>
+      <link_name>base_link</link_name>
+      <control_joint_name>servo_2</control_joint_name>
+          <control_joint_name>servo_2</control_joint_name>
+      <control_joint_rad_to_cl>-12.0</control_joint_rad_to_cl>
+    </plugin>
+    <plugin
+      filename="gz-sim-joint-position-controller-system" name="gz::sim::systems::JointPositionController">
+      <joint_name>servo_2</joint_name>
+      <sub_topic>servo_2</sub_topic>
+    </plugin>
+    <plugin filename="gz-sim-multicopter-motor-model-system" name="gz::sim::systems::MulticopterMotorModel">
+      <jointName>rotor_0_joint</jointName>
+      <linkName>rotor_0</linkName>
+      <turningDirection>ccw</turningDirection>
+      <timeConstantUp>0.0125</timeConstantUp>
+      <timeConstantDown>0.025</timeConstantDown>
+      <maxRotVelocity>1500</maxRotVelocity>
+      <motorConstant>2e-05</motorConstant>
+      <momentConstant>0.06</momentConstant>
+      <commandSubTopic>command/motor_speed</commandSubTopic>
+      <motorNumber>0</motorNumber>
+      <rotorDragCoefficient>0.000106428</rotorDragCoefficient>
+      <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
+      <rotorVelocitySlowdownSim>20</rotorVelocitySlowdownSim>
+      <motorType>velocity</motorType>
+    </plugin>
+    <plugin filename="gz-sim-multicopter-motor-model-system" name="gz::sim::systems::MulticopterMotorModel">
+      <jointName>rotor_1_joint</jointName>
+      <linkName>rotor_1</linkName>
+      <turningDirection>ccw</turningDirection>
+      <timeConstantUp>0.0125</timeConstantUp>
+      <timeConstantDown>0.025</timeConstantDown>
+      <maxRotVelocity>1500</maxRotVelocity>
+      <motorConstant>2e-05</motorConstant>
+      <momentConstant>0.06</momentConstant>
+      <commandSubTopic>command/motor_speed</commandSubTopic>
+      <motorNumber>1</motorNumber>
+      <rotorDragCoefficient>0.000106428</rotorDragCoefficient>
+      <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
+      <rotorVelocitySlowdownSim>20</rotorVelocitySlowdownSim>
+      <motorType>velocity</motorType>
+    </plugin>
+    <plugin
+      filename="gz-sim-multicopter-motor-model-system"
+      name="gz::sim::systems::MulticopterMotorModel">
+      <jointName>rotor_2_joint</jointName>
+      <linkName>rotor_2</linkName>
+      <turningDirection>cw</turningDirection>
+      <timeConstantUp>0.0125</timeConstantUp>
+      <timeConstantDown>0.025</timeConstantDown>
+      <maxRotVelocity>1500</maxRotVelocity>
+      <motorConstant>2e-05</motorConstant>
+      <momentConstant>0.06</momentConstant>
+      <commandSubTopic>command/motor_speed</commandSubTopic>
+      <motorNumber>2</motorNumber>
+      <rotorDragCoefficient>0.000106428</rotorDragCoefficient>
+      <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
+      <rotorVelocitySlowdownSim>20</rotorVelocitySlowdownSim>
+      <motorType>velocity</motorType>
+    </plugin>
+    <plugin
+      filename="gz-sim-multicopter-motor-model-system"
+      name="gz::sim::systems::MulticopterMotorModel">
+      <jointName>rotor_3_joint</jointName>
+      <linkName>rotor_3</linkName>
+      <turningDirection>cw</turningDirection>
+      <timeConstantUp>0.0125</timeConstantUp>
+      <timeConstantDown>0.025</timeConstantDown>
+      <maxRotVelocity>1500</maxRotVelocity>
+      <motorConstant>2e-05</motorConstant>
+      <momentConstant>0.06</momentConstant>
+      <commandSubTopic>command/motor_speed</commandSubTopic>
+      <motorNumber>3</motorNumber>
+      <rotorDragCoefficient>0.000106428</rotorDragCoefficient>
+      <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
+      <rotorVelocitySlowdownSim>20</rotorVelocitySlowdownSim>
+      <motorType>velocity</motorType>
+    </plugin>
+    <plugin
+      filename="gz-sim-multicopter-motor-model-system"
+      name="gz::sim::systems::MulticopterMotorModel">
+      <jointName>rotor_puller_joint</jointName>
+      <linkName>rotor_puller</linkName>
+      <turningDirection>cw</turningDirection>
+      <timeConstantUp>0.0125</timeConstantUp>
+      <timeConstantDown>0.025</timeConstantDown>
+      <maxRotVelocity>3500</maxRotVelocity>
+      <motorConstant>8.54858e-06</motorConstant>
+      <momentConstant>0.01</momentConstant>
+      <commandSubTopic>command/motor_speed</commandSubTopic>
+      <motorNumber>4</motorNumber>
+      <rotorDragCoefficient>0.000106428</rotorDragCoefficient>
+      <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
+      <rotorVelocitySlowdownSim>20</rotorVelocitySlowdownSim>
+      <motorType>velocity</motorType>
+    </plugin>
+    <static>0</static>
+  </model>
+</sdf>
@@ -215,6 +215,18 @@
          </friction>
        </surface>
      </collision>
+      <sensor name="air_pressure_sensor" type="air_pressure">
+        <always_on>1</always_on>
+        <update_rate>50</update_rate>
+        <air_pressure>
+          <pressure>
+            <noise type="gaussian">
+              <mean>0</mean>
+              <stddev>0.01</stddev>
+            </noise>
+          </pressure>
+        </air_pressure>
+      </sensor>
      <sensor name="imu_sensor" type="imu">
        <always_on>1</always_on>
        <update_rate>250</update_rate>
@@ -290,7 +302,6 @@
          <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">
@@ -363,7 +374,6 @@
          <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">
@@ -436,7 +446,6 @@
          <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">
@@ -509,10 +518,9 @@
          <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">
+   <plugin filename="gz-sim-multicopter-motor-model-system" name="gz::sim::systems::MulticopterMotorModel">
      <jointName>rotor_0_joint</jointName>
      <linkName>rotor_0</linkName>
      <turningDirection>ccw</turningDirection>
@@ -528,7 +536,7 @@
      <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
      <motorType>velocity</motorType>
    </plugin>
-    <plugin filename="ignition-gazebo-multicopter-motor-model-system" name="ignition::gazebo::systems::MulticopterMotorModel">
+    <plugin filename="gz-sim-multicopter-motor-model-system" name="gz::sim::systems::MulticopterMotorModel">
      <jointName>rotor_1_joint</jointName>
      <linkName>rotor_1</linkName>
      <turningDirection>ccw</turningDirection>
@@ -544,7 +552,7 @@
      <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
      <motorType>velocity</motorType>
    </plugin>
-    <plugin filename="ignition-gazebo-multicopter-motor-model-system" name="ignition::gazebo::systems::MulticopterMotorModel">
+    <plugin filename="gz-sim-multicopter-motor-model-system" name="gz::sim::systems::MulticopterMotorModel">
      <jointName>rotor_2_joint</jointName>
      <linkName>rotor_2</linkName>
      <turningDirection>cw</turningDirection>
@@ -560,7 +568,7 @@
      <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
      <motorType>velocity</motorType>
    </plugin>
-    <plugin filename="ignition-gazebo-multicopter-motor-model-system" name="ignition::gazebo::systems::MulticopterMotorModel">
+    <plugin filename="gz-sim-multicopter-motor-model-system" name="gz::sim::systems::MulticopterMotorModel">
      <jointName>rotor_3_joint</jointName>
      <linkName>rotor_3</linkName>
      <turningDirection>cw</turningDirection>
@@ -0,0 +1,11 @@
+<?xml version="1.0"?>
+<model>
+  <name>x500-vision</name>
+  <version>1.0</version>
+  <sdf version="1.9">model.sdf</sdf>
+  <author>
+    <name>Jaeyoung Lim</name>
+    <email>jalim@ethz.ch</email>
+  </author>
+  <description>Model of the X500 with a odometry/external vision input.</description>
+</model>
@@ -0,0 +1,13 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<sdf version='1.9'>
+  <model name='x500-vision'>
+    <include merge='true'>
+      <uri>x500</uri>
+    </include>
+      <plugin
+        filename="gz-sim-odometry-publisher-system"
+        name="gz::sim::systems::OdometryPublisher">
+        <dimensions>3</dimensions>
+      </plugin>
+  </model>
+</sdf>
@@ -5,21 +5,22 @@
      <real_time_factor>1.0</real_time_factor>
      <real_time_update_rate>250</real_time_update_rate>
    </physics>
-    <plugin name='ignition::gazebo::systems::Physics' filename='ignition-gazebo-physics-system'/>
-    <plugin name='ignition::gazebo::systems::UserCommands' filename='ignition-gazebo-user-commands-system'/>
-    <plugin name='ignition::gazebo::systems::SceneBroadcaster' filename='ignition-gazebo-scene-broadcaster-system'/>
-    <plugin name='ignition::gazebo::systems::Contact' filename='ignition-gazebo-contact-system'/>
-    <plugin name='ignition::gazebo::systems::Imu' filename='ignition-gazebo-imu-system'/>
-    <plugin name='ignition::gazebo::systems::Sensors' filename='ignition-gazebo-sensors-system'>
+    <plugin name='gz::sim::systems::Physics' filename='gz-sim-physics-system'/>
+    <plugin name='gz::sim::systems::UserCommands' filename='gz-sim-user-commands-system'/>
+    <plugin name='gz::sim::systems::SceneBroadcaster' filename='gz-sim-scene-broadcaster-system'/>
+    <plugin name='gz::sim::systems::Contact' filename='gz-sim-contact-system'/>
+    <plugin name='gz::sim::systems::Imu' filename='gz-sim-imu-system'/>
+    <plugin name='gz::sim::systems::AirPressure' filename='gz-sim-air-pressure-system'/>
+    <plugin name='gz::sim::systems::Sensors' filename='gz-sim-sensors-system'>
      <render_engine>ogre2</render_engine>
    </plugin>
    <gui fullscreen='false'>
      <plugin name='3D View' filename='GzScene3D'>
-        <ignition-gui>
+        <gz-gui>
          <title>3D View</title>
          <property type='bool' key='showTitleBar'>0</property>
          <property type='string' key='state'>docked</property>
-        </ignition-gui>
+        </gz-gui>
        <engine>ogre2</engine>
        <scene>scene</scene>
        <ambient_light>0.5984631152222222 0.5984631152222222 0.5984631152222222</ambient_light>
@@ -27,7 +28,7 @@
        <camera_pose>-6 0 6 0 0.5 0</camera_pose>
      </plugin>
      <plugin name='World control' filename='WorldControl'>
-        <ignition-gui>
+        <gz-gui>
          <title>World control</title>
          <property type='bool' key='showTitleBar'>0</property>
          <property type='bool' key='resizable'>0</property>
@@ -39,13 +40,13 @@
            <line own='left' target='left'/>
            <line own='bottom' target='bottom'/>
          </anchors>
-        </ignition-gui>
+        </gz-gui>
        <play_pause>1</play_pause>
        <step>1</step>
        <start_paused>1</start_paused>
      </plugin>
      <plugin name='World stats' filename='WorldStats'>
-        <ignition-gui>
+        <gz-gui>
          <title>World stats</title>
          <property type='bool' key='showTitleBar'>0</property>
          <property type='bool' key='resizable'>0</property>
@@ -57,7 +58,7 @@
            <line own='right' target='right'/>
            <line own='bottom' target='bottom'/>
          </anchors>
-        </ignition-gui>
+        </gz-gui>
        <sim_time>1</sim_time>
        <real_time>1</real_time>
        <real_time_factor>1</real_time_factor>
@@ -46,7 +46,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL_L1=y
+CONFIG_MODULES_FW_PATH_NAVIGATION=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -17,6 +17,7 @@ CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_SENSORS=y
 # CONFIG_SENSORS_VEHICLE_AIRSPEED is not set
 # CONFIG_SENSORS_VEHICLE_AIR_DATA is not set
+# CONFIG_SENSORS_VEHICLE_ACCELERATION is not set
 # CONFIG_SENSORS_VEHICLE_GPS_POSITION is not set
 # CONFIG_SENSORS_VEHICLE_MAGNETOMETER is not set
 CONFIG_SYSTEMCMDS_PARAM=y
@@ -29,4 +29,5 @@ CONFIG_SYSTEMCMDS_REBOOT=y
 CONFIG_SYSTEMCMDS_TOP=y
 CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
 CONFIG_SYSTEMCMDS_UORB=y
+CONFIG_SYSTEMCMDS_VER=y
 CONFIG_SYSTEMCMDS_WORK_QUEUE=y
@@ -42,3 +42,9 @@
 //--------------------------------------------//---------------------------//----------------
 #define DMACHAN_SPI1_RX    DMAMAP_SPI1_RX_2   // DMA2, Stream 2, Channel 3
 #define DMACHAN_SPI1_TX    DMAMAP_SPI1_TX_2   // DMA2, Stream 5, Channel 3
+
+// Assigned in timer_config.cpp
+
+// Timer 2                                    /* DMA1, Stream 7, Channel 3 DMAMAP_TIM2_UP_2 */
+// Timer 3                                    /* DMA1, Stream 2, Channel 5 DMAMAP_TIM3_UP   */
+// Timer 4                                    /* DMA1, Stream 6, Channel 2 DMAMAP_TIM4_UP   */
@@ -62,7 +62,8 @@
 #define GPIO_TIM1_CH3         /* PA10 */  (GPIO_TIM1_CH3_1|GPIO_OPENDRAIN|GPIO_SPEED_2MHz)

 /* PWM Outputs */
-#define DIRECT_PWM_OUTPUT_CHANNELS  6 // Actually 8
+#define BOARD_NUM_IO_TIMERS         3
+#define DIRECT_PWM_OUTPUT_CHANNELS  8

 #define GPIO_TIM2_CH1_RESET   /* PA0  */  (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0)
 #define GPIO_TIM2_CH2_RESET   /* PA1  */  (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN1)
@@ -34,9 +34,9 @@
 #include <px4_arch/io_timer_hw_description.h>

 constexpr io_timers_t io_timers[MAX_IO_TIMERS] = {
-	initIOTimer(Timer::Timer2),
-	initIOTimer(Timer::Timer3),
-	//initIOTimer(Timer::Timer4),
+	initIOTimer(Timer::Timer2, DMA{DMA::Index1, DMA::Stream7, DMA::Channel3}),
+	initIOTimer(Timer::Timer3, DMA{DMA::Index1, DMA::Stream2, DMA::Channel5}),
+	initIOTimer(Timer::Timer4, DMA{DMA::Index1, DMA::Stream6, DMA::Channel2}),
 };

 constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = {
@@ -46,8 +46,8 @@ constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = {
 	initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel1}, {GPIO::PortB, GPIO::Pin4}),
 	initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel2}, {GPIO::PortB, GPIO::Pin5}),
 	initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel3}, {GPIO::PortB, GPIO::Pin0}),
-	//initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel4}, {GPIO::PortB, GPIO::Pin1}),
-	//initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel2}, {GPIO::PortB, GPIO::Pin7}),
+	initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel4}, {GPIO::PortB, GPIO::Pin1}),
+	initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel2}, {GPIO::PortB, GPIO::Pin7}),
 };

 constexpr io_timers_channel_mapping_t io_timers_channel_mapping =
@@ -43,7 +43,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL_L1=y
+CONFIG_MODULES_FW_PATH_NAVIGATION=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -47,14 +47,14 @@ then
 	fi
 fi

-# Internal SPI bus IIM42652
-iim42652 -R 3 -s -b 1 start
+# Internal SPI bus IIM42652 with SPIX measured frequency of 32.051kHz
+iim42652 -R 3 -s -b 1 -C 32051 start

-# Internal SPI bus ICM42688p
-icm42688p -R 9 -s -b 2 start
+# Internal SPI bus ICM42688p with SPIX measured frequency of 32.051kHz
+icm42688p -R 9 -s -b 2 -C 32051 start

-# Internal SPI bus ICM42688p
-icm42688p -R 6 -s -b 3 start
+# Internal SPI bus ICM42688p with SPIX measured frequency of 32.051kHz
+icm42688p -R 6 -s -b 3 -C 32051 start

 # Internal magnetometer on I2C
 bmm150 -I start
@@ -379,7 +379,7 @@

 #define GPIO_UART5_RX    GPIO_UART5_RX_3    /* PD2  */
 #define GPIO_UART5_TX    GPIO_UART5_TX_3    /* PC12 */
-// GPIO_UART5_RTS   no remap                /* PC8  */
+// GPIO_UART5_RTS  No remap                 /* PC8  */
 // GPIO_UART5_CTS  No remap                 /* PC9  */

 #define GPIO_USART6_RX   GPIO_USART6_RX_1   /* PC7 */
@@ -387,8 +387,8 @@

 #define GPIO_UART7_RX    GPIO_UART7_RX_4    /* PF6  */
 #define GPIO_UART7_TX    GPIO_UART7_TX_3    /* PE8  */
-#define GPIO_UART7_RTS   GPIO_UART7_RTS_2   /* PF8  */
-#define GPIO_UART7_CTS   GPIO_UART7_CTS_1   /* PE10 */
+#define GPIO_UART7_RTS   (GPIO_UART7_RTS_2 | GPIO_PULLDOWN)   /* PF8  */
+#define GPIO_UART7_CTS   (GPIO_UART7_CTS_1 | GPIO_PULLDOWN)   /* PE10 */

 #define GPIO_UART8_RX    GPIO_UART8_RX_1    /* PE0 */
 #define GPIO_UART8_TX    GPIO_UART8_TX_1    /* PE1 */
@@ -31,14 +31,19 @@
 #
 ############################################################################
 if("${PX4_BOARD_LABEL}" STREQUAL  "bootloader")
+	add_compile_definitions(BOOTLOADER)
 	add_library(drivers_board
 		bootloader_main.c
+		init.c
 		usb.c
+		timer_config.cpp
 	)
 	target_link_libraries(drivers_board
 		PRIVATE
 			nuttx_arch # sdio
 			nuttx_drivers # sdio
+			px4_layer #gpio
+			arch_io_pins # iotimer
 			bootloader
 	)
 	target_include_directories(drivers_board PRIVATE ${PX4_SOURCE_DIR}/platforms/nuttx/src/bootloader/common)
@@ -53,6 +58,8 @@ else()
 		manifest.c
 		sdio.c
 		spi.cpp
+		spix_sync.c
+		spix_sync.h
 		timer_config.cpp
 		usb.c
 	)
@@ -125,7 +125,6 @@
 /* SPI */

 #define SPI6_nRESET_EXTERNAL1       /* PF10 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN10)
-#define GPIO_SYNC                   /* PE9  */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_100MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN9)

 /* I2C busses */

@@ -251,8 +250,20 @@

 /* PWM
 */
-#define DIRECT_PWM_OUTPUT_CHANNELS   9
+#define DIRECT_PWM_OUTPUT_CHANNELS   8
+#define BOARD_PWM_FREQ	             1024000

+#define GPIO_FMU_CH1                    /* PI0  */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTI|GPIO_PIN0)
+#define GPIO_FMU_CH2                    /* PH12 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTH|GPIO_PIN12)
+#define GPIO_FMU_CH3                    /* PH11 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTH|GPIO_PIN11)
+#define GPIO_FMU_CH4                    /* PH10 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTH|GPIO_PIN10)
+#define GPIO_FMU_CH5                    /* PD13 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTD|GPIO_PIN13)
+#define GPIO_FMU_CH6                    /* PD14 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTD|GPIO_PIN14)
+#define GPIO_FMU_CH7                    /* PH6  */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTH|GPIO_PIN6)
+#define GPIO_FMU_CH8                    /* PH9  */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTH|GPIO_PIN9)
+
+#define GPIO_FMU_CAP                    /* PE11 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTE|GPIO_PIN11)
+#define GPIO_SPIX_SYNC                  /* PE9  */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTE|GPIO_PIN9)

 /* Power supply control and monitoring GPIOs */

@@ -458,7 +469,6 @@
 		GPIO_VDD_3V3_SPEKTRUM_POWER_EN,   \
 		GPIO_VDD_3V3_SD_CARD_EN,          \
 		GPIO_PD15,                        \
-		GPIO_SYNC,                        \
 		SPI6_nRESET_EXTERNAL1,            \
 		GPIO_ETH_POWER_EN,                \
 		GPIO_NFC_GPIO,                    \
@@ -466,7 +476,17 @@
 		GPIO_nSAFETY_SWITCH_LED_OUT_INIT, \
 		GPIO_SAFETY_SWITCH_IN,            \
 		GPIO_PG6,                         \
-		GPIO_nARMED_INIT                  \
+		GPIO_nARMED_INIT,                 \
+		GPIO_FMU_CH1,     	          \
+		GPIO_FMU_CH2,     	          \
+		GPIO_FMU_CH3,     	          \
+		GPIO_FMU_CH4,     	          \
+		GPIO_FMU_CH5,     	          \
+		GPIO_FMU_CH6,     	          \
+		GPIO_FMU_CH7,     	          \
+		GPIO_FMU_CH8,     	          \
+		GPIO_FMU_CAP,     	          \
+		GPIO_SPIX_SYNC                    \
 	}

 #define BOARD_ENABLE_CONSOLE_BUFFER
@@ -474,7 +494,8 @@
 #define PX4_I2C_BUS_MTD      4,5


-#define BOARD_NUM_IO_TIMERS 5
+#define BOARD_NUM_IO_TIMERS  3
+#define BOARD_SPIX_SYNC_FREQ 32000

 __BEGIN_DECLS

@@ -46,6 +46,7 @@
 ****************************************************************************/

 #include "board_config.h"
+#include "spix_sync.h"

 #include <stdbool.h>
 #include <stdio.h>
@@ -279,5 +280,9 @@ __EXPORT int board_app_initialize(uintptr_t arg)

 #endif /* CONFIG_MMCSD */

+	/* Configure the SPIX_SYNC output */
+	spix_sync_servo_init(BOARD_SPIX_SYNC_FREQ);
+	spix_sync_servo_set(0, 150);
+
 	return OK;
 }
@@ -0,0 +1,311 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2023 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name Airmind nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+* @file spix_sync.c
+*
+*
+*/
+
+#include <px4_platform_common/px4_config.h>
+
+#include <board_config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <ctype.h>
+
+
+#include <perf/perf_counter.h>
+#include <systemlib/err.h>
+#include <systemlib/px4_macros.h>
+
+#include <px4_arch/io_timer.h>
+
+#include "spix_sync.h"
+
+#define REG(_tmr, _reg) (*(volatile uint32_t *)(spix_sync_timers[_tmr].base + _reg))
+
+#define rCR1(_tmr)      REG(_tmr, STM32_GTIM_CR1_OFFSET)
+#define rCR2(_tmr)      REG(_tmr, STM32_GTIM_CR2_OFFSET)
+#define rSMCR(_tmr)     REG(_tmr, STM32_GTIM_SMCR_OFFSET)
+#define rDIER(_tmr)     REG(_tmr, STM32_GTIM_DIER_OFFSET)
+#define rSR(_tmr)       REG(_tmr, STM32_GTIM_SR_OFFSET)
+#define rEGR(_tmr)      REG(_tmr, STM32_GTIM_EGR_OFFSET)
+#define rCCMR1(_tmr)    REG(_tmr, STM32_GTIM_CCMR1_OFFSET)
+#define rCCMR2(_tmr)    REG(_tmr, STM32_GTIM_CCMR2_OFFSET)
+#define rCCER(_tmr)     REG(_tmr, STM32_GTIM_CCER_OFFSET)
+#define rCNT(_tmr)      REG(_tmr, STM32_GTIM_CNT_OFFSET)
+#define rPSC(_tmr)      REG(_tmr, STM32_GTIM_PSC_OFFSET)
+#define rARR(_tmr)      REG(_tmr, STM32_GTIM_ARR_OFFSET)
+#define rCCR1(_tmr)     REG(_tmr, STM32_GTIM_CCR1_OFFSET)
+#define rCCR2(_tmr)     REG(_tmr, STM32_GTIM_CCR2_OFFSET)
+#define rCCR3(_tmr)     REG(_tmr, STM32_GTIM_CCR3_OFFSET)
+#define rCCR4(_tmr)     REG(_tmr, STM32_GTIM_CCR4_OFFSET)
+#define rDCR(_tmr)      REG(_tmr, STM32_GTIM_DCR_OFFSET)
+#define rDMAR(_tmr)     REG(_tmr, STM32_GTIM_DMAR_OFFSET)
+#define rBDTR(_tmr)     REG(_tmr, STM32_ATIM_BDTR_OFFSET)
+
+#if !defined(BOARD_PWM_FREQ)
+#define BOARD_PWM_FREQ 1000000
+#endif
+
+unsigned
+spix_sync_timer_get_period(unsigned timer)
+{
+	return (rARR(timer));
+}
+
+static void spix_sync_timer_init_timer(unsigned timer, unsigned rate)
+{
+	if (spix_sync_timers[timer].base) {
+
+		irqstate_t flags = px4_enter_critical_section();
+
+		/* enable the timer clock before we try to talk to it */
+
+		modifyreg32(spix_sync_timers[timer].clock_register, 0, spix_sync_timers[timer].clock_bit);
+
+		/* disable and configure the timer */
+		rCR1(timer) = 0;
+		rCR2(timer) = 0;
+		rSMCR(timer) = 0;
+		rDIER(timer) = 0;
+		rCCER(timer) = 0;
+		rCCMR1(timer) = 0;
+		rCCMR2(timer) = 0;
+		rCCR1(timer) = 0;
+		rCCR2(timer) = 0;
+		rCCR3(timer) = 0;
+		rCCR4(timer) = 0;
+		rCCER(timer) = 0;
+		rDCR(timer) = 0;
+
+		if ((spix_sync_timers[timer].base == STM32_TIM1_BASE) || (spix_sync_timers[timer].base == STM32_TIM8_BASE)) {
+
+			/* master output enable = on */
+
+			rBDTR(timer) = ATIM_BDTR_MOE;
+		}
+
+		/* If the timer clock source provided as clock_freq is the STM32_APBx_TIMx_CLKIN
+		 * then configure the timer to free-run at 1MHz.
+		 * Otherwise, other frequencies are attainable by adjusting .clock_freq accordingly.
+		 */
+
+		rPSC(timer) = (spix_sync_timers[timer].clock_freq / BOARD_PWM_FREQ) - 1;
+
+		/* configure the timer to update at the desired rate */
+
+		rARR(timer) = (BOARD_PWM_FREQ / rate) - 1;
+
+		/* generate an update event; reloads the counter and all registers */
+		rEGR(timer) = GTIM_EGR_UG;
+
+		px4_leave_critical_section(flags);
+	}
+
+}
+
+void spix_sync_channel_init(unsigned channel)
+{
+	/* Only initialize used channels */
+
+	if (spix_sync_channels[channel].timer_channel) {
+
+		unsigned timer = spix_sync_channels[channel].timer_index;
+
+		/* configure the GPIO first */
+		px4_arch_configgpio(spix_sync_channels[channel].gpio_out);
+
+		uint16_t polarity = spix_sync_channels[channel].masks;
+
+		/* configure the channel */
+		switch (spix_sync_channels[channel].timer_channel) {
+		case 1:
+			rCCMR1(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR1_OC1M_SHIFT) | GTIM_CCMR1_OC1PE;
+			rCCER(timer) |= polarity | GTIM_CCER_CC1E;
+			break;
+
+		case 2:
+			rCCMR1(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR1_OC2M_SHIFT) | GTIM_CCMR1_OC2PE;
+			rCCER(timer) |= polarity | GTIM_CCER_CC2E;
+			break;
+
+		case 3:
+			rCCMR2(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR2_OC3M_SHIFT) | GTIM_CCMR2_OC3PE;
+			rCCER(timer) |= polarity | GTIM_CCER_CC3E;
+			break;
+
+		case 4:
+			rCCMR2(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR2_OC4M_SHIFT) | GTIM_CCMR2_OC4PE;
+			rCCER(timer) |= polarity | GTIM_CCER_CC4E;
+			break;
+		}
+	}
+}
+
+int
+spix_sync_servo_set(unsigned channel, uint8_t  cvalue)
+{
+	if (channel >= arraySize(spix_sync_channels)) {
+		return -1;
+	}
+
+	unsigned timer = spix_sync_channels[channel].timer_index;
+
+	/* test timer for validity */
+	if ((spix_sync_timers[timer].base == 0) ||
+	    (spix_sync_channels[channel].gpio_out == 0)) {
+		return -1;
+	}
+
+	unsigned period = spix_sync_timer_get_period(timer);
+
+	unsigned value = (unsigned)cvalue * period / 255;
+
+	/* configure the channel */
+	if (value > 0) {
+		value--;
+	}
+
+
+	switch (spix_sync_channels[channel].timer_channel) {
+	case 1:
+		rCCR1(timer) = value;
+		break;
+
+	case 2:
+		rCCR2(timer) = value;
+		break;
+
+	case 3:
+		rCCR3(timer) = value;
+		break;
+
+	case 4:
+		rCCR4(timer) = value;
+		break;
+
+	default:
+		return -1;
+	}
+
+	return 0;
+}
+
+unsigned spix_sync_servo_get(unsigned channel)
+{
+	if (channel >= 3) {
+		return 0;
+	}
+
+	unsigned timer = spix_sync_channels[channel].timer_index;
+	uint16_t value = 0;
+
+	/* test timer for validity */
+	if ((spix_sync_timers[timer].base == 0) ||
+	    (spix_sync_channels[channel].timer_channel == 0)) {
+		return 0;
+	}
+
+	/* configure the channel */
+	switch (spix_sync_channels[channel].timer_channel) {
+	case 1:
+		value = rCCR1(timer);
+		break;
+
+	case 2:
+		value = rCCR2(timer);
+		break;
+
+	case 3:
+		value = rCCR3(timer);
+		break;
+
+	case 4:
+		value = rCCR4(timer);
+		break;
+	}
+
+	unsigned period = spix_sync_timer_get_period(timer);
+	return ((value + 1) * 255 / period);
+}
+
+int spix_sync_servo_init(unsigned rate)
+{
+	/* do basic timer initialisation first */
+	for (unsigned i = 0; i < arraySize(spix_sync_timers); i++) {
+		spix_sync_timer_init_timer(i, rate);
+	}
+
+	/* now init channels */
+	for (unsigned i = 0; i < arraySize(spix_sync_channels); i++) {
+		spix_sync_channel_init(i);
+	}
+
+	spix_sync_servo_arm(true);
+	return OK;
+}
+
+void
+spix_sync_servo_deinit(void)
+{
+	/* disable the timers */
+	spix_sync_servo_arm(false);
+}
+void
+spix_sync_servo_arm(bool armed)
+{
+	/* iterate timers and arm/disarm appropriately */
+	for (unsigned i = 0; i < arraySize(spix_sync_timers); i++) {
+		if (spix_sync_timers[i].base != 0) {
+			if (armed) {
+				/* force an update to preload all registers */
+				rEGR(i) = GTIM_EGR_UG;
+
+				/* arm requires the timer be enabled */
+				rCR1(i) |= GTIM_CR1_CEN | GTIM_CR1_ARPE;
+
+			} else {
+				rCR1(i) = 0;
+			}
+		}
+	}
+}
@@ -0,0 +1,42 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2023 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+__BEGIN_DECLS
+void spix_sync_channel_init(unsigned channel);
+int spix_sync_servo_set(unsigned channel, uint8_t  value);
+unsigned spix_sync_servo_get(unsigned channel);
+int spix_sync_servo_init(unsigned rate);
+void spix_sync_servo_deinit(void);
+void spix_sync_servo_arm(bool armed);
+unsigned spix_sync_timer_get_period(unsigned timer);
+__END_DECLS
@@ -60,8 +60,8 @@ constexpr io_timers_t io_timers[MAX_IO_TIMERS] = {
 	initIOTimer(Timer::Timer5, DMA{DMA::Index1}),
 	initIOTimer(Timer::Timer4, DMA{DMA::Index1}),
 	initIOTimer(Timer::Timer12),
-	initIOTimer(Timer::Timer1),
-	initIOTimer(Timer::Timer2),
+	//initIOTimer(Timer::Timer1),
+	//initIOTimer(Timer::Timer2),
 };

 constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = {
@@ -73,8 +73,18 @@ constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = {
 	initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel3}, {GPIO::PortD, GPIO::Pin14}),
 	initIOTimerChannel(io_timers, {Timer::Timer12, Timer::Channel1}, {GPIO::PortH, GPIO::Pin6}),
 	initIOTimerChannel(io_timers, {Timer::Timer12, Timer::Channel2}, {GPIO::PortH, GPIO::Pin9}),
-	initIOTimerChannelCapture(io_timers, {Timer::Timer1, Timer::Channel2}, {GPIO::PortE, GPIO::Pin11}),
+	//initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel2}, {GPIO::PortE, GPIO::Pin11}),
+	//initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel1}, {GPIO::PortE, GPIO::Pin9}),
 };

 constexpr io_timers_channel_mapping_t io_timers_channel_mapping =
 	initIOTimerChannelMapping(io_timers, timer_io_channels);
+
+
+constexpr io_timers_t spix_sync_timers[MAX_SPIX_SYNC_TIMERS] = {
+	initIOTimer(Timer::Timer1),
+};
+
+constexpr timer_io_channels_t spix_sync_channels[MAX_SPIX_SYNC_TIMERS] = {
+	initIOTimerChannel(spix_sync_timers, {Timer::Timer1, Timer::Channel1}, {GPIO::PortE, GPIO::Pin9}),
+};
@@ -43,7 +43,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL_L1=y
+CONFIG_MODULES_FW_PATH_NAVIGATION=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -31,7 +31,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL_L1=y
+CONFIG_MODULES_FW_PATH_NAVIGATION=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -50,7 +50,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL_L1=y
+CONFIG_MODULES_FW_PATH_NAVIGATION=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -51,7 +51,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL_L1=y
+CONFIG_MODULES_FW_PATH_NAVIGATION=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
--- a/Show More
+++ b/Show More