Set feedforward heightrate setpoint in offboard mode

Fix feedforward acceleration setpoints for fixedwing offboard position
This commit fixes feedforward acceleration setpoints for fixedwing offboard position control. Previously when acceleration feedforward inputs were sent, negative curvature accelerations were not being computed properly
2026-06-12 02:00:06 +08:00 · 2023-02-24 08:17:27 +01:00 · 2022-10-19 09:15:13 +02:00 · 2022-10-18 18:38:25 -04:00 · 2022-10-18 14:19:16 -04:00 · 2022-10-18 14:19:16 -04:00
290 changed files with 14258 additions and 10270 deletions
@@ -937,7 +937,7 @@ void printTopics() {
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_odometry" || true'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_rates_setpoint" || true'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_status" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_status_flags" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener failsafe_flags" || true'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vtol_vehicle_status" || true'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener yaw_estimator_status" || true'
 }
@@ -23,7 +23,6 @@ jobs:
          "shellcheck_all",
          "NO_NINJA_BUILD=1 px4_fmu-v5_default",
          "NO_NINJA_BUILD=1 px4_sitl_default",
-          "NO_NINJA_BUILD=1 px4_sitl_gps_base",
          "BUILD_MICRORTPS_AGENT=1 px4_sitl_rtps",
          "airframe_metadata",
          "module_documentation",
@@ -0,0 +1,44 @@
+name: Failsafe Simulator Build
+
+on:
+  push:
+    branches:
+    - 'main'
+  pull_request:
+    branches:
+    - '*'
+
+jobs:
+  build:
+    runs-on: ubuntu-latest
+    defaults:
+      run:
+        shell: bash
+    strategy:
+      fail-fast: false
+      matrix:
+        check: [
+          "failsafe_web",
+        ]
+    container:
+      image: px4io/px4-dev-nuttx-focal:2021-09-08
+      options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined
+    steps:
+    - uses: actions/checkout@v1
+      with:
+        token: ${{ secrets.ACCESS_TOKEN }}
+
+    - name: check environment
+      run: |
+          export
+          ulimit -a
+    - name: install emscripten
+      run: |
+        git clone https://github.com/emscripten-core/emsdk.git _emscripten_sdk
+        cd _emscripten_sdk
+        ./emsdk install latest
+        ./emsdk activate latest
+    - name: ${{matrix.check}}
+      run: |
+        . ./_emscripten_sdk/emsdk_env.sh
+        make ${{matrix.check}}
@@ -9,15 +9,15 @@
 [submodule "Tools/simulation/jmavsim/jMAVSim"]
 	path = Tools/simulation/jmavsim/jMAVSim
 	url = https://github.com/PX4/jMAVSim.git
-	branch = master
+	branch = main
 [submodule "Tools/simulation/gazebo/sitl_gazebo"]
 	path = Tools/simulation/gazebo/sitl_gazebo
 	url = https://github.com/PX4/PX4-SITL_gazebo.git
-	branch = master
+	branch = main
 [submodule "src/drivers/gps/devices"]
 	path = src/drivers/gps/devices
 	url = https://github.com/PX4/PX4-GPSDrivers.git
-	branch = master
+	branch = main
 [submodule "src/modules/micrortps_bridge/micro-CDR"]
 	path = src/modules/micrortps_bridge/micro-CDR
 	url = https://github.com/PX4/Micro-CDR.git
@@ -53,6 +53,7 @@
 [submodule "src/lib/events/libevents"]
 	path = src/lib/events/libevents
 	url = https://github.com/mavlink/libevents.git
+	branch = main
 [submodule "src/lib/crypto/libtomcrypt"]
 	path = src/lib/crypto/libtomcrypt
 	url = https://github.com/PX4/libtomcrypt.git
@@ -6,11 +6,6 @@ CONFIG:
      buildType: RelWithDebInfo
      settings:
        CONFIG: px4_sitl_default
-    px4_sitl_gps_base:
-      short: px4_sitl_gps_base
-      buildType: RelWithDebInfo
-      settings:
-        CONFIG: px4_sitl_gps_base
    px4_sitl_rtps:
      short: px4_sitl_rtps
      buildType: RelWithDebInfo
@@ -162,6 +162,35 @@ pipeline {
          }
        }

