ADD TODO

* Add link to image
* Update link to nice permalink

Co-authored-by: Chris Seto <chris1seto@gmail.com>

.ci/Jenkinsfile-hardware

@@ -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'
 }

.github/workflows/failsafe_sim.yml

@@ -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}}

Jenkinsfile

@@ -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')

Makefile

@@ -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

README.md

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

ROMFS/px4fmu_common/init.d/airframes/50003_aion_robotics_r1_rover

@@ -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
+

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

@@ -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

ROMFS/px4fmu_common/init.d/rc.fw_apps

@@ -18,6 +18,8 @@ control_allocator start
 #
 # Start attitude controller.
 #
+# fw_rate_control start
+dob_rate_control start
 fw_att_control start
 fw_pos_control_l1 start
 airspeed_selector start

Tools/module_config/output_groups_from_timer_config.py

@@ -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))

Tools/px4events/srcparser.py

@@ -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:

boards/airmind/mindpx-v2/default.px4board

@@ -47,6 +47,7 @@ CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
+CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_GYRO_CALIBRATION=y

boards/atl/mantis-edu/default.px4board

@@ -55,3 +55,4 @@ CONFIG_SYSTEMCMDS_VER=y
 CONFIG_SYSTEMCMDS_WORK_QUEUE=y
 CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
+CONFIG_MODULES_FW_RATE_CONTROL=y

boards/av/x-v1/default.px4board

@@ -44,6 +44,7 @@ CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
+CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_GYRO_CALIBRATION=y

boards/beaglebone/blue/default.px4board

@@ -30,6 +30,7 @@ CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
+CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_GIMBAL=y

boards/cuav/nora/default.px4board

@@ -51,6 +51,7 @@ CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
+CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_GYRO_CALIBRATION=y

boards/cuav/x7pro/default.px4board

@@ -52,6 +52,7 @@ CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
+CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_GYRO_CALIBRATION=y

boards/cubepilot/cubeorange/default.px4board

@@ -45,6 +45,7 @@ CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
+CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_GIMBAL=y

boards/cubepilot/cubeyellow/default.px4board

@@ -48,6 +48,7 @@ CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
+CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_GYRO_CALIBRATION=y

boards/emlid/navio2/default.px4board

@@ -32,6 +32,7 @@ CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
+CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_GIMBAL=y

boards/holybro/durandal-v1/default.px4board

@@ -46,6 +46,7 @@ CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
+CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_GYRO_CALIBRATION=y

boards/holybro/kakuteh7/default.px4board

@@ -39,6 +39,7 @@ CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
+CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_GIMBAL=y

boards/holybro/pix32v5/default.px4board

@@ -53,6 +53,7 @@ CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
+CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_GYRO_CALIBRATION=y

boards/matek/h743-mini/default.px4board

@@ -37,6 +37,7 @@ CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
+CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y

boards/matek/h743-slim/default.px4board

@@ -36,6 +36,7 @@ CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
+CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y

boards/matek/h743/default.px4board

@@ -37,6 +37,7 @@ CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
+CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y

boards/modalai/fc-v1/default.px4board

@@ -48,6 +48,7 @@ CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
+CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_GIMBAL=y

boards/modalai/fc-v2/default.px4board

@@ -46,6 +46,7 @@ CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
+CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_GIMBAL=y

boards/mro/ctrl-zero-classic/default.px4board

@@ -45,6 +45,7 @@ CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
+CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_GYRO_CALIBRATION=y

boards/mro/ctrl-zero-f7-oem/default.px4board

@@ -45,6 +45,7 @@ CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
+CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_GYRO_CALIBRATION=y

boards/mro/ctrl-zero-f7/default.px4board

@@ -45,6 +45,7 @@ CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
+CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_GYRO_CALIBRATION=y

boards/mro/ctrl-zero-h7-oem/default.px4board

@@ -44,6 +44,7 @@ CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
+CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_GIMBAL=y

boards/mro/ctrl-zero-h7/default.px4board

@@ -45,6 +45,7 @@ CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
+CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_GIMBAL=y

boards/mro/pixracerpro/default.px4board

@@ -44,6 +44,7 @@ CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
+CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_GIMBAL=y

boards/mro/x21-777/default.px4board

@@ -47,6 +47,7 @@ CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
+CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_GYRO_CALIBRATION=y

boards/mro/x21/default.px4board

@@ -48,6 +48,7 @@ CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
+CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_GYRO_CALIBRATION=y

boards/nxp/fmuk66-e/default.px4board

@@ -47,6 +47,7 @@ CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
+CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_GYRO_CALIBRATION=y

boards/nxp/fmuk66-v3/default.px4board

@@ -50,6 +50,7 @@ CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
+CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_GIMBAL=y

boards/nxp/fmurt1062-v1/src/board_config.h

@@ -236,6 +236,7 @@
  */
 
 #define DIRECT_PWM_OUTPUT_CHANNELS  8
+#define BOARD_NUM_IO_TIMERS         8
 
 // Input Capture not supported on MVP
 

boards/nxp/fmurt1062-v1/src/timer_config.cpp

@@ -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] = {

boards/px4/fmu-v2/fixedwing.px4board

@@ -9,4 +9,5 @@ CONFIG_DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_I2C=y
 CONFIG_DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_SERIAL=y
 CONFIG_MODULES_AIRSPEED_SELECTOR=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
+CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL_L1=y

boards/px4/fmu-v3/default.px4board

@@ -50,6 +50,7 @@ CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
+CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_GIMBAL=y

boards/px4/fmu-v4/default.px4board

@@ -51,6 +51,7 @@ CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
+CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_GIMBAL=y

boards/px4/fmu-v4pro/default.px4board

@@ -49,6 +49,7 @@ CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
+CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_GYRO_CALIBRATION=y

boards/px4/fmu-v5/default.px4board

@@ -55,6 +55,7 @@ CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
+CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_GIMBAL=y

boards/px4/fmu-v5x/default.px4board

@@ -58,6 +58,7 @@ CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
+CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_GIMBAL=y

boards/px4/fmu-v6c/default.px4board

@@ -40,6 +40,7 @@ CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
+CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_GIMBAL=y

boards/px4/fmu-v6u/default.px4board

@@ -48,6 +48,7 @@ CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
+CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_GIMBAL=y

boards/px4/fmu-v6x/default.px4board

@@ -49,6 +49,7 @@ CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
+CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_GIMBAL=y

boards/px4/raspberrypi/default.px4board

@@ -29,6 +29,7 @@ CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
+CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_GIMBAL=y

boards/px4/sitl/default.px4board

@@ -12,10 +12,12 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
+CONFIG_MODULES_DOB_RATE_CONTROL=y
 CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
+CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_GIMBAL=y

boards/scumaker/pilotpi/default.px4board

@@ -29,6 +29,7 @@ CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
+CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_GYRO_CALIBRATION=y

boards/sky-drones/smartap-airlink/default.px4board

@@ -51,6 +51,7 @@ CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
+CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_GIMBAL=y

msg/CMakeLists.txt

@@ -56,11 +56,9 @@ set(msg_files
 	camera_capture.msg
 	camera_status.msg
 	camera_trigger.msg
-	can_frame.msg
 	cellular_status.msg
 	collision_constraints.msg
 	collision_report.msg
-	commander_state.msg
 	control_allocator_status.msg
 	cpuload.msg
 	differential_pressure.msg
@@ -82,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
@@ -203,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

msg/action_request.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_*

msg/can_frame.msg

@@ -1,15 +0,0 @@
-uint64 timestamp	# time since system start (microseconds)
-
-uint32 id
-
-bool is_rtr
-bool is_extended
-bool is_error
-
-uint8 dlc
-
-uint8[8] data
-
-uint8 ORB_QUEUE_LENGTH = 8
-
-# TOPICS can_frame can_frame_out can_frame_in

msg/commander_state.msg

@@ -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

msg/estimator_aid_source_1d.msg

@@ -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

msg/estimator_aid_source_2d.msg

@@ -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

msg/estimator_aid_source_3d.msg

@@ -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

msg/estimator_bias.msg

@@ -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)
 

msg/failsafe_flags.msg

@@ -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
+
+

msg/geofence_result.msg

@@ -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

msg/mission_result.msg

@@ -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

msg/vehicle_command.msg

@@ -96,7 +96,6 @@ uint16 VEHICLE_CMD_DO_VTOL_TRANSITION = 3000    	# Command VTOL transition
 uint16 VEHICLE_CMD_ARM_AUTHORIZATION_REQUEST = 3001	# Request arm authorization
 uint16 VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY = 30001	# Prepare a payload deployment in the flight plan
 uint16 VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY = 30002	# Control a pre-programmed payload deployment
-uint16 VEHICLE_CMD_CAN_FORWARD = 32000
 uint16 VEHICLE_CMD_FIXED_MAG_CAL_YAW = 42006            # Magnetometer calibration based on provided known yaw. This allows for fast calibration using WMM field tables in the vehicle, given only the known yaw of the vehicle. If Latitude and longitude are both zero then use the current vehicle location.
 uint16 VEHICLE_CMD_DO_WINCH = 42600			# Command to operate winch.
 

msg/vehicle_status.msg

@@ -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,7 +104,6 @@ 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
@@ -120,3 +116,9 @@ 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
+

msg/vehicle_status_flags.msg

@@ -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
-

platforms/common/include/px4_platform_common/events.h

@@ -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

platforms/nuttx/src/px4/nxp/imxrt/include/px4_arch/io_timer.h

@@ -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 */

platforms/nuttx/src/px4/nxp/imxrt/include/px4_arch/io_timer_hw_description.h

@@ -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;
 }

platforms/nuttx/src/px4/nxp/imxrt/io_pins/io_timer.c

@@ -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;
 }
 

platforms/posix/cmake/sitl_tests.cmake

@@ -47,7 +47,6 @@ endforeach()
 
 # standalone tests
 set(cmd_tests
-	commander_tests
 	controllib_test
 	lightware_laser_test
 	rc_tests

src/drivers/uavcan/uavcan_main.cpp

@@ -130,10 +130,6 @@ UavcanNode::~UavcanNode()
 		} while (_instance);
 	}
 
-	_node.removeRxFrameListener();
-	delete _rx_frame_listener_uorb;
-	_rx_frame_listener_uorb = nullptr;
-
 	// Removing the sensor bridges
 	_sensor_bridges.clear();
 
@@ -502,17 +498,6 @@ UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events)
 
 	fill_node_info();
 
-	// install frame listener
-	int32_t uavcan_frame_dbg = 0;
-	param_get(param_find("UAVCAN_FRAME_DBG"), &uavcan_frame_dbg);
-
-	if (uavcan_frame_dbg == 1) {
-		PX4_INFO("UAVCAN FRAME DBG enabled");
-		_rx_frame_listener_uorb = new RxFrameUorbPublisher();
-		_node.getDispatcher().installRxFrameListener(_rx_frame_listener_uorb);
-		_mirror_to_uorb = true;
-	}
-
 	int ret = _beep_controller.init();
 
 	if (ret < 0) {
@@ -704,22 +689,6 @@ UavcanNode::Run()
 		update_params();
 	}
 
-	if (_mirror_to_uorb && _can_frame_in_sub.updated()) {
-		can_frame_s can_frame;
-
-		if (_can_frame_in_sub.copy(&can_frame)) {
-			uavcan::CanFrame frame{};
-			frame.id = can_frame.id;
-			memcpy(frame.data, can_frame.data, sizeof(can_frame.data));
-			frame.dlc = can_frame.dlc;
-
-			uavcan::MonotonicTime tx_deadline = _node.getMonotonicTime() + uavcan::MonotonicDuration::fromUSec(1000);
-			uint8_t iface_mask = 3;
-
-			_node.injectTxFrame(frame, tx_deadline, iface_mask, uavcan::CanTxQueue::Volatile);
-		}
-	}
-
 	// Check for parameter requests (get/set/list)
 	if (_param_request_sub.updated() && !_param_list_in_progress && !_param_in_progress && !_count_in_progress) {
 		uavcan_parameter_request_s request{};
@@ -908,13 +877,6 @@ UavcanNode::Run()
 					break;
 				}
 			}
-
-		} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_CAN_FORWARD) {
-			if (_rx_frame_listener_uorb == nullptr) {
-				_rx_frame_listener_uorb = new RxFrameUorbPublisher();
-				_node.getDispatcher().installRxFrameListener(_rx_frame_listener_uorb);
-				_mirror_to_uorb = true;
-			}
 		}
 
 		if (acknowledge) {

src/drivers/uavcan/uavcan_main.hpp

@@ -74,7 +74,6 @@
 #include <uORB/Publication.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionInterval.hpp>
-#include <uORB/topics/can_frame.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/uavcan_parameter_request.h>
 #include <uORB/topics/uavcan_parameter_value.h>
@@ -316,26 +315,4 @@ private:
 	bool _check_fw{false};
 
 	UavcanServers   *_servers{nullptr};
-
-
-	struct RxFrameUorbPublisher : public uavcan::IRxFrameListener {
-		void handleRxFrame(const uavcan::CanRxFrame &frame, uavcan::CanIOFlags flags) override
-		{
-			can_frame_s can_frame{};
-			can_frame.id = frame.id;
-			//can_frame.iface_index = frame.iface_index;
-			can_frame.dlc = frame.dlc;
-			memcpy(&can_frame.data, &frame.data, sizeof(frame.data));
-			can_frame.timestamp = hrt_absolute_time();
-			_can_frame_pub.publish(can_frame);
-		}
-	private:
-		uORB::Publication<can_frame_s> _can_frame_pub{ORB_ID(can_frame_out)};
-	};
-
-	RxFrameUorbPublisher *_rx_frame_listener_uorb{nullptr};
-
-	uORB::Subscription _can_frame_in_sub{ORB_ID(can_frame_in)};
-
-	bool _mirror_to_uorb{false};
 };

src/drivers/uavcan/uavcan_params.c