+        stage('failsafe docs') {
+          agent {
+            docker { image 'px4io/px4-dev-base-focal:2021-08-18' }
+          }
+          steps {
+            sh '''#!/bin/bash -l
+			echo $0;
+            git clone https://github.com/emscripten-core/emsdk.git _emscripten_sdk;
+            cd _emscripten_sdk;
+            ./emsdk install latest;
+            ./emsdk activate latest;
+            . ./_emscripten_sdk/emsdk_env.sh;
+			make failsafe_web;
+			cd build/px4_sitl_default_failsafe_web;
+			mkdir -p failsafe_sim;
+			cp index.* parameters.json failsafe_sim;
+			'''
+            dir('build/px4_sitl_default_failsafe_web') {
+              archiveArtifacts(artifacts: 'failsafe_sim/*')
+              stash includes: 'failsafe_sim/*', name: 'failsafe_sim'
+            }
+          }
+          post {
+            always {
+              sh 'make distclean; git clean -ff -x -d .'
+            }
+          }
+        }
+
        stage('uORB graphs') {
          agent {
            docker {
@@ -203,6 +232,7 @@ pipeline {
            unstash 'metadata_parameters'
            unstash 'metadata_module_documentation'
            unstash 'msg_documentation'
+            unstash 'failsafe_sim'
            unstash 'uorb_graph'
            withCredentials([usernamePassword(credentialsId: 'px4buildbot_github_personal_token', passwordVariable: 'GIT_PASS', usernameVariable: 'GIT_USER')]) {
              sh('git clone https://${GIT_USER}:${GIT_PASS}@github.com/PX4/PX4-user_guide.git')
@@ -211,6 +241,7 @@ pipeline {
              sh('cp -R modules/*.md PX4-user_guide/en/modules/')
              sh('cp -R graph_*.json PX4-user_guide/.vuepress/public/en/middleware/')
              sh('cp -R msg_docs/*.md PX4-user_guide/en/msg_docs/')
+              sh('cp -R failsafe_sim/* PX4-user_guide/.vuepress/public/en/config/failsafe')
              sh('cd PX4-user_guide; git status; git add .; git commit -a -m "Update PX4 Firmware metadata `date`" || true')
              sh('cd PX4-user_guide; git push origin main || true')
              sh('rm -rf PX4-user_guide')
@@ -575,3 +575,13 @@ update_px4_ros_com:

 update_px4_msgs:
 	@Tools/update_px4_ros2_bridge.sh --ws_dir ${ROS2_WS_DIR} --px4_msgs
+
+.PHONY: failsafe_web run_failsafe_web_server
+failsafe_web:
+	@if ! command -v emcc; then echo -e "Install emscripten first: https://emscripten.org/docs/getting_started/downloads.html\nAnd source the env: source <path>/emsdk_env.sh"; exit 1; fi
+	@$(MAKE) --no-print-directory px4_sitl_default failsafe_test parameters_xml \
+		PX4_CMAKE_BUILD_TYPE=Release BUILD_DIR_SUFFIX=_failsafe_web \
+		CMAKE_ARGS="-DCMAKE_CXX_COMPILER=em++ -DCMAKE_C_COMPILER=emcc"
+run_failsafe_web_server: failsafe_web
+	@cd build/px4_sitl_default_failsafe_web && \
+		python3 -m http.server
@@ -4,7 +4,7 @@

 [![Nuttx Targets](https://github.com/PX4/PX4-Autopilot/workflows/Nuttx%20Targets/badge.svg)](https://github.com/PX4/PX4-Autopilot/actions?query=workflow%3A%22Nuttx+Targets%22?branch=master) [![SITL Tests](https://github.com/PX4/PX4-Autopilot/workflows/SITL%20Tests/badge.svg?branch=master)](https://github.com/PX4/PX4-Autopilot/actions?query=workflow%3A%22SITL+Tests%22)

-[![Slack](/.github/slack.svg)](https://join.slack.com/t/px4/shared_invite/zt-si4xo5qs-R4baYFmMjlrT4rQK5yUnaA)
+[![Discord Shield](https://discordapp.com/api/guilds/1022170275984457759/widget.png?style=shield)](https://discord.gg/dronecode)

 This repository holds the [PX4](http://px4.io) flight control solution for drones, with the main applications located in the [src/modules](https://github.com/PX4/PX4-Autopilot/tree/main/src/modules) directory. It also contains the PX4 Drone Middleware Platform, which provides drivers and middleware to run drones.

@@ -41,6 +41,20 @@ elif [ "$PX4_SIMULATOR" = "gz" ]; then
 	fi

 	gz_world=$( ign topic -l | grep -m 1 -e "/world/.*/clock" | sed 's/\/world\///g; s/\/clock//g' )
+	
+	gz_version_major=$( ign gazebo --versions | sed 's/\..*//g' )
+	gz_version_minor=$( ign gazebo --versions | sed 's/'"${gz_version_major}"\.'//; s/\..*//g' )
+	gz_version_point=$( ign gazebo --versions | sed 's/'"${gz_version_major}"\.'//; s/'"${gz_version_minor}"\.'//')
+
+	if [ "$gz_version_major" -gt "6" ] || { [ "$gz_version_major" -eq "6" ] && [ "$gz_version_minor" -gt "12" ]; } || { [ "$gz_version_major" -eq "6" ] && [ "$gz_version_minor" -eq "12" ] && [ "$gz_version_point" -gt "0" ]; }; then
+		echo "INFO  [init] using latest version of MultiCopterMotor plugin."
+	else
+		echo "WARN  [init] using older version of MultiCopterMotor plugin, please update to latest gazebo > 6.12.0."
+		if [ "$PX4_SIM_MODEL" = "x500" ]; then
+			PX4_SIM_MODEL="x500-Legacy"
+			echo "WARN  [init] setting PX4_SIM_MODEL -> $PX4_SIM_MODEL from x500 till gazebo > 6.12.0"
+		fi
+	fi

 	if [ -z $gz_world ]; then

@@ -38,7 +38,6 @@ param set-default MIS_YAW_TMT 10
 param set-default MPC_ACC_HOR_MAX 2
 param set-default MPC_ACC_HOR_MAX 2
 param set-default MPC_THR_MIN 0.1
-param set-default MPC_TKO_SPEED 1
 param set-default MPC_XY_P 0.8
 param set-default MPC_XY_VEL_D_ACC 0.1
 param set-default MPC_XY_VEL_I_ACC 4
@@ -65,7 +65,6 @@ param set-default COM_SPOOLUP_TIME 1.5
 param set-default MPC_THR_HOVER 0.45
 param set-default MPC_TILTMAX_AIR 25
 param set-default MPC_TKO_RAMP_T 1.8
-param set-default MPC_TKO_SPEED 1
 param set-default MPC_VEL_MANUAL 3
 param set-default MPC_XY_CRUISE 3
 param set-default MPC_XY_VEL_MAX 3.5
@@ -63,7 +63,6 @@ param set-default MPC_ACC_UP_MAX 4
 param set-default MPC_MAN_Y_MAX 120
 param set-default MPC_TILTMAX_AIR 45
 param set-default MPC_THR_HOVER 0.3
-param set-default MPC_TKO_SPEED 1
 param set-default MPC_VEL_MANUAL 5
 param set-default MPC_XY_CRUISE 5
 param set-default MPC_XY_VEL_MAX 5
@@ -64,7 +64,6 @@ param set-default MPC_MANTHR_MIN 0
 param set-default MPC_MAN_Y_MAX 120
 param set-default MPC_TILTMAX_AIR 45
 param set-default MPC_THR_HOVER 0.3
-param set-default MPC_TKO_SPEED 1
 param set-default MPC_VEL_MANUAL 5
 param set-default MPC_XY_CRUISE 5
 param set-default MPC_XY_VEL_MAX 5
@@ -53,7 +53,6 @@ param set-default MPC_XY_VEL_P_ACC 2.6
 param set-default MPC_XY_VEL_I_ACC 1.2
 param set-default MPC_XY_VEL_D_ACC 0.2
 param set-default MPC_TKO_RAMP_T 1
-param set-default MPC_TKO_SPEED 1.1
 param set-default MPC_VEL_MANUAL 3

 param set-default BAT1_SOURCE 0
@@ -62,7 +62,6 @@ param set-default MPC_XY_VEL_P_ACC 2.6
 param set-default MPC_XY_VEL_I_ACC 1.2
 param set-default MPC_XY_VEL_D_ACC 0.2
 param set-default MPC_TKO_RAMP_T 1
-param set-default MPC_TKO_SPEED 1.1
 param set-default MPC_VEL_MANUAL 3

 param set-default BAT1_SOURCE 0
@@ -0,0 +1,75 @@
+#!/bin/sh
+#
+# @name Aion Robotics R1 UGV
+#
+# @url https://www.aionrobotics.com/r1
+#
+# @type Rover
+# @class Rover
+#
+# @board px4_fmu-v2 exclude
+# @board bitcraze_crazyflie exclude
+#
+
+. ${R}etc/init.d/rc.rover_defaults
+
+param set-default BAT1_N_CELLS 4
+
+param set-default EKF2_GBIAS_INIT 0.01
+param set-default EKF2_ANGERR_INIT 0.01
+param set-default EKF2_MAG_TYPE 1
+
+param set-default FW_AIRSPD_MIN 0
+param set-default FW_AIRSPD_TRIM 1
+param set-default FW_AIRSPD_MAX 3
+
+param set-default GND_SP_CTRL_MODE 1
+param set-default GND_L1_DIST 5
+param set-default GND_L1_PERIOD 3
+param set-default GND_THR_CRUISE 0.7
+param set-default GND_THR_MAX 0.5
+
+# Because this is differential drive, it can make a turn with radius 0.
+# This corresponds to a turn angle of pi radians.
+# If a special case is made for differential-drive, this will need to change.
+param set-default GND_MAX_ANG 3.142
+param set-default GND_WHEEL_BASE 0.3
+
+# TODO: Set to -1.0, to allow reversing. This will require many changes in the codebase
+# to support negative throttle.
+param set-default GND_THR_MIN 0
+param set-default GND_SPEED_P 0.25
+param set-default GND_SPEED_I 3
+param set-default GND_SPEED_D 0.001
+param set-default GND_SPEED_IMAX 0.125
+param set-default GND_SPEED_THR_SC 1
+
+param set-default MIS_LTRMIN_ALT 0.01
+param set-default MIS_TAKEOFF_ALT 0.01
+
+param set-default NAV_ACC_RAD 0.5
+
+# Enable Airspeed check circuit breaker because Rovers will have no airspeed sensor
+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
+param set-default PWM_MAIN_FUNC1 101
+param set-default PWM_MAIN_FUNC2 102
+param set-default PWM_MAIN_DIS1 1500
+param set-default PWM_MAIN_DIS2 1500
+param set-default PWM_MAIN_TIM0 50
+param set-default PWM_MAIN_TIM1 50
+
@@ -118,6 +118,7 @@ px4_add_romfs_files(

 	50000_generic_ground_vehicle
 	50004_nxpcup_car_dfrobot_gpx
+	50003_aion_robotics_r1_rover

 	# [60000, 61000] (Unmanned) Underwater Robots
 	60000_uuv_generic
@@ -324,6 +324,27 @@ else
 	rc_update start
 	manual_control start

+	# Start camera trigger, capture and PPS before pwm_out as they might access
+	# pwm pins
+	if param greater -s TRIG_MODE 0
+	then
+		camera_trigger start
+		camera_feedback start
+	fi
+	# PPS capture driver
+	if param greater -s PPS_CAP_ENABLE 0
+	then
+		pps_capture start
+	fi
+	# Camera capture driver
+	if param greater -s CAM_CAP_FBACK 0
+	then
+		if camera_capture start
+		then
+			camera_capture on
+		fi
+	fi
+
 	#
 	# Sensors System (start before Commander so Preflight checks are properly run).
 	# Commander needs to be this early for in-air-restarts.
@@ -390,12 +411,6 @@ else
 		mag_bias_estimator start
 	fi

-	if param greater -s TRIG_MODE 0
-	then
-		camera_trigger start
-		camera_feedback start
-	fi
-
 	#
 	# Optional board mavlink streams: rc.board_mavlink
 	#
@@ -416,21 +431,6 @@ else
 	# Must be started after the serial config is read
 	rc_input start $RC_INPUT_ARGS

-	# PPS capture driver (before pwm_out)
-	if param greater -s PPS_CAP_ENABLE 0
-	then
-		pps_capture start
-	fi
-
-	# Camera capture driver (before pwm_out)
-	if param greater -s CAM_CAP_FBACK 0
-	then
-		if camera_capture start
-		then
-			camera_capture on
-		fi
-	fi
-
 	#
 	# Play the startup tune (if not disabled or there is an error)
 	#
@@ -489,7 +489,7 @@ actuators = {

 with open(output_file, 'w') as outfile:
    indent = 2 if verbose else None
-    json.dump(actuators, outfile, indent=indent)
+    json.dump(actuators, outfile, indent=indent, sort_keys=True)

 if compress:
    save_compressed(output_file)
@@ -32,16 +32,16 @@ def extract_timer(line):
    # Try format: initIOTimer(Timer::Timer5, DMA{DMA::Index1, DMA::Stream0, DMA::Channel6}),
    search = re.search('Timer::([0-9a-zA-Z_]+)[,\)]', line, re.IGNORECASE)
    if search:
-        return search.group(1)
+        return search.group(1), 'generic'

    # nxp rt1062 format: initIOPWM(PWM::FlexPWM2),
    search = re.search('PWM::Flex([0-9a-zA-Z_]+)[,\)]', line, re.IGNORECASE)
    if search:
-        return search.group(1)
+        return search.group(1), 'imxrt'

-    return None
+    return None, 'unknown'

-def extract_timer_from_channel(line):
+def extract_timer_from_channel(line, num_channels_already_found):
    # Try format: initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel1}, {GPIO::PortA, GPIO::Pin0}),
    search = re.search('Timer::([0-9a-zA-Z_]+), ', line, re.IGNORECASE)
    if search:
@@ -50,7 +50,8 @@ def extract_timer_from_channel(line):
    # nxp rt1062 format: initIOTimerChannel(io_timers, {PWM::PWM2_PWM_A, PWM::Submodule0}, IOMUX::Pad::GPIO_B0_06),
    search = re.search('PWM::(PWM[0-9]+)[_,\)]', line, re.IGNORECASE)
    if search:
-        return search.group(1)
+        # imxrt uses a 1:1 timer group to channel association
+        return str(num_channels_already_found)

    return None

@@ -72,7 +73,14 @@ def get_timer_groups(timer_config_file, verbose=False):
        line = line.strip()
        if len(line) == 0 or line.startswith('//'):
            continue
-        timer = extract_timer(line)
+        timer, timer_type = extract_timer(line)
+
+        if timer_type == 'imxrt':
+            if verbose: print('imxrt timer found')
+            max_num_channels = 16 # Just add a fixed number of timers
+            timers = [str(i) for i in range(max_num_channels)]
+            dshot_support = {str(i): False for i in range(max_num_channels)}
+            break

        if timer:
            if verbose: print('found timer def: {:}'.format(timer))
@@ -101,7 +109,7 @@ def get_timer_groups(timer_config_file, verbose=False):
            continue

        if verbose: print('--'+line+'--')
-        timer = extract_timer_from_channel(line)
+        timer = extract_timer_from_channel(line, len(channel_timers))

        if timer:
            if verbose: print('Found timer: {:} in channel line {:}'.format(timer, line))
@@ -211,6 +211,11 @@ class SourceParser(object):

                    ignore_event = False

+                    def parse_message(s):
+                        assert s[0] == '"', "Argument must be a string literal: {:}".format(s)
+                        # unescape \x, to treat the string the same as the C++ compiler
+                        return s[1:-1].encode("utf-8").decode('unicode_escape')
+
                    # extract function arguments
                    args_split = self._parse_arguments(args)
                    if call == "events::send" or call == "send":
@@ -228,8 +233,7 @@ class SourceParser(object):
                            else:
                                raise Exception("Could not extract event ID from {:}".format(args_split[0]))
                            event.name = event_name
-                            # unescape \x, to treat the string the same as the C++ compiler
-                            event.message = args_split[2][1:-1].encode("utf-8").decode('unicode_escape')
+                            event.message = parse_message(args_split[2])
                    elif call in ['reporter.healthFailure', 'reporter.armingCheckFailure']:
                        assert len(args_split) == num_args + 5, \
                            "Unexpected Number of arguments for: {:}, {:}".format(args_split, num_args)
@@ -239,7 +243,7 @@ class SourceParser(object):
                        else:
                            raise Exception("Could not extract event ID from {:}".format(args_split[2]))
                        event.name = event_name
-                        event.message = args_split[4][1:-1]
+                        event.message = parse_message(args_split[4])
                        if 'health' in call:
                            event.group = "health"
                        else:
@@ -77,11 +77,23 @@ except ImportError as e:
    print("")
    sys.exit(1)

+
+# Define time to use time.time() by default
+def _time():
+    return time.time()
+
 # Detect python version
 if sys.version_info[0] < 3:
    runningPython3 = False
 else:
    runningPython3 = True
+    if sys.version_info[1] >=3:
+        # redefine to use monotonic time when available
+        def _time():
+            try:
+                return time.monotonic()
+            except Exception:
+                return time.time()

 class FirmwareNotSuitableException(Exception):
    def __init__(self, message):
@@ -230,7 +242,7 @@ class uploader(object):

    def open(self):
        # upload timeout
-        timeout = time.time() + 0.2
+        timeout = _time() + 0.2

        # attempt to open the port while it exists and until timeout occurs
        while self.port is not None:
@@ -240,7 +252,7 @@ class uploader(object):
            except AttributeError:
                portopen = self.port.isOpen()

-            if not portopen and time.time() < timeout:
+            if not portopen and _time() < timeout:
                try:
                    self.port.open()
                except OSError:
@@ -415,16 +427,16 @@ class uploader(object):
                    uploader.EOC)

        # erase is very slow, give it 30s
-        deadline = time.time() + 30.0
-        while time.time() < deadline:
+        deadline = _time() + 30.0
+        while _time() < deadline:

            usualEraseDuration = 15.0
-            estimatedTimeRemaining = deadline-time.time()
+            estimatedTimeRemaining = deadline-_time()
            if estimatedTimeRemaining >= usualEraseDuration:
                self.__drawProgressBar(label, 30.0-estimatedTimeRemaining, usualEraseDuration)
            else:
                self.__drawProgressBar(label, 10.0, 10.0)
-                sys.stdout.write(" (timeout: %d seconds) " % int(deadline-time.time()))
+                sys.stdout.write(" (timeout: %d seconds) " % int(deadline-_time()))
                sys.stdout.flush()

            if self.__trySync():
@@ -572,7 +584,7 @@ class uploader(object):
    # upload the firmware
    def upload(self, fw, force=False, boot_delay=None):
        # Make sure we are doing the right thing
-        start = time.time()
+        start = _time()
        if self.board_type != fw.property('board_id'):
            msg = "Firmware not suitable for this board (Firmware board_type=%u board_id=%u)" % (
                self.board_type, fw.property('board_id'))
@@ -668,7 +680,7 @@ class uploader(object):
        print("\nRebooting.", end='')
        self.__reboot()
        self.port.close()
-        print(" Elapsed Time %3.3f\n" % (time.time() - start))
+        print(" Elapsed Time %3.3f\n" % (_time() - start))

    def __next_baud_flightstack(self):
        if self.baudrate_flightstack_idx + 1 >= len(self.baudrate_flightstack):
@@ -2,7 +2,7 @@

 set -e

-## Bash script to setup PX4 development environment on Ubuntu LTS (20.04, 18.04, 16.04).
+## Bash script to setup PX4 development environment on Ubuntu LTS (22.04, 20.04, 18.04).
 ## Can also be used in docker.
 ##
 ## Installs:
@@ -0,0 +1,11 @@
+<?xml version="1.0"?>
+<model>
+  <name>x500-Legacy</name>
+  <version>1.0</version>
+  <sdf version="1.9">model.sdf</sdf>
+  <author>
+    <name>Benjamin Perseghetti</name>
+    <email>bperseghetti@rudislabs.com</email>
+  </author>
+  <description>Model of the NXP HoverGames Drone development kit (KIT-HGDRONEK66). The PX4 software compatible kit provides mechanical, RC remote and other components needed to evaluate the RDDRONE-FMUK66 reference design. The FMU includes 100Base-T1 Automotive Ethernet, dual CAN transceivers, as well as SE050 secure element, and works with add on boards NavQPlus, MR-T1ETH8, MR-T1ADAPT, and CAN-nodes such as UCANS32K1SIC.  Kit may be used with, and contains the components needed for the HoverGames.com coding challenges.</description>
+</model>
@@ -0,0 +1,76 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<sdf version='1.9'>
+  <model name='x500-Legacy'>
+    <include merge='true'>
+      <uri>model://x500-NoPlugin</uri>
+    </include>
+    <plugin filename="ignition-gazebo-multicopter-motor-model-system" name="ignition::gazebo::systems::MulticopterMotorModel">
+      <robotNamespace>/x500-Legacy_0</robotNamespace>
+      <jointName>rotor_0_joint</jointName>
+      <linkName>rotor_0</linkName>
+      <turningDirection>ccw</turningDirection>
+      <timeConstantUp>0.0125</timeConstantUp>
+      <timeConstantDown>0.025</timeConstantDown>
+      <maxRotVelocity>1000.0</maxRotVelocity>
+      <motorConstant>8.54858e-06</motorConstant>
+      <momentConstant>0.016</momentConstant>
+      <commandSubTopic>command/motor_speed</commandSubTopic>
+      <motorNumber>0</motorNumber>
+      <rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
+      <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
+      <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
+      <motorType>velocity</motorType>
+    </plugin>
+    <plugin filename="ignition-gazebo-multicopter-motor-model-system" name="ignition::gazebo::systems::MulticopterMotorModel">
+      <robotNamespace>/x500-Legacy_0</robotNamespace>
+      <jointName>rotor_1_joint</jointName>
+      <linkName>rotor_1</linkName>
+      <turningDirection>ccw</turningDirection>
+      <timeConstantUp>0.0125</timeConstantUp>
+      <timeConstantDown>0.025</timeConstantDown>
+      <maxRotVelocity>1000.0</maxRotVelocity>
+      <motorConstant>8.54858e-06</motorConstant>
+      <momentConstant>0.016</momentConstant>
+      <commandSubTopic>command/motor_speed</commandSubTopic>
+      <motorNumber>1</motorNumber>
+      <rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
+      <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
+      <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
+      <motorType>velocity</motorType>
+    </plugin>
+    <plugin filename="ignition-gazebo-multicopter-motor-model-system" name="ignition::gazebo::systems::MulticopterMotorModel">
+      <robotNamespace>/x500-Legacy_0</robotNamespace>
+      <jointName>rotor_2_joint</jointName>
+      <linkName>rotor_2</linkName>
+      <turningDirection>cw</turningDirection>
+      <timeConstantUp>0.0125</timeConstantUp>
+      <timeConstantDown>0.025</timeConstantDown>
+      <maxRotVelocity>1000.0</maxRotVelocity>
+      <motorConstant>8.54858e-06</motorConstant>
+      <momentConstant>0.016</momentConstant>
+      <commandSubTopic>command/motor_speed</commandSubTopic>
+      <motorNumber>2</motorNumber>
+      <rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
+      <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
+      <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
+      <motorType>velocity</motorType>
+    </plugin>
+    <plugin filename="ignition-gazebo-multicopter-motor-model-system" name="ignition::gazebo::systems::MulticopterMotorModel">
+      <robotNamespace>/x500-Legacy_0</robotNamespace>
+      <jointName>rotor_3_joint</jointName>
+      <linkName>rotor_3</linkName>
+      <turningDirection>cw</turningDirection>
+      <timeConstantUp>0.0125</timeConstantUp>
+      <timeConstantDown>0.025</timeConstantDown>
+      <maxRotVelocity>1000.0</maxRotVelocity>
+      <motorConstant>8.54858e-06</motorConstant>
+      <momentConstant>0.016</momentConstant>
+      <commandSubTopic>command/motor_speed</commandSubTopic>
+      <motorNumber>3</motorNumber>
+      <rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
+      <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
+      <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
+      <motorType>velocity</motorType>
+    </plugin>
+  </model>
+</sdf>
@@ -0,0 +1,11 @@
+<?xml version="1.0"?>
+<model>
+  <name>x500-NoPlugin</name>
+  <version>1.0</version>
+  <sdf version="1.9">model.sdf</sdf>
+  <author>
+    <name>Benjamin Perseghetti</name>
+    <email>bperseghetti@rudislabs.com</email>
+  </author>
+  <description>Model of the NXP HoverGames Drone development kit (KIT-HGDRONEK66). The PX4 software compatible kit provides mechanical, RC remote and other components needed to evaluate the RDDRONE-FMUK66 reference design. The FMU includes 100Base-T1 Automotive Ethernet, dual CAN transceivers, as well as SE050 secure element, and works with add on boards NavQPlus, MR-T1ETH8, MR-T1ADAPT, and CAN-nodes such as UCANS32K1SIC.  Kit may be used with, and contains the components needed for the HoverGames.com coding challenges.</description>
+</model>
@@ -0,0 +1,516 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<sdf version='1.9'>
+  <model name='x500-NoPlugin'>
+    <pose>0 0 .24 0 0 0</pose>
+    <self_collide>false</self_collide>
+    <static>false</static>
+    <link name="base_link">
+      <inertial>
+        <mass>2.0</mass>
+        <inertia>
+          <ixx>0.02166666666666667</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>0.02166666666666667</iyy>
+          <iyz>0</iyz>
+          <izz>0.04000000000000001</izz>
+        </inertia>
+      </inertial>
+      <gravity>true</gravity>
+      <velocity_decay/>
+      <visual name="base_link_visual">
+        <pose>0 0 .025 0 0 3.141592654</pose>
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://x500-NoPlugin/meshes/NXP-HGD-CF.dae</uri>
+          </mesh>
+        </geometry>
+      </visual>
+      <visual name="5010_motor_base_0">
+        <pose>0.174 0.174 .032 0 0 -.45</pose>
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://x500-NoPlugin/meshes/5010Base.dae</uri>
+          </mesh>
+        </geometry>
+      </visual>
+      <visual name="5010_motor_base_1">
+        <pose>-0.174 0.174 .032 0 0 -.45</pose>
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://x500-NoPlugin/meshes/5010Base.dae</uri>
+          </mesh>
+        </geometry>
+      </visual>
+      <visual name="5010_motor_base_2">
+        <pose>0.174 -0.174 .032 0 0 -.45</pose>
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://x500-NoPlugin/meshes/5010Base.dae</uri>
+          </mesh>
+        </geometry>
+      </visual>
+      <visual name="5010_motor_base_3">
+        <pose>-0.174 -0.174 .032 0 0 -.45</pose>
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://x500-NoPlugin/meshes/5010Base.dae</uri>
+          </mesh>
+        </geometry>
+      </visual>
+      <visual name="NXP_FMUK66_FRONT">
+        <pose>0.047 .001 .043 1 0 1.57</pose>
+        <cast_shadows>false</cast_shadows>
+        <geometry>
+          <plane>
+            <normal>0 0 1</normal>
+            <size>.013 .007</size>
+          </plane>
+        </geometry>
+        <material>
+          <diffuse>1.0 1.0 1.0</diffuse>
+          <specular>1.0 1.0 1.0</specular>
+          <pbr>
+            <metal>
+              <albedo_map>model://x500-NoPlugin/materials/textures/nxp.png</albedo_map>
+            </metal>
+          </pbr>
+        </material>
+      </visual>
+      <visual name="NXP_FMUK66_TOP">
+        <pose>-0.023 0 .0515 0 0 -1.57</pose>
+        <cast_shadows>false</cast_shadows>
+        <geometry>
+          <plane>
+            <normal>0 0 1</normal>
+            <size>.013 .007</size>
+          </plane>
+        </geometry>
+        <material>
+          <diffuse>1.0 1.0 1.0</diffuse>
+          <specular>1.0 1.0 1.0</specular>
+          <pbr>
+            <metal>
+              <albedo_map>model://x500-NoPlugin/materials/textures/nxp.png</albedo_map>
+            </metal>
+          </pbr>
+        </material>
+      </visual>
+      <visual name="RDDRONE_FMUK66_TOP">
+        <pose>-.03 0 .0515 0 0 -1.57</pose>
+        <cast_shadows>false</cast_shadows>
+        <geometry>
+          <plane>
+            <normal>0 0 1</normal>
+            <size>.032 .0034</size>
+          </plane>
+        </geometry>
+        <material>
+          <diffuse>1.0 1.0 1.0</diffuse>
+          <specular>1.0 1.0 1.0</specular>
+          <pbr>
+            <metal>
+              <albedo_map>model://x500-NoPlugin/materials/textures/rd.png</albedo_map>
+            </metal>
+          </pbr>
+        </material>
+      </visual>
+      <collision name="base_link_collision_0">
+        <pose>0 0 .007 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.35355339059327373 0.35355339059327373 0.05</size>
+          </box>
+        </geometry>
+        <surface>
+          <contact>
+            <ode>
+              <min_depth>0.001</min_depth>
+              <max_vel>0</max_vel>
+            </ode>
+          </contact>
+          <friction>
+            <ode/>
+          </friction>
+        </surface>
+      </collision>
+      <collision name="base_link_collision_1">
+        <pose>0 -0.098 -.123 -0.35 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.015 0.015 0.21</size>
+          </box>
+        </geometry>
+        <surface>
+          <contact>
+            <ode>
+              <min_depth>0.001</min_depth>
+              <max_vel>0</max_vel>
+            </ode>
+          </contact>
+          <friction>
+            <ode/>
+          </friction>
+        </surface>
+      </collision>
+      <collision name="base_link_collision_2">
+        <pose>0 0.098 -.123 0.35 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.015 0.015 0.21</size>
+          </box>
+        </geometry>
+        <surface>
+          <contact>
+            <ode>
+              <min_depth>0.001</min_depth>
+              <max_vel>0</max_vel>
+            </ode>
+          </contact>
+          <friction>
+            <ode/>
+          </friction>
+        </surface>
+      </collision>
+      <collision name="base_link_collision_3">
+        <pose>0 -0.132 -.2195 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.25 0.015 0.015</size>
+          </box>
+        </geometry>
+        <surface>
+          <contact>
+            <ode>
+              <min_depth>0.001</min_depth>
+              <max_vel>0</max_vel>
+            </ode>
+          </contact>
+          <friction>
+            <ode/>
+          </friction>
+        </surface>
+      </collision>
+      <collision name="base_link_collision_4">
+        <pose>0 0.132 -.2195 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.25 0.015 0.015</size>
+          </box>
+        </geometry>
+        <surface>
+          <contact>
+            <ode>
+              <min_depth>0.001</min_depth>
+              <max_vel>0</max_vel>
+            </ode>
+          </contact>
+          <friction>
+            <ode/>
+          </friction>
+        </surface>
+      </collision>
+      <sensor name="imu_sensor" type="imu">
+        <always_on>1</always_on>
+        <update_rate>250</update_rate>
+      </sensor>
+    </link>
+    <link name="rotor_0">
+      <gravity>true</gravity>
+      <self_collide>false</self_collide>
+      <velocity_decay/>
+      <pose>0.174 -0.174 0.06 0 0 0</pose>
+      <inertial>
+          <mass>0.016076923076923075</mass>
+          <inertia>
+            <ixx>3.8464910483993325e-07</ixx>
+            <iyy>2.6115851691700804e-05</iyy>
+            <izz>2.649858234714004e-05</izz>
+          </inertia>
+        </inertial>
+      <visual name="rotor_0_visual">
+        <pose>-0.022 -0.14638461538461536 -0.016 0 0 0</pose>
+        <geometry>
+          <mesh>
+            <scale>0.8461538461538461 0.8461538461538461 0.8461538461538461</scale>
+            <uri>model://x500-NoPlugin/meshes/1345_prop_ccw.stl</uri>
+          </mesh>
+        </geometry>
+        <material>
+          <script>
+            <name>Gazebo/DarkGrey</name>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+          </script>
+        </material>
+      </visual>
+      <visual name="rotor_0_visual_motor_bell">
+        <pose>0 0 -.032 0 0 0</pose>
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://x500-NoPlugin/meshes/5010Bell.dae</uri>
+          </mesh>
+        </geometry>
+      </visual>
+      <collision name="rotor_0_collision">
+        <pose>0 0 0 0 0 0 </pose>
+        <geometry>
+          <box>
+            <size>0.2792307692307692 0.016923076923076923 0.0008461538461538462</size>
+          </box>
+        </geometry>
+        <surface>
+          <contact>
+            <ode>
+              <min_depth>0.001</min_depth>
+              <max_vel>0</max_vel>
+            </ode>
+          </contact>
+          <friction>
+            <ode/>
+          </friction>
+        </surface>
+      </collision>
+    </link>
+    <joint name="rotor_0_joint" type="revolute">
+      <parent>base_link</parent>
+      <child>rotor_0</child>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <limit>
+          <lower>-1e+16</lower>
+          <upper>1e+16</upper>
+        </limit>
+        <dynamics>
+          <spring_reference>0</spring_reference>
+          <spring_stiffness>0</spring_stiffness>
+        </dynamics>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+    </joint>
+    <link name="rotor_1">
+      <gravity>true</gravity>
+      <self_collide>false</self_collide>
+      <velocity_decay/>
+      <pose>-0.174 0.174 0.06 0 0 0</pose>
+      <inertial>
+          <mass>0.016076923076923075</mass>
+          <inertia>
+            <ixx>3.8464910483993325e-07</ixx>
+            <iyy>2.6115851691700804e-05</iyy>
+            <izz>2.649858234714004e-05</izz>
+          </inertia>
+        </inertial>
+      <visual name="rotor_1_visual">
+        <pose>-0.022 -0.14638461538461536 -0.016 0 0 0</pose>
+        <geometry>
+          <mesh>
+            <scale>0.8461538461538461 0.8461538461538461 0.8461538461538461</scale>
+            <uri>model://x500-NoPlugin/meshes/1345_prop_ccw.stl</uri>
+          </mesh>
+        </geometry>
+        <material>
+          <script>
+            <name>Gazebo/DarkGrey</name>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+          </script>
+        </material>
+      </visual>
+      <visual name="rotor_1_visual_motor_top">
+        <pose>0 0 -.032 0 0 0</pose>
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://x500-NoPlugin/meshes/5010Bell.dae</uri>
+          </mesh>
+        </geometry>
+      </visual>
+      <collision name="rotor_1_collision">
+        <pose>0 0 0 0 0 0 </pose>
+        <geometry>
+          <box>
+            <size>0.2792307692307692 0.016923076923076923 0.0008461538461538462</size>
+          </box>
+        </geometry>
+        <surface>
+          <contact>
+            <ode>
+              <min_depth>0.001</min_depth>
+              <max_vel>0</max_vel>
+            </ode>
+          </contact>
+          <friction>
+            <ode/>
+          </friction>
+        </surface>
+      </collision>
+    </link>
+    <joint name="rotor_1_joint" type="revolute">
+      <parent>base_link</parent>
+      <child>rotor_1</child>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <limit>
+          <lower>-1e+16</lower>
+          <upper>1e+16</upper>
+        </limit>
+        <dynamics>
+          <spring_reference>0</spring_reference>
+          <spring_stiffness>0</spring_stiffness>
+        </dynamics>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+    </joint>
+    <link name="rotor_2">
+      <gravity>true</gravity>
+      <self_collide>false</self_collide>
+      <velocity_decay/>
+      <pose>0.174 0.174 0.06 0 0 0</pose>
+      <inertial>
+          <mass>0.016076923076923075</mass>
+          <inertia>
+            <ixx>3.8464910483993325e-07</ixx>
+            <iyy>2.6115851691700804e-05</iyy>
+            <izz>2.649858234714004e-05</izz>
+          </inertia>
+        </inertial>
+      <visual name="rotor_2_visual">
+        <pose>-0.022 -0.14638461538461536 -0.016 0 0 0</pose>
+        <geometry>
+          <mesh>
+            <scale>0.8461538461538461 0.8461538461538461 0.8461538461538461</scale>
+            <uri>model://x500-NoPlugin/meshes/1345_prop_cw.stl</uri>
+          </mesh>
+        </geometry>
+        <material>
+          <script>
+            <name>Gazebo/DarkGrey</name>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+          </script>
+        </material>
+      </visual>
+      <visual name="rotor_2_visual_motor_top">
+        <pose>0 0 -.032 0 0 0</pose>
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://x500-NoPlugin/meshes/5010Bell.dae</uri>
+          </mesh>
+        </geometry>
+      </visual>
+      <collision name="rotor_2_collision">
+        <pose>0 0 0 0 0 0 </pose>
+        <geometry>
+          <box>
+            <size>0.2792307692307692 0.016923076923076923 0.0008461538461538462</size>
+          </box>
+        </geometry>
+        <surface>
+          <contact>
+            <ode>
+              <min_depth>0.001</min_depth>
+              <max_vel>0</max_vel>
+            </ode>
+          </contact>
+          <friction>
+            <ode/>
+          </friction>
+        </surface>
+      </collision>
+    </link>
+    <joint name="rotor_2_joint" type="revolute">
+      <parent>base_link</parent>
+      <child>rotor_2</child>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <limit>
+          <lower>-1e+16</lower>
+          <upper>1e+16</upper>
+        </limit>
+        <dynamics>
+          <spring_reference>0</spring_reference>
+          <spring_stiffness>0</spring_stiffness>
+        </dynamics>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+    </joint>
+    <link name="rotor_3">
+      <gravity>true</gravity>
+      <self_collide>false</self_collide>
+      <velocity_decay/>
+      <pose>-0.174 -0.174 0.06 0 0 0</pose>
+      <inertial>
+          <mass>0.016076923076923075</mass>
+          <inertia>
+            <ixx>3.8464910483993325e-07</ixx>
+            <iyy>2.6115851691700804e-05</iyy>
+            <izz>2.649858234714004e-05</izz>
+          </inertia>
+        </inertial>
+      <visual name="rotor_3_visual">
+        <pose>-0.022 -0.14638461538461536 -0.016 0 0 0</pose>
+        <geometry>
+          <mesh>
+            <scale>0.8461538461538461 0.8461538461538461 0.8461538461538461</scale>
+            <uri>model://x500-NoPlugin/meshes/1345_prop_cw.stl</uri>
+          </mesh>
+        </geometry>
+        <material>
+          <script>
+            <name>Gazebo/DarkGrey</name>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+          </script>
+        </material>
+      </visual>
+      <visual name="rotor_3_visual_motor_top">
+        <pose>0 0 -.032 0 0 0</pose>
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://x500-NoPlugin/meshes/5010Bell.dae</uri>
+          </mesh>
+        </geometry>
+      </visual>
+      <collision name="rotor_3_collision">
+        <pose>0 0 0 0 0 0 </pose>
+        <geometry>
+          <box>
+            <size>0.2792307692307692 0.016923076923076923 0.0008461538461538462</size>
+          </box>
+        </geometry>
+        <surface>
+          <contact>
+            <ode>
+              <min_depth>0.001</min_depth>
+              <max_vel>0</max_vel>
+            </ode>
+          </contact>
+          <friction>
+            <ode/>
+          </friction>
+        </surface>
+      </collision>
+    </link>
+    <joint name="rotor_3_joint" type="revolute">
+      <parent>base_link</parent>
+      <child>rotor_3</child>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <limit>
+          <lower>-1e+16</lower>
+          <upper>1e+16</upper>
+        </limit>
+        <dynamics>
+          <spring_reference>0</spring_reference>
+          <spring_stiffness>0</spring_stiffness>
+        </dynamics>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+    </joint>
+  </model>
+</sdf>
@@ -2,7 +2,71 @@
 <sdf version='1.9'>
  <model name='x500'>
    <include merge='true'>
-      <uri>https://fuel.gazebosim.org/1.0/RudisLaboratories/models/x500-Base</uri>
+      <uri>model://x500-NoPlugin</uri>
    </include>
+    <plugin filename="ignition-gazebo-multicopter-motor-model-system" name="ignition::gazebo::systems::MulticopterMotorModel">
+      <jointName>rotor_0_joint</jointName>
+      <linkName>rotor_0</linkName>
+      <turningDirection>ccw</turningDirection>
+      <timeConstantUp>0.0125</timeConstantUp>
+      <timeConstantDown>0.025</timeConstantDown>
+      <maxRotVelocity>1000.0</maxRotVelocity>
+      <motorConstant>8.54858e-06</motorConstant>
+      <momentConstant>0.016</momentConstant>
+      <commandSubTopic>command/motor_speed</commandSubTopic>
+      <motorNumber>0</motorNumber>
+      <rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
+      <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
+      <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
+      <motorType>velocity</motorType>
+    </plugin>
+    <plugin filename="ignition-gazebo-multicopter-motor-model-system" name="ignition::gazebo::systems::MulticopterMotorModel">
+      <jointName>rotor_1_joint</jointName>
+      <linkName>rotor_1</linkName>
+      <turningDirection>ccw</turningDirection>
+      <timeConstantUp>0.0125</timeConstantUp>
+      <timeConstantDown>0.025</timeConstantDown>
+      <maxRotVelocity>1000.0</maxRotVelocity>
+      <motorConstant>8.54858e-06</motorConstant>
+      <momentConstant>0.016</momentConstant>
+      <commandSubTopic>command/motor_speed</commandSubTopic>
+      <motorNumber>1</motorNumber>
+      <rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
+      <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
+      <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
+      <motorType>velocity</motorType>
+    </plugin>
+    <plugin filename="ignition-gazebo-multicopter-motor-model-system" name="ignition::gazebo::systems::MulticopterMotorModel">
+      <jointName>rotor_2_joint</jointName>
+      <linkName>rotor_2</linkName>
+      <turningDirection>cw</turningDirection>
+      <timeConstantUp>0.0125</timeConstantUp>
+      <timeConstantDown>0.025</timeConstantDown>
+      <maxRotVelocity>1000.0</maxRotVelocity>
+      <motorConstant>8.54858e-06</motorConstant>
+      <momentConstant>0.016</momentConstant>
+      <commandSubTopic>command/motor_speed</commandSubTopic>
+      <motorNumber>2</motorNumber>
+      <rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
+      <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
+      <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
+      <motorType>velocity</motorType>
+    </plugin>
+    <plugin filename="ignition-gazebo-multicopter-motor-model-system" name="ignition::gazebo::systems::MulticopterMotorModel">
+      <jointName>rotor_3_joint</jointName>
+      <linkName>rotor_3</linkName>
+      <turningDirection>cw</turningDirection>
+      <timeConstantUp>0.0125</timeConstantUp>
+      <timeConstantDown>0.025</timeConstantDown>
+      <maxRotVelocity>1000.0</maxRotVelocity>
+      <motorConstant>8.54858e-06</motorConstant>
+      <momentConstant>0.016</momentConstant>
+      <commandSubTopic>command/motor_speed</commandSubTopic>
+      <motorNumber>3</motorNumber>
+      <rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
+      <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
+      <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
+      <motorType>velocity</motorType>
+    </plugin>
  </model>
 </sdf>
@@ -127,7 +127,6 @@ CONFIG_MTD_RAMTRON=y
 CONFIG_NETDEV_PHY_IOCTL=y
 CONFIG_NETINIT_THREAD=y
 CONFIG_NETINIT_THREAD_PRIORITY=49
-CONFIG_NETUTILS_TELNETD=y
 CONFIG_NET_ARP_IPIN=y
 CONFIG_NET_ARP_SEND=y
 CONFIG_NET_BROADCAST=y
@@ -157,7 +156,6 @@ CONFIG_NSH_DISABLE_UNAME=y
 CONFIG_NSH_DISABLE_WGET=y
 CONFIG_NSH_DISABLE_XD=y
 CONFIG_NSH_FILEIOSIZE=512
-CONFIG_NSH_IOBUFFER_SIZE=256
 CONFIG_NSH_LINELEN=128
 CONFIG_NSH_MAXARGUMENTS=15
 CONFIG_NSH_NESTDEPTH=8
@@ -165,8 +163,6 @@ CONFIG_NSH_READLINE=y
 CONFIG_NSH_ROMFSETC=y
 CONFIG_NSH_ROMFSSECTSIZE=128
 CONFIG_NSH_STRERROR=y
-CONFIG_NSH_TELNETD_CLIENTSTACKSIZE=1280
-CONFIG_NSH_TELNETD_DAEMONSTACKSIZE=1024
 CONFIG_NSH_VARS=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
@@ -204,10 +200,6 @@ CONFIG_SYSTEM_CUTERM_DEFAULT_BAUD=57600
 CONFIG_SYSTEM_NSH=y
 CONFIG_SYSTEM_PING=y
 CONFIG_TASK_NAME_SIZE=24
-CONFIG_TELNET_IOTHREAD_STACKSIZE=512
-CONFIG_TELNET_MAXLCLIENTS=1
-CONFIG_TELNET_RXBUFFER_SIZE=128
-CONFIG_TELNET_TXBUFFER_SIZE=128
 CONFIG_UART0_IFLOWCONTROL=y
 CONFIG_UART0_OFLOWCONTROL=y
 CONFIG_UART1_RXBUFSIZE=600
@@ -95,7 +95,9 @@ CONFIG_KINETIS_SDHC=y
 CONFIG_KINETIS_SERIALBRK_BSDCOMPAT=y
 CONFIG_KINETIS_SPI0=y
 CONFIG_KINETIS_SPI1=y
+CONFIG_KINETIS_SPI1_DMA=y
 CONFIG_KINETIS_SPI2=y
+CONFIG_KINETIS_SPI_DMA=y
 CONFIG_KINETIS_UART0=y
 CONFIG_KINETIS_UART0_RXDMA=y
 CONFIG_KINETIS_UART1=y
@@ -125,7 +127,6 @@ CONFIG_MTD_RAMTRON=y
 CONFIG_NETDEV_PHY_IOCTL=y
 CONFIG_NETINIT_THREAD=y
 CONFIG_NETINIT_THREAD_PRIORITY=49
-CONFIG_NETUTILS_TELNETD=y
 CONFIG_NET_ARP_IPIN=y
 CONFIG_NET_ARP_SEND=y
 CONFIG_NET_BROADCAST=y
@@ -155,7 +156,6 @@ CONFIG_NSH_DISABLE_UNAME=y
 CONFIG_NSH_DISABLE_WGET=y
 CONFIG_NSH_DISABLE_XD=y
 CONFIG_NSH_FILEIOSIZE=512
-CONFIG_NSH_IOBUFFER_SIZE=256
 CONFIG_NSH_LINELEN=128
 CONFIG_NSH_MAXARGUMENTS=15
 CONFIG_NSH_NESTDEPTH=8
@@ -163,8 +163,6 @@ CONFIG_NSH_READLINE=y
 CONFIG_NSH_ROMFSETC=y
 CONFIG_NSH_ROMFSSECTSIZE=128
 CONFIG_NSH_STRERROR=y
-CONFIG_NSH_TELNETD_CLIENTSTACKSIZE=1280
-CONFIG_NSH_TELNETD_DAEMONSTACKSIZE=1024
 CONFIG_NSH_VARS=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
@@ -202,10 +200,6 @@ CONFIG_SYSTEM_CUTERM_DEFAULT_BAUD=57600
 CONFIG_SYSTEM_NSH=y
 CONFIG_SYSTEM_PING=y
 CONFIG_TASK_NAME_SIZE=24
-CONFIG_TELNET_IOTHREAD_STACKSIZE=512
-CONFIG_TELNET_MAXLCLIENTS=1
-CONFIG_TELNET_RXBUFFER_SIZE=128
-CONFIG_TELNET_TXBUFFER_SIZE=128
 CONFIG_UART1_RXBUFSIZE=600
 CONFIG_UART1_TXBUFSIZE=1100
 CONFIG_UART4_BAUD=57600
@@ -236,6 +236,7 @@
 */

 #define DIRECT_PWM_OUTPUT_CHANNELS  8
+#define BOARD_NUM_IO_TIMERS         8

 // Input Capture not supported on MVP

@@ -76,9 +76,14 @@
 #define rENBL         REG(IMXRT_TMR_ENBL_OFFSET)

 constexpr io_timers_t io_timers[MAX_IO_TIMERS] = {
-	initIOPWM(PWM::FlexPWM2),
-	initIOPWM(PWM::FlexPWM3),
-	initIOPWM(PWM::FlexPWM4),
+	initIOPWM(PWM::FlexPWM2, PWM::Submodule0),
+	initIOPWM(PWM::FlexPWM2, PWM::Submodule1),
+	initIOPWM(PWM::FlexPWM2, PWM::Submodule2),
+	initIOPWM(PWM::FlexPWM2, PWM::Submodule3),
+	initIOPWM(PWM::FlexPWM3, PWM::Submodule2),
+	initIOPWM(PWM::FlexPWM3, PWM::Submodule0),
+	initIOPWM(PWM::FlexPWM4, PWM::Submodule2),
+	initIOPWM(PWM::FlexPWM4, PWM::Submodule0),
 };

 constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = {
@@ -100,7 +100,7 @@ else
 	# V3 build hwtypecmp supports V2|V2M|V30
 	if ! ver hwtypecmp V2M
 	then
-		mpu9250 -s -b 1 -R 14 start
+		mpu9250 -s -b 1 -R 14 -q start
 	# else: On the PixhawkMini the mpu9250 has been disabled due to HW errata
 	fi

@@ -100,7 +100,7 @@ else
 	# V3 build hwtypecmp supports V2|V2M|V30
 	if ! ver hwtypecmp V2M
 	then
-		mpu9250 -s -b 1 -R 14 start
+		mpu9250 -s -b 1 -R 14 -q start
 	# else: On the PixhawkMini the mpu9250 has been disabled due to HW errata
 	fi

@@ -29,6 +29,7 @@ CONFIG_MODULES_LOGGER=y
 CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
+CONFIG_MAVLINK_DIALECT="development"
 CONFIG_MODULES_MC_ATT_CONTROL=y
 CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
@@ -1,27 +0,0 @@
-CONFIG_PLATFORM_POSIX=y
-CONFIG_BOARD_TESTING=y
-CONFIG_BOARD_ETHERNET=y
-CONFIG_DRIVERS_GPS=y
-CONFIG_MODULES_DATAMAN=y
-CONFIG_MODULES_EVENTS=y
-CONFIG_MODULES_GPS_BASE_STATION=y
-CONFIG_MODULES_LOAD_MON=y
-CONFIG_MODULES_LOGGER=y
-CONFIG_MODULES_MAVLINK=y
-CONFIG_MODULES_REPLAY=y
-CONFIG_COMMON_SIMULATION=y
-CONFIG_SYSTEMCMDS_DYN=y
-CONFIG_SYSTEMCMDS_FAILURE=y
-CONFIG_SYSTEMCMDS_PARAM=y
-CONFIG_SYSTEMCMDS_PERF=y
-CONFIG_SYSTEMCMDS_SD_BENCH=y
-CONFIG_SYSTEMCMDS_SHUTDOWN=y
-CONFIG_SYSTEMCMDS_SYSTEM_TIME=y
-CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
-CONFIG_SYSTEMCMDS_UORB=y
-CONFIG_SYSTEMCMDS_VER=y
-CONFIG_SYSTEMCMDS_WORK_QUEUE=y
-CONFIG_EXAMPLES_HELLO=y
-CONFIG_EXAMPLES_PX4_MAVLINK_DEBUG=y
-CONFIG_EXAMPLES_PX4_SIMPLE_APP=y
-CONFIG_EXAMPLES_WORK_ITEM=y
@@ -59,7 +59,6 @@ set(msg_files
 	cellular_status.msg
 	collision_constraints.msg
 	collision_report.msg
-	commander_state.msg
 	control_allocator_status.msg
 	cpuload.msg
 	differential_pressure.msg
@@ -81,6 +80,7 @@ set(msg_files
 	estimator_status_flags.msg
 	event.msg
 	failure_detector_status.msg
+	failsafe_flags.msg
 	follow_target.msg
 	follow_target_estimator.msg
 	follow_target_status.msg
@@ -202,7 +202,6 @@ set(msg_files
 	vehicle_rates_setpoint.msg
 	vehicle_roi.msg
 	vehicle_status.msg
-	vehicle_status_flags.msg
 	vehicle_thrust_setpoint.msg
 	vehicle_torque_setpoint.msg
 	vehicle_trajectory_bezier.msg
@@ -16,4 +16,4 @@ uint8 SOURCE_RC_SWITCH = 1
 uint8 SOURCE_RC_BUTTON = 2
 uint8 SOURCE_RC_MODE_SLOT = 3

-uint8 mode # for ACTION_SWITCH_MODE what mode is requested according to commander_state.MAIN_STATE_
+uint8 mode # for ACTION_SWITCH_MODE what mode is requested according to vehicle_status_s::NAVIGATION_STATE_*
@@ -1,24 +0,0 @@
-# Main state, i.e. what user wants. Controlled by RC or from ground station via telemetry link.
-uint64 timestamp			# time since system start (microseconds)
-
-uint8 MAIN_STATE_MANUAL             = 0
-uint8 MAIN_STATE_ALTCTL             = 1
-uint8 MAIN_STATE_POSCTL             = 2
-uint8 MAIN_STATE_AUTO_MISSION       = 3
-uint8 MAIN_STATE_AUTO_LOITER        = 4
-uint8 MAIN_STATE_AUTO_RTL           = 5
-uint8 MAIN_STATE_ACRO               = 6
-uint8 MAIN_STATE_OFFBOARD           = 7
-uint8 MAIN_STATE_STAB               = 8
-# LEGACY RATTITUDE                  = 9
-uint8 MAIN_STATE_AUTO_TAKEOFF       = 10
-uint8 MAIN_STATE_AUTO_LAND          = 11
-uint8 MAIN_STATE_AUTO_FOLLOW_TARGET = 12
-uint8 MAIN_STATE_AUTO_PRECLAND      = 13
-uint8 MAIN_STATE_ORBIT              = 14
-uint8 MAIN_STATE_AUTO_VTOL_TAKEOFF  = 15
-uint8 MAIN_STATE_MAX                = 16
-
-uint8 main_state
-
-uint16 main_state_changes
@@ -19,6 +19,7 @@ bool innovation_rejected     # true if the observation has been rejected
 bool fused                   # true if the sample was successfully fused

 # TOPICS estimator_aid_source_1d
-# TOPICS estimator_aid_src_baro_hgt estimator_aid_src_rng_hgt estimator_aid_src_airspeed
+# TOPICS estimator_aid_src_baro_hgt estimator_aid_src_ev_hgt estimator_aid_src_gnss_hgt estimator_aid_src_rng_hgt
+# TOPICS estimator_aid_src_airspeed estimator_aid_src_sideslip
 # TOPICS estimator_aid_src_fake_hgt
 # TOPICS estimator_aid_src_mag_heading estimator_aid_src_gnss_yaw estimator_aid_src_ev_yaw
@@ -5,7 +5,7 @@ uint8 estimator_instance

 uint32 device_id

-uint64[2] time_last_fuse
+uint64 time_last_fuse

 float32[2] observation
 float32[2] observation_variance
@@ -14,9 +14,10 @@ float32[2] innovation
 float32[2] innovation_variance
 float32[2] test_ratio

-bool[2] fusion_enabled       # true when measurements are being fused
-bool[2] innovation_rejected  # true if the observation has been rejected
-bool[2] fused                # true if the sample was successfully fused
+bool fusion_enabled          # true when measurements are being fused
+bool innovation_rejected     # true if the observation has been rejected
+bool fused                   # true if the sample was successfully fused

 # TOPICS estimator_aid_source_2d
-# TOPICS estimator_aid_src_fake_pos
+# TOPICS estimator_aid_src_ev_pos estimator_aid_src_fake_pos estimator_aid_src_gnss_pos
+# TOPICS estimator_aid_src_aux_vel estimator_aid_src_optical_flow
@@ -5,7 +5,7 @@ uint8 estimator_instance

 uint32 device_id

-uint64[3] time_last_fuse
+uint64 time_last_fuse

 float32[3] observation
 float32[3] observation_variance
@@ -14,10 +14,11 @@ float32[3] innovation
 float32[3] innovation_variance
 float32[3] test_ratio

-bool[3] fusion_enabled       # true when measurements are being fused
-bool[3] innovation_rejected  # true if the observation has been rejected
-bool[3] fused                # true if the sample was successfully fused
+bool fusion_enabled          # true when measurements are being fused
+bool innovation_rejected     # true if the observation has been rejected
+bool fused                   # true if the sample was successfully fused

 # TOPICS estimator_aid_source_3d
-# TOPICS estimator_aid_src_gnss_pos estimator_aid_src_gnss_vel
-# TOPICS estimator_aid_src_mag estimator_aid_src_aux_vel
+# TOPICS estimator_aid_src_ev_vel
+# TOPICS estimator_aid_src_gnss_vel
+# TOPICS estimator_aid_src_mag
@@ -1,7 +1,7 @@
 uint64 timestamp                # time since system start (microseconds)
 uint64 timestamp_sample         # the timestamp of the raw data (microseconds)

-uint32 baro_device_id		# unique device ID for the sensor that does not change between power cycles
+uint32 device_id		# unique device ID for the sensor that does not change between power cycles
 float32 bias			# estimated barometric altitude bias (m)
 float32 bias_var		# estimated barometric altitude bias variance (m^2)

@@ -0,0 +1,57 @@
+# Input flags for the failsafe state machine set by the arming & health checks.
+#
+# Flags must be named such that false == no failure (e.g. _invalid, _unhealthy, _lost)
+# The flag comments are used as label for the failsafe state machine simulation
+
+uint64 timestamp				# time since system start (microseconds)
+
+# Per-mode requirements
+uint32 mode_req_angular_velocity
+uint32 mode_req_attitude
+uint32 mode_req_local_alt
+uint32 mode_req_local_position
+uint32 mode_req_local_position_relaxed
+uint32 mode_req_global_position
+uint32 mode_req_mission
+uint32 mode_req_offboard_signal
+uint32 mode_req_home_position
+uint32 mode_req_prevent_arming    # if set, cannot arm while in this mode
+uint32 mode_req_other             # other requirements, not covered above (for external modes)
+
+
+# Mode requirements
+bool angular_velocity_invalid         # Angular velocity invalid
+bool attitude_invalid                 # Attitude invalid
+bool local_altitude_invalid           # Local altitude invalid
+bool local_position_invalid           # Local position estimate invalid
+bool local_position_invalid_relaxed   # Local position with reduced accuracy requirements invalid (e.g. flying with optical flow)
+bool local_velocity_invalid           # Local velocity estimate invalid
+bool global_position_invalid          # Global position estimate invalid
+bool gps_position_invalid             # GPS position invalid
+bool auto_mission_missing             # No mission available
+bool offboard_control_signal_lost     # Offboard signal lost
+bool home_position_invalid            # No home position available
+
+# Control links
+bool manual_control_signal_lost       # Manual control (RC) signal lost
+bool gcs_connection_lost              # GCS connection lost
+
+# Battery
+uint8 battery_warning                 # Battery warning level
+bool battery_low_remaining_time       # Low battery based on remaining flight time
+bool battery_unhealthy                # Battery unhealthy
+
+# Other
+bool primary_geofence_breached        # Primary Geofence breached
+bool mission_failure                  # Mission failure
+bool vtol_transition_failure          # VTOL transition failed (quadchute)
+bool wind_limit_exceeded              # Wind limit exceeded
+bool flight_time_limit_exceeded       # Maximum flight time exceeded
+
+# Failure detector
+bool fd_critical_failure              # Critical failure (attitude/altitude limit exceeded, or external ATS)
+bool fd_esc_arming_failure            # ESC failed to arm
+bool fd_imbalanced_prop               # Imbalanced propeller detected
+bool fd_motor_failure                 # Motor failure
+
+
@@ -1,12 +1,14 @@
-uint64 timestamp                # time since system start (microseconds)
-uint8 GF_ACTION_NONE = 0        # no action on geofence violation
-uint8 GF_ACTION_WARN = 1        # critical mavlink message
-uint8 GF_ACTION_LOITER = 2      # switch to AUTO|LOITER
-uint8 GF_ACTION_RTL = 3         # switch to AUTO|RTL
-uint8 GF_ACTION_TERMINATE = 4   # flight termination
-uint8 GF_ACTION_LAND = 5        # switch to AUTO|LAND
+uint64 timestamp                        # time since system start (microseconds)
+uint8 GF_ACTION_NONE = 0                # no action on geofence violation
+uint8 GF_ACTION_WARN = 1                # critical mavlink message
+uint8 GF_ACTION_LOITER = 2              # switch to AUTO|LOITER
+uint8 GF_ACTION_RTL = 3                 # switch to AUTO|RTL
+uint8 GF_ACTION_TERMINATE = 4           # flight termination
+uint8 GF_ACTION_LAND = 5                # switch to AUTO|LAND

-bool geofence_violated          # true if the geofence is violated
-uint8 geofence_action           # action to take when geofence is violated
+uint8 geofence_violation_reason         # one of geofence_violation_reason_t::*

-bool home_required              # true if the geofence requires a valid home position
+bool primary_geofence_breached          # true if the primary geofence is breached
+uint8 primary_geofence_action           # action to take when the primary geofence is breached
+
+bool home_required                      # true if the geofence requires a valid home position
@@ -14,9 +14,6 @@ bool warning			# true if mission is valid, but has potentially problematic items
 bool finished			# true if mission has been completed
 bool failure			# true if the mission cannot continue or be completed for some reason

-bool stay_in_failsafe		# true if the commander should not switch out of the failsafe mode
-bool flight_termination		# true if the navigator demands a flight termination from the commander app
-
 bool item_do_jump_changed	# true if the number of do jumps remaining has changed
 uint16 item_changed_index	# indicate which item has changed
 uint16 item_do_jump_remaining	# set to the number of do jumps remaining for that item
@@ -9,18 +9,12 @@ uint8 SETPOINT_TYPE_TAKEOFF=3	# takeoff setpoint
 uint8 SETPOINT_TYPE_LAND=4	# land setpoint, altitude must be ignored, descend until landing
 uint8 SETPOINT_TYPE_IDLE=5	# do nothing, switch off motors or keep at idle speed (MC)

-uint8 VELOCITY_FRAME_LOCAL_NED = 1 # MAV_FRAME_LOCAL_NED
-uint8 VELOCITY_FRAME_BODY_NED = 8 # MAV_FRAME_BODY_NED
-
 bool valid			# true if setpoint is valid
 uint8 type			# setpoint type to adjust behavior of position controller

 float32 vx			# local velocity setpoint in m/s in NED
 float32 vy			# local velocity setpoint in m/s in NED
 float32 vz			# local velocity setpoint in m/s in NED
-bool velocity_valid		# true if local velocity setpoint valid
-uint8 velocity_frame		# to set velocity setpoints in NED or body
-bool alt_valid		# do not set for 3D position control. Set to true if you want z-position control while doing vx,vy velocity control.

 float64 lat			# latitude, in deg
 float64 lon			# longitude, in deg
@@ -45,6 +45,7 @@ bool heartbeat_type_gimbal                  # MAV_TYPE_GIMBAL
 bool heartbeat_type_adsb                    # MAV_TYPE_ADSB
 bool heartbeat_type_camera                  # MAV_TYPE_CAMERA
 bool heartbeat_type_parachute               # MAV_TYPE_PARACHUTE
+bool heartbeat_type_open_drone_id           # MAV_TYPE_ODID

 # Heartbeats per component
 bool heartbeat_component_telemetry_radio    # MAV_COMP_ID_TELEMETRY_RADIO
@@ -58,4 +59,5 @@ bool heartbeat_component_uart_bridge        # MAV_COMP_ID_UART_BRIDGE

 # Misc component health
 bool avoidance_system_healthy
+bool open_drone_id_system_healthy
 bool parachute_system_healthy
@@ -33,8 +33,9 @@ uint8 ARM_DISARM_REASON_UNIT_TEST = 13

 uint64 nav_state_timestamp # time when current nav_state activated

-# Navigation state "what should vehicle do"
-uint8 nav_state
+uint8 nav_state_user_intention                  # Mode that the user selected (might be different from nav_state in a failsafe situation)
+
+uint8 nav_state                                 # Currently active mode
 uint8 NAVIGATION_STATE_MANUAL = 0               # Manual mode
 uint8 NAVIGATION_STATE_ALTCTL = 1               # Altitude control mode
 uint8 NAVIGATION_STATE_POSCTL = 2               # Position control mode
@@ -83,12 +84,11 @@ uint8 VEHICLE_TYPE_ROVER = 3
 uint8 VEHICLE_TYPE_AIRSHIP = 4

 bool failsafe # true if system is in failsafe state (e.g.:RTL, Hover, Terminate, ...)
-uint64 failsafe_timestamp # time when failsafe was activated
+bool failsafe_and_user_took_over # true if system is in failsafe state but the user took over control

 # Link loss
-bool rc_signal_lost              # true if RC reception lost
-bool data_link_lost              # datalink to GCS lost
-uint8 data_link_lost_counter     # counts unique data link lost events
+bool gcs_connection_lost              # datalink to GCS lost
+uint8 gcs_connection_lost_counter     # counts unique GCS connection lost events
 bool high_latency_data_link_lost # Set to true if the high latency data link (eg. RockBlock Iridium 9603 telemetry module) is lost

 # VTOL flags
@@ -97,9 +97,6 @@ bool is_vtol_tailsitter  # True if the system performs a 90° pitch down rotatio
 bool in_transition_mode  # True if VTOL is doing a transition
 bool in_transition_to_fw # True if VTOL is doing a transition from MC to FW

-bool mission_failure # Set to true if mission could not continue/finish
-bool geofence_violated
-
 # MAVLink identification
 uint8 system_type  # system type, contains mavlink MAV_TYPE
 uint8 system_id	   # system id, contains MAVLink's system ID field
@@ -107,15 +104,21 @@ uint8 component_id # subsystem / component id, contains MAVLink's component ID f

 bool safety_button_available # Set to true if a safety button is connected
 bool safety_off # Set to true if safety is off
-bool auto_mission_available

 bool power_input_valid                            # set if input power is valid
 bool usb_connected                                # set to true (never cleared) once telemetry received from usb link

+bool open_drone_id_system_present
+bool open_drone_id_system_healthy
+
 bool parachute_system_present
 bool parachute_system_healthy

 bool avoidance_system_required                    # Set to true if avoidance system is enabled via COM_OBS_AVOID parameter
 bool avoidance_system_valid                       # Status of the obstacle avoidance system

+bool rc_calibration_in_progress
+bool calibration_enabled
+
+bool pre_flight_checks_pass		# true if all checks necessary to arm pass

@@ -1,43 +0,0 @@
-# TODO: rename to failsafe_flags (will be input to failsafe state machine)
-#
-
-uint64 timestamp				# time since system start (microseconds)
-
-# Per-mode requirements
-uint32 mode_req_angular_velocity
-uint32 mode_req_attitude
-uint32 mode_req_local_position
-uint32 mode_req_local_position_relaxed
-uint32 mode_req_global_position
-uint32 mode_req_local_alt
-uint32 mode_req_mission
-uint32 mode_req_offboard_signal
-uint32 mode_req_home_position
-uint32 mode_req_prevent_arming    # if set, cannot arm while in this mode
-uint32 mode_req_other             # other requirements, not covered above (for external modes)
-
-
-bool calibration_enabled
-bool pre_flight_checks_pass		# true if all checks necessary to arm pass
-bool auto_mission_available
-bool angular_velocity_valid
-bool attitude_valid
-bool local_altitude_valid
-bool local_position_valid		# set to true by the commander app if the quality of the local position estimate is good enough to use for navigation
-bool local_position_valid_relaxed	# Local position with reduced accuracy requirements (e.g. flying with optical flow)
-bool local_velocity_valid		# set to true by the commander app if the quality of the local horizontal velocity data is good enough to use for navigation
-bool global_position_valid		# set to true by the commander app if the quality of the global position estimate is good enough to use for navigation
-bool gps_position_valid
-bool home_position_valid		# indicates a valid home position (a valid home position is not always a valid launch)
-
-bool offboard_control_signal_lost
-
-bool rc_signal_found_once
-bool rc_calibration_in_progress
-bool vtol_transition_failure                        # Set to true if vtol transition failed
-
-bool battery_low_remaining_time
-bool battery_unhealthy
-
-uint8 battery_warning
-
@@ -39,7 +39,7 @@
 // It does not print arguments.
 #if 0
 #include <px4_platform_common/log.h>
-#define CONSOLE_PRINT_EVENT(log_level, id, str) PX4_INFO_RAW("Event 0x%08x: %s\n", id, str)
+#define CONSOLE_PRINT_EVENT(log_level, id, str) PX4_INFO_RAW("Event 0x%08" PRIx32 ": %s\n", id, str)
 #else
 #define CONSOLE_PRINT_EVENT(log_level, id, str)
 #endif
@@ -45,7 +45,11 @@
 #pragma once
 __BEGIN_DECLS
 /* configuration limits */
-#define MAX_IO_TIMERS			4
+#ifdef BOARD_NUM_IO_TIMERS
+#define MAX_IO_TIMERS     BOARD_NUM_IO_TIMERS
+#else
+#define MAX_IO_TIMERS     4
+#endif
 #define MAX_TIMER_IO_CHANNELS	16

 #define MAX_LED_TIMERS			2
@@ -78,6 +82,7 @@ typedef uint16_t io_timer_channel_allocation_t; /* big enough to hold MAX_TIMER_
 */
 typedef struct io_timers_t {
 	uint32_t  base;                /* Base address of the timer */
+	uint32_t  submodle;            /* Which Submodule */
 	uint32_t  clock_register;      /* SIM_SCGCn */
 	uint32_t  clock_bit;           /* SIM_SCGCn bit pos */
 	uint32_t  vectorno;            /* IRQ number */
@@ -56,7 +56,6 @@ static inline constexpr timer_io_channels_t initIOTimerChannel(const io_timers_t
 		PWM::FlexPWMConfig pwm_config, IOMUX::Pad pad)
 {
 	timer_io_channels_t ret{};
-
 	PWM::FlexPWM pwm{};

 	// FlexPWM Muxing Options
@@ -591,7 +590,7 @@ static inline constexpr timer_io_channels_t initIOTimerChannel(const io_timers_t
 	const uint32_t timer_base = getFlexPWMBaseRegister(pwm);

 	for (int i = 0; i < MAX_IO_TIMERS; ++i) {
-		if (io_timers_conf[i].base == timer_base) {
+		if (io_timers_conf[i].base == timer_base && io_timers_conf[i].submodle == ret.sub_module) {
 			ret.timer_index = i;
 			break;
 		}
@@ -602,11 +601,11 @@ static inline constexpr timer_io_channels_t initIOTimerChannel(const io_timers_t
 	return ret;
 }

-static inline constexpr io_timers_t initIOPWM(PWM::FlexPWM pwm)
+static inline constexpr io_timers_t initIOPWM(PWM::FlexPWM pwm, PWM::FlexPWMSubmodule sub)
 {
 	io_timers_t ret{};

 	ret.base = getFlexPWMBaseRegister(pwm);
-
+	ret.submodle = sub;
 	return ret;
 }
@@ -431,46 +431,69 @@ static inline void io_timer_set_PWM_mode(unsigned channel)

 void io_timer_trigger(unsigned channel_mask)
 {
+	// Any Channels in oneshot?
+
 	int oneshots = io_timer_get_mode_channels(IOTimerChanMode_OneShot) & channel_mask;
-	struct {
-		uint32_t base;
-		uint16_t triggers;
-	} action_cache[MAX_IO_TIMERS] = {0};
-	int actions = 0;

-	/* Pre-calculate the list of channels to Trigger */
-	int mask;
+	// Yes do triggering
+	if (oneshots) {
+		struct {
+			uint32_t base;
+			uint16_t triggers;
+		} action_cache[MAX_IO_TIMERS] = {0};

-	for (int timer = 0; timer < MAX_IO_TIMERS; timer++) {
-		action_cache[actions].base = io_timers[timer].base;
+		int actions = 0;

-		if (action_cache[actions].base) {
-			uint32_t first_channel_index = io_timers_channel_mapping.element[timer].first_channel_index;
-			uint32_t last_channel_index = first_channel_index + io_timers_channel_mapping.element[timer].channel_count;
+		// Get the Timer modules addresses
+		for (int timer = 0; timer < MAX_IO_TIMERS && io_timers[timer].base != 0; timer++) {
+			if (action_cache[actions].base == 0) {
+				action_cache[actions].base = io_timers[timer].base;
+
+			} else if (action_cache[actions].base != io_timers[timer].base) {
+				actions++;
+				action_cache[actions].base = io_timers[timer].base;
+			}
+		}
+
+
+		/* Pre-calculate the list of channels to Trigger for each Timer module */
+
+		int mask;
+		uint32_t channel = 0;
+
+		for (actions = 0; actions < MAX_IO_TIMERS; actions++) {
+			if (action_cache[actions].base == 0) {
+				// All have been processed
+				break;
+			}
+
+			// Group the bits from channels on the same timer
+			for (; channel < MAX_TIMER_IO_CHANNELS && timer_io_channels[channel].val_offset;  channel++) {
+
+				if (io_timers[timer_io_channels[channel].timer_index].base != action_cache[actions].base) {
+					break;
+				}

-			for (uint32_t channel = first_channel_index; channel < last_channel_index; channel++) {
 				mask = get_channel_mask(channel);

 				if (oneshots & mask) {
 					action_cache[actions].triggers |= timer_io_channels[channel].sub_module_bits;
 				}
 			}
-
-			actions++;
 		}
+
+		/* Now do them all with the shortest delay in between */
+
+		irqstate_t flags = px4_enter_critical_section();
+
+		for (actions = 0; action_cache[actions].base != 0 &&  actions < MAX_IO_TIMERS; actions++) {
+			_REG16(action_cache[actions].base, IMXRT_FLEXPWM_MCTRL_OFFSET) |= (action_cache[actions].triggers >> MCTRL_LDOK_SHIFT)
+					<< MCTRL_CLDOK_SHIFT ;
+			_REG16(action_cache[actions].base, IMXRT_FLEXPWM_MCTRL_OFFSET) |= action_cache[actions].triggers;
+		}
+
+		px4_leave_critical_section(flags);
 	}
-
-	/* Now do them all with the shortest delay in between */
-
-	irqstate_t flags = px4_enter_critical_section();
-
-	for (actions = 0; action_cache[actions].base != 0 &&  actions < MAX_IO_TIMERS; actions++) {
-		_REG16(action_cache[actions].base, IMXRT_FLEXPWM_MCTRL_OFFSET) |= (action_cache[actions].triggers >> MCTRL_LDOK_SHIFT)
-				<< MCTRL_CLDOK_SHIFT ;
-		_REG16(action_cache[actions].base, IMXRT_FLEXPWM_MCTRL_OFFSET) |= action_cache[actions].triggers;
-	}
-
-	px4_leave_critical_section(flags);
 }

 int io_timer_init_timer(unsigned timer, io_timer_channel_mode_t mode)
@@ -685,44 +708,71 @@ int io_timer_set_enable(bool state, io_timer_channel_mode_t mode, io_timer_chann

 	}

-	struct {
-		uint32_t sm_ens;
-		uint32_t base;
-		uint32_t io_index;
-		uint32_t gpios[MAX_TIMER_IO_CHANNELS];
-	} action_cache[MAX_IO_TIMERS];
+	if (masks) {
+		struct {
+			uint32_t sm_ens;
+			uint32_t base;
+			uint32_t io_index;
+			uint32_t gpios[MAX_TIMER_IO_CHANNELS];
+		} action_cache[MAX_IO_TIMERS];

-	memset(action_cache, 0, sizeof(action_cache));
+		unsigned int actions = 0;
+		memset(action_cache, 0, sizeof(action_cache));

-	for (int chan_index = 0; masks != 0 && chan_index < MAX_TIMER_IO_CHANNELS; chan_index++) {
-		if (masks & (1 << chan_index)) {
-			masks &= ~(1 << chan_index);
+		for (int timer = 0; timer < MAX_IO_TIMERS && io_timers[timer].base != 0; timer++) {
+			if (action_cache[actions].base == 0) {
+				action_cache[actions].base = io_timers[timer].base;

-			if (io_timer_validate_channel_index(chan_index) == 0) {
-				uint32_t timer_index = channels_timer(chan_index);
-				action_cache[timer_index].base = io_timers[timer_index].base;
-				action_cache[timer_index].sm_ens |= MCTRL_RUN(1 << timer_io_channels[chan_index].sub_module) |
-								    timer_io_channels[chan_index].sub_module_bits;
-				action_cache[timer_index].gpios[action_cache[timer_index].io_index++] = timer_io_channels[chan_index].gpio_out;
+			} else if (action_cache[actions].base != io_timers[timer].base) {
+				actions++;
+				action_cache[actions].base = io_timers[timer].base;
 			}
 		}
-	}

-	irqstate_t flags = px4_enter_critical_section();
+		int chan_index = 0;
+
+		for (actions = 0; actions < MAX_IO_TIMERS; actions++) {
+			if (action_cache[actions].base == 0) {
+				// All have been processed
+				break;
+			}
+
+			for (; masks != 0 && chan_index < MAX_TIMER_IO_CHANNELS && timer_io_channels[chan_index].val_offset; chan_index++) {
+
+				if (masks & (1 << chan_index)) {
+
+					if (io_timers[timer_io_channels[chan_index].timer_index].base != action_cache[actions].base) {
+						break;
+					}
+
+					masks &= ~(1 << chan_index);
+
+					action_cache[actions].sm_ens |= MCTRL_RUN(1 << timer_io_channels[chan_index].sub_module) |
+									timer_io_channels[chan_index].sub_module_bits;
+					action_cache[actions].gpios[action_cache[actions].io_index++] = timer_io_channels[chan_index].gpio_out;
+				}
+			}
+		}
+
+		irqstate_t flags = px4_enter_critical_section();
+
+		for (actions = 0; actions < arraySize(action_cache); actions++) {
+			if (action_cache[actions].base == 0) {
+				break;
+			}

-	for (unsigned actions = 0; actions < arraySize(action_cache); actions++) {
-		if (action_cache[actions].base != 0) {
 			for (unsigned int index = 0; index < action_cache[actions].io_index; index++) {
 				if (action_cache[actions].gpios[index]) {
 					px4_arch_configgpio(action_cache[actions].gpios[index]);
 				}
-
-				_REG16(action_cache[actions].base, IMXRT_FLEXPWM_MCTRL_OFFSET) = action_cache[actions].sm_ens;
 			}
+
+			_REG16(action_cache[actions].base, IMXRT_FLEXPWM_MCTRL_OFFSET) = action_cache[actions].sm_ens;
 		}
+
+		px4_leave_critical_section(flags);
 	}

-	px4_leave_critical_section(flags);
 	return 0;
 }

@@ -47,7 +47,6 @@ endforeach()

 # standalone tests
 set(cmd_tests
-	commander_tests
 	controllib_test
 	lightware_laser_test
 	rc_tests
@@ -35,7 +35,7 @@ set(HEXAGON_SDK_INCLUDES
 	${HEXAGON_SDK_ROOT}/incs
 	${HEXAGON_SDK_ROOT}/incs/stddef
 	${HEXAGON_SDK_ROOT}/rtos/qurt/computev66/include/qurt
-	# ${HEXAGON_SDK_ROOT}/rtos/qurt/computev66/include/posix
+	${HEXAGON_SDK_ROOT}/rtos/qurt/computev66/include/posix
 	${HEXAGON_SDK_ROOT}/tools/HEXAGON_Tools/8.4.05/Tools/target/hexagon/include
 	)

@@ -100,7 +100,6 @@ set(ARCHCPUFLAGS
 add_definitions(
 	-D __QURT
 	-D _PROVIDE_POSIX_TIME_DECLS
-	-D _TIMER_T
 	-D _HAS_C9X
 	-D restrict=__restrict__
 	-D noreturn_function=
@@ -966,8 +966,8 @@ void ModalaiEsc::mix_turtle_mode(uint16_t outputs[MAX_ACTUATORS])
 					     float)_parameters.turtle_motor_percent / 100.f);

 		// Add a little bit to the motorOutputMin so props aren't spinning when sticks are centered
-		float dead_band_rpm = ((float)_parameters.turtle_motor_deadband / 100.0f) * _rpm_fullscale;
-		motor_output = (motor_output < _rpm_turtle_min + dead_band_rpm) ? 0.0f : (motor_output -  dead_band_rpm);
+		motor_output = (motor_output < _rpm_turtle_min + _parameters.turtle_motor_deadband) ? 0.0f :
+			       (motor_output - _parameters.turtle_motor_deadband);

 		// using the output map here for clarity as PX4 motors are 1-4
 		switch (_output_map[i].number) {
@@ -253,6 +253,18 @@ MS5611::collect()
 		return ret;
 	}

+	// According to the sensor docs:
+	// If the conversion is not executed before the ADC read command, or the
+	// ADC read command is repeated, it will give 0 as the output result.
+	//
+	// We have seen 0 during the init phase on I2C, therefore, we add this
+	// protection in.
+	if (raw == 0) {
+		perf_count(_comms_errors);
+		perf_end(_sample_perf);
+		return ret;
+	}
+
 	/* handle a measurement */
 	if (_measure_phase == 0) {

@@ -54,6 +54,7 @@ extern "C" __EXPORT int gy_us42_main(int argc, char *argv[])
 	BusCLIArguments cli{true, false};
 	cli.rotation = (Rotation)distance_sensor_s::ROTATION_DOWNWARD_FACING;
 	cli.default_i2c_frequency = 100000;
+	cli.i2c_address = GY_US42_BASEADDR;

 	while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) {
 		switch (ch) {
@@ -38,7 +38,7 @@

 void MPU9250_I2C::print_usage()
 {
-	PRINT_MODULE_USAGE_NAME("mpu9520_i2c", "driver");
+	PRINT_MODULE_USAGE_NAME("mpu9250_i2c", "driver");
 	PRINT_MODULE_USAGE_SUBCATEGORY("imu");
 	PRINT_MODULE_USAGE_COMMAND("start");
 	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
@@ -38,7 +38,7 @@

 void MPU9250::print_usage()
 {
-	PRINT_MODULE_USAGE_NAME("mpu9520", "driver");
+	PRINT_MODULE_USAGE_NAME("mpu9250", "driver");
 	PRINT_MODULE_USAGE_SUBCATEGORY("imu");
 	PRINT_MODULE_USAGE_COMMAND("start");
 	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true);
@@ -37,9 +37,9 @@ parameters:
                16: (unused) PITCH_ANGLE
                17: (unused) ROLL_ANGLE
                18: (unused) CROSSHAIRS
-                19: (unused) AVG_CELL_VOLTAGE
+                19: AVG_CELL_VOLTAGE
                20: (unused) HORIZON_SIDEBARS
-                21: (unused) POWER
+                21: POWER
            default: 16383

        # OSD Log Level
@@ -71,25 +71,44 @@

 // Currently working elements positions (hardcoded)

+/* center col
+
+Speed Power Alt
+Rssi cell_voltage mah
+craft name
+
+*/
+
 // Left
 const uint16_t osd_gps_lat_pos = 2048;
 const uint16_t osd_gps_lon_pos = 2080;
 const uint16_t osd_gps_sats_pos = 2112;
-const uint16_t osd_rssi_value_pos = 2176;

 // Center
-const uint16_t osd_home_dir_pos = 2093;
-const uint16_t osd_craft_name_pos = 2543;
+// Top
 const uint16_t osd_disarmed_pos = 2125;
+const uint16_t osd_home_dir_pos = 2093;
+const uint16_t osd_home_dist_pos = 2095;
+
+// Bottom row 1
+const uint16_t osd_gps_speed_pos = 2413;
+const uint16_t osd_power_pos = 2415;
+const uint16_t osd_altitude_pos = 2416;
+
+// Bottom Row 2
+const uint16_t osd_rssi_value_pos = 2445;
+const uint16_t osd_avg_cell_voltage_pos = 2446;
+const uint16_t osd_mah_drawn_pos = 2449;
+
+// Bottom Row 3
+const uint16_t osd_craft_name_pos = 2480;

 // Right
 const uint16_t osd_main_batt_voltage_pos = 2073;
 const uint16_t osd_current_draw_pos = 2103;
-const uint16_t osd_mah_drawn_pos = 2138;
-const uint16_t osd_altitude_pos = 2233;
-const uint16_t osd_numerical_vario_pos = 2267;
-const uint16_t osd_gps_speed_pos = 2299;
-const uint16_t osd_home_dist_pos = 2331;
+
+
+const uint16_t osd_numerical_vario_pos = LOCATION_HIDDEN;

 MspOsd::MspOsd(const char *device) :
 	ModuleParams(nullptr),
@@ -147,15 +166,18 @@ void MspOsd::SendConfig()
 	msp_osd_config.osd_numerical_vario_pos = enabled(SymbolIndex::NUMERICAL_VARIO) ? osd_numerical_vario_pos :
 			LOCATION_HIDDEN;

+	msp_osd_config.osd_power_pos = enabled(SymbolIndex::POWER) ? osd_power_pos : LOCATION_HIDDEN;
+	msp_osd_config.osd_avg_cell_voltage_pos = enabled(SymbolIndex::AVG_CELL_VOLTAGE) ? osd_avg_cell_voltage_pos :
+			LOCATION_HIDDEN;
+
 	// possibly available, but not currently used
 	msp_osd_config.osd_flymode_pos = 			LOCATION_HIDDEN;
 	msp_osd_config.osd_esc_tmp_pos = 			LOCATION_HIDDEN;
 	msp_osd_config.osd_pitch_angle_pos = 			LOCATION_HIDDEN;
 	msp_osd_config.osd_roll_angle_pos = 			LOCATION_HIDDEN;
 	msp_osd_config.osd_crosshairs_pos = 			LOCATION_HIDDEN;
-	msp_osd_config.osd_avg_cell_voltage_pos = 		LOCATION_HIDDEN;
+
 	msp_osd_config.osd_horizon_sidebars_pos = 		LOCATION_HIDDEN;
-	msp_osd_config.osd_power_pos = 				LOCATION_HIDDEN;

 	// Not implemented or not available
 	msp_osd_config.osd_artificial_horizon_pos = 		LOCATION_HIDDEN;
@@ -261,7 +261,7 @@ msp_status_BF_t construct_STATUS(const vehicle_status_s &vehicle_status)
 			break;

 		default:
-			status_BF.flight_mode_flags = 0;
+			status_BF.flight_mode_flags |= 0;
 			break;
 		}
 	}
@@ -277,10 +277,9 @@ msp_analog_t construct_ANALOG(const battery_status_s &battery_status, const inpu
 	msp_analog_t analog {0};

 	analog.vbat = battery_status.voltage_v * 10; // bottom right... v * 10
-	analog.rssi = (uint16_t)((input_rc.rssi * 1023.0f) / 100.0f);
+	analog.rssi = (uint16_t)((input_rc.link_quality * 1023.0f) / 100.0f);
 	analog.amperage = battery_status.current_a * 100; // main amperage
 	analog.mAhDrawn = battery_status.discharged_mah; // unused
-
 	return analog;
 }

@@ -290,8 +289,8 @@ msp_battery_state_t construct_BATTERY_STATE(const battery_status_s &battery_stat
 	msp_battery_state_t battery_state = {0};

 	// MSP_BATTERY_STATE
-	battery_state.amperage = battery_status.current_a; // not used?
-	battery_state.batteryVoltage = (uint16_t)(battery_status.voltage_v * 400.0f);  // OK
+	battery_state.amperage = battery_status.current_a * 100.0f; // Used for power element
+	battery_state.batteryVoltage = (uint16_t)((battery_status.voltage_v / battery_status.cell_count) * 400.0f);  // OK
 	battery_state.mAhDrawn = battery_status.discharged_mah ; // OK
 	battery_state.batteryCellCount = battery_status.cell_count;
 	battery_state.batteryCapacity = battery_status.capacity; // not used?
@@ -318,14 +317,24 @@ msp_raw_gps_t construct_RAW_GPS(const sensor_gps_s &vehicle_gps_position,
 		raw_gps.lat = vehicle_gps_position.lat;
 		raw_gps.lon = vehicle_gps_position.lon;
 		raw_gps.alt =  vehicle_gps_position.alt / 10;
-		//raw_gps.groundCourse = vehicle_gps_position_struct
+
+		float course = math::degrees(vehicle_gps_position.cog_rad);
+
+		if (course < 0) {
+			course += 360.0f;
+		}
+
+		raw_gps.groundCourse = course * 100.0f; // centidegrees

 	} else {
 		raw_gps.lat = 0;
 		raw_gps.lon = 0;
 		raw_gps.alt = 0;
+		raw_gps.groundCourse = 0; // centidegrees
 	}

+	raw_gps.groundCourse = 0; // centidegrees
+
 	if (vehicle_gps_position.fix_type == 0
 	    || vehicle_gps_position.fix_type == 1) {
 		raw_gps.fixType = MSP_GPS_NO_FIX;
@@ -367,20 +376,24 @@ msp_comp_gps_t construct_COMP_GPS(const home_position_s &home_position,
 	if (home_position.valid_hpos
 	    && home_position.valid_lpos
 	    && estimator_status.solution_status_flags & (1 << 4)) {
-		float bearing_to_home = get_bearing_to_next_waypoint(vehicle_global_position.lat,
-					vehicle_global_position.lon,
-					home_position.lat, home_position.lon);
+		float bearing_to_home = math::degrees(get_bearing_to_next_waypoint(vehicle_global_position.lat,
+						      vehicle_global_position.lon,
+						      home_position.lat, home_position.lon));
+
+		if (bearing_to_home < 0) {
+			bearing_to_home += 360.0f;
+		}

 		float distance_to_home = get_distance_to_next_waypoint(vehicle_global_position.lat,
 					 vehicle_global_position.lon,
 					 home_position.lat, home_position.lon);

 		comp_gps.distanceToHome = (int16_t)distance_to_home; // meters
-		comp_gps.directionToHome = bearing_to_home; // degrees
+		comp_gps.directionToHome = bearing_to_home;

 	} else {
 		comp_gps.distanceToHome = 0; // meters
-		comp_gps.directionToHome = 0; // degrees
+		comp_gps.directionToHome = 0;
 	}

 	comp_gps.heartbeat = heartbeat;
@@ -396,7 +409,17 @@ msp_attitude_t construct_ATTITUDE(const vehicle_attitude_s &vehicle_attitude)
 	matrix::Eulerf euler_attitude(matrix::Quatf(vehicle_attitude.q));
 	attitude.pitch = math::degrees(euler_attitude.theta()) * 10;
 	attitude.roll = math::degrees(euler_attitude.phi()) * 10;
-	attitude.yaw = math::degrees(euler_attitude.psi()) * 10;
+	//attitude.yaw = math::degrees(euler_attitude.psi()) * 10;
+
+	float yaw_fixed = math::degrees(euler_attitude.psi());
+
+	if (yaw_fixed < 0) {
+		yaw_fixed += 360.0f;
+	}
+
+	attitude.yaw = yaw_fixed;
+
+	//attitude.yaw = 360;

 	return attitude;
 }
@@ -90,6 +90,7 @@ add_custom_command(OUTPUT ${component_general_json} ${component_general_json}.xz
 		--version-file ${PX4_BINARY_DIR}/src/lib/version/build_git_version.h
 	COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/generate_crc.py
 		${component_general_json}
+		${PX4_BINARY_DIR}/events/all_events.json.xz
 		--output ${component_information_header}
 	DEPENDS
 		generate_component_general.py
@@ -74,7 +74,7 @@ for metadata_type_tuple in args.type:
 component_general['metadataTypes'] = metadata_types

 with open(filename, 'w') as outfile:
-    json.dump(component_general, outfile)
+    json.dump(component_general, outfile, sort_keys=True)

 if compress:
    save_compressed(filename)
@@ -2,6 +2,7 @@

 import argparse
 import os
+import hashlib

 parser = argparse.ArgumentParser(description="""Generate the COMPONENT_INFORMATION checksums (CRC32) header file""")
 parser.add_argument('--output', metavar='checksums.h', help='output file')
@@ -30,6 +31,16 @@ def crc_update(buf, crc_table, crc):

 crc_table = create_table()

+def sha256sum(filename):
+    h  = hashlib.sha256()
+    b  = bytearray(128*1024)
+    mv = memoryview(b)
+    with open(filename, 'rb', buffering=0) as f:
+        for n in iter(lambda : f.readinto(mv), 0):
+            h.update(mv[:n])
+    return h.hexdigest()
+
+
 with open(filename, 'w') as outfile:
    outfile.write("#pragma once\n")
    outfile.write("#include <stdint.h>\n")
@@ -38,10 +49,12 @@ with open(filename, 'w') as outfile:
        file_crc = 0
        for line in open(filename, "rb"):
            file_crc = crc_update(line, crc_table, file_crc)
+        file_sha256 = sha256sum(filename)

        basename = os.path.basename(filename)
        identifier = basename.split('.')[0]
        outfile.write("static constexpr uint32_t {:}_crc = {:};\n".format(identifier, file_crc))
+        outfile.write("static constexpr const char *{:}_sha256 = \"{:}\";\n".format(identifier, file_sha256))


    outfile.write("}\n")
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2012-2017 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2022 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
@@ -169,7 +169,7 @@ public:

 	union DeviceId {
 		struct DeviceStructure devid_s;
-		uint32_t devid;
+		uint32_t devid{0};
 	};

 	uint32_t get_device_id() const { return _device_id.devid; }
@@ -268,8 +268,8 @@ protected:
 	Device(uint8_t devtype, const char *name, DeviceBusType bus_type, uint8_t bus, uint8_t address) : _name(name)
 	{
 		set_device_type(devtype);
-		_device_id.devid_s.bus_type = bus_type;
-		_device_id.devid_s.bus = bus;
+		set_device_bus_type(bus_type);
+		set_device_bus(bus);
 		set_device_address(address);
 	}

@@ -276,41 +276,83 @@
                        }
                    }
                },
-                "failsafe_reason_t": {
+                "failsafe_cause_t": {
                    "type": "uint8_t",
-                    "description": "Reason for entering failsafe",
+                    "description": "Cause for entering failsafe",
                    "entries": {
                        "0": {
-                            "name": "no_rc",
-                            "description": "No manual control stick input"
+                            "name": "generic",
+                            "description": ""
                        },
                        "1": {
-                            "name": "no_offboard",
-                            "description": "No offboard control inputs"
+                            "name": "manual_control_loss",
+                            "description": "manual control loss"
                        },
                        "2": {
-                            "name": "no_rc_and_no_offboard",
-                            "description": "No manual control stick and no offboard control inputs"
+                            "name": "gcs_connection_loss",
+                            "description": "GCS connection loss"
                        },
                        "3": {
-                            "name": "no_local_position",
-                            "description": "No local position estimate"
+                            "name": "low_battery_level",
+                            "description": "low battery level"
                        },
                        "4": {
-                            "name": "no_global_position",
-                            "description": "No global position estimate"
+                            "name": "critical_battery_level",
+                            "description": "critical battery level"
                        },
                        "5": {
-                            "name": "no_datalink",
-                            "description": "No datalink"
+                            "name": "emergency_battery_level",
+                            "description": "emergency battery level"
+                        }
+                    }
+                },
+                "failsafe_action_t": {
+                    "type": "uint8_t",
+                    "description": "failsafe action",
+                    "entries": {
+                        "0": {
+                            "name": "none",
+                            "description": ""
+                        },
+                        "1": {
+                            "name": "warn",
+                            "description": "warning"
+                        },
+                        "2": {
+                            "name": "fallback_posctrl",
+                            "description": "fallback to position control"
+                        },
+                        "3": {
+                            "name": "fallback_altctrl",
+                            "description": "fallback to altitude control"
+                        },
+                        "4": {
+                            "name": "fallback_stabilized",
+                            "description": "fallback to stabilized"
+                        },
+                        "5": {
+                            "name": "hold",
+                            "description": "hold"
                        },
                        "6": {
-                            "name": "no_rc_and_no_datalink",
-                            "description": "No RC and no datalink"
+                            "name": "rtl",
+                            "description": "RTL"
                        },
                        "7": {
-                            "name": "no_gps",
-                            "description": "No valid GPS"
+                            "name": "land",
+                            "description": "land"
+                        },
+                        "8": {
+                            "name": "descend",
+                            "description": "descend"
+                        },
+                        "9": {
+                            "name": "disarm",
+                            "description": "disarm"
+                        },
+                        "10": {
+                            "name": "terminate",
+                            "description": "terminate"
                        }
                    }
                },
@@ -373,6 +415,10 @@
                        "13": {
                            "name": "rc_button",
                            "description": "RC (button)"
+                        },
+                        "14": {
+                            "name": "failsafe",
+                            "description": "failsafe"
                        }
                    }
                },
@@ -577,6 +623,24 @@
                            "description": "Reduce throttle!"
                        }
                    }
+                },
+                "geofence_violation_reason_t": {
+                    "type": "uint8_t",
+                    "description": "Reason for geofence violation",
+                    "entries": {
+                        "0": {
+                            "name": "dist_to_home_exceeded",
+                            "description": "maximum distance to home exceeded"
+                        },
+                        "1": {
+                            "name": "max_altitude_exceeded",
+                            "description": "maximum altitude exceeded"
+                        },
+                        "2": {
+                            "name": "fence_violation",
+                            "description": "approaching or outside geofence"
+                        }
+                    }
                }
            },
            "navigation_mode_groups": {
@@ -147,6 +147,7 @@ add_custom_command(OUTPUT px4_parameters.hpp
 		px_generate_params.py
 		templates/px4_parameters.hpp.jinja
 	)
+add_custom_target(parameters_header DEPENDS px4_parameters.hpp)

 set(SRCS)

@@ -114,7 +114,7 @@ class JsonOutput():


        #Json string output.
-        self.output = json.dumps(all_json,indent=2)
+        self.output = json.dumps(all_json, indent=2, sort_keys=True)


    def Save(self, filename):
@@ -4,7 +4,6 @@ Param source code generation script.
 """
 from __future__ import print_function
 import xml.etree.ElementTree as ET
-import codecs
 import argparse
 import sys

@@ -50,9 +50,7 @@ import argparse
 from px4params import srcscanner, srcparser, injectxmlparams, xmlout, markdownout, jsonout

 import lzma #to create .xz file
-import re
 import json
-import codecs

 def save_compressed(filename):
    #create lzma compressed version
@@ -39,9 +39,12 @@
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/vehicle_status_flags.h>

-#include "../../state_machine_helper.h" // TODO: get independent of transition_result_t
+typedef enum {
+	TRANSITION_DENIED = -1,
+	TRANSITION_NOT_CHANGED = 0,
+	TRANSITION_CHANGED
+} transition_result_t;

 using arm_disarm_reason_t = events::px4::enums::arm_disarm_reason_t;

@@ -242,7 +242,6 @@ TEST(ArmStateMachineTest, ArmingStateTransitionTest)
 	};

 	struct vehicle_status_s status {};
-	struct vehicle_status_flags_s status_flags {};
 	struct actuator_armed_s armed {};

 	size_t cArmingTransitionTests = sizeof(rgArmingTransitionTests) / sizeof(rgArmingTransitionTests[0]);
@@ -254,7 +253,7 @@ TEST(ArmStateMachineTest, ArmingStateTransitionTest)
 		arm_state_machine.forceArmState(test->current_state.arming_state);
 		status.hil_state = test->hil_state;

-		HealthAndArmingChecks health_and_arming_checks(nullptr, status_flags, status);
+		HealthAndArmingChecks health_and_arming_checks(nullptr, status);

 		// Attempt transition
 		transition_result_t result = arm_state_machine.arming_state_transition(
@@ -33,6 +33,7 @@

 add_subdirectory(failure_detector)
 add_subdirectory(HealthAndArmingChecks)
+add_subdirectory(failsafe)
 add_subdirectory(Arming)
 add_subdirectory(ModeUtil)

@@ -51,12 +52,12 @@ px4_add_module(
 		factory_calibration_storage.cpp
 		gyro_calibration.cpp
 		HomePosition.cpp
+		UserModeIntention.cpp
 		level_calibration.cpp
 		lm_fit.cpp
 		mag_calibration.cpp
 		rc_calibration.cpp
 		Safety.cpp
-		state_machine_helper.cpp
 		worker_thread.cpp
 	DEPENDS
 		circuit_breaker
@@ -69,10 +70,7 @@ px4_add_module(
 		sensor_calibration
 		world_magnetic_model
 		mode_util
+		failsafe
 	)

-if(PX4_TESTING)
-	add_subdirectory(commander_tests)
-endif()
-
 px4_add_unit_gtest(SRC mag_calibration_test.cpp LINKLIBS modules__commander)
@@ -36,11 +36,12 @@
 /*   Helper classes  */
 #include "Arming/ArmStateMachine/ArmStateMachine.hpp"
 #include "failure_detector/FailureDetector.hpp"
+#include "failsafe/failsafe.h"
 #include "Safety.hpp"
-#include "state_machine_helper.h"
 #include "worker_thread.hpp"
 #include "HealthAndArmingChecks/HealthAndArmingChecks.hpp"
 #include "HomePosition.hpp"
+#include "UserModeIntention.hpp"

 #include <lib/controllib/blocks.hpp>
 #include <lib/hysteresis/hysteresis.h>
@@ -57,7 +58,6 @@
 #include <uORB/topics/vehicle_command_ack.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/vehicle_status_flags.h>

 // subscriptions
 #include <uORB/Subscription.hpp>
@@ -68,7 +68,6 @@
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/cpuload.h>
 #include <uORB/topics/distance_sensor.h>
-#include <uORB/topics/geofence_result.h>
 #include <uORB/topics/iridiumsbd_status.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/mission_result.h>
@@ -84,7 +83,6 @@
 #include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vtol_vehicle_status.h>
-#include <uORB/topics/wind.h>

 using math::constrain;
 using systemlib::Hysteresis;
@@ -117,12 +115,11 @@ public:

 	void enable_hil();

-	void get_circuit_breaker_params();
-
 private:
 	void answer_command(const vehicle_command_s &cmd, uint8_t result);

 	transition_result_t arm(arm_disarm_reason_t calling_reason, bool run_preflight_checks = true);
+
 	transition_result_t disarm(arm_disarm_reason_t calling_reason, bool forced = false);

 	void battery_status_check();
@@ -132,9 +129,11 @@ private:
 	/**
 	 * Checks the status of all available data links and handles switching between different system telemetry states.
 	 */
-	void data_link_check();
+	void dataLinkCheck();

-	void manual_control_check();
+	void manualControlCheck();
+
+	void offboardControlCheck();

 	/**
 	 * @brief Handle incoming vehicle command relavant to Commander
@@ -146,110 +145,43 @@ private:
 	 */
 	bool handle_command(const vehicle_command_s &cmd);

-	unsigned handle_command_actuator_test(const vehicle_command_s &cmd);
+	unsigned handleCommandActuatorTest(const vehicle_command_s &cmd);

 	void executeActionRequest(const action_request_s &action_request);

-	void offboard_control_update();
+	void printRejectMode(uint8_t nav_state);

-	void print_reject_mode(uint8_t main_state);
+	void updateControlMode();

-	void update_control_mode();
-
-	bool shutdown_if_allowed();
+	bool shutdownIfAllowed();

 	void send_parachute_command();

-	void checkWindSpeedThresholds();
 	void checkForMissionUpdate();
+
 	void handlePowerButtonState();
+
 	void systemPowerUpdate();
+
 	void landDetectorUpdate();
+
 	void safetyButtonUpdate();
+
 	void vtolStatusUpdate();
+
 	void updateTunes();
+
 	void checkWorkerThread();
+
 	bool getPrearmState() const;

 	void handleAutoDisarm();

+	bool handleModeIntentionAndFailsafe();
+
 	void updateParameters();

-	void check_and_inform_ready_for_takeoff();
-
-	DEFINE_PARAMETERS(
-
-		(ParamInt<px4::params::NAV_DLL_ACT>) _param_nav_dll_act,
-		(ParamInt<px4::params::COM_DL_LOSS_T>) _param_com_dl_loss_t,
-
-		(ParamInt<px4::params::COM_RC_OVERRIDE>) _param_com_rc_override,
-
-		(ParamInt<px4::params::COM_HLDL_LOSS_T>) _param_com_hldl_loss_t,
-		(ParamInt<px4::params::COM_HLDL_REG_T>) _param_com_hldl_reg_t,
-
-		(ParamFloat<px4::params::COM_RC_LOSS_T>) _param_com_rc_loss_t,
-		(ParamInt<px4::params::NAV_RCL_ACT>) _param_nav_rcl_act,
-		(ParamFloat<px4::params::COM_RCL_ACT_T>) _param_com_rcl_act_t,
-		(ParamInt<px4::params::COM_RCL_EXCEPT>) _param_com_rcl_except,
-
-		(ParamBool<px4::params::COM_HOME_EN>) _param_com_home_en,
-		(ParamBool<px4::params::COM_HOME_IN_AIR>) _param_com_home_in_air,
-
-		(ParamInt<px4::params::COM_POSCTL_NAVL>) _param_com_posctl_navl,	/* failsafe response to loss of navigation accuracy */
-
-		(ParamInt<px4::params::COM_LOW_BAT_ACT>) _param_com_low_bat_act,
-		(ParamFloat<px4::params::COM_BAT_ACT_T>) _param_com_bat_act_t,
-		(ParamInt<px4::params::COM_IMB_PROP_ACT>) _param_com_imb_prop_act,
-		(ParamFloat<px4::params::COM_DISARM_LAND>) _param_com_disarm_land,
-		(ParamFloat<px4::params::COM_DISARM_PRFLT>) _param_com_disarm_preflight,
-
-		(ParamBool<px4::params::COM_OBS_AVOID>) _param_com_obs_avoid,
-
-		(ParamInt<px4::params::COM_FLT_PROFILE>) _param_com_flt_profile,
-
-		(ParamFloat<px4::params::COM_OBC_LOSS_T>) _param_com_obc_loss_t,
-
-		(ParamFloat<px4::params::COM_WIND_WARN>) _param_com_wind_warn,
-
-		// Quadchute
-		(ParamInt<px4::params::COM_QC_ACT>) _param_com_qc_act,
-
-		// Offboard
-		(ParamFloat<px4::params::COM_OF_LOSS_T>) _param_com_of_loss_t,
-		(ParamInt<px4::params::COM_OBL_ACT>) _param_com_obl_act,
-		(ParamInt<px4::params::COM_OBL_RC_ACT>) _param_com_obl_rc_act,
-
-		(ParamInt<px4::params::COM_PREARM_MODE>) _param_com_prearm_mode,
-		(ParamBool<px4::params::COM_FORCE_SAFETY>) _param_com_force_safety,
-		(ParamBool<px4::params::COM_MOT_TEST_EN>) _param_com_mot_test_en,
-
-		(ParamFloat<px4::params::COM_KILL_DISARM>) _param_com_kill_disarm,
-		(ParamFloat<px4::params::COM_LKDOWN_TKO>) _param_com_lkdown_tko,
-
-		// Engine failure
-		(ParamInt<px4::params::COM_ACT_FAIL_ACT>) _param_com_actuator_failure_act,
-
-		(ParamInt<px4::params::COM_FLIGHT_UUID>) _param_flight_uuid,
-		(ParamInt<px4::params::COM_TAKEOFF_ACT>) _param_takeoff_finished_action,
-
-		// Circuit breakers
-		(ParamInt<px4::params::CBRK_SUPPLY_CHK>) _param_cbrk_supply_chk,
-		(ParamInt<px4::params::CBRK_USB_CHK>) _param_cbrk_usb_chk,
-		(ParamInt<px4::params::CBRK_AIRSPD_CHK>) _param_cbrk_airspd_chk,
-		(ParamInt<px4::params::CBRK_FLIGHTTERM>) _param_cbrk_flightterm,
-		(ParamInt<px4::params::CBRK_VTOLARMING>) _param_cbrk_vtolarming,
-
-		(ParamInt<px4::params::COM_FLT_TIME_MAX>) _param_com_flt_time_max,
-		(ParamFloat<px4::params::COM_WIND_MAX>) _param_com_wind_max,
-
-		(ParamFloat<px4::params::COM_SPOOLUP_TIME>) _param_com_spoolup_time
-	)
-
-	// optional parameters
-	param_t _param_mav_comp_id{PARAM_INVALID};
-	param_t _param_mav_sys_id{PARAM_INVALID};
-	param_t _param_mav_type{PARAM_INVALID};
-	param_t _param_rc_map_fltmode{PARAM_INVALID};
+	void checkAndInformReadyForTakeoff();

 	enum class PrearmedMode {
 		DISABLED = 0,
@@ -262,110 +194,91 @@ private:
 		OFFBOARD_MODE_BIT = (1 << 1),
 	};

-	enum class ActuatorFailureActions {
-		DISABLED = 0,
-		AUTO_LOITER = 1,
-		AUTO_LAND = 2,
-		AUTO_RTL = 3,
-		TERMINATE = 4,
-	};
-
 	/* Decouple update interval and hysteresis counters, all depends on intervals */
 	static constexpr uint64_t COMMANDER_MONITORING_INTERVAL{10_ms};
 	static constexpr uint64_t INAIR_RESTART_HOLDOFF_INTERVAL{500_ms};

-	ArmStateMachine _arm_state_machine{};
+	vehicle_status_s        _vehicle_status{};

-	bool		_geofence_loiter_on{false};
-	bool		_geofence_rtl_on{false};
-	bool		_geofence_land_on{false};
-	bool		_geofence_warning_action_on{false};
-	bool		_geofence_violated_prev{false};
+	ArmStateMachine		_arm_state_machine{};
+	Failsafe		_failsafe_instance{this};
+	FailsafeBase		&_failsafe{_failsafe_instance};
+	FailureDetector		_failure_detector{this};
+	HealthAndArmingChecks	_health_and_arming_checks{this, _vehicle_status};
+	Safety			_safety{};
+	UserModeIntention	_user_mode_intention{this, _vehicle_status, _health_and_arming_checks};
+	WorkerThread 		_worker_thread{};

-	bool         _circuit_breaker_flight_termination_disabled{false};
-
-	bool		_rtl_time_actions_done{false};
-
-	FailureDetector	_failure_detector;
-	bool		_flight_termination_triggered{false};
-	bool		_lockdown_triggered{false};
-	bool            _imbalanced_propeller_check_triggered{false};
+	const failsafe_flags_s &_failsafe_flags{_health_and_arming_checks.failsafeFlags()};
+	HomePosition 		_home_position{_failsafe_flags};


-	hrt_abstime	_datalink_last_heartbeat_gcs{0};
-	hrt_abstime	_datalink_last_heartbeat_avoidance_system{0};
-	hrt_abstime	_datalink_last_heartbeat_onboard_controller{0};
-	hrt_abstime	_datalink_last_heartbeat_parachute_system{0};
-	bool		_onboard_controller_lost{false};
-	bool		_avoidance_system_lost{false};
-	bool		_parachute_system_lost{true};
+	Hysteresis _auto_disarm_landed{false};
+	Hysteresis _auto_disarm_killed{false};

-	hrt_abstime	_high_latency_datalink_heartbeat{0};
-	hrt_abstime	_high_latency_datalink_lost{0};
+	hrt_abstime _datalink_last_heartbeat_open_drone_id_system{0};
+	hrt_abstime _datalink_last_heartbeat_avoidance_system{0};
+	hrt_abstime _datalink_last_heartbeat_gcs{0};
+	hrt_abstime _datalink_last_heartbeat_onboard_controller{0};
+	hrt_abstime _datalink_last_heartbeat_parachute_system{0};

-	uint8_t		_battery_warning{battery_status_s::BATTERY_WARNING_NONE};
-	hrt_abstime	_battery_failsafe_timestamp{0};
-	Hysteresis	_auto_disarm_landed{false};
-	Hysteresis	_auto_disarm_killed{false};
-	Hysteresis	_offboard_available{false};
+	hrt_abstime _last_print_mode_reject_time{0};	///< To remember when last notification was sent

-	hrt_abstime	_last_print_mode_reject_time{0};	///< To remember when last notification was sent
-	bool            _mode_switch_mapped{false};
+	hrt_abstime _high_latency_datalink_heartbeat{0};
+	hrt_abstime _high_latency_datalink_lost{0};

-	bool		_last_overload{false};
-
-	hrt_abstime	_last_valid_manual_control_setpoint{0};
-
-	bool		_is_throttle_above_center{false};
-	bool		_is_throttle_low{false};
-
-	hrt_abstime	_boot_timestamp{0};
-	hrt_abstime	_last_disarmed_timestamp{0};
-	hrt_abstime	_overload_start{0};		///< time when CPU overload started
+	hrt_abstime _boot_timestamp{0};
+	hrt_abstime _last_disarmed_timestamp{0};
+	hrt_abstime _overload_start{0};		///< time when CPU overload started

 	hrt_abstime _led_armed_state_toggle{0};
 	hrt_abstime _led_overload_toggle{0};

-	hrt_abstime _last_termination_message_sent{0};
+	hrt_abstime _last_health_and_arming_check{0};

-	bool		_status_changed{true};
-	bool		_arm_tune_played{false};
-	bool		_was_armed{false};
-	bool		_failsafe_old{false};	///< check which state machines for changes, clear "changed" flag
-	bool		_have_taken_off_since_arming{false};
+	uint8_t		_battery_warning{battery_status_s::BATTERY_WARNING_NONE};
+
+	bool _failsafe_user_override_request{false}; ///< override request due to stick movements
+
+	bool _flight_termination_triggered{false};
+	bool _lockdown_triggered{false};
+
+	bool _open_drone_id_system_lost{true};
+	bool _avoidance_system_lost{false};
+	bool _onboard_controller_lost{false};
+	bool _parachute_system_lost{true};
+
+	bool _last_overload{false};
+	bool _mode_switch_mapped{false};
+
+	bool _is_throttle_above_center{false};
+	bool _is_throttle_low{false};
+
+	bool _arm_tune_played{false};
+	bool _was_armed{false};
+	bool _have_taken_off_since_arming{false};
+	bool _status_changed{true};

-	geofence_result_s	_geofence_result{};
 	vehicle_land_detected_s	_vehicle_land_detected{};
-	vtol_vehicle_status_s	_vtol_vehicle_status{};
-
-	hrt_abstime _last_wind_warning{0};

 	// commander publications
 	actuator_armed_s        _actuator_armed{};
-	commander_state_s       _commander_state{};
 	vehicle_control_mode_s  _vehicle_control_mode{};
-	vehicle_status_s        _vehicle_status{};
-	vehicle_status_flags_s  _vehicle_status_flags{};
-
-	Safety _safety;
-
-	WorkerThread _worker_thread;
+	vtol_vehicle_status_s	_vtol_vehicle_status{};

 	// Subscriptions
-	uORB::Subscription					_action_request_sub {ORB_ID(action_request)};
+	uORB::Subscription					_action_request_sub{ORB_ID(action_request)};
 	uORB::Subscription					_cpuload_sub{ORB_ID(cpuload)};
-	uORB::Subscription					_geofence_result_sub{ORB_ID(geofence_result)};
 	uORB::Subscription					_iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)};
-	uORB::Subscription					_vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
 	uORB::Subscription					_manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
 	uORB::Subscription					_system_power_sub{ORB_ID(system_power)};
 	uORB::Subscription					_vehicle_command_sub{ORB_ID(vehicle_command)};
+	uORB::Subscription					_vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
 	uORB::Subscription					_vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)};
-	uORB::Subscription					_wind_sub{ORB_ID(wind)};

 	uORB::SubscriptionInterval				_parameter_update_sub{ORB_ID(parameter_update), 1_s};

-	uORB::SubscriptionMultiArray<telemetry_status_s>        _telemetry_status_subs{ORB_ID::telemetry_status};
+	uORB::SubscriptionMultiArray<telemetry_status_s>	_telemetry_status_subs{ORB_ID::telemetry_status};

 #if defined(BOARD_HAS_POWER_CONTROL)
 	uORB::Subscription					_power_button_state_sub {ORB_ID(power_button_state)};
@@ -376,19 +289,41 @@ private:

 	// Publications
 	uORB::Publication<actuator_armed_s>			_actuator_armed_pub{ORB_ID(actuator_armed)};
-	uORB::Publication<commander_state_s>			_commander_state_pub{ORB_ID(commander_state)};
-	uORB::Publication<failure_detector_status_s>		_failure_detector_status_pub{ORB_ID(failure_detector_status)};
 	uORB::Publication<actuator_test_s>			_actuator_test_pub{ORB_ID(actuator_test)};
-	uORB::Publication<vehicle_control_mode_s>		_vehicle_control_mode_pub{ORB_ID(vehicle_control_mode)};
-	uORB::Publication<vehicle_status_flags_s>		_vehicle_status_flags_pub{ORB_ID(vehicle_status_flags)};
-	uORB::Publication<vehicle_status_s>			_vehicle_status_pub{ORB_ID(vehicle_status)};
-
+	uORB::Publication<failure_detector_status_s>		_failure_detector_status_pub{ORB_ID(failure_detector_status)};
 	uORB::Publication<vehicle_command_ack_s>		_vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)};
+	uORB::Publication<vehicle_control_mode_s>		_vehicle_control_mode_pub{ORB_ID(vehicle_control_mode)};
+	uORB::Publication<vehicle_status_s>			_vehicle_status_pub{ORB_ID(vehicle_status)};

 	orb_advert_t _mavlink_log_pub{nullptr};

 	perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
 	perf_counter_t _preflight_check_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": preflight check")};
-	HealthAndArmingChecks _health_and_arming_checks;
-	HomePosition _home_position{_vehicle_status_flags};
+
+	// optional parameters
+	param_t _param_mav_comp_id{PARAM_INVALID};
+	param_t _param_mav_sys_id{PARAM_INVALID};
+	param_t _param_mav_type{PARAM_INVALID};
+	param_t _param_rc_map_fltmode{PARAM_INVALID};
+
+	DEFINE_PARAMETERS(
+
+		(ParamFloat<px4::params::COM_DISARM_LAND>)  _param_com_disarm_land,
+		(ParamFloat<px4::params::COM_DISARM_PRFLT>) _param_com_disarm_preflight,
+		(ParamInt<px4::params::COM_DL_LOSS_T>)      _param_com_dl_loss_t,
+		(ParamInt<px4::params::COM_HLDL_LOSS_T>)    _param_com_hldl_loss_t,
+		(ParamInt<px4::params::COM_HLDL_REG_T>)     _param_com_hldl_reg_t,
+		(ParamBool<px4::params::COM_HOME_EN>)       _param_com_home_en,
+		(ParamBool<px4::params::COM_HOME_IN_AIR>)   _param_com_home_in_air,
+		(ParamInt<px4::params::COM_FLT_PROFILE>)    _param_com_flt_profile,
+		(ParamBool<px4::params::COM_FORCE_SAFETY>)  _param_com_force_safety,
+		(ParamFloat<px4::params::COM_KILL_DISARM>)  _param_com_kill_disarm,
+		(ParamBool<px4::params::COM_MOT_TEST_EN>)   _param_com_mot_test_en,
+		(ParamBool<px4::params::COM_OBS_AVOID>)     _param_com_obs_avoid,
+		(ParamFloat<px4::params::COM_OBC_LOSS_T>)   _param_com_obc_loss_t,
+		(ParamInt<px4::params::COM_PREARM_MODE>)    _param_com_prearm_mode,
+		(ParamInt<px4::params::COM_RC_OVERRIDE>)    _param_com_rc_override,
+		(ParamInt<px4::params::COM_FLIGHT_UUID>)    _param_flight_uuid,
+		(ParamInt<px4::params::COM_TAKEOFF_ACT>)    _param_takeoff_finished_action
+	)
 };
--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
Jaeyoung Lim	0bdd780bdd	Set feedforward heightrate setpoint in offboard mode	2023-02-24 08:17:27 +01:00
Jaeyoung Lim	7e49147bcf	Fix feedforward acceleration setpoints for fixedwing offboard position This commit fixes feedforward acceleration setpoints for fixedwing offboard position control. Previously when acceleration feedforward inputs were sent, negative curvature accelerations were not being computed properly	2022-10-19 09:15:13 +02:00
Daniel Agar	b71fc63162	ekf2: fix sideslip timeout (typo)	2022-10-18 18:38:25 -04:00
Daniel Agar	0d2ff6e224	ekf2: test ekf_logger further reduce std::precision to minimize false positives	2022-10-18 14:19:16 -04:00
Daniel Agar	535415a537	ekf2: add OF estimator aid src status	2022-10-18 14:19:16 -04:00
Matthias Grob	75c63aee2a	Fix confusion between trajectory_setpoint and vehicle_local_postion_setpoint	2022-10-17 16:18:00 -04:00
Daniel Agar	7c237fca74	boards: px4_sitl use Mavlink development dialect	2022-10-17 16:16:36 -04:00
Daniel Agar	743200df22	mavlink: move mavlink dialect selection to kconfig	2022-10-17 16:16:36 -04:00
Daniel Agar	d792a3ff5b	simulator_mavlink: use existing development/common mavlink generation if available	2022-10-17 16:16:36 -04:00
Hamish Willee	2a9801f191	Default mavlink dialect should be common.xml	2022-10-17 16:16:36 -04:00
bresch	8b9ac2d7f3	ekf2: remove old mag 3D fusion auto-code	2022-10-17 16:14:56 -04:00
bresch	77a36219c6	ekf2_test: compare 3D mag fusion sympy vs symforce	2022-10-17 16:14:56 -04:00
bresch	b92cbe12a0	ekf2: migrate mag 3D fusion to symforce ekf2: merge mag 3d innov var, Hx and Kx computation to reduce flash Slightly less code produced, almost no performance change ekf2_mag3D: do not pre-compute Kalman gains The vector of Kalaman gains is not too expensive to compute using matrix-vector multiplication. Pre-generating it using CSE takes a lot of flash space for little benefit.	2022-10-17 16:14:56 -04:00
Silvan Fuhrer	5ea8c6e507	FlightTaskAuto: remove unused _getTargetVelocityXY() Inclusive velocity_valid field in position_setpoint message that's then no longer needed. Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2022-10-17 16:12:15 -04:00
Silvan Fuhrer	42c613a0c7	position_setpoint.msg: remove unused alt_valid Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2022-10-17 16:12:15 -04:00
Silvan Fuhrer	42dd9b5063	position_setpoint.msg: remove unused velocity_frame field Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2022-10-17 16:12:15 -04:00
Daniel Agar	0d6766d14d	limit vehicle_command subscription updates per cycle - this is a precaution to eliminate the possibility of getting stuck in a loop trying to keep up with a high rate publication that could be coming from a higher priority task	2022-10-17 16:10:51 -04:00
Daniel Agar	677f3e9294	sensors/vehicle_optical_flow: limit gyro updates and generation lost error per cycle - in the worst case scenario printing the error message can take longer than the next gyro publication, so combined with an infinite loop you could get stuck here	2022-10-17 13:45:04 -04:00
Alex Klimaj	8545164869	sensors/vehicle_optical_flow: only get gyro if optical flow delta angle not available (#20416 )	2022-10-17 10:23:36 -04:00
Daniel Agar	116bb6049f	ekf2: fusion helper consistency - yaw fusion use measurementUpdate() - fuseVelPosHeight() respect _accel_bias_inhibit (like measurementUpdate())	2022-10-17 10:17:31 -04:00
Silvan Fuhrer	7bc90c7f00	FW rate controller gains: put default for all IMAX gains to 0.4 The previous default for yaw and roll was 0.2, which is very low, and for wheel was 1, which is very high. A value of 0.4 makes sense to me for all axes. Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2022-10-17 10:27:09 +02:00
Silvan Fuhrer	4bbe8f3c0d	FW rate controller gains: open up some limitis for the rate controller gains Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2022-10-17 10:27:09 +02:00
Silvan Fuhrer	e2d1e79614	FW rate controller gains: reduce max FW_YR_I from 50 to 1 I guess this was a left over from times where the controller had a completely different structure, a value of 50 is well outside of reasonable range (50x the default). Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2022-10-17 10:27:09 +02:00
Silvan Fuhrer	ea44c89366	FW rate controller gains: reduce max FF gains to 1 The previous max of 10 for the FF gains seems a bit very high for me, well outside of reasonable reange. Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2022-10-17 10:27:09 +02:00
bresch	458e5a6b0e	ekf2: update change indicator	2022-10-14 11:42:23 -04:00
bresch	1b4092abbb	symforce: temporarily remove custom cmake command otherwise the generated files are removed with make clean	2022-10-14 11:42:23 -04:00
bresch	a8a3107c05	ekf2: remove old covariance prediction code	2022-10-14 11:42:23 -04:00
bresch	df084d65e3	ekf2_test: compare covariance prediction sympy vs symforce	2022-10-14 11:42:23 -04:00
bresch	a4e511b90e	ekf2: migrate covariance prediction to SymForce	2022-10-14 11:42:23 -04:00
chris1seto	079dfdf209	drivers/osd/msp_osd: cleanup and small fixes/additions (#20399 ) - Fixed clearing arming word if flightmode unknown - Added power and cell voltage elements - New OSD layout - Fixed home direction/distance are now correct Co-authored-by: Chris Seto <chris1seto@gmail.com>	2022-10-14 10:54:14 -04:00
Beat Küng	3c290d812d	commander: use printRejectMode() also for rtl, takeoff, land & mission commands	2022-10-13 16:05:25 -04:00
Beat Küng	d542ffc10c	refactor vehicle_status_flags: rename to failsafe_flags	2022-10-13 16:05:25 -04:00
Beat Küng	f9c8e760b1	CI: add failsafe web build & deployment to the user guide	2022-10-13 16:05:25 -04:00
chris1seto	0ddba3ea90	Fix Discord badge not a link in README (#20406 ) * Add link to image * Update link to nice permalink Co-authored-by: Chris Seto <chris1seto@gmail.com>	2022-10-13 11:05:28 -07:00
Daniel Agar	2de990fd4b	estimator_aid_source split GNSS pos (3d) -> pos (2d) + hgt - per estimator air source status only keep a single set of flags and timestamp that applies to the entire source	2022-10-13 11:28:50 -04:00
David Sidrane	8a9a091ff3	nxp_fmurt1062-v1:Timer configuration for 1 channel per group (timer)	2022-10-13 08:17:51 +02:00
David Sidrane	3be8d52c8e	imxrt:io_timers Support 1 channel per group (timer)	2022-10-13 08:17:51 +02:00
David Sidrane	7f7137320a	fix imxrt: use 1:1 timer group to channel association	2022-10-13 08:17:51 +02:00
chris1seto	7e9ec325f7	Convert README to Discord (#20400 )	2022-10-12 21:41:00 -07:00
bresch	bdd043f27f	ekf2: fix sideslip fusion sign	2022-10-12 16:14:47 +02:00
bresch	53865118fb	ekf2: remove old sideslip fusion code	2022-10-12 09:55:35 -04:00
bresch	f7c749c9cd	ekf2_test: compare sideslip fusion of Sympy and SymForce	2022-10-12 09:55:35 -04:00
bresch	d0f92bfbd5	ekf2_test: create helper functions for auto-generated code diff	2022-10-12 09:55:35 -04:00
bresch	5f54f6fcda	ekf2: migrate sideslip fusion to SymForce - split fusion into update (compute innov and innov_var) and actual fusion to the state vector - use estimator_aid_source_1d struct to group the data	2022-10-12 09:55:35 -04:00
Beat Küng	c09be0f0ac	airframes: add r1 again This was dropped in `f454dcef6b`	2022-10-12 07:35:35 +02:00
Beat Küng	9b7a8d4568	vehicle_status_flags: add group 'Other'	2022-10-11 22:31:20 -04:00
Beat Küng	f7819f5dba	commander: reorder update calls Moves the arming checks before the command handling. This reduces the chance of race conditions. However it does not prevent them! The SDK will need to check when offboard is ready to run/arm to fix this. Specifically this is for sitl offboard tests, where offboard_control_mode is updated and immediately after a mode switch into offboard is commanded.	2022-10-11 22:31:20 -04:00
Beat Küng	38d3739b6d	refactor commander: rename rc_signal_lost -> manual_control_signal_lost, data_link_lost -> gcs_connection_lost	2022-10-11 22:31:20 -04:00
Beat Küng	e2d8ca73a5	commander: add unit tests for failsafe state machine	2022-10-11 22:31:20 -04:00
Beat Küng	3d1da597dd	commander: run arming checks on _offboard_control_mode_sub update	2022-10-11 22:31:20 -04:00
Beat Küng	6ee2252b8c	events: handle events being generated before a mavlink module started - do not print dropped events warnings - make sure current sequence is still sent with reset flag	2022-10-11 22:31:20 -04:00
Beat Küng	3fcdf40a47	events: use PRIx32 to print uint32	2022-10-11 22:31:20 -04:00
Beat Küng	acaa50a448	geofence: add and report reason for violation	2022-10-11 22:31:20 -04:00
mcsauder	ebc88afe46	Apply Google Style to Commander Private methods, rename geofence message geofence_violation to primary_geofence_breached.	2022-10-11 22:31:20 -04:00
Beat Küng	693af897b3	commander: check if battery was already disconnected on arming If so, don't report. Happens e.g. with USB powered pixhawk.	2022-10-11 22:31:20 -04:00
Beat Küng	24142bc014	commander: RcCalibrationChecks: avoid param access on each cycle reduces cpu usage a bit	2022-10-11 22:31:20 -04:00
Beat Küng	c72c580a0b	commander: run arming checks @ 10Hz Required for things like RC loss.	2022-10-11 22:31:20 -04:00
Beat Küng	6fda555cba	commander: move ownership of vehicle_status_flags_s to HealthAndArmingChecks	2022-10-11 22:31:20 -04:00
Beat Küng	cf2eb69d25	px4events/srcparser: ensure message is a string literal	2022-10-11 22:31:20 -04:00
Beat Küng	500a896a56	commander: print reason for mode rejection & report for GCS switches as well	2022-10-11 22:31:20 -04:00
Beat Küng	e4bb219d10	vehicle_status_flags: cleanup message, move non-failsafe flags to vehicle_status	2022-10-11 22:31:20 -04:00
Beat Küng	ae6377dfa0	mission_result: remove unused fields	2022-10-11 22:31:20 -04:00
Beat Küng	455b885f86	commander: use new failsafe state machine and add user intention class	2022-10-11 22:31:20 -04:00
Beat Küng	a04230faa1	commander: add failsafe state machine with webassembly simulation to run the simulation: install sdk: https://emscripten.org/docs/getting_started/downloads.html make run_failsafe_web_server open http://0.0.0.0:8000/ Co-authored-by: Konrad <konrad@auterion.com>	2022-10-11 22:31:20 -04:00
Beat Küng	82911e48be	fix commander: check estimator type & avoid timestamp wrap-around Before it was possible that the publication timestamp was never than 'now', leading to wrap-around.	2022-10-11 22:31:20 -04:00
Beat Küng	27f8298bb9	fix commander: add local position requirement to NAVIGATION_STATE_AUTO_LAND	2022-10-11 22:31:20 -04:00
Beat Küng	57c7b5e843	flight_mode_manager: reduce error verbosity & remove mode switching While disarmed, commander allows to be in any mode now.	2022-10-11 22:31:20 -04:00
Beat Küng	31dfdea12e	commander: remove state_machine_helper tests	2022-10-11 22:31:20 -04:00
Beat Küng	e9387cac1d	mavlink: move get_px4_custom_mode to px4_custom_mode.h	2022-10-11 22:31:20 -04:00
Silvan Fuhrer	9159f020cb	mavlink local_position_ned: always publish on update, not just when xy and v_xy valid Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2022-10-11 00:47:32 -04:00
Daniel Agar	ce609144b0	simulation/gz_bridge: fix implicit floating-point conversions	2022-10-09 14:11:19 -04:00
Daniel Agar	9010029e0d	lib/drivers/device: Device.hpp fully init devid - constructor use available setters	2022-10-09 13:49:39 -04:00
Daniel Agar	f3964513c7	sensors: fix accel/gyro/mag calibration offset units	2022-10-09 13:02:38 -04:00
Daniel Agar	ee4821ed0a	commander: relax COM_ARM_MAG_ANG default to minimize false positives - this check is mainly intended for catching blatant setup problems like a magnetometer that's mounted 90 or 180 degrees off	2022-10-09 12:41:06 -04:00
fkaiser	30150f723a	Tools/px_uploader.py: use monotonic clock if available (#20352 ) Signed-off-by: fkaiser <fabian.kaiser@wingtra.com>	2022-10-06 09:49:05 -04:00
alexklimaj	064f3f86bc	Increase mag calibration max_var_allowed	2022-10-06 08:44:36 -04:00
Hamish Willee	022aa13aa1	mpu9250_i2c - also needs docs update	2022-10-05 20:26:25 -04:00
Hamish Willee	6d612b1ba4	MPU9250 driver doc incorrect	2022-10-05 20:26:25 -04:00
Daniel Agar	8cc39096cb	load_mon: NuttX cpuload use system times for calculation - this is to minimize the impact of any load_mon scheduling jitter in the sampled load percentage	2022-10-05 14:21:38 -04:00
benjinne	29b031c862	Tools/setup/ubuntu.sh update comment to include 22.04	2022-10-05 13:41:47 -04:00
Daniel Agar	5f0a539633	gps_blending: fix blending init to best instance - output GPS publication defaults to best input instance - blended states are explicitly cleared and then populated with weighted blend	2022-10-05 10:26:48 -04:00
Beat Küng	f79dad1e63	fix ROMFS: run camera trigger, capture and PPS before pwm_out As they might need to reserve the pwm pins.	2022-10-05 09:13:35 -04:00
Matthias Grob	46e6e83e14	Commander: allow disarmin not-landed boat like rover This is a hotfix and a boat should never even detect a takeoff in normal operation.	2022-10-05 09:07:18 -04:00
Beat Küng	dafead6f20	logger: add compressed events metadata file if it exists in the ROMFS Allows to decode events not (yet) on main. The file is currently ~15KB.	2022-10-05 07:44:55 +02:00
Beat Küng	02035d94aa	metadata: sort json output ensures the files don't change arbitrarily	2022-10-05 07:44:55 +02:00
modaltb	06a0aedbdb	modalai_esc: directly using deadband param value instead of converting it to RPM	2022-10-04 20:14:59 -04:00
Thomas Debrunner	0af87ec745	mavlink: initial OPEN_DRONE_ID_BASIC_ID/OPEN_DRONE_ID_LOCATION support	2022-10-04 14:40:59 -04:00
Matthias Grob	b179427b4c	Remove opinionated non-default MPC_TKO_SPEED from airframes	2022-10-04 11:36:57 -04:00
pandafeng1999	e4b4df4e5d	drivers/distance_sensor/gy_us42: add the specified i2c address	2022-10-04 11:35:45 -04:00
Matthias Grob	f96507bb22	vtol_takeoff: fix comment typo	2022-10-04 10:01:04 -04:00
Peter van der Perk	c807d6079d	FMUK66-V3 Enable DMA on SPI	2022-10-04 09:46:04 -04:00
Peter van der Perk	0cf2ecedb9	FMUK66 Disable Telnet to save RAM	2022-10-04 09:46:04 -04:00
Julian Oes	5c77bbcb4c	ms5611: ignore reading 0 This prevents publishing a negative pressure which leads to a NAN altitude estimate further down the line.	2022-10-04 09:37:53 -04:00
Beat Küng	04b1cbb423	vtol_att_control: standard: also do blending for FW controls So that the sum of the control is always 1.	2022-10-04 09:54:11 +02:00
Silvan Fuhrer	6db92b4011	ControlAllocation: remove actuator trim value from actuator_sp to calculate allocated control Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2022-10-04 07:51:25 +02:00
Zachary Lowell	4520186878	qurt: replacing qurt threads with pthread	2022-10-03 18:01:04 -04:00
bresch	4b687beb3b	ekf2: remove old airspeed fusion code	2022-10-03 10:59:42 -04:00
bresch	299e6058e3	ekf2_test: test airspeed symforce vs sympy generated code	2022-10-03 10:59:42 -04:00
bresch	29ebef1f74	ekf2: migrate fuse_airspeed to SymForce	2022-10-03 10:59:42 -04:00
Daniel Agar	41cda14126	ekf2: add symforce code generation helper target	2022-10-03 10:59:42 -04:00
Daniel Agar	c9441bb48a	boards: px4_fmu-v2/v3 allow optional mpu9250 probe to fail without error	2022-10-02 21:40:18 -04:00
Benjamin Perseghetti	aa2b47845a	new Gazebo simulation handle older gz versions (#20342 )	2022-10-02 21:30:30 -04:00
PX4 BuildBot	f4d2e176ae	Update submodule sitl_gazebo to latest Sun Oct 2 12:38:54 UTC 2022 - sitl_gazebo in PX4/Firmware (`2bd1ac005f`): https://github.com/PX4/PX4-SITL_gazebo/commit/b968405a617005ba0a793a8b4703913d4c6eff5f - sitl_gazebo current upstream: https://github.com/PX4/PX4-SITL_gazebo/commit/e80432759540c91f85a012f47aa6ebb0ee9de7e4 - Changes: https://github.com/PX4/PX4-SITL_gazebo/compare/b968405a617005ba0a793a8b4703913d4c6eff5f...e80432759540c91f85a012f47aa6ebb0ee9de7e4 e804327 2022-10-01 Thomas Stauber - set lidar measurements to 0.0 if out of range (#914) 7ed1da6 2022-09-27 Karthik S - Advanced Lift-Drag Plugin (#901) 669d7f5 2022-09-08 Oleg Kalachev - Fix local addr assignment for qgc and sdk sockets	2022-10-02 10:54:31 -04:00
PX4 BuildBot	f7a5c91fb3	Update submodule libcanard to latest Sun Oct 2 12:38:56 UTC 2022 - libcanard in PX4/Firmware (8e144eece9e9cd5a23dbeb6723591f214cad631a): https://github.com/opencyphal/libcanard/commit/db87ea32aa092c48ea103963138b6346dd3e9008 - libcanard current upstream: https://github.com/opencyphal/libcanard/commit/2e3b11f6b8325080c160d38521b169b0bbb6b1c7 - Changes: https://github.com/opencyphal/libcanard/compare/db87ea32aa092c48ea103963138b6346dd3e9008...2e3b11f6b8325080c160d38521b169b0bbb6b1c7 2e3b11f 2022-07-15 Pavel Kirienko - Follow-up for #197 -- fix minor issues discovered by Sonar (#198) 106ceaf 2022-07-11 Dmitry Ponomarev - Fix cast-align warnings on ARM 8953fe6 2022-07-06 Dmitry Ponomarev - Add ARM build to github workflow	2022-10-02 10:54:05 -04:00
PX4 BuildBot	644a836d0a	Update submodule libevents to latest Sun Oct 2 12:39:02 UTC 2022 - libevents in PX4/Firmware (ddc1e6d6c5c2707be5522d4c4d7434403202cf45): https://github.com/mavlink/libevents/commit/82dabdb914d7bd640c281900e2852d0afc074b68 - libevents current upstream: https://github.com/mavlink/libevents/commit/a89808bffd1efb0543e66f17a7fa12dfce5e6bf0 - Changes: https://github.com/mavlink/libevents/compare/82dabdb914d7bd640c281900e2852d0afc074b68...a89808bffd1efb0543e66f17a7fa12dfce5e6bf0 a89808b 2022-09-26 Beat Küng - combine.py: sort json keys	2022-10-02 10:53:28 -04:00
PX4 BuildBot	cf0cd4ebf2	Update submodule mavlink to latest Sun Oct 2 12:39:04 UTC 2022 - mavlink in PX4/Firmware (65bd1fd4c000a7fa0de2a0e8dce1ba9b2a62fa86): https://github.com/mavlink/mavlink/commit/c46af523263ff0dad59afe018fe387c1df030442 - mavlink current upstream: https://github.com/mavlink/mavlink/commit/dda5a18ddb002a871ba301bb584893ee6378e2f3 - Changes: https://github.com/mavlink/mavlink/compare/c46af523263ff0dad59afe018fe387c1df030442...dda5a18ddb002a871ba301bb584893ee6378e2f3 dda5a18d 2022-09-28 WickedShell - Support a return to default speed for DO_CHANGE_SPEED (#1890) 4e04dbbf 2022-09-24 Hamish Willee - MAV_FRAME_BODY_FRD - match to implementations and sanity (#1894) 89a61214 2022-09-23 Hamish Willee - DO_CHANGE_SPEED - clarify lifetime of speed setting (#1892) 11414f88 2022-09-21 Søren Friis - Clarify swarm fields (#1891) 5cdf8b60 2022-09-08 Hamish Willee - HOME_POSITION - clarify local frame means NED (#1886) 551e3b49 2022-09-07 Julian Oes - Fix CI (#1888) d99ee9b6 2022-09-07 Julian Oes - pymavlink: update submodule to latest master (#1887) 92d2a58d 2022-09-01 Hamish Willee - common.xml:TIMESYNC docs and extensions (#1878) c424dc65 2022-09-01 Julian Oes - minimal: try to make component ID less confusing (#1876) 153939bd 2022-09-01 Hamish Willee - Fix typo MAV_CMD_DO_SET_MISSION_CURRENT 5043a1e0 2022-09-01 Hamish Willee - MAV_CMD_DO_SET_MISSION_CURRENT - allow mission resetting (#1870) 90aa7bfc 2022-08-31 Chikirev Sirguy - Update common.xml (#1815) 554ccadd 2022-08-31 Hamish Willee - common: OpenDroneID messages no longer WIP (#1882) 06448cf6 2022-08-24 Hamish Willee - MAV_ODID_ARM_STATUS entries update to naming convention (#1883) ff649224 2022-08-18 Patrick José Pereira - Fix gibberish descriptions ualberta.xml (#1881) 11f4b236 2022-08-18 Hamish Willee - Mission item - document values for autocontinue (#1868) fb8ac31b 2022-08-18 Julian Oes - development: remove MISSION_CHANGED (#1879) 76b794c2 2022-08-18 Julian Oes - common: extend MISSION_CURRENT (#1869) 1ae3edc4 2022-08-15 Andrew Tridgell - common: added OPEN_DRONE_ID_SYSTEM_UPDATE message (#1880) aa25f4df 2022-08-11 Andrew Tridgell - common: added OPEN_DRONE_ID_ARM_STATUS (#1873)	2022-10-02 10:53:00 -04:00
Daniel Agar	2bd1ac005f	.gitmodules update branches master -> main	2022-10-01 12:22:22 -04:00