@@ -77,18 +77,6 @@ PARAM_DEFINE_INT32(UAVCAN_NODE_ID, 1);
  */
 PARAM_DEFINE_INT32(UAVCAN_BITRATE, 1000000);
 
-/**
- * UAVCAN CAN frame debug
- *
- * Publish UAVCAN CAN frames to ORB_ID(can_frame_out)
- * CAN frames published to ORB_ID(can_frame_in) are injected into UAVCAN
- *
- * @boolean
- * @reboot_required true
- * @group UAVCAN
- */
-PARAM_DEFINE_INT32(UAVCAN_FRAME_DBG, 0);
-
 /**
  * UAVCAN rangefinder minimum range
  *

src/lib/drivers/device/Device.hpp

@@ -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);
 	}
 

src/lib/events/enums.json

@@ -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": {

src/lib/parameters/CMakeLists.txt

@@ -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)
 

src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.hpp

@@ -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;
 

src/modules/commander/Arming/ArmStateMachine/ArmStateMachineTest.cpp

@@ -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(

src/modules/commander/CMakeLists.txt

@@ -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)

src/modules/commander/Commander.hpp

@@ -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,112 +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};
-	hrt_abstime	_datalink_last_heartbeat_open_drone_id_system{0};
-	bool		_onboard_controller_lost{false};
-	bool		_avoidance_system_lost{false};
-	bool		_parachute_system_lost{true};
-	bool		_open_drone_id_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)};
@@ -378,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
+	)
 };

src/modules/commander/HealthAndArmingChecks/CMakeLists.txt

@@ -53,6 +53,14 @@ px4_add_library(health_and_arming_checks
 	checks/sdcardCheck.cpp
 	checks/parachuteCheck.cpp
 	checks/batteryCheck.cpp
+	checks/windCheck.cpp
+	checks/geofenceCheck.cpp
+	checks/homePositionCheck.cpp
+	checks/flightTimeCheck.cpp
+	checks/missionCheck.cpp
+	checks/rcAndDataLinkCheck.cpp
+	checks/vtolCheck.cpp
+	checks/offboardCheck.cpp
 )
 add_dependencies(health_and_arming_checks mode_util)
 

src/modules/commander/HealthAndArmingChecks/Common.cpp

@@ -172,12 +172,13 @@ void Report::reset()
 	_results[_current_result].reset();
 	_next_buffer_idx = 0;
 	_buffer_overflowed = false;
+	_results_changed = false;
 }
 
 void Report::prepare(uint8_t vehicle_type)
 {
 	// Get mode requirements before running any checks (in particular the mode checks require them)
-	mode_util::getModeRequirements(vehicle_type, _status_flags);
+	mode_util::getModeRequirements(vehicle_type, _failsafe_flags);
 }
 
 NavModes Report::getModeGroup(uint8_t nav_state) const
@@ -186,16 +187,18 @@ NavModes Report::getModeGroup(uint8_t nav_state) const
 	return (NavModes)(1u << nav_state);
 }
 
-void Report::finalize()
+bool Report::finalize()
 {
 	_results[_current_result].arming_checks.valid = true;
 	_already_reported = false;
+	_results_changed = _results[0] != _results[1];
+	return _results_changed;
 }
 
 bool Report::report(bool is_armed, bool force)
 {
 	const hrt_abstime now = hrt_absolute_time();
-	const bool has_difference = _had_unreported_difference || _results[0] != _results[1];
+	const bool has_difference = _had_unreported_difference || _results_changed;
 
 	if (now - _last_report < _min_reporting_interval && !force) {
 		if (has_difference) {

src/modules/commander/HealthAndArmingChecks/Common.hpp

@@ -38,7 +38,7 @@
 #include <px4_platform_common/module_params.h>
 #include <uORB/topics/health_report.h>
 #include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/vehicle_status_flags.h>
+#include <uORB/topics/failsafe_flags.h>
 #include <systemlib/mavlink_log.h>
 
 #include <stdint.h>
@@ -188,11 +188,11 @@ public:
 		}
 	};
 
-	Report(vehicle_status_flags_s &status_flags, hrt_abstime min_reporting_interval = 2_s)
-		: _min_reporting_interval(min_reporting_interval), _status_flags(status_flags) { }
+	Report(failsafe_flags_s &failsafe_flags, hrt_abstime min_reporting_interval = 2_s)
+		: _min_reporting_interval(min_reporting_interval), _failsafe_flags(failsafe_flags) { }
 	~Report() = default;
 
-	vehicle_status_flags_s &failsafeFlags() { return _status_flags; }
+	failsafe_flags_s &failsafeFlags() { return _failsafe_flags; }
 
 	orb_advert_t *mavlink_log_pub() { return _mavlink_log_pub; }
 
@@ -254,6 +254,8 @@ public:
 
 	const HealthResults &healthResults() const { return _results[_current_result].health; }
 	const ArmingCheckResults &armingCheckResults() const { return _results[_current_result].arming_checks; }
+
+	bool modePreventsArming(uint8_t nav_state) const { return _failsafe_flags.mode_req_prevent_arming & (1u << nav_state); }
 private:
 
 	/**
@@ -321,7 +323,10 @@ private:
 	 */
 	void reset();
 	void prepare(uint8_t vehicle_type);
-	void finalize();
+	/**
+	 * Called after all checks are run. Returns true if the results changed
+	 */
+	bool finalize();
 
 	bool report(bool is_armed, bool force);
 
@@ -335,12 +340,13 @@ private:
 
 	bool _already_reported{false};
 	bool _had_unreported_difference{false}; ///< true if there was a difference not reported yet (due to rate limitation)
+	bool _results_changed{false};
 	hrt_abstime _last_report{0};
 
 	Results _results[2]; ///< Previous and current results to check for changes
 	int _current_result{0};
 
-	vehicle_status_flags_s &_status_flags;
+	failsafe_flags_s &_failsafe_flags;
 
 	orb_advert_t *_mavlink_log_pub{nullptr}; ///< mavlink log publication for legacy reporting
 };

src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp

@@ -33,12 +33,23 @@
 
 #include "HealthAndArmingChecks.hpp"
 
-HealthAndArmingChecks::HealthAndArmingChecks(ModuleParams *parent, vehicle_status_flags_s &status_flags,
-		vehicle_status_s &status)
+HealthAndArmingChecks::HealthAndArmingChecks(ModuleParams *parent, vehicle_status_s &status)
 	: ModuleParams(parent),
 	  _context(status),
-	  _reporter(status_flags)
+	  _reporter(_failsafe_flags)
 {
+	// Initialize mode requirements to invalid
+	_failsafe_flags.angular_velocity_invalid = true;
+	_failsafe_flags.attitude_invalid = true;
+	_failsafe_flags.local_altitude_invalid = true;
+	_failsafe_flags.local_position_invalid = true;
+	_failsafe_flags.local_position_invalid_relaxed = true;
+	_failsafe_flags.local_velocity_invalid = true;
+	_failsafe_flags.global_position_invalid = true;
+	_failsafe_flags.gps_position_invalid = true;
+	_failsafe_flags.auto_mission_missing = true;
+	_failsafe_flags.offboard_control_signal_lost = true;
+	_failsafe_flags.home_position_invalid = true;
 }
 
 bool HealthAndArmingChecks::update(bool force_reporting)
@@ -55,9 +66,10 @@ bool HealthAndArmingChecks::update(bool force_reporting)
 		_checks[i]->checkAndReport(_context, _reporter);
 	}
 
-	_reporter.finalize();
+	const bool results_changed = _reporter.finalize();
+	const bool reported = _reporter.report(_context.isArmed(), force_reporting);
 
-	if (_reporter.report(_context.isArmed(), force_reporting)) {
+	if (reported) {
 
 		// LEGACY start
 		// Run the checks again, this time with the mavlink publication set.
@@ -85,10 +97,17 @@ bool HealthAndArmingChecks::update(bool force_reporting)
 		_reporter.getHealthReport(health_report);
 		health_report.timestamp = hrt_absolute_time();
 		_health_report_pub.publish(health_report);
-		return true;
 	}
 
-	return false;
+	// Check if we need to publish the failsafe flags
+	const hrt_abstime now = hrt_absolute_time();
+
+	if (now - _failsafe_flags.timestamp > 500_ms || results_changed) {
+		_failsafe_flags.timestamp = now;
+		_failsafe_flags_pub.publish(_failsafe_flags);
+	}
+
+	return reported;
 }
 
 void HealthAndArmingChecks::updateParams()

src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp

@@ -38,6 +38,7 @@
 #include <px4_platform_common/module_params.h>
 #include <uORB/Publication.hpp>
 #include <uORB/topics/health_report.h>
+#include <uORB/topics/failsafe_flags.h>
 
 #include "checks/accelerometerCheck.hpp"
 #include "checks/airspeedCheck.hpp"
@@ -51,6 +52,7 @@
 #include "checks/imuConsistencyCheck.hpp"
 #include "checks/magnetometerCheck.hpp"
 #include "checks/manualControlCheck.hpp"
+#include "checks/homePositionCheck.hpp"
 #include "checks/modeCheck.hpp"
 #include "checks/parachuteCheck.hpp"
 #include "checks/powerCheck.hpp"
@@ -58,12 +60,19 @@
 #include "checks/sdcardCheck.hpp"
 #include "checks/systemCheck.hpp"
 #include "checks/batteryCheck.hpp"
+#include "checks/windCheck.hpp"
+#include "checks/geofenceCheck.hpp"
+#include "checks/flightTimeCheck.hpp"
+#include "checks/missionCheck.hpp"
+#include "checks/rcAndDataLinkCheck.hpp"
+#include "checks/vtolCheck.hpp"
+#include "checks/offboardCheck.hpp"
 
 
 class HealthAndArmingChecks : public ModuleParams
 {
 public:
-	HealthAndArmingChecks(ModuleParams *parent, vehicle_status_flags_s &status_flags, vehicle_status_s &status);
+	HealthAndArmingChecks(ModuleParams *parent, vehicle_status_s &status);
 	~HealthAndArmingChecks() = default;
 
 	/**
@@ -84,6 +93,13 @@ public:
 	 */
 	bool canRun(uint8_t nav_state) const { return _reporter.canRun(nav_state); }
 
+	/**
+	 * Query the mode requirements: check if a mode prevents arming
+	 */
+	bool modePreventsArming(uint8_t nav_state) const { return _reporter.modePreventsArming(nav_state); }
+
+	const failsafe_flags_s &failsafeFlags() const { return _failsafe_flags; }
+
 protected:
 	void updateParams() override;
 private:
@@ -91,7 +107,10 @@ private:
 	Report _reporter;
 	orb_advert_t _mavlink_log_pub{nullptr};
 
+	failsafe_flags_s _failsafe_flags{};
+
 	uORB::Publication<health_report_s> _health_report_pub{ORB_ID(health_report)};
+	uORB::Publication<failsafe_flags_s> _failsafe_flags_pub{ORB_ID(failsafe_flags)};
 
 	// all checks
 	AccelerometerChecks _accelerometer_checks;
@@ -106,6 +125,7 @@ private:
 	ImuConsistencyChecks _imu_consistency_checks;
 	MagnetometerChecks _magnetometer_checks;
 	ManualControlChecks _manual_control_checks;
+	HomePositionChecks _home_position_checks;
 	ModeChecks _mode_checks;
 	ParachuteChecks _parachute_checks;
 	PowerChecks _power_checks;
@@ -113,6 +133,13 @@ private:
 	SdCardChecks _sd_card_checks;
 	SystemChecks _system_checks;
 	BatteryChecks _battery_checks;
+	WindChecks _wind_checks;
+	GeofenceChecks _geofence_checks;
+	FlightTimeChecks _flight_time_checks;
+	MissionChecks _mission_checks;
+	RcAndDataLinkChecks _rc_and_data_link_checks;
+	VtolChecks _vtol_checks;
+	OffboardChecks _offboard_checks;
 
 	HealthAndArmingCheckBase *_checks[30] = {
 		&_accelerometer_checks,
@@ -127,13 +154,21 @@ private:
 		&_imu_consistency_checks,
 		&_magnetometer_checks,
 		&_manual_control_checks,
-		&_mode_checks, // must be after _estimator_checks
+		&_home_position_checks,
+		&_mission_checks,
+		&_offboard_checks, // must be after _estimator_checks
+		&_mode_checks, // must be after _estimator_checks, _home_position_checks, _mission_checks, _offboard_checks
 		&_parachute_checks,
 		&_power_checks,
 		&_rc_calibration_checks,
 		&_sd_card_checks,
-		&_system_checks,
+		&_system_checks, // must be after _estimator_checks & _home_position_checks
 		&_battery_checks,
+		&_wind_checks,
+		&_geofence_checks, // must be after _home_position_checks
+		&_flight_time_checks,
+		&_rc_and_data_link_checks,
+		&_vtol_checks,
 	};
 };
 

src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecksTest.cpp

@@ -63,8 +63,8 @@ public:
 
 TEST_F(ReporterTest, basic_no_checks)
 {
-	vehicle_status_flags_s status_flags{};
-	Report reporter{status_flags, 0_s};
+	failsafe_flags_s failsafe_flags{};
+	Report reporter{failsafe_flags, 0_s};
 	ASSERT_FALSE(reporter.canArm(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION));
 
 	reporter.reset();
@@ -83,8 +83,8 @@ TEST_F(ReporterTest, basic_no_checks)
 
 TEST_F(ReporterTest, basic_fail_all_modes)
 {
-	vehicle_status_flags_s status_flags{};
-	Report reporter{status_flags, 0_s};
+	failsafe_flags_s failsafe_flags{};
+	Report reporter{failsafe_flags, 0_s};
 
 	// ensure arming is always denied with a NavModes::All failure
 	for (uint8_t nav_state = 0; nav_state < vehicle_status_s::NAVIGATION_STATE_MAX; ++nav_state) {
@@ -101,8 +101,8 @@ TEST_F(ReporterTest, basic_fail_all_modes)
 
 TEST_F(ReporterTest, arming_checks_mode_category)
 {
-	vehicle_status_flags_s status_flags{};
-	Report reporter{status_flags, 0_s};
+	failsafe_flags_s failsafe_flags{};
+	Report reporter{failsafe_flags, 0_s};
 
 	// arming must still be possible for non-relevant failures
 	reporter.reset();
@@ -130,8 +130,8 @@ TEST_F(ReporterTest, arming_checks_mode_category)
 
 TEST_F(ReporterTest, arming_checks_mode_category2)
 {
-	vehicle_status_flags_s status_flags{};
-	Report reporter{status_flags, 0_s};
+	failsafe_flags_s failsafe_flags{};
+	Report reporter{failsafe_flags, 0_s};
 
 	// A matching mode category must deny arming
 	reporter.reset();
@@ -153,8 +153,8 @@ TEST_F(ReporterTest, arming_checks_mode_category2)
 
 TEST_F(ReporterTest, reporting)
 {
-	vehicle_status_flags_s status_flags{};
-	Report reporter{status_flags, 0_s};
+	failsafe_flags_s failsafe_flags{};
+	Report reporter{failsafe_flags, 0_s};
 
 	uORB::Subscription event_sub{ORB_ID(event)};
 	event_sub.subscribe();
@@ -247,8 +247,8 @@ TEST_F(ReporterTest, reporting)
 
 TEST_F(ReporterTest, reporting_multiple)
 {
-	vehicle_status_flags_s status_flags{};
-	Report reporter{status_flags, 0_s};
+	failsafe_flags_s failsafe_flags{};
+	Report reporter{failsafe_flags, 0_s};
 
 	uORB::Subscription event_sub{ORB_ID(event)};
 	event_sub.subscribe();

src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp

@@ -113,9 +113,13 @@ void BatteryChecks::checkAndReport(const Context &context, Report &reporter)
 			battery_required_count++;
 		}
 
+		if (!_last_armed && context.isArmed()) {
+			_battery_connected_at_arming[index] = battery.connected;
+		}
+
 		if (context.isArmed()) {
 
-			if (!battery.connected) {
+			if (!battery.connected && _battery_connected_at_arming[index]) { // If disconnected after arming
 				/* EVENT
 				 */
 				reporter.healthFailure<uint8_t>(NavModes::All, health_component_t::battery, events::ID("check_battery_disconnected"),
@@ -199,7 +203,8 @@ void BatteryChecks::checkAndReport(const Context &context, Report &reporter)
 		reporter.failsafeFlags().battery_warning = worst_warning;
 	}
 
-	if (reporter.failsafeFlags().battery_warning > battery_status_s::BATTERY_WARNING_NONE) {
+	if (reporter.failsafeFlags().battery_warning > battery_status_s::BATTERY_WARNING_NONE
+	    && reporter.failsafeFlags().battery_warning < battery_status_s::BATTERY_WARNING_FAILED) {
 		bool critical_or_higher = reporter.failsafeFlags().battery_warning >= battery_status_s::BATTERY_WARNING_CRITICAL;
 		NavModes affected_modes = critical_or_higher ? NavModes::All : NavModes::None;
 		events::LogLevel log_level = critical_or_higher ? events::Log::Critical : events::Log::Warning;
@@ -222,12 +227,13 @@ void BatteryChecks::checkAndReport(const Context &context, Report &reporter)
 		// There is at least one connected battery (in any slot)
 		|| num_connected_batteries < battery_required_count
 		// No currently-connected batteries have any fault
-		|| battery_has_fault;
+		|| battery_has_fault
+		|| reporter.failsafeFlags().battery_warning == battery_status_s::BATTERY_WARNING_FAILED;
 
 	if (reporter.failsafeFlags().battery_unhealthy && !battery_has_fault) { // faults are reported above already
 		/* EVENT
 		 * @description
-		 * Make sure all batteries are connected.
+		 * Make sure all batteries are connected and operational.
 		 */
 		reporter.healthFailure(NavModes::All, health_component_t::battery, events::ID("check_battery_unhealthy"),
 				       events::Log::Error, "Battery unhealthy");
@@ -241,6 +247,7 @@ void BatteryChecks::checkAndReport(const Context &context, Report &reporter)
 		reporter.setIsPresent(health_component_t::battery);
 	}
 
+	_last_armed = context.isArmed();
 }
 
 void BatteryChecks::rtlEstimateCheck(const Context &context, Report &reporter, float worst_battery_time_s)

src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.hpp

@@ -53,5 +53,7 @@ private:
 
 	uORB::SubscriptionMultiArray<battery_status_s, battery_status_s::MAX_INSTANCES> _battery_status_subs{ORB_ID::battery_status};
 	uORB::Subscription					_rtl_time_estimate_sub{ORB_ID(rtl_time_estimate)};
+	bool _last_armed{false};
+	bool _battery_connected_at_arming[battery_status_s::MAX_INSTANCES] {};
 
 };

src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp

@@ -102,7 +102,7 @@ void EstimatorChecks::checkAndReport(const Context &context, Report &reporter)
 		}
 	}
 
-	if (missing_data) {
+	if (missing_data && _param_sys_mc_est_group.get() == 2) {
 		/* EVENT
 		 */
 		reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate,
@@ -119,11 +119,11 @@ void EstimatorChecks::checkAndReport(const Context &context, Report &reporter)
 	}
 
 	// set mode requirements
-	const bool condition_gps_position_was_valid = reporter.failsafeFlags().gps_position_valid;
+	const bool condition_gps_position_was_valid = !reporter.failsafeFlags().gps_position_invalid;
 	setModeRequirementFlags(context, pre_flt_fail_innov_heading, pre_flt_fail_innov_vel_horiz, lpos, vehicle_gps_position,
 				reporter.failsafeFlags());
 
-	if (condition_gps_position_was_valid && !reporter.failsafeFlags().gps_position_valid) {
+	if (condition_gps_position_was_valid && reporter.failsafeFlags().gps_position_invalid) {
 		gpsNoLongerValid(context, reporter);
 	}
 }
@@ -658,7 +658,7 @@ void EstimatorChecks::gpsNoLongerValid(const Context &context, Report &reporter)
 	PX4_DEBUG("GPS no longer valid");
 
 	// report GPS failure if armed and the global position estimate is still valid
-	if (context.isArmed() && reporter.failsafeFlags().global_position_valid) {
+	if (context.isArmed() && !reporter.failsafeFlags().global_position_invalid) {
 		if (reporter.mavlink_log_pub()) {
 			mavlink_log_warning(reporter.mavlink_log_pub(), "GPS no longer valid\t");
 		}
@@ -670,7 +670,7 @@ void EstimatorChecks::gpsNoLongerValid(const Context &context, Report &reporter)
 
 void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_flt_fail_innov_heading,
 		bool pre_flt_fail_innov_vel_horiz,
-		const vehicle_local_position_s &lpos, const sensor_gps_s &vehicle_gps_position, vehicle_status_flags_s &failsafe_flags)
+		const vehicle_local_position_s &lpos, const sensor_gps_s &vehicle_gps_position, failsafe_flags_s &failsafe_flags)
 {
 	// The following flags correspond to mode requirements, and are reported in the corresponding mode checks
 
@@ -706,26 +706,26 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f
 		}
 	}
 
-	failsafe_flags.global_position_valid =
-		checkPosVelValidity(now, xy_valid, gpos.eph, _param_com_pos_fs_eph.get(), gpos.timestamp,
-				    _last_gpos_fail_time_us, failsafe_flags.global_position_valid);
+	failsafe_flags.global_position_invalid =
+		!checkPosVelValidity(now, xy_valid, gpos.eph, _param_com_pos_fs_eph.get(), gpos.timestamp,
+				     _last_gpos_fail_time_us, !failsafe_flags.global_position_invalid);
 
-	failsafe_flags.local_position_valid =
-		checkPosVelValidity(now, xy_valid, lpos.eph, _param_com_pos_fs_eph.get(), lpos.timestamp,
-				    _last_lpos_fail_time_us, failsafe_flags.local_position_valid);
+	failsafe_flags.local_position_invalid =
+		!checkPosVelValidity(now, xy_valid, lpos.eph, _param_com_pos_fs_eph.get(), lpos.timestamp,
+				     _last_lpos_fail_time_us, !failsafe_flags.local_position_invalid);
 
-	failsafe_flags.local_position_valid_relaxed =
-		checkPosVelValidity(now, xy_valid, lpos.eph, lpos_eph_threshold_relaxed, lpos.timestamp,
-				    _last_lpos_relaxed_fail_time_us, failsafe_flags.local_position_valid);
+	failsafe_flags.local_position_invalid_relaxed =
+		!checkPosVelValidity(now, xy_valid, lpos.eph, lpos_eph_threshold_relaxed, lpos.timestamp,
+				     _last_lpos_relaxed_fail_time_us, !failsafe_flags.local_position_invalid_relaxed);
 
-	failsafe_flags.local_velocity_valid =
-		checkPosVelValidity(now, v_xy_valid, lpos.evh, _param_com_vel_fs_evh.get(), lpos.timestamp,
-				    _last_lvel_fail_time_us, failsafe_flags.local_velocity_valid);
+	failsafe_flags.local_velocity_invalid =
+		!checkPosVelValidity(now, v_xy_valid, lpos.evh, _param_com_vel_fs_evh.get(), lpos.timestamp,
+				     _last_lvel_fail_time_us, !failsafe_flags.local_velocity_invalid);
 
 
 	// altitude
-	failsafe_flags.local_altitude_valid = lpos.z_valid
-					      && (now - lpos.timestamp < (_param_com_pos_fs_delay.get() * 1_s));
+	failsafe_flags.local_altitude_invalid = !lpos.z_valid
+						|| (now - lpos.timestamp > (_param_com_pos_fs_delay.get() * 1_s));
 
 
 	// attitude
@@ -740,10 +740,11 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f
 							&& (fabsf(q(3)) <= 1.f + eps);
 		const bool norm_in_tolerance = fabsf(1.f - q.norm()) <= eps;
 
-		failsafe_flags.attitude_valid = now - attitude.timestamp < 1_s && norm_in_tolerance && no_element_larger_than_one;
+		failsafe_flags.attitude_invalid = now > attitude.timestamp + 1_s || !norm_in_tolerance
+						  || !no_element_larger_than_one;
 
 	} else {
-		failsafe_flags.attitude_valid = false;
+		failsafe_flags.attitude_invalid = true;
 	}
 
 	// angular velocity
@@ -753,13 +754,13 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f
 			&& now < angular_velocity.timestamp + 1_s;
 	const bool condition_angular_velocity_finite = PX4_ISFINITE(angular_velocity.xyz[0])
 			&& PX4_ISFINITE(angular_velocity.xyz[1]) && PX4_ISFINITE(angular_velocity.xyz[2]);
-	const bool angular_velocity_valid = condition_angular_velocity_time_valid
-					    && condition_angular_velocity_finite;
+	const bool angular_velocity_invalid = !condition_angular_velocity_time_valid
+					      || !condition_angular_velocity_finite;
 
-	if (failsafe_flags.angular_velocity_valid && !angular_velocity_valid) {
+	if (!failsafe_flags.angular_velocity_invalid && angular_velocity_invalid) {
 		const char err_str[] {"angular velocity no longer valid"};
 
-		if (!condition_angular_velocity_time_valid) {
+		if (!condition_angular_velocity_time_valid && angular_velocity.timestamp != 0) {
 			PX4_ERR("%s (timeout)", err_str);
 
 		} else if (!condition_angular_velocity_finite) {
@@ -767,7 +768,7 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f
 		}
 	}
 
-	failsafe_flags.angular_velocity_valid = angular_velocity_valid;
+	failsafe_flags.angular_velocity_invalid = angular_velocity_invalid;
 
 
 	// gps
@@ -781,10 +782,10 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f
 		bool no_jamming = (vehicle_gps_position.jamming_state != sensor_gps_s::JAMMING_STATE_CRITICAL);
 
 		_vehicle_gps_position_valid.set_state_and_update(time && fix && eph && epv && evh && no_jamming, now);
-		failsafe_flags.gps_position_valid = _vehicle_gps_position_valid.get_state();
+		failsafe_flags.gps_position_invalid = !_vehicle_gps_position_valid.get_state();
 
 	} else {
-		failsafe_flags.gps_position_valid = false;
+		failsafe_flags.gps_position_invalid = true;
 	}
 }
 

src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp

@@ -69,7 +69,7 @@ private:
 	void gpsNoLongerValid(const Context &context, Report &reporter) const;
 	void setModeRequirementFlags(const Context &context, bool pre_flt_fail_innov_heading, bool pre_flt_fail_innov_vel_horiz,
 				     const vehicle_local_position_s &lpos, const sensor_gps_s &vehicle_gps_position,
-				     vehicle_status_flags_s &failsafe_flags);
+				     failsafe_flags_s &failsafe_flags);
 
 	bool checkPosVelValidity(const hrt_abstime &now, const bool data_valid, const float data_accuracy,
 				 const float required_accuracy,
@@ -105,6 +105,7 @@ private:
 	bool _position_reliant_on_optical_flow{false};
 
 	DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
+					(ParamInt<px4::params::SYS_MC_EST_GROUP>) _param_sys_mc_est_group,
 					(ParamInt<px4::params::SENS_IMU_MODE>) _param_sens_imu_mode,
 					(ParamInt<px4::params::COM_ARM_MAG_STR>) _param_com_arm_mag_str,
 					(ParamFloat<px4::params::COM_ARM_EKF_HGT>) _param_com_arm_ekf_hgt,

src/modules/commander/HealthAndArmingChecks/checks/failureDetectorCheck.cpp

@@ -35,81 +35,125 @@
 
 void FailureDetectorChecks::checkAndReport(const Context &context, Report &reporter)
 {
-	if (context.status().failure_detector_status != vehicle_status_s::FAILURE_NONE) {
-		if (context.status().failure_detector_status & vehicle_status_s::FAILURE_ROLL) {
-			/* EVENT
-			 * @description
-			 * The vehicle exceeded the maximum configured roll angle.
-			 *
-			 * <profile name="dev">
-			 * This check can be configured via <param>FD_FAIL_R</param> parameter.
-			 * </profile>
-			 */
-			reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_failure_detector_roll"),
-						    events::Log::Critical, "Attitude failure detected");
+	if (context.status().failure_detector_status & vehicle_status_s::FAILURE_ROLL) {
+		/* EVENT
+		 * @description
+		 * The vehicle exceeded the maximum configured roll angle.
+		 *
+		 * <profile name="dev">
+		 * This check can be configured via <param>FD_FAIL_R</param> parameter.
+		 * </profile>
+		 */
+		reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_failure_detector_roll"),
+					    events::Log::Critical, "Attitude failure detected");
 
-			if (reporter.mavlink_log_pub()) {
-				mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Attitude failure (roll)");
-			}
-
-		} else if (context.status().failure_detector_status & vehicle_status_s::FAILURE_PITCH) {
-			/* EVENT
-			 * @description
-			 * The vehicle exceeded the maximum configured pitch angle.
-			 *
-			 * <profile name="dev">
-			 * This check can be configured via <param>FD_FAIL_P</param> parameter.
-			 * </profile>
-			 */
-			reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_failure_detector_pitch"),
-						    events::Log::Critical, "Attitude failure detected");
-
-			if (reporter.mavlink_log_pub()) {
-				mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Attitude failure (pitch)");
-			}
+		if (reporter.mavlink_log_pub()) {
+			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Attitude failure (roll)");
 		}
 
-		if (context.status().failure_detector_status & vehicle_status_s::FAILURE_ALT) {
-			/* EVENT
-			 */
-			reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_failure_detector_alt"),
-						    events::Log::Critical, "Altitude failure detected");
+	} else if (context.status().failure_detector_status & vehicle_status_s::FAILURE_PITCH) {
+		/* EVENT
+		 * @description
+		 * The vehicle exceeded the maximum configured pitch angle.
+		 *
+		 * <profile name="dev">
+		 * This check can be configured via <param>FD_FAIL_P</param> parameter.
+		 * </profile>
+		 */
+		reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_failure_detector_pitch"),
+					    events::Log::Critical, "Attitude failure detected");
 
-			if (reporter.mavlink_log_pub()) {
-				mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Altitude failure");
-			}
-		}
-
-		if (context.status().failure_detector_status & vehicle_status_s::FAILURE_EXT) {
-			/* EVENT
-			 * @description
-			 * <profile name="dev">
-			 * This check can be configured via <param>FD_EXT_ATS_EN</param> parameter.
-			 * </profile>
-			 */
-			reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_failure_detector_ext"),
-						    events::Log::Critical, "Failure triggered by external system");
-
-			if (reporter.mavlink_log_pub()) {
-				mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Failure triggered by external system");
-			}
-		}
-
-		if (context.status().failure_detector_status & vehicle_status_s::FAILURE_ARM_ESC) {
-			/* EVENT
-			 * @description
-			 * One or more ESCs failed to arm.
-			 *
-			 * <profile name="dev">
-			 * This check can be configured via <param>FD_ESCS_EN</param> parameter.
-			 * </profile>
-			 */
-			reporter.healthFailure(NavModes::All, health_component_t::motors_escs, events::ID("check_failure_detector_arm_esc"),
-					       events::Log::Critical, "ESC failure");
-
-			if (reporter.mavlink_log_pub()) {
-				mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: ESC failure detected");
-			}
+		if (reporter.mavlink_log_pub()) {
+			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Attitude failure (pitch)");
 		}
 	}
+
+	if (context.status().failure_detector_status & vehicle_status_s::FAILURE_ALT) {
+		/* EVENT
+		 */
+		reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_failure_detector_alt"),
+					    events::Log::Critical, "Altitude failure detected");
+
+		if (reporter.mavlink_log_pub()) {
+			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Altitude failure");
+		}
+	}
+
+	if (context.status().failure_detector_status & vehicle_status_s::FAILURE_EXT) {
+		/* EVENT
+		 * @description
+		 * <profile name="dev">
+		 * This check can be configured via <param>FD_EXT_ATS_EN</param> parameter.
+		 * </profile>
+		 */
+		reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_failure_detector_ext"),
+					    events::Log::Critical, "Failure triggered by external system");
+
+		if (reporter.mavlink_log_pub()) {
+			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Failure triggered by external system");
+		}
+	}
+
+	reporter.failsafeFlags().fd_critical_failure = context.status().failure_detector_status &
+			(vehicle_status_s::FAILURE_ROLL | vehicle_status_s::FAILURE_PITCH | vehicle_status_s::FAILURE_ALT |
+			 vehicle_status_s::FAILURE_EXT);
+
+
+	reporter.failsafeFlags().fd_esc_arming_failure = context.status().failure_detector_status &
+			vehicle_status_s::FAILURE_ARM_ESC;
+
+	if (reporter.failsafeFlags().fd_esc_arming_failure) {
+		/* EVENT
+		 * @description
+		 * One or more ESCs failed to arm.
+		 *
+		 * <profile name="dev">
+		 * This check can be configured via <param>FD_ESCS_EN</param> parameter.
+		 * </profile>
+		 */
+		reporter.healthFailure(NavModes::All, health_component_t::motors_escs, events::ID("check_failure_detector_arm_esc"),
+				       events::Log::Critical, "ESC failure");
+
+		if (reporter.mavlink_log_pub()) {
+			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: ESC failure detected");
+		}
+	}
+
+	reporter.failsafeFlags().fd_imbalanced_prop = context.status().failure_detector_status &
+			vehicle_status_s::FAILURE_IMBALANCED_PROP;
+
+	if (reporter.failsafeFlags().fd_imbalanced_prop) {
+		/* EVENT
+		 * @description
+		 * Check that all propellers are mounted correctly and are not damaged.
+		 *
+		 * <profile name="dev">
+		 * This check can be configured via <param>FD_IMB_PROP_THR</param> and <param>COM_IMB_PROP_ACT</param> parameters.
+		 * </profile>
+		 */
+		reporter.healthFailure(NavModes::All, health_component_t::system, events::ID("check_failure_detector_imbalanced_prop"),
+				       events::Log::Critical, "Imbalanced propeller detected");
+
+		if (reporter.mavlink_log_pub()) {
+			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Imbalanced propeller detected");
+		}
+	}
+
+	reporter.failsafeFlags().fd_motor_failure = context.status().failure_detector_status & vehicle_status_s::FAILURE_MOTOR;
+
+	if (reporter.failsafeFlags().fd_motor_failure) {
+		/* EVENT
+		 * @description
+		 * <profile name="dev">
+		 * This check can be configured via <param>FD_ACT_EN</param> parameter.
+		 * </profile>
+		 */
+		reporter.healthFailure(NavModes::All, health_component_t::motors_escs, events::ID("check_failure_detector_motor"),
+				       events::Log::Critical, "Motor failure detected");
+
+		if (reporter.mavlink_log_pub()) {
+			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Motor failure detected");
+		}
+	}
+
 }

src/modules/commander/HealthAndArmingChecks/checks/flightTimeCheck.cpp

@@ -0,0 +1,58 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "flightTimeCheck.hpp"
+
+void FlightTimeChecks::checkAndReport(const Context &context, Report &reporter)
+{
+	if (_param_com_flt_time_max.get() > FLT_EPSILON && context.status().takeoff_time != 0 &&
+	    (hrt_absolute_time() - context.status().takeoff_time) > (1_s * _param_com_flt_time_max.get())) {
+		reporter.failsafeFlags().flight_time_limit_exceeded = true;
+		/* EVENT
+		 * @description
+		 * <profile name="dev">
+		 * This check can be configured via <param>COM_FLT_TIME_MAX</param> parameter.
+		 * </profile>
+		 */
+		reporter.armingCheckFailure(NavModes::All, health_component_t::system,
+					    events::ID("check_flight_time_limit"),
+					    events::Log::Error, "Maximum flight time reached");
+
+		if (reporter.mavlink_log_pub()) {
+			mavlink_log_critical(reporter.mavlink_log_pub(), "Maximum flight time reached\t");
+		}
+
+	} else {
+		reporter.failsafeFlags().flight_time_limit_exceeded = false;
+	}
+}

src/modules/commander/HealthAndArmingChecks/checks/flightTimeCheck.hpp

@@ -0,0 +1,50 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "../Common.hpp"
+
+class FlightTimeChecks : public HealthAndArmingCheckBase
+{
+public:
+	FlightTimeChecks() = default;
+	~FlightTimeChecks() = default;
+
+	void checkAndReport(const Context &context, Report &reporter) override;
+
+private:
+	DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
+					(ParamInt<px4::params::COM_FLT_TIME_MAX>) _param_com_flt_time_max
+				       )
+};

src/modules/commander/HealthAndArmingChecks/checks/geofenceCheck.cpp

@@ -0,0 +1,78 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "geofenceCheck.hpp"
+
+void GeofenceChecks::checkAndReport(const Context &context, Report &reporter)
+{
+	geofence_result_s geofence_result;
+
+	if (!_geofence_result_sub.copy(&geofence_result)) {
+		geofence_result = {};
+	}
+
+	reporter.failsafeFlags().primary_geofence_breached = geofence_result.primary_geofence_breached;
+
+	if (geofence_result.primary_geofence_action != 0 && reporter.failsafeFlags().primary_geofence_breached) {
+		/* EVENT
+		 * @description
+		 * <profile name="dev">
+		 * This check can be configured via <param>GF_ACTION</param> parameter.
+		 * </profile>
+		 */
+		reporter.armingCheckFailure<events::px4::enums::geofence_violation_reason_t>(NavModes::All, health_component_t::system,
+				events::ID("check_gf_violation"),
+				events::Log::Error, "Geofence violation: {1}",
+				(events::px4::enums::geofence_violation_reason_t)geofence_result.geofence_violation_reason);
+
+		if (reporter.mavlink_log_pub()) {
+			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Geofence violation");
+		}
+	}
+
+	if (geofence_result.primary_geofence_action == geofence_result_s::GF_ACTION_RTL
+	    && reporter.failsafeFlags().home_position_invalid) {
+		/* EVENT
+		 * @description
+		 * <profile name="dev">
+		 * This check can be configured via <param>GF_ACTION</param> parameter.
+		 * </profile>
+		 */
+		reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_gf_no_home"),
+					    events::Log::Error, "Geofence RTL requires valid home");
+
+		if (reporter.mavlink_log_pub()) {
+			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Geofence RTL requires valid home");
+		}
+	}
+}

src/modules/commander/HealthAndArmingChecks/checks/geofenceCheck.hpp

@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *   Author: Simon Wilks <sjwilks@gmail.com>
+ *   Copyright (c) 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
@@ -32,21 +31,21 @@
  *
  ****************************************************************************/
 
-/**
- * @file commander_tests.cpp
- * Commander unit tests. Run the tests as follows:
- *   nsh> commander_tests
- *
- */
+#pragma once
 
-#include <systemlib/err.h>
+#include "../Common.hpp"
 
-#include "state_machine_helper_test.h"
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/geofence_result.h>
 
-extern "C" __EXPORT int commander_tests_main(int argc, char *argv[]);
-
-
-int commander_tests_main(int argc, char *argv[])
+class GeofenceChecks : public HealthAndArmingCheckBase
 {
-	return stateMachineHelperTest() ? 0 : -1;
-}
+public:
+	GeofenceChecks() = default;
+	~GeofenceChecks() = default;
+
+	void checkAndReport(const Context &context, Report &reporter) override;
+
+private:
+	uORB::Subscription _geofence_result_sub{ORB_ID(geofence_result)};
+};

src/modules/commander/HealthAndArmingChecks/checks/homePositionCheck.cpp

@@ -0,0 +1,48 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "homePositionCheck.hpp"
+
+void HomePositionChecks::checkAndReport(const Context &context, Report &reporter)
+{
+	home_position_s home_position;
+
+	if (_home_position_sub.copy(&home_position)) {
+		reporter.failsafeFlags().home_position_invalid = !home_position.valid_alt || !home_position.valid_hpos;
+
+	} else {
+		reporter.failsafeFlags().home_position_invalid = true;
+	}
+
+	// No need to report, as it's a mode requirement
+}

src/modules/commander/HealthAndArmingChecks/checks/homePositionCheck.hpp

@@ -0,0 +1,51 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "../Common.hpp"
+
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/home_position.h>
+
+class HomePositionChecks : public HealthAndArmingCheckBase
+{
+public:
+	HomePositionChecks() = default;
+	~HomePositionChecks() = default;
+
+	void checkAndReport(const Context &context, Report &reporter) override;
+
+private:
+	uORB::Subscription _home_position_sub{ORB_ID(home_position)};
+};

src/modules/commander/HealthAndArmingChecks/checks/missionCheck.cpp

@@ -0,0 +1,61 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "missionCheck.hpp"
+
+void MissionChecks::checkAndReport(const Context &context, Report &reporter)
+{
+	reporter.failsafeFlags().mission_failure = false;
+	reporter.failsafeFlags().auto_mission_missing = true;
+	mission_result_s mission_result;
+
+	if (_mission_result_sub.copy(&mission_result) && mission_result.valid) {
+		reporter.failsafeFlags().mission_failure = mission_result.failure;
+
+		if (reporter.failsafeFlags().mission_failure) {
+			// navigator sends out the exact reason
+			/* EVENT
+			 */
+			reporter.armingCheckFailure(NavModes::All, health_component_t::system,
+						    events::ID("check_mission_failure"),
+						    events::Log::Error, "Mission cannot be completed");
+
+			if (reporter.mavlink_log_pub()) {
+				mavlink_log_critical(reporter.mavlink_log_pub(), "Mission cannot be completed\t");
+			}
+		}
+
+		// This is a mode requirement, no need to report
+		reporter.failsafeFlags().auto_mission_missing = mission_result.instance_count <= 0;
+	}
+}