Update submodule

Use  control allocation helicopter
Set yaw ccw
Simplify swashplate geometry

.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-posix/airframes/1080_helicopter

@@ -0,0 +1,34 @@
+#!/bin/sh
+#
+# @name 3DR Iris Quadrotor SITL
+#
+# @type Quadrotor Wide
+#
+# @maintainer Julian Oes <julian@oes.ch>
+#
+
+. ${R}etc/init.d/rc.heli_defaults
+
+
+# Disable PID gains for initial setup. These should be enabled after setting the FF gain.
+# P is expected to be lower than FF.
+param set-default MC_ROLLRATE_P 0
+param set-default MC_ROLLRATE_I 0
+param set-default MC_ROLLRATE_D 0
+param set-default MC_ROLLRATE_FF 0.1
+param set-default MC_PITCHRATE_P 0
+param set-default MC_PITCHRATE_I 0
+param set-default MC_PITCHRATE_D 0
+param set-default MC_PITCHRATE_FF 0.1
+
+param set-default CA_AIRFRAME 10
+param set-default CA_HELI_YAW_CCW 1
+param set-default CA_SP0_ANG0 0
+param set-default CA_SP0_ANG1 120
+param set-default CA_SP0_ANG2 240
+
+param set-default PWM_MAIN_FUNC1 101
+param set-default PWM_MAIN_FUNC2 102
+param set-default PWM_MAIN_FUNC3 201
+param set-default PWM_MAIN_FUNC4 202
+param set-default PWM_MAIN_FUNC5 203

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

@@ -71,6 +71,7 @@ px4_add_romfs_files(
 	1061_r1_rover
 	1062_tf-r1
 	1070_boat
+	1080_helicopter
 	3010_quadrotor_x
 	3011_hexarotor_x
 

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

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

msg/CMakeLists.txt

@@ -59,7 +59,6 @@ set(msg_files
 	cellular_status.msg
 	collision_constraints.msg
 	collision_report.msg
-	commander_state.msg
 	control_allocator_status.msg
 	cpuload.msg
 	differential_pressure.msg
@@ -81,6 +80,7 @@ set(msg_files
 	estimator_status_flags.msg
 	event.msg
 	failure_detector_status.msg
+	failsafe_flags.msg
 	follow_target.msg
 	follow_target_estimator.msg
 	follow_target_status.msg
@@ -202,7 +202,6 @@ set(msg_files
 	vehicle_rates_setpoint.msg
 	vehicle_roi.msg
 	vehicle_status.msg
-	vehicle_status_flags.msg
 	vehicle_thrust_setpoint.msg
 	vehicle_torque_setpoint.msg
 	vehicle_trajectory_bezier.msg

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/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_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/osd/msp_osd/module.yaml

@@ -37,9 +37,9 @@ parameters:
                 16: (unused) PITCH_ANGLE
                 17: (unused) ROLL_ANGLE
                 18: (unused) CROSSHAIRS
-                19: (unused) AVG_CELL_VOLTAGE
+                19: AVG_CELL_VOLTAGE
                 20: (unused) HORIZON_SIDEBARS
-                21: (unused) POWER
+                21: POWER
             default: 16383
 
         # OSD Log Level

src/drivers/osd/msp_osd/msp_osd.cpp

@@ -71,25 +71,44 @@
 
 // Currently working elements positions (hardcoded)
 
+/* center col
+
+Speed Power Alt
+Rssi cell_voltage mah
+craft name
+
+*/
+
 // Left
 const uint16_t osd_gps_lat_pos = 2048;
 const uint16_t osd_gps_lon_pos = 2080;
 const uint16_t osd_gps_sats_pos = 2112;
-const uint16_t osd_rssi_value_pos = 2176;
 
 // Center
-const uint16_t osd_home_dir_pos = 2093;
-const uint16_t osd_craft_name_pos = 2543;
+// Top
 const uint16_t osd_disarmed_pos = 2125;
+const uint16_t osd_home_dir_pos = 2093;
+const uint16_t osd_home_dist_pos = 2095;
+
+// Bottom row 1
+const uint16_t osd_gps_speed_pos = 2413;
+const uint16_t osd_power_pos = 2415;
+const uint16_t osd_altitude_pos = 2416;
+
+// Bottom Row 2
+const uint16_t osd_rssi_value_pos = 2445;
+const uint16_t osd_avg_cell_voltage_pos = 2446;
+const uint16_t osd_mah_drawn_pos = 2449;
+
+// Bottom Row 3
+const uint16_t osd_craft_name_pos = 2480;
 
 // Right
 const uint16_t osd_main_batt_voltage_pos = 2073;
 const uint16_t osd_current_draw_pos = 2103;
-const uint16_t osd_mah_drawn_pos = 2138;
-const uint16_t osd_altitude_pos = 2233;
-const uint16_t osd_numerical_vario_pos = 2267;
-const uint16_t osd_gps_speed_pos = 2299;
-const uint16_t osd_home_dist_pos = 2331;
+
+
+const uint16_t osd_numerical_vario_pos = LOCATION_HIDDEN;
 
 MspOsd::MspOsd(const char *device) :
 	ModuleParams(nullptr),
@@ -147,15 +166,18 @@ void MspOsd::SendConfig()
 	msp_osd_config.osd_numerical_vario_pos = enabled(SymbolIndex::NUMERICAL_VARIO) ? osd_numerical_vario_pos :
 			LOCATION_HIDDEN;
 
+	msp_osd_config.osd_power_pos = enabled(SymbolIndex::POWER) ? osd_power_pos : LOCATION_HIDDEN;
+	msp_osd_config.osd_avg_cell_voltage_pos = enabled(SymbolIndex::AVG_CELL_VOLTAGE) ? osd_avg_cell_voltage_pos :
+			LOCATION_HIDDEN;
+
 	// possibly available, but not currently used
 	msp_osd_config.osd_flymode_pos = 			LOCATION_HIDDEN;
 	msp_osd_config.osd_esc_tmp_pos = 			LOCATION_HIDDEN;
 	msp_osd_config.osd_pitch_angle_pos = 			LOCATION_HIDDEN;
 	msp_osd_config.osd_roll_angle_pos = 			LOCATION_HIDDEN;
 	msp_osd_config.osd_crosshairs_pos = 			LOCATION_HIDDEN;
-	msp_osd_config.osd_avg_cell_voltage_pos = 		LOCATION_HIDDEN;
+
 	msp_osd_config.osd_horizon_sidebars_pos = 		LOCATION_HIDDEN;
-	msp_osd_config.osd_power_pos = 				LOCATION_HIDDEN;
 
 	// Not implemented or not available
 	msp_osd_config.osd_artificial_horizon_pos = 		LOCATION_HIDDEN;

src/drivers/osd/msp_osd/uorb_to_msp.cpp

@@ -261,7 +261,7 @@ msp_status_BF_t construct_STATUS(const vehicle_status_s &vehicle_status)
 			break;
 
 		default:
-			status_BF.flight_mode_flags = 0;
+			status_BF.flight_mode_flags |= 0;
 			break;
 		}
 	}
@@ -277,10 +277,9 @@ msp_analog_t construct_ANALOG(const battery_status_s &battery_status, const inpu
 	msp_analog_t analog {0};
 
 	analog.vbat = battery_status.voltage_v * 10; // bottom right... v * 10
-	analog.rssi = (uint16_t)((input_rc.rssi * 1023.0f) / 100.0f);
+	analog.rssi = (uint16_t)((input_rc.link_quality * 1023.0f) / 100.0f);
 	analog.amperage = battery_status.current_a * 100; // main amperage
 	analog.mAhDrawn = battery_status.discharged_mah; // unused
-
 	return analog;
 }
 
@@ -290,8 +289,8 @@ msp_battery_state_t construct_BATTERY_STATE(const battery_status_s &battery_stat
 	msp_battery_state_t battery_state = {0};
 
 	// MSP_BATTERY_STATE
-	battery_state.amperage = battery_status.current_a; // not used?
-	battery_state.batteryVoltage = (uint16_t)(battery_status.voltage_v * 400.0f);  // OK
+	battery_state.amperage = battery_status.current_a * 100.0f; // Used for power element
+	battery_state.batteryVoltage = (uint16_t)((battery_status.voltage_v / battery_status.cell_count) * 400.0f);  // OK
 	battery_state.mAhDrawn = battery_status.discharged_mah ; // OK
 	battery_state.batteryCellCount = battery_status.cell_count;
 	battery_state.batteryCapacity = battery_status.capacity; // not used?
@@ -318,14 +317,24 @@ msp_raw_gps_t construct_RAW_GPS(const sensor_gps_s &vehicle_gps_position,
 		raw_gps.lat = vehicle_gps_position.lat;
 		raw_gps.lon = vehicle_gps_position.lon;
 		raw_gps.alt =  vehicle_gps_position.alt / 10;
-		//raw_gps.groundCourse = vehicle_gps_position_struct
+
+		float course = math::degrees(vehicle_gps_position.cog_rad);
+
+		if (course < 0) {
+			course += 360.0f;
+		}
+
+		raw_gps.groundCourse = course * 100.0f; // centidegrees
 
 	} else {
 		raw_gps.lat = 0;
 		raw_gps.lon = 0;
 		raw_gps.alt = 0;
+		raw_gps.groundCourse = 0; // centidegrees
 	}
 
+	raw_gps.groundCourse = 0; // centidegrees
+
 	if (vehicle_gps_position.fix_type == 0
 	    || vehicle_gps_position.fix_type == 1) {
 		raw_gps.fixType = MSP_GPS_NO_FIX;
@@ -367,20 +376,24 @@ msp_comp_gps_t construct_COMP_GPS(const home_position_s &home_position,
 	if (home_position.valid_hpos
 	    && home_position.valid_lpos
 	    && estimator_status.solution_status_flags & (1 << 4)) {
-		float bearing_to_home = get_bearing_to_next_waypoint(vehicle_global_position.lat,
-					vehicle_global_position.lon,
-					home_position.lat, home_position.lon);
+		float bearing_to_home = math::degrees(get_bearing_to_next_waypoint(vehicle_global_position.lat,
+						      vehicle_global_position.lon,
+						      home_position.lat, home_position.lon));
+
+		if (bearing_to_home < 0) {
+			bearing_to_home += 360.0f;
+		}
 
 		float distance_to_home = get_distance_to_next_waypoint(vehicle_global_position.lat,
 					 vehicle_global_position.lon,
 					 home_position.lat, home_position.lon);
 
 		comp_gps.distanceToHome = (int16_t)distance_to_home; // meters
-		comp_gps.directionToHome = bearing_to_home; // degrees
+		comp_gps.directionToHome = bearing_to_home;
 
 	} else {
 		comp_gps.distanceToHome = 0; // meters
-		comp_gps.directionToHome = 0; // degrees
+		comp_gps.directionToHome = 0;
 	}
 
 	comp_gps.heartbeat = heartbeat;
@@ -396,7 +409,17 @@ msp_attitude_t construct_ATTITUDE(const vehicle_attitude_s &vehicle_attitude)
 	matrix::Eulerf euler_attitude(matrix::Quatf(vehicle_attitude.q));
 	attitude.pitch = math::degrees(euler_attitude.theta()) * 10;
 	attitude.roll = math::degrees(euler_attitude.phi()) * 10;
-	attitude.yaw = math::degrees(euler_attitude.psi()) * 10;
+	//attitude.yaw = math::degrees(euler_attitude.psi()) * 10;
+
+	float yaw_fixed = math::degrees(euler_attitude.psi());
+
+	if (yaw_fixed < 0) {
+		yaw_fixed += 360.0f;
+	}
+
+	attitude.yaw = yaw_fixed;
+
+	//attitude.yaw = 360;
 
 	return attitude;
 }

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

src/modules/commander/HealthAndArmingChecks/checks/missionCheck.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,11 +31,21 @@
  *
  ****************************************************************************/
 
-/**
- * @file state_machine_helper_test.h
- */
-
 #pragma once
 
-bool stateMachineHelperTest(void);
+#include "../Common.hpp"
 
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/mission_result.h>
+
+class MissionChecks : public HealthAndArmingCheckBase
+{
+public:
+	MissionChecks() = default;
+	~MissionChecks() = default;
+
+	void checkAndReport(const Context &context, Report &reporter) override;
+
+private:
+	uORB::Subscription _mission_result_sub{ORB_ID(mission_result)};
+};

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

@@ -42,7 +42,7 @@ void ModeChecks::checkAndReport(const Context &context, Report &reporter)
 	// Failing mode requirements generally also clear the can_run bits which prevents mode switching and
 	// might trigger a failsafe if already in that mode.
 
-	if (!reporter.failsafeFlags().angular_velocity_valid && reporter.failsafeFlags().mode_req_angular_velocity != 0) {
+	if (reporter.failsafeFlags().angular_velocity_invalid && reporter.failsafeFlags().mode_req_angular_velocity != 0) {
 		/* EVENT
 		 * @description
 		 * Make sure the gyroscope is providing valid data.
@@ -53,7 +53,7 @@ void ModeChecks::checkAndReport(const Context &context, Report &reporter)
 		reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_angular_velocity);
 	}
 
-	if (!reporter.failsafeFlags().attitude_valid && reporter.failsafeFlags().mode_req_attitude != 0) {
+	if (reporter.failsafeFlags().attitude_invalid && reporter.failsafeFlags().mode_req_attitude != 0) {
 		/* EVENT
 		 * @description
 		 * Wait until the estimator initialized
@@ -66,11 +66,11 @@ void ModeChecks::checkAndReport(const Context &context, Report &reporter)
 
 	NavModes local_position_modes = NavModes::None;
 
-	if (!reporter.failsafeFlags().local_position_valid && reporter.failsafeFlags().mode_req_local_position != 0) {
+	if (reporter.failsafeFlags().local_position_invalid && reporter.failsafeFlags().mode_req_local_position != 0) {
 		local_position_modes = (NavModes)reporter.failsafeFlags().mode_req_local_position;
 	}
 
-	if (!reporter.failsafeFlags().local_position_valid_relaxed
+	if (reporter.failsafeFlags().local_position_invalid_relaxed
 	    && reporter.failsafeFlags().mode_req_local_position_relaxed != 0) {
 		local_position_modes = local_position_modes | (NavModes)reporter.failsafeFlags().mode_req_local_position_relaxed;
 	}
@@ -84,7 +84,7 @@ void ModeChecks::checkAndReport(const Context &context, Report &reporter)
 		reporter.clearCanRunBits(local_position_modes);
 	}
 
-	if (!reporter.failsafeFlags().global_position_valid && reporter.failsafeFlags().mode_req_global_position != 0) {
+	if (reporter.failsafeFlags().global_position_invalid && reporter.failsafeFlags().mode_req_global_position != 0) {
 		/* EVENT
 		 */
 		reporter.armingCheckFailure((NavModes)reporter.failsafeFlags().mode_req_global_position, health_component_t::system,
@@ -93,7 +93,7 @@ void ModeChecks::checkAndReport(const Context &context, Report &reporter)
 		reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_global_position);
 	}
 
-	if (!reporter.failsafeFlags().local_altitude_valid && reporter.failsafeFlags().mode_req_local_alt != 0) {
+	if (reporter.failsafeFlags().local_altitude_invalid && reporter.failsafeFlags().mode_req_local_alt != 0) {
 		/* EVENT
 		 */
 		reporter.armingCheckFailure((NavModes)reporter.failsafeFlags().mode_req_local_alt, health_component_t::system,
@@ -102,21 +102,31 @@ void ModeChecks::checkAndReport(const Context &context, Report &reporter)
 		reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_local_alt);
 	}
 
-	if (!reporter.failsafeFlags().auto_mission_available && reporter.failsafeFlags().mode_req_mission != 0) {
+	NavModes mission_required_modes = (NavModes)reporter.failsafeFlags().mode_req_mission;
+
+	if (_param_com_arm_mis_req.get()) {
+		mission_required_modes = NavModes::All;
+	}
+
+	if (reporter.failsafeFlags().auto_mission_missing && mission_required_modes != NavModes::None) {
 		/* EVENT
 		 * @description
 		 * Upload a mission first.
+		 *
+		 * <profile name="dev">
+		 * This check can be configured via <param>COM_ARM_MIS_REQ</param> parameter.
+		 * </profile>
 		 */
-		reporter.armingCheckFailure((NavModes)reporter.failsafeFlags().mode_req_mission, health_component_t::system,
+		reporter.armingCheckFailure(mission_required_modes, health_component_t::system,
 					    events::ID("check_modes_mission"),
-					    events::Log::Info, "No mission available");
+					    events::Log::Info, "No valid mission available");
 		reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_mission);
 	}
 
 	if (reporter.failsafeFlags().offboard_control_signal_lost && reporter.failsafeFlags().mode_req_offboard_signal != 0) {
 		/* EVENT
 		 * @description
-		 * The offboard component is not sending setpoints.
+		 * The offboard component is not sending setpoints or the required estimate (e.g. position) is missing.
 		 */
 		reporter.armingCheckFailure((NavModes)reporter.failsafeFlags().mode_req_offboard_signal, health_component_t::system,
 					    events::ID("check_modes_offboard_signal"),
@@ -124,7 +134,7 @@ void ModeChecks::checkAndReport(const Context &context, Report &reporter)
 		reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_offboard_signal);
 	}
 
-	if (!reporter.failsafeFlags().home_position_valid && reporter.failsafeFlags().mode_req_home_position != 0) {
+	if (reporter.failsafeFlags().home_position_invalid && reporter.failsafeFlags().mode_req_home_position != 0) {
 		/* EVENT
 		 */
 		reporter.armingCheckFailure((NavModes)reporter.failsafeFlags().mode_req_home_position, health_component_t::system,

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

@@ -48,4 +48,7 @@ public:
 private:
 	void checkArmingRequirement(const Context &context, Report &reporter);
 
+	DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
+					(ParamBool<px4::params::COM_ARM_MIS_REQ>) _param_com_arm_mis_req
+				       );
 };

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

@@ -0,0 +1,70 @@
+/****************************************************************************
+ *
+ *   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 "offboardCheck.hpp"
+
+using namespace time_literals;
+
+OffboardChecks::OffboardChecks()
+{
+	_offboard_available.set_hysteresis_time_from(true, _param_com_of_loss_t.get() * 1_s);
+}
+
+void OffboardChecks::checkAndReport(const Context &context, Report &reporter)
+{
+	reporter.failsafeFlags().offboard_control_signal_lost = true;
+
+	offboard_control_mode_s offboard_control_mode;
+
+	if (_offboard_control_mode_sub.copy(&offboard_control_mode)) {
+		bool offboard_available = offboard_control_mode.position || offboard_control_mode.velocity
+					  || offboard_control_mode.acceleration || offboard_control_mode.attitude || offboard_control_mode.body_rate
+					  || offboard_control_mode.actuator;
+
+		if (offboard_control_mode.position && reporter.failsafeFlags().local_position_invalid) {
+			offboard_available = false;
+
+		} else if (offboard_control_mode.velocity && reporter.failsafeFlags().local_velocity_invalid) {
+			offboard_available = false;
+
+		} else if (offboard_control_mode.acceleration && reporter.failsafeFlags().local_velocity_invalid) {
+			// OFFBOARD acceleration handled by position controller
+			offboard_available = false;
+		}
+
+		_offboard_available.set_state_and_update(offboard_available, hrt_absolute_time());
+
+		// This is a mode requirement, no need to report
+		reporter.failsafeFlags().offboard_control_signal_lost = !_offboard_available.get_state();
+	}
+}

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

@@ -0,0 +1,57 @@
+/****************************************************************************
+ *
+ *   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/offboard_control_mode.h>
+#include <lib/hysteresis/hysteresis.h>
+
+class OffboardChecks : public HealthAndArmingCheckBase
+{
+public:
+	OffboardChecks();
+	~OffboardChecks() = default;
+
+	void checkAndReport(const Context &context, Report &reporter) override;
+
+private:
+	uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)};
+	systemlib::Hysteresis _offboard_available{false};
+
+	DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
+					(ParamFloat<px4::params::COM_OF_LOSS_T>) _param_com_of_loss_t
+				       );
+};

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

@@ -0,0 +1,129 @@
+/****************************************************************************
+ *
+ *   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 "rcAndDataLinkCheck.hpp"
+
+using namespace time_literals;
+
+void RcAndDataLinkChecks::checkAndReport(const Context &context, Report &reporter)
+{
+	// RC
+	bool rc_is_optional = true;
+
+	if (_param_com_rc_in_mode.get() == 4) { // RC disabled
+		reporter.failsafeFlags().manual_control_signal_lost = false;
+
+	} else {
+
+		manual_control_setpoint_s manual_control_setpoint;
+
+		if (!_manual_control_setpoint_sub.copy(&manual_control_setpoint)) {
+			manual_control_setpoint = {};
+			reporter.failsafeFlags().manual_control_signal_lost = true;
+		}
+
+		// Check if RC is valid
+		if (!manual_control_setpoint.valid
+		    || hrt_elapsed_time(&manual_control_setpoint.timestamp) > _param_com_rc_loss_t.get() * 1_s) {
+
+			if (!reporter.failsafeFlags().manual_control_signal_lost && _last_valid_manual_control_setpoint > 0) {
+
+				events::send(events::ID("commander_rc_lost"), {events::Log::Critical, events::LogInternal::Info},
+					     "Manual control lost");
+			}
+
+			reporter.failsafeFlags().manual_control_signal_lost = true;
+
+		} else {
+			reporter.setIsPresent(health_component_t::remote_control);
+
+			if (reporter.failsafeFlags().manual_control_signal_lost && _last_valid_manual_control_setpoint > 0) {
+				float elapsed = hrt_elapsed_time(&_last_valid_manual_control_setpoint) * 1e-6f;
+				events::send<float>(events::ID("commander_rc_regained"), events::Log::Info,
+						    "Manual control regained after {1:.1} s", elapsed);
+			}
+
+			reporter.failsafeFlags().manual_control_signal_lost = false;
+			_last_valid_manual_control_setpoint = manual_control_setpoint.timestamp;
+		}
+
+
+		if (reporter.failsafeFlags().manual_control_signal_lost) {
+
+			NavModes affected_modes = rc_is_optional ? NavModes::None : NavModes::All;
+			events::LogLevel log_level = rc_is_optional ? events::Log::Info : events::Log::Error;
+			/* EVENT
+			 * @description
+			 * <profile name="dev">
+			 * This check can be configured via <param>COM_RC_IN_MODE</param> parameter.
+			 * </profile>
+			 */
+			reporter.armingCheckFailure(affected_modes, health_component_t::remote_control, events::ID("check_rc_dl_no_rc"),
+						    log_level, "No manual control input");
+
+			if (reporter.mavlink_log_pub()) {
+				mavlink_log_info(reporter.mavlink_log_pub(), "Preflight Fail: No manual control input\t");
+			}
+		}
+	}
+
+	// GCS connection
+	reporter.failsafeFlags().gcs_connection_lost = context.status().gcs_connection_lost;
+
+	if (reporter.failsafeFlags().gcs_connection_lost) {
+
+		// Prevent arming if we neither have RC nor a GCS connection. TODO: disabled for now due to MAVROS tests
+		bool gcs_connection_required =  _param_nav_dll_act.get() > 0
+						/*|| (rc_is_optional && reporter.failsafeFlags().manual_control_signal_lost) */;
+		NavModes affected_modes = gcs_connection_required ? NavModes::All : NavModes::None;
+		events::LogLevel log_level = gcs_connection_required ? events::Log::Error : events::Log::Info;
+		/* EVENT
+		 * @description
+		 * To arm, at least a data link or manual control (RC) must be present.
+		 *
+		 * <profile name="dev">
+		 * This check can be configured via <param>NAV_DLL_ACT</param> parameter.
+		 * </profile>
+		 */
+		reporter.armingCheckFailure(affected_modes, health_component_t::communication_links,
+					    events::ID("check_rc_dl_no_dllink"),
+					    log_level, "No connection to the ground control station");
+
+		if (reporter.mavlink_log_pub()) {
+			mavlink_log_warning(reporter.mavlink_log_pub(), "Preflight Fail: No connection to the ground control station\t");
+		}
+
+	} else {
+		reporter.setIsPresent(health_component_t::communication_links);
+	}
+}

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

@@ -0,0 +1,59 @@
+/****************************************************************************
+ *
+ *   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/manual_control_setpoint.h>
+
+class RcAndDataLinkChecks : public HealthAndArmingCheckBase
+{
+public:
+	RcAndDataLinkChecks() = default;
+	~RcAndDataLinkChecks() = default;
+
+	void checkAndReport(const Context &context, Report &reporter) override;
+
+private:
+	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
+
+	hrt_abstime	_last_valid_manual_control_setpoint{0};
+
+	DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
+					(ParamInt<px4::params::COM_RC_IN_MODE>) _param_com_rc_in_mode,
+					(ParamFloat<px4::params::COM_RC_LOSS_T>) _param_com_rc_loss_t,
+					(ParamInt<px4::params::NAV_DLL_ACT>) _param_nav_dll_act
+				       )
+};

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

@@ -62,13 +62,11 @@ RcCalibrationChecks::RcCalibrationChecks()
 		sprintf(nbuf, "RC%d_MAX", i + 1);
 		_param_handles[i].max = param_find(nbuf);
 
-		sprintf(nbuf, "RC%d_REV", i + 1);
-		_param_handles[i].rev = param_find(nbuf);
-
 		sprintf(nbuf, "RC%d_DZ", i + 1);
 		_param_handles[i].dz = param_find(nbuf);
 	}
 
+	updateParams();
 }
 
 void RcCalibrationChecks::checkAndReport(const Context &context, Report &reporter)
@@ -82,18 +80,10 @@ void RcCalibrationChecks::checkAndReport(const Context &context, Report &reporte
 	}
 
 	for (unsigned i = 0; i < input_rc_s::RC_INPUT_MAX_CHANNELS; i++) {
-		/* initialize values to values failing the check */
-		float param_min = 0.0f;
-		float param_max = 0.0f;
-		float param_trim = 0.0f;
-		float param_rev = 0.0f;
-		float param_dz = RC_INPUT_MAX_DEADZONE_US * 2.0f;
-
-		param_get(_param_handles[i].min, &param_min);
-		param_get(_param_handles[i].trim, &param_trim);
-		param_get(_param_handles[i].max, &param_max);
-		param_get(_param_handles[i].rev, &param_rev);
-		param_get(_param_handles[i].dz, &param_dz);
+		float param_min = _param_values[i].min;
+		float param_max = _param_values[i].max;
+		float param_trim = _param_values[i].trim;
+		float param_dz = _param_values[i].dz;
 
 		/* assert min..center..max ordering */
 		if (param_min < RC_INPUT_LOWEST_MIN_US) {
@@ -175,3 +165,21 @@ void RcCalibrationChecks::checkAndReport(const Context &context, Report &reporte
 		}
 	}
 }
+
+void RcCalibrationChecks::updateParams()
+{
+	HealthAndArmingCheckBase::updateParams();
+
+	for (unsigned i = 0; i < input_rc_s::RC_INPUT_MAX_CHANNELS; i++) {
+		/* initialize values to values failing the check */
+		_param_values[i].min = 0.0f;
+		_param_values[i].max = 0.0f;
+		_param_values[i].trim = 0.0f;
+		_param_values[i].dz = RC_INPUT_MAX_DEADZONE_US * 2.0f;
+
+		param_get(_param_handles[i].min, &_param_values[i].min);
+		param_get(_param_handles[i].trim, &_param_values[i].trim);
+		param_get(_param_handles[i].max, &_param_values[i].max);
+		param_get(_param_handles[i].dz, &_param_values[i].dz);
+	}
+}

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

@@ -47,14 +47,23 @@ public:
 	void checkAndReport(const Context &context, Report &reporter) override;
 
 private:
+	void updateParams() override;
+
 	struct ParamHandles {
 		param_t min;
 		param_t trim;
 		param_t max;
-		param_t rev;
 		param_t dz;
 	};
+	struct ParamValues {
+		float min;
+		float trim;
+		float max;
+		float dz;
+	};
+
 	ParamHandles _param_handles[input_rc_s::RC_INPUT_MAX_CHANNELS];
+	ParamValues _param_values[input_rc_s::RC_INPUT_MAX_CHANNELS];
 
 	DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
 					(ParamInt<px4::params::COM_RC_IN_MODE>) _param_com_rc_in_mode

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

@@ -72,29 +72,9 @@ void SystemChecks::checkAndReport(const Context &context, Report &reporter)
 		}
 	}
 
-	// Arm Requirements: mission
-	if (_param_com_arm_mis_req.get() && !context.isArmed()) {
-		if (!context.status().auto_mission_available) {
-			/* EVENT
-			 * @description
-			 * <profile name="dev">
-			 * This check can be configured via <param>COM_ARM_MIS_REQ</param> parameter.
-			 * </profile>
-			 */
-			reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_system_no_mission"),
-						    events::Log::Error, "No valid mission");
-
-			if (reporter.mavlink_log_pub()) {
-				mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: No valid mission");
-			}
-		}
-	}
-
-	reporter.failsafeFlags().auto_mission_available = context.status().auto_mission_available;
-
 	// Global position required
 	if (!_param_com_arm_wo_gps.get() && !context.isArmed()) {
-		if (!reporter.failsafeFlags().global_position_valid) {
+		if (reporter.failsafeFlags().global_position_invalid) {
 			/* EVENT
 			 * @description
 			 * <profile name="dev">
@@ -108,7 +88,7 @@ void SystemChecks::checkAndReport(const Context &context, Report &reporter)
 				mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Global position required");
 			}
 
-		} else if (!reporter.failsafeFlags().home_position_valid) {
+		} else if (reporter.failsafeFlags().home_position_invalid) {
 			/* EVENT
 			 * @description
 			 * <profile name="dev">
@@ -191,21 +171,6 @@ void SystemChecks::checkAndReport(const Context &context, Report &reporter)
 		}
 	}
 
-	if (_param_gf_action.get() != 0 && context.status().geofence_violated) {
-		/* 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_system_gf_violation"),
-					    events::Log::Error, "Vehicle outside geofence");
-
-		if (reporter.mavlink_log_pub()) {
-			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Vehicle outside geofence");
-		}
-	}
-
 	// Arm Requirements: authorization
 	if (_param_com_arm_auth_req.get() != 0 && !context.isArmed()) {
 		if (arm_auth_check() != vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED) {
@@ -215,8 +180,4 @@ void SystemChecks::checkAndReport(const Context &context, Report &reporter)
 						    events::Log::Error, "Arm authorization denied");
 		}
 	}
-
-	if (reporter.failsafeFlags().rc_signal_found_once) {
-		reporter.setIsPresent(health_component_t::remote_control);
-	}
 }

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

@@ -52,7 +52,6 @@ private:
 	DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
 					(ParamInt<px4::params::CBRK_VTOLARMING>) _param_cbrk_vtolarming,
 					(ParamInt<px4::params::CBRK_USB_CHK>) _param_cbrk_usb_chk,
-					(ParamBool<px4::params::COM_ARM_MIS_REQ>) _param_com_arm_mis_req,
 					(ParamBool<px4::params::COM_ARM_WO_GPS>) _param_com_arm_wo_gps,
 					(ParamInt<px4::params::COM_ARM_AUTH_REQ>) _param_com_arm_auth_req,
 					(ParamInt<px4::params::GF_ACTION>) _param_gf_action

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

@@ -0,0 +1,56 @@
+/****************************************************************************
+ *
+ *   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 "vtolCheck.hpp"
+
+using namespace time_literals;
+
+void VtolChecks::checkAndReport(const Context &context, Report &reporter)
+{
+	vtol_vehicle_status_s vtol_vehicle_status;
+
+	if (_vtol_vehicle_status_sub.copy(&vtol_vehicle_status)) {
+		reporter.failsafeFlags().vtol_transition_failure = vtol_vehicle_status.vtol_transition_failsafe;
+
+		if (reporter.failsafeFlags().vtol_transition_failure) {
+			/* EVENT
+			 */
+			reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_vtol_transition_failure"),
+						    events::Log::Error, "VTOL transition failure");
+
+			if (reporter.mavlink_log_pub()) {
+				mavlink_log_info(reporter.mavlink_log_pub(), "Preflight Fail: VTOL transition failure\t");
+			}
+		}
+	}
+}

src/modules/commander/HealthAndArmingChecks/checks/vtolCheck.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/vtol_vehicle_status.h>
+
+class VtolChecks : public HealthAndArmingCheckBase
+{
+public:
+	VtolChecks() = default;
+	~VtolChecks() = default;
+
+	void checkAndReport(const Context &context, Report &reporter) override;
+
+private:
+	uORB::Subscription _vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)};
+};

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

@@ -0,0 +1,79 @@
+/****************************************************************************
+ *
+ *   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 "windCheck.hpp"
+
+void WindChecks::checkAndReport(const Context &context, Report &reporter)
+{
+	if (_param_com_wind_warn.get() < FLT_EPSILON && _param_com_wind_max.get() < FLT_EPSILON) {
+		reporter.failsafeFlags().wind_limit_exceeded = false;
+		return;
+	}
+
+	wind_s wind_estimate;
+	const hrt_abstime now = hrt_absolute_time();
+
+	if (_wind_sub.copy(&wind_estimate)) {
+		const matrix::Vector2f wind(wind_estimate.windspeed_north, wind_estimate.windspeed_east);
+
+		// publish a warning if it's the first since in air or 60s have passed since the last warning
+		const bool warning_timeout_passed = _last_wind_warning == 0 || now - _last_wind_warning > 60_s;
+
+		reporter.failsafeFlags().wind_limit_exceeded = _param_com_wind_max.get() > FLT_EPSILON
+				&& wind.longerThan(_param_com_wind_max.get());
+
+		if (reporter.failsafeFlags().wind_limit_exceeded) {
+
+			/* EVENT
+			 * @description
+			 * <profile name="dev">
+			 * This check can be configured via <param>COM_WIND_MAX</param> parameter.
+			 * </profile>
+			 */
+			reporter.armingCheckFailure<float>(NavModes::All, health_component_t::system,
+							   events::ID("check_wind_too_high"),
+							   events::Log::Warning, "Wind speed is above limit ({1:.1m/s})", wind.norm());
+
+		} else if (_param_com_wind_warn.get() > FLT_EPSILON
+			   && wind.longerThan(_param_com_wind_warn.get())
+			   && warning_timeout_passed
+			   && context.status().nav_state != vehicle_status_s::NAVIGATION_STATE_AUTO_RTL
+			   && context.status().nav_state != vehicle_status_s::NAVIGATION_STATE_AUTO_LAND) {
+
+			events::send<float>(events::ID("check_high_wind_warning"),
+			{events::Log::Warning, events::LogInternal::Info},
+			"High wind speed detected ({1:.1m/s}), landing advised", wind.norm());
+			_last_wind_warning = now;
+		}
+	}
+}

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

@@ -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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "../Common.hpp"
+
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/wind.h>
+
+class WindChecks : public HealthAndArmingCheckBase
+{
+public:
+	WindChecks() = default;
+	~WindChecks() = default;
+
+	void checkAndReport(const Context &context, Report &reporter) override;
+
+private:
+	uORB::Subscription _wind_sub{ORB_ID(wind)};
+	hrt_abstime _last_wind_warning{0};
+
+	DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
+					(ParamFloat<px4::params::COM_WIND_MAX>) _param_com_wind_max,
+					(ParamFloat<px4::params::COM_WIND_WARN>) _param_com_wind_warn
+
+				       )
+};

src/modules/commander/HomePosition.cpp

@@ -37,8 +37,8 @@
 #include <lib/geo/geo.h>
 #include "commander_helper.h"
 
-HomePosition::HomePosition(const vehicle_status_flags_s &vehicle_status_flags)
-	: _vehicle_status_flags(vehicle_status_flags)
+HomePosition::HomePosition(const failsafe_flags_s &failsafe_flags)
+	: _failsafe_flags(failsafe_flags)
 {
 }
 
@@ -69,7 +69,7 @@ bool HomePosition::hasMovedFromCurrentHomeLocation()
 			eph = gpos.eph;
 			epv = gpos.epv;
 
-		} else if (_vehicle_status_flags.gps_position_valid) {
+		} else if (!_failsafe_flags.gps_position_invalid) {
 			sensor_gps_s gps;
 			_vehicle_gps_position_sub.copy(&gps);
 			const double lat = static_cast<double>(gps.lat) * 1e-7;
@@ -98,7 +98,7 @@ bool HomePosition::setHomePosition(bool force)
 	bool updated = false;
 	home_position_s home{};
 
-	if (_vehicle_status_flags.local_position_valid) {
+	if (!_failsafe_flags.local_position_invalid) {
 		// Set home position in local coordinates
 		const vehicle_local_position_s &lpos = _local_position_sub.get();
 		_heading_reset_counter = lpos.heading_reset_counter; // TODO: should not be here
@@ -107,14 +107,14 @@ bool HomePosition::setHomePosition(bool force)
 		updated = true;
 	}
 
-	if (_vehicle_status_flags.global_position_valid) {
+	if (!_failsafe_flags.global_position_invalid) {
 		// Set home using the global position estimate (fused INS/GNSS)
 		const vehicle_global_position_s &gpos = _global_position_sub.get();
 		fillGlobalHomePos(home, gpos);
 		setHomePosValid();
 		updated = true;
 
-	} else if (_vehicle_status_flags.gps_position_valid) {
+	} else if (!_failsafe_flags.gps_position_invalid) {
 		// Set home using GNSS position
 		sensor_gps_s gps_pos;
 		_vehicle_gps_position_sub.copy(&gps_pos);
@@ -184,7 +184,7 @@ void HomePosition::setInAirHomePosition()
 	const bool local_home_valid = home.valid_lpos;
 
 	if (local_home_valid && !global_home_valid) {
-		if (_vehicle_status_flags.local_position_valid && _vehicle_status_flags.global_position_valid) {
+		if (!_failsafe_flags.local_position_invalid && !_failsafe_flags.global_position_invalid) {
 			// Back-compute lon, lat and alt of home position given the local home position
 			// and current positions in local and global (GNSS fused) frames
 			const vehicle_local_position_s &lpos = _local_position_sub.get();
@@ -203,7 +203,7 @@ void HomePosition::setInAirHomePosition()
 			home.timestamp = hrt_absolute_time();
 			_home_position_pub.update();
 
-		} else if (_vehicle_status_flags.local_position_valid && _vehicle_status_flags.gps_position_valid) {
+		} else if (!_failsafe_flags.local_position_invalid && !_failsafe_flags.gps_position_invalid) {
 			// Back-compute lon, lat and alt of home position given the local home position
 			// and current positions in local and global (GNSS raw) frames
 			const vehicle_local_position_s &lpos = _local_position_sub.get();
@@ -231,7 +231,7 @@ void HomePosition::setInAirHomePosition()
 	} else if (!local_home_valid && global_home_valid) {
 		const vehicle_local_position_s &lpos = _local_position_sub.get();
 
-		if (_vehicle_status_flags.local_position_valid && lpos.xy_global && lpos.z_global) {
+		if (!_failsafe_flags.local_position_invalid && lpos.xy_global && lpos.z_global) {
 			// Back-compute x, y and z of home position given the global home position
 			// and the global reference of the local frame
 			MapProjection ref_pos{lpos.ref_lat, lpos.ref_lon};
@@ -326,9 +326,9 @@ void HomePosition::update(bool set_automatically, bool check_if_changed)
 	}
 
 	if (check_if_changed && set_automatically) {
-		const bool can_set_home_lpos_first_time = !home.valid_lpos && _vehicle_status_flags.local_position_valid;
+		const bool can_set_home_lpos_first_time = !home.valid_lpos && !_failsafe_flags.local_position_invalid;
 		const bool can_set_home_gpos_first_time = ((!home.valid_hpos || !home.valid_alt)
-				&& (_vehicle_status_flags.global_position_valid || _vehicle_status_flags.gps_position_valid));
+				&& (!_failsafe_flags.global_position_invalid || !_failsafe_flags.gps_position_invalid));
 		const bool can_set_home_alt_first_time = (!home.valid_alt && lpos.z_global);
 
 		if (can_set_home_lpos_first_time

src/modules/commander/HomePosition.hpp

@@ -39,12 +39,12 @@
 #include <uORB/topics/sensor_gps.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_local_position.h>
-#include <uORB/topics/vehicle_status_flags.h>
+#include <uORB/topics/failsafe_flags.h>
 
 class HomePosition
 {
 public:
-	HomePosition(const vehicle_status_flags_s &vehicle_status_flags);
+	HomePosition(const failsafe_flags_s &failsafe_flags);
 	~HomePosition() = default;
 
 	bool setHomePosition(bool force = false);
@@ -74,5 +74,5 @@ private:
 
 	uint8_t							_heading_reset_counter{0};
 	bool							_valid{false};
-	const vehicle_status_flags_s				&_vehicle_status_flags;
+	const failsafe_flags_s					&_failsafe_flags;
 };

src/modules/commander/ModeUtil/CMakeLists.txt

@@ -31,7 +31,9 @@
 #
 ############################################################################
 
-px4_add_library(mode_util
+add_library(mode_util
 	control_mode.cpp
 	mode_requirements.cpp
 )
+add_dependencies(mode_util uorb_headers prebuild_targets)
+

src/modules/commander/ModeUtil/conversions.hpp

@@ -0,0 +1,110 @@
+/****************************************************************************
+ *
+ *   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 <px4_platform_common/events.h>
+#include <uORB/topics/vehicle_status.h>
+
+#include <stdint.h>
+
+using navigation_mode_t = events::px4::enums::navigation_mode_t;
+
+namespace mode_util
+{
+
+static inline navigation_mode_t navigation_mode(uint8_t nav_state)
+{
+	switch (nav_state) {
+	case vehicle_status_s::NAVIGATION_STATE_MANUAL: return navigation_mode_t::manual;
+
+	case vehicle_status_s::NAVIGATION_STATE_ALTCTL: return navigation_mode_t::altctl;
+
+	case vehicle_status_s::NAVIGATION_STATE_POSCTL: return navigation_mode_t::posctl;
+
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: return navigation_mode_t::auto_mission;
+
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: return navigation_mode_t::auto_loiter;
+
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: return navigation_mode_t::auto_rtl;
+
+	case vehicle_status_s::NAVIGATION_STATE_ACRO: return navigation_mode_t::acro;
+
+	case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: return navigation_mode_t::offboard;
+
+	case vehicle_status_s::NAVIGATION_STATE_STAB: return navigation_mode_t::stab;
+
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF: return navigation_mode_t::auto_takeoff;
+
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND: return navigation_mode_t::auto_land;
+
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET: return navigation_mode_t::auto_follow_target;
+
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND: return navigation_mode_t::auto_precland;
+
+	case vehicle_status_s::NAVIGATION_STATE_ORBIT: return navigation_mode_t::orbit;
+
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF: return navigation_mode_t::auto_vtol_takeoff;
+	}
+
+	static_assert(vehicle_status_s::NAVIGATION_STATE_MAX  == 23, "code requires update");
+
+	return navigation_mode_t::unknown;
+}
+
+const char *const nav_state_names[vehicle_status_s::NAVIGATION_STATE_MAX] = {
+	"MANUAL",
+	"ALTCTL",
+	"POSCTL",
+	"AUTO_MISSION",
+	"AUTO_LOITER",
+	"AUTO_RTL",
+	"6: unallocated",
+	"7: unallocated",
+	"AUTO_LANDENGFAIL",
+	"9: unallocated",
+	"ACRO",
+	"11: UNUSED",
+	"DESCEND",
+	"TERMINATION",
+	"OFFBOARD",
+	"STAB",
+	"16: UNUSED2",
+	"AUTO_TAKEOFF",
+	"AUTO_LAND",
+	"AUTO_FOLLOW_TARGET",
+	"AUTO_PRECLAND",
+	"ORBIT"
+};
+
+} // namespace mode_util

src/modules/commander/ModeUtil/mode_requirements.cpp

@@ -43,7 +43,7 @@ static inline void setRequirement(uint8_t nav_state, uint32_t &mode_requirement)
 }
 
 
-void getModeRequirements(uint8_t vehicle_type, vehicle_status_flags_s &flags)
+void getModeRequirements(uint8_t vehicle_type, failsafe_flags_s &flags)
 {
 	flags.mode_req_angular_velocity = 0;
 	flags.mode_req_attitude = 0;
@@ -130,6 +130,7 @@ void getModeRequirements(uint8_t vehicle_type, vehicle_status_flags_s &flags)
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LAND, flags.mode_req_angular_velocity);
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LAND, flags.mode_req_attitude);
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LAND, flags.mode_req_local_alt);
+	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LAND, flags.mode_req_local_position);
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LAND, flags.mode_req_prevent_arming);
 
 	// NAVIGATION_STATE_AUTO_FOLLOW_TARGET

src/modules/commander/ModeUtil/mode_requirements.hpp

@@ -33,7 +33,7 @@
 
 #pragma once
 
-#include <uORB/topics/vehicle_status_flags.h>
+#include <uORB/topics/failsafe_flags.h>
 
 namespace mode_util
 {
@@ -43,7 +43,7 @@ namespace mode_util
  * @param vehicle_type one of vehicle_status_s::VEHICLE_TYPE_*
  * @param flags output flags, all mode_req_* entries are set
  */
-void getModeRequirements(uint8_t vehicle_type, vehicle_status_flags_s &flags);
+void getModeRequirements(uint8_t vehicle_type, failsafe_flags_s &flags);
 
 
 } // namespace mode_util

src/modules/commander/UserModeIntention.cpp

@@ -0,0 +1,83 @@
+/****************************************************************************
+ *
+ *   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 "UserModeIntention.hpp"
+
+UserModeIntention::UserModeIntention(ModuleParams *parent, const vehicle_status_s &vehicle_status,
+				     const HealthAndArmingChecks &health_and_arming_checks)
+	: ModuleParams(parent), _vehicle_status(vehicle_status), _health_and_arming_checks(health_and_arming_checks)
+{
+}
+
+bool UserModeIntention::change(uint8_t user_intended_nav_state, bool allow_fallback, bool force)
+{
+	_ever_had_mode_change = true;
+	_had_mode_change = true;
+
+	if (_user_intented_nav_state == user_intended_nav_state) {
+		return true;
+	}
+
+	// Always allow mode change while disarmed
+	bool always_allow = force || !isArmed();
+	bool allow_change = true;
+
+	if (!always_allow) {
+		allow_change = _health_and_arming_checks.canRun(user_intended_nav_state);
+
+		// Check fallback
+		if (!allow_change && allow_fallback && _param_com_posctl_navl.get() == 0) {
+			if (user_intended_nav_state == vehicle_status_s::NAVIGATION_STATE_POSCTL) {
+				allow_change = _health_and_arming_checks.canRun(vehicle_status_s::NAVIGATION_STATE_ALTCTL);
+				// We still use the original user intended mode. The failsafe state machine will then set the
+				// fallback and once can_run becomes true, the actual user intended mode will be selected.
+			}
+		}
+	}
+
+	if (allow_change) {
+		_user_intented_nav_state = user_intended_nav_state;
+
+		if (!_health_and_arming_checks.modePreventsArming(user_intended_nav_state)) {
+			_nav_state_after_disarming = user_intended_nav_state;
+		}
+	}
+
+	return allow_change;
+}
+
+void UserModeIntention::onDisarm()
+{
+	_user_intented_nav_state = _nav_state_after_disarming;
+}

src/modules/commander/UserModeIntention.hpp

@@ -0,0 +1,85 @@
+/****************************************************************************
+ *
+ *   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 <uORB/topics/vehicle_status.h>
+#include "HealthAndArmingChecks/HealthAndArmingChecks.hpp"
+#include <px4_platform_common/module_params.h>
+
+class UserModeIntention : ModuleParams
+{
+public:
+	UserModeIntention(ModuleParams *parent, const vehicle_status_s &vehicle_status,
+			  const HealthAndArmingChecks &health_and_arming_checks);
+	~UserModeIntention() = default;
+
+	/**
+	 * Change the user intended mode
+	 * @param user_intended_nav_state new mode
+	 * @param allow_fallback allow to fallback to a lower mode if current mode cannot run
+	 * @param force always set if true
+	 * @return true if successfully set (also if unchanged)
+	 */
+	bool change(uint8_t user_intended_nav_state, bool allow_fallback = false, bool force = false);
+
+	uint8_t get() const { return _user_intented_nav_state; }
+
+	/**
+	 * Change the user intention to the last user intended mode where arming is possible
+	 */
+	void onDisarm();
+
+	/**
+	 * Returns false if there has not been any mode change yet
+	 */
+	bool everHadModeChange() const { return _ever_had_mode_change; }
+
+	bool getHadModeChangeAndClear() { bool ret = _had_mode_change; _had_mode_change = false; return ret; }
+
+private:
+	bool isArmed() const { return _vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED; }
+
+	const vehicle_status_s &_vehicle_status;
+	const HealthAndArmingChecks &_health_and_arming_checks;
+
+	uint8_t _user_intented_nav_state{vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER}; ///< Current user intended mode
+	uint8_t _nav_state_after_disarming{vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER}; ///< Mode that is switched into after landing/disarming
+
+	bool _ever_had_mode_change{false}; ///< true if there was ever a mode change call (also if the same mode as already set)
+	bool _had_mode_change{false}; ///< true if there was a mode change call since the last getHadModeChangeAndClear()
+
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::COM_POSCTL_NAVL>) _param_com_posctl_navl
+	);
+};

src/modules/commander/commander_params.c

@@ -90,9 +90,9 @@ PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f);
 PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f);
 
 /**
- * Datalink loss time threshold
+ * GCS connection loss time threshold
  *
- * After this amount of seconds without datalink the data link lost mode triggers
+ * After this amount of seconds without datalink, the GCS connection lost mode triggers
  *
  * @group Commander
  * @unit s
@@ -283,13 +283,14 @@ PARAM_DEFINE_INT32(COM_ARM_SWISBTN, 0);
 PARAM_DEFINE_INT32(COM_LOW_BAT_ACT, 0);
 
 /**
- * Delay between battery state change and failsafe reaction
+ * Delay between failsafe condition triggered and failsafe reaction
  *
- * Battery state requires action -> wait COM_BAT_ACT_T seconds in Hold mode
- * for the user to realize and take a custom action
- * -> React with failsafe action COM_LOW_BAT_ACT
+ * Before entering failsafe (RTL, Land, Hold), wait COM_FAIL_ACT_T seconds in Hold mode
+ * for the user to realize.
+ * During that time the user cannot take over control.
+ * Afterwards the configured failsafe action is triggered and the user may take over.
  *
- * A zero value disables the delay.
+ * A zero value disables the delay and the user cannot take over via stick movements (switching modes is still allowed).
  *
  * @group Commander
  * @unit s
@@ -297,7 +298,7 @@ PARAM_DEFINE_INT32(COM_LOW_BAT_ACT, 0);
  * @max 25.0
  * @decimal 3
  */
-PARAM_DEFINE_FLOAT(COM_BAT_ACT_T, 5.f);
+PARAM_DEFINE_FLOAT(COM_FAIL_ACT_T, 5.f);
 
 /**
  * Imbalanced propeller failsafe mode
@@ -319,7 +320,7 @@ PARAM_DEFINE_INT32(COM_IMB_PROP_ACT, 0);
 /**
  * Time-out to wait when offboard connection is lost before triggering offboard lost action.
  *
- * See COM_OBL_ACT and COM_OBL_RC_ACT to configure action.
+ * See COM_OBL_RC_ACT to configure action.
  *
  * @group Commander
  * @unit s
@@ -329,27 +330,10 @@ PARAM_DEFINE_INT32(COM_IMB_PROP_ACT, 0);
  */
 PARAM_DEFINE_FLOAT(COM_OF_LOSS_T, 1.0f);
 
-/**
- * Set offboard loss failsafe mode
- *
- * The offboard loss failsafe will only be entered after a timeout,
- * set by COM_OF_LOSS_T in seconds.
- *
- * @value -1 Disabled
- * @value  0 Land mode
- * @value  1 Hold mode
- * @value  2 Return mode
- * @value  3 Terminate
- * @value  4 Lockdown
- *
- * @group Commander
- */
-PARAM_DEFINE_INT32(COM_OBL_ACT, 0);
-
 /**
  * Set command after a quadchute
  *
- * @value -1 No action: stay in current flight mode
+ * @value -1 Warning only
  * @value  0 Return mode
  * @value  1 Land mode
  * @value  2 Hold mode
@@ -358,12 +342,11 @@ PARAM_DEFINE_INT32(COM_OBL_ACT, 0);
 PARAM_DEFINE_INT32(COM_QC_ACT, 0);
 
 /**
- * Set offboard loss failsafe mode when RC is available
+ * Set offboard loss failsafe mode
  *
  * The offboard loss failsafe will only be entered after a timeout,
  * set by COM_OF_LOSS_T in seconds.
  *
- * @value -1 Disabled
  * @value  0 Position mode
  * @value  1 Altitude mode
  * @value  2 Manual
@@ -371,7 +354,7 @@ PARAM_DEFINE_INT32(COM_QC_ACT, 0);
  * @value  4 Land mode
  * @value  5 Hold mode
  * @value  6 Terminate
- * @value  7 Lockdown
+ * @value  7 Disarm
  * @group Commander
  */
 PARAM_DEFINE_INT32(COM_OBL_RC_ACT, 0);
@@ -676,10 +659,10 @@ PARAM_DEFINE_INT32(COM_ARM_MIS_REQ, 0);
  *
  * If Altitude/Manual is selected: assume use of remote control after fallback. Switch to Altitude mode if a height estimate is available, else switch to MANUAL.
  *
- * If Land/Terminate is selected: assume no use of remote control after fallback. Switch to Land mode if a height estimate is available, else switch to TERMINATION.
+ * If Land/Descend is selected: assume no use of remote control after fallback. Switch to Land mode if a height estimate is available, else switch to Descend.
  *
  * @value 0 Altitude/Manual
- * @value 1 Land/Terminate
+ * @value 1 Land/Descend
  *
  * @group Commander
  */
@@ -802,9 +785,9 @@ PARAM_DEFINE_INT32(COM_FLIGHT_UUID, 0);
 PARAM_DEFINE_INT32(COM_TAKEOFF_ACT, 0);
 
 /**
- * Set data link loss failsafe mode
+ * Set GCS connection loss failsafe mode
  *
- * The data link loss failsafe will only be entered after a timeout,
+ * The GCS connection loss failsafe will only be entered after a timeout,
  * set by COM_DL_LOSS_T in seconds. Once the timeout occurs the selected
  * action will be executed.
  *
@@ -813,7 +796,7 @@ PARAM_DEFINE_INT32(COM_TAKEOFF_ACT, 0);
  * @value 2 Return mode
  * @value 3 Land mode
  * @value 5 Terminate
- * @value 6 Lockdown
+ * @value 6 Disarm
  * @min 0
  * @max 6
  *
@@ -832,7 +815,7 @@ PARAM_DEFINE_INT32(NAV_DLL_ACT, 0);
  * @value 2 Return mode
  * @value 3 Land mode
  * @value 5 Terminate
- * @value 6 Lockdown
+ * @value 6 Disarm
  * @min 1
  * @max 6
  *
@@ -862,7 +845,7 @@ PARAM_DEFINE_INT32(COM_RCL_EXCEPT, 0);
  *
  * @min 0
  * @max 3
- * @value 0 Disabled
+ * @value 0 Warning only
  * @value 1 Hold mode
  * @value 2 Land mode
  * @value 3 Return mode
@@ -988,8 +971,8 @@ PARAM_DEFINE_INT32(COM_POWER_COUNT, 1);
 /**
  * Timeout for detecting a failure after takeoff
  *
- * A non-zero, positive value specifies the timeframe in seconds within failure detector is allowed to put the vehicle into
- * a lockdown state if attitude exceeds the limits defined in FD_FAIL_P and FD_FAIL_R.
+ * A non-zero, positive value specifies the timeframe in seconds within failure detector is allowed to disarm the vehicle
+ * if attitude exceeds the limits defined in FD_FAIL_P and FD_FAIL_R.
  * The check is not executed for flight modes that do support acrobatic maneuvers, e.g: Acro (MC/FW) and Manual (FW).
  * A zero or negative value means that the check is disabled.
  *

src/modules/commander/commander_tests/CMakeLists.txt

@@ -1,41 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2015 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.
-#
-############################################################################
-px4_add_module(
-	MODULE modules__commander__commander_tests
-	MAIN commander_tests
-	SRCS
-		commander_tests.cpp
-		state_machine_helper_test.cpp
-		../state_machine_helper.cpp
-	DEPENDS
-	)

src/modules/commander/commander_tests/state_machine_helper_test.cpp

@@ -1,283 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *   Author: Simon Wilks <sjwilks@gmail.com>
- *
- * 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.
- *
- ****************************************************************************/
-
-/**
- * @file state_machine_helper_test.cpp
- * System state machine unit test.
- *
- */
-
-#include "state_machine_helper_test.h"
-
-#include "../state_machine_helper.h"
-#include <unit_test.h>
-#include "../Arming/ArmStateMachine/ArmStateMachine.hpp"
-
-class StateMachineHelperTest : public UnitTest
-{
-public:
-	StateMachineHelperTest() = default;
-	~StateMachineHelperTest() override = default;
-
-	bool run_tests() override;
-
-private:
-	bool mainStateTransitionTest();
-};
-
-bool StateMachineHelperTest::mainStateTransitionTest()
-{
-	// This structure represent a single test case for testing Main State transitions.
-	typedef struct {
-		const char *assertMsg;				// Text to show when test case fails
-		uint8_t	 condition_bits;			// Bits for various condition_* values
-		uint8_t	from_state;				// State prior to transition request
-		main_state_t to_state;				// State to transition to
-		transition_result_t	expected_transition_result;	// Expected result from main_state_transition call
-	} MainTransitionTest_t;
-
-	// Bits for condition_bits
-#define MTT_ALL_NOT_VALID		0
-#define MTT_ROTARY_WING			1 << 0
-#define MTT_LOC_ALT_VALID		1 << 1
-#define MTT_LOC_POS_VALID		1 << 2
-#define MTT_HOME_POS_VALID		1 << 3
-#define MTT_GLOBAL_POS_VALID		1 << 4
-
-	static const MainTransitionTest_t rgMainTransitionTests[] = {
-
-		// TRANSITION_NOT_CHANGED tests
-
-		{
-			"no transition: identical states",
-			MTT_ALL_NOT_VALID,
-			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_NOT_CHANGED
-		},
-
-		// TRANSITION_CHANGED tests
-
-		{
-			"transition: MANUAL to ACRO - rotary",
-			MTT_ROTARY_WING,
-			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ACRO, TRANSITION_CHANGED
-		},
-
-		{
-			"transition: MANUAL to ACRO - not rotary",
-			MTT_ALL_NOT_VALID,
-			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ACRO, TRANSITION_CHANGED
-		},
-
-		{
-			"transition: ACRO to MANUAL",
-			MTT_ALL_NOT_VALID,
-			commander_state_s::MAIN_STATE_ACRO, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED
-		},
-
-		{
-			"transition: MANUAL to AUTO_MISSION - global position valid, home position valid",
-			MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
-			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_MISSION, TRANSITION_CHANGED
-		},
-
-		{
-			"transition: AUTO_MISSION to MANUAL - global position valid, home position valid",
-			MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
-			commander_state_s::MAIN_STATE_AUTO_MISSION, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED
-		},
-
-		{
-			"transition: MANUAL to AUTO_LOITER - global position valid",
-			MTT_GLOBAL_POS_VALID,
-			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_LOITER, TRANSITION_CHANGED
-		},
-
-		{
-			"transition: AUTO_LOITER to MANUAL - global position valid",
-			MTT_GLOBAL_POS_VALID,
-			commander_state_s::MAIN_STATE_AUTO_LOITER, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED
-		},
-
-		{
-			"transition: MANUAL to AUTO_RTL - global position valid, home position valid",
-			MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
-			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_RTL, TRANSITION_CHANGED
-		},
-
-		{
-			"transition: AUTO_RTL to MANUAL - global position valid, home position valid",
-			MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
-			commander_state_s::MAIN_STATE_AUTO_RTL, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED
-		},
-
-		{
-			"transition: MANUAL to ALTCTL - not rotary",
-			MTT_LOC_ALT_VALID,
-			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED
-		},
-
-		{
-			"transition: MANUAL to ALTCTL - rotary, global position not valid, local altitude valid",
-			MTT_ROTARY_WING | MTT_LOC_ALT_VALID,
-			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED
-		},
-
-		{
-			"transition: MANUAL to ALTCTL - rotary, global position valid, local altitude not valid",
-			MTT_ROTARY_WING | MTT_GLOBAL_POS_VALID,
-			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED
-		},
-
-		{
-			"transition: ALTCTL to MANUAL - local altitude valid",
-			MTT_LOC_ALT_VALID,
-			commander_state_s::MAIN_STATE_ALTCTL, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED
-		},
-
-		{
-			"transition: MANUAL to POSCTL - local position not valid, global position valid",
-			MTT_GLOBAL_POS_VALID,
-			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_POSCTL, TRANSITION_CHANGED
-		},
-
-		{
-			"transition: MANUAL to POSCTL - local position valid, global position not valid",
-			MTT_LOC_POS_VALID,
-			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_POSCTL, TRANSITION_CHANGED
-		},
-
-		{
-			"transition: POSCTL to MANUAL - local position valid, global position valid",
-			MTT_LOC_POS_VALID,
-			commander_state_s::MAIN_STATE_POSCTL, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED
-		},
-
-		// TRANSITION_DENIED tests
-
-		{
-			"no transition: MANUAL to AUTO_MISSION - global position not valid",
-			MTT_ALL_NOT_VALID,
-			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_MISSION, TRANSITION_DENIED
-		},
-
-		{
-			"no transition: MANUAL to AUTO_LOITER - global position not valid",
-			MTT_ALL_NOT_VALID,
-			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_LOITER, TRANSITION_DENIED
-		},
-
-		{
-			"no transition: MANUAL to AUTO_RTL - global position not valid, home position not valid",
-			MTT_ALL_NOT_VALID,
-			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED
-		},
-
-		{
-			"no transition: MANUAL to AUTO_RTL - global position not valid, home position valid",
-			MTT_HOME_POS_VALID,
-			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED
-		},
-
-		{
-			"no transition: MANUAL to AUTO_RTL - global position valid, home position not valid",
-			MTT_GLOBAL_POS_VALID,
-			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED
-		},
-
-		{
-			"transition: MANUAL to ALTCTL - not rotary",
-			MTT_ALL_NOT_VALID,
-			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ALTCTL, TRANSITION_DENIED
-		},
-
-		{
-			"no transition: MANUAL to ALTCTL - rotary, global position not valid, local altitude not valid",
-			MTT_ROTARY_WING,
-			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ALTCTL, TRANSITION_DENIED
-		},
-
-		{
-			"no transition: MANUAL to POSCTL - local position not valid, global position not valid",
-			MTT_ALL_NOT_VALID,
-			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_POSCTL, TRANSITION_DENIED
-		},
-	};
-
-	size_t cMainTransitionTests = sizeof(rgMainTransitionTests) / sizeof(rgMainTransitionTests[0]);
-
-	for (size_t i = 0; i < cMainTransitionTests; i++) {
-		const MainTransitionTest_t *test = &rgMainTransitionTests[i];
-
-		// Setup initial machine state
-		struct vehicle_status_s current_vehicle_status = {};
-		struct commander_state_s current_commander_state = {};
-		struct vehicle_status_flags_s current_status_flags = {};
-
-		current_commander_state.main_state = test->from_state;
-		current_vehicle_status.vehicle_type = (test->condition_bits & MTT_ROTARY_WING) ?
-						      vehicle_status_s::VEHICLE_TYPE_ROTARY_WING : vehicle_status_s::VEHICLE_TYPE_FIXED_WING;
-		current_status_flags.local_altitude_valid = test->condition_bits & MTT_LOC_ALT_VALID;
-		current_status_flags.local_position_valid = test->condition_bits & MTT_LOC_POS_VALID;
-		current_status_flags.home_position_valid = test->condition_bits & MTT_HOME_POS_VALID;
-		current_status_flags.global_position_valid = test->condition_bits & MTT_GLOBAL_POS_VALID;
-		current_status_flags.auto_mission_available = true;
-
-		// Attempt transition
-		transition_result_t result = main_state_transition(current_vehicle_status, test->to_state, current_status_flags,
-					     current_commander_state);
-
-		// Validate result of transition
-		ut_compare(test->assertMsg, test->expected_transition_result, result);
-
-		if (test->expected_transition_result == result) {
-			if (test->expected_transition_result == TRANSITION_CHANGED) {
-				ut_compare(test->assertMsg, test->to_state, current_commander_state.main_state);
-
-			} else {
-				ut_compare(test->assertMsg, test->from_state, current_commander_state.main_state);
-			}
-		}
-	}
-
-	return true;
-}
-
-bool StateMachineHelperTest::run_tests()
-{
-	ut_run_test(mainStateTransitionTest);
-
-	return (_tests_failed == 0);
-}
-
-ut_declare_test(stateMachineHelperTest, StateMachineHelperTest)

src/modules/commander/failsafe/CMakeLists.txt

@@ -0,0 +1,82 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+# Extract information from failsafe msg file
+set(failsafe_flags_msg_file ${PX4_SOURCE_DIR}/msg/failsafe_flags.msg)
+set(generated_uorb_struct_field_mapping_header ${PX4_BINARY_DIR}/generated_uorb_struct_field_mapping.h)
+set(html_template_file ${CMAKE_CURRENT_SOURCE_DIR}/emscripten_template.html)
+set(html_output_file ${PX4_BINARY_DIR}/failsafe_html_template.html)
+add_custom_command(OUTPUT ${generated_uorb_struct_field_mapping_header} ${html_output_file}
+	COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/parse_flags_from_msg.py
+		${failsafe_flags_msg_file} ${generated_uorb_struct_field_mapping_header}
+		${html_template_file} ${html_output_file}
+	DEPENDS
+		${failsafe_flags_msg_file}
+		${html_template_file}
+		${CMAKE_CURRENT_SOURCE_DIR}/parse_flags_from_msg.py
+		COMMENT "Extracting info from failsafe flags msg file"
+)
+add_custom_target(failsafe_uorb_struct_header DEPENDS ${generated_uorb_struct_field_mapping_header})
+
+# Webassembly failsafe testing (expects CMAKE_CXX_COMPILER = "em++")
+add_executable(failsafe_test EXCLUDE_FROM_ALL
+	emscripten.cpp
+	failsafe.cpp
+	framework.cpp
+	)
+set_property(TARGET failsafe_test PROPERTY UNITY_BUILD ON) # avoids some method calls to e.g. param_name or param_type
+set_property(TARGET failsafe_test PROPERTY LINK_DEPENDS ${html_output_file})
+add_dependencies(failsafe_test failsafe_uorb_struct_header uorb_headers parameters_header events_header mode_util)
+target_compile_definitions(failsafe_test PUBLIC EMSCRIPTEN_BUILD PRINTF_LOG MODULE_NAME=\"failsafe\")
+set_target_properties(failsafe_test PROPERTIES OUTPUT_NAME "index.html")
+
+get_target_property(failsafe_test_compile_options failsafe_test COMPILE_OPTIONS)
+list(REMOVE_ITEM failsafe_test_compile_options "$<$<COMPILE_LANGUAGE:CXX>:-fno-rtti>")
+set_property(TARGET failsafe_test PROPERTY COMPILE_OPTIONS ${failsafe_test_compile_options})
+
+target_link_libraries(failsafe_test mode_util embind)
+# TODO: use target_link_options() when updating to cmake 3.13 or newer
+#target_link_options(failsafe_test PUBLIC --shell-file ${html_output_file} --no-entry -sLLD_REPORT_UNDEFINED -s NO_EXIT_RUNTIME=1)
+target_link_libraries(failsafe_test
+	"--shell-file ${html_output_file}"
+	"--no-entry -sLLD_REPORT_UNDEFINED"
+	"-s NO_EXIT_RUNTIME=1")
+
+px4_add_library(failsafe
+	failsafe.cpp
+	framework.cpp
+)
+
+px4_add_functional_gtest(SRC failsafe_test.cpp
+	LINKLIBS failsafe mode_util
+)

src/modules/commander/failsafe/emscripten.cpp

@@ -0,0 +1,221 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#define PARAM_IMPLEMENTATION
+#include <parameters/param.h>
+
+#include "failsafe.h"
+#include "../ModeUtil/mode_requirements.hpp"
+#include <uORB/topics/vehicle_status.h>
+
+#include <emscripten/emscripten.h>
+#include <emscripten/bind.h>
+#include <emscripten/html5.h>
+#include <cstdio>
+#include <string>
+#include <map>
+#include <vector>
+
+using namespace emscripten;
+
+#include <generated_uorb_struct_field_mapping.h>
+
+// parameter storage
+struct Param {
+	union param_value_u val;
+};
+
+class FailsafeTest : public ModuleParams
+{
+public:
+	FailsafeTest() : ModuleParams(nullptr) {}
+
+	void updateParams() override
+	{
+		ModuleParams::updateParams();
+	}
+
+	FailsafeBase &current() { return _failsafe; }
+	std::map<param_t, Param> &params() { return _used_params; }
+private:
+	std::map<param_t, Param> _used_params;
+	Failsafe _failsafe{this};
+};
+
+static FailsafeTest failsafe_instance;
+
+
+int param_get(param_t param, void *val)
+{
+	std::map<param_t, Param> &used_params = failsafe_instance.params();
+	auto param_iter = used_params.find(param);
+
+	if (param_iter != used_params.end()) {
+		memcpy(val, &param_iter->second.val, sizeof(param_iter->second.val));
+		return 0;
+	}
+
+	printf("Error: param %i not found\n", param);
+	return -1;
+}
+
+void param_set_used(param_t param)
+{
+	std::map<param_t, Param> &used_params = failsafe_instance.params();
+
+	if (used_params.find(param) != used_params.end()) {
+		return;
+	}
+
+	Param p;
+	memcpy(&p.val, &px4::parameters[param].val, sizeof(p.val));
+	used_params[param] = p;
+}
+
+std::vector<std::string> get_used_params()
+{
+	std::vector<std::string> ret;
+	std::map<param_t, Param> &used_params = failsafe_instance.params();
+
+	for (const auto &param_iter : used_params) {
+		ret.push_back(px4::parameters[param_iter.first].name);
+	}
+
+	return ret;
+}
+param_value_u get_param_value(const std::string &name)
+{
+	std::map<param_t, Param> &used_params = failsafe_instance.params();
+
+	for (const auto &param_iter : used_params) {
+		if (name == px4::parameters[param_iter.first].name) {
+			return param_iter.second.val;
+		}
+	}
+
+	printf("Error: param %s not used\n", name.c_str());
+	return {};
+}
+
+int get_param_value_int32(const std::string &name)
+{
+	return get_param_value(name).i;
+}
+
+float get_param_value_float(const std::string &name)
+{
+	return get_param_value(name).f;
+}
+
+void set_param_value(const std::string &name, const param_value_u value)
+{
+	std::map<param_t, Param> &used_params = failsafe_instance.params();
+
+	for (auto &param_iter : used_params) {
+		if (name == px4::parameters[param_iter.first].name) {
+			param_iter.second.val = value;
+			break;
+		}
+	}
+
+	failsafe_instance.updateParams();
+}
+
+void set_param_value_int32(const std::string &name, int value)
+{
+	param_value_u param_value;
+	param_value.i = value;
+	set_param_value(name, param_value);
+}
+
+void set_param_value_float(const std::string &name, float value)
+{
+	param_value_u param_value;
+	param_value.f = value;
+	set_param_value(name, param_value);
+}
+
+int failsafe_update(bool armed, bool vtol_in_transition_mode, bool mission_finished,
+		    bool user_override, uint8_t user_intended_mode, uint8_t vehicle_type,
+		    failsafe_flags_s status_flags)
+{
+	uint64_t time_ms = emscripten_date_now();
+	FailsafeBase::State state{};
+	state.armed = armed;
+	state.vtol_in_transition_mode = vtol_in_transition_mode;
+	state.mission_finished = mission_finished;
+	state.user_intended_mode = user_intended_mode;
+	state.vehicle_type = vehicle_type;
+	mode_util::getModeRequirements(vehicle_type, status_flags);
+	return failsafe_instance.current().update(time_ms * 1000, state, false, user_override, status_flags);
+}
+
+int selected_action()
+{
+	FailsafeBase::Action action = failsafe_instance.current().selectedAction();
+
+	if (action == FailsafeBase::Action::Disarm) {
+		printf("Disarming\n");
+	}
+
+	return (int)action;
+}
+
+bool user_takeover_active()
+{
+	return failsafe_instance.current().userTakeoverActive();
+}
+
+std::string action_str(int action)
+{
+	return FailsafeBase::actionStr((FailsafeBase::Action)action);
+}
+
+EMSCRIPTEN_BINDINGS(failsafe)
+{
+	class_<failsafe_flags_s>("state")
+	.constructor<>()
+	UORB_STRUCT_FIELD_MAPPING
+	;
+
+	function("failsafe_update", &failsafe_update);
+	function("action_str", &action_str);
+	function("get_used_params", &get_used_params);
+	function("set_param_value_int32", &set_param_value_int32);
+	function("set_param_value_float", &set_param_value_float);
+	function("get_param_value_int32", &get_param_value_int32);
+	function("get_param_value_float", &get_param_value_float);
+	function("user_takeover_active", &user_takeover_active);
+	function("selected_action", &selected_action);
+	register_vector<std::string>("vector<string>");
+}

src/modules/commander/failsafe/emscripten_template.html

@@ -0,0 +1,411 @@
+<!doctype html>
+<html lang="en-us">
+  <head>
+    <meta charset="utf-8">
+    <link rel="icon" href="data:;base64,iVBORw0KGgo=">
+    <meta http-equiv="Content-Type" content="text/html; charset=utf-8">
+    <title>Failsafe State Machine</title>
+    <style>
+      html * {
+          font-family: Helvetica, sans-serif;
+      }
+      .title {
+          margin-left: 10px;
+          margin-bottom: 0px;
+      }
+      h3 {
+          margin-bottom: 5px;
+      }
+      h5 {
+          margin-top: 5px;
+          margin-bottom: 0px;
+      }
+      input[type=text] {
+          text-align: right;
+      }
+      p {
+          margin-top: 8px;
+          margin-bottom: 8px;
+      }
+      button {
+          margin-top: 6px;
+      }
+      .checkbox-field {
+          display: flex;
+          flex-direction: row;
+      }
+      .box {
+          background-color: rgb(231, 233, 235);
+          border-bottom-left-radius: 5px;
+          border-bottom-right-radius: 5px;
+          border-top-left-radius: 5px;
+          border-top-right-radius: 5px;
+          box-shadow: none;
+          box-sizing: border-box;
+          line-height: 22.5px;
+          margin-bottom: 10px;
+          margin-left: 10px;
+          margin-right: 10px;
+          margin-top: 20px;
+          padding-bottom: 16px;
+          padding-left: 20px;
+          padding-right: 20px;
+          padding-top: 8px
+      }
+      table td {
+          vertical-align: top;
+      }
+
+      .tooltip {
+          position: relative;
+          background: rgba(0,0,0,0.2);
+          padding: 0px 5px;
+          border-radius: 100%;
+          font-size: 90%;
+          width: 11px;
+          display: inline-block;
+          margin-left: 8px;
+          text-align: center;
+          color: white;
+      }
+
+      .tooltip .tooltiptext {
+          visibility: hidden;
+          width: 200px;
+          background-color: #555;
+          color: #fff;
+          text-align: center;
+          border-radius: 6px;
+          padding: 5px 0;
+          position: absolute;
+          z-index: 1;
+          top: 125%;
+          left: 50%;
+          margin-left: -100px;
+          opacity: 0;
+          transition: opacity 0.3s;
+      }
+
+      .tooltip:hover .tooltiptext {
+          visibility: visible;
+          opacity: 1;
+      }
+
+      .emscripten { padding-right: 0; display: block; }
+      textarea.emscripten {
+          font-family: monospace;
+          width: 100%;
+          white-space: pre;
+          overflow-wrap: normal;
+          overflow-x: scroll;
+      }
+      div.emscripten { text-align: center; }
+      div.emscripten_border { border: 1px solid black; }
+      /* the canvas *must not* have any border or padding, or mouse coords will be wrong */
+      canvas.emscripten { border: 0px none; background-color: black; }
+
+      .spinner {
+        height: 50px;
+        width: 50px;
+        margin: 0px auto;
+        -webkit-animation: rotation .8s linear infinite;
+        -moz-animation: rotation .8s linear infinite;
+        -o-animation: rotation .8s linear infinite;
+        animation: rotation 0.8s linear infinite;
+        border-left: 10px solid rgb(0,150,240);
+        border-right: 10px solid rgb(0,150,240);
+        border-bottom: 10px solid rgb(0,150,240);
+        border-top: 10px solid rgb(100,0,200);
+        border-radius: 100%;
+        background-color: rgb(200,100,250);
+      }
+      @-webkit-keyframes rotation {
+        from {-webkit-transform: rotate(0deg);}
+        to {-webkit-transform: rotate(360deg);}
+      }
+      @-moz-keyframes rotation {
+        from {-moz-transform: rotate(0deg);}
+        to {-moz-transform: rotate(360deg);}
+      }
+      @-o-keyframes rotation {
+        from {-o-transform: rotate(0deg);}
+        to {-o-transform: rotate(360deg);}
+      }
+      @keyframes rotation {
+        from {transform: rotate(0deg);}
+        to {transform: rotate(360deg);}
+      }
+
+    </style>
+  </head>
+  <body>
+    <figure style="overflow:visible;" id="spinner"><div class="spinner"></div><center style="margin-top:0.5em"><strong>emscripten</strong></center></figure>
+    <div class="emscripten" id="status">Downloading...</div>
+    <div class="emscripten">
+      <progress value="0" max="100" id="progress" hidden=1></progress>
+    </div>
+    <h1 class="title" id="title">Failsafe State Machine Simulation</h1>
+    <table>
+        <tr>
+            <td><div class="box"><h3>State</h3>
+                    <table>
+                        <tr>
+                            <td><label for="vehicle_type">Vehicle Type:&nbsp;</label> </td>
+                            <td><select id="vehicle_type">
+                                <option value="0">Unknown/other</option>
+                                <option value="1" selected>Multirotor</option>
+                                <option value="2">Fixed wing</option>
+                                <option value="3">Rover</option>
+                                <option value="4">Airship</option>
+                            </select>
+                            </td>
+                            <td><div class='tooltip'>?<span class='tooltiptext'>For VTOLs the type switches between Multirotor and Fixed Wing</span></div></td>
+                        </tr>
+                        <tr>
+                            <td><label for="armed">Armed:</label></td>
+                            <td><input type="checkbox" id="armed" name="armed" checked></td>
+                            <td></td>
+                        </tr>
+                        <tr>
+                            <td><label for="vtol_in_transition_mode">VTOL in Transition Mode:</label></td>
+                            <td><input type="checkbox" id="vtol_in_transition_mode" name="vtol_in_transition_mode"></td>
+                            <td></td>
+                        </tr>
+                        <tr>
+                            <td><label for="mission_finished">Mission Finished:</label></td>
+                            <td><input type="checkbox" id="mission_finished" name="mission_finished"></td>
+                            <td></td>
+                        </tr>
+                        <tr>
+                            <td><label for="intended_nav_state">Intended Mode:&nbsp;</label></td>
+                            <td><select id="intended_nav_state">
+                                <option value="0">MANUAL</option>
+                                <option value="10">ACRO</option>
+                                <option value="15">STAB</option>
+                                <option value="1">ALTCTL</option>
+                                <option value="2" selected>POSCTL</option>
+                                <option value="3">AUTO_MISSION</option>
+                                <option value="4">AUTO_LOITER</option>
+                                <option value="5">AUTO_RTL</option>
+                                <option value="17">AUTO_TAKEOFF</option>
+                                <option value="18">AUTO_LAND</option>
+                                <option value="19">AUTO_FOLLOW_TARGET</option>
+                                <option value="20">AUTO_PRECLAND</option>
+                                <option value="22">AUTO_VTOL_TAKEOFF</option>
+                                <option value="14">OFFBOARD</option>
+                                <option value="21">ORBIT</option>
+                            </select>
+                            </td>
+                            <td></td>
+                        </tr>
+                        <tr>
+                            <td colspan="3">
+                                <button onclick="moveRCSticks()">Move RC Sticks (user takeover request)</button>
+                            </td>
+                        </tr>
+                    </table>
+                </div>
+                <div class="box"><h3>Parameters</h3>
+                    <div id="parameters"></div>
+                </div></td>
+            <td><div class="box"><h3>Conditions</h3>
+                    {{{ HTML_CONDITIONS }}}
+                </div>
+                <div class="box"><h3>Output</h3>
+                <p>Failsafe action:&nbsp;<b id="action"></b></p>
+                <p>User takeover active:&nbsp;<span id="user_takeover_active"></span></p>
+                <p><div>Console output (debug):</div>
+                <textarea class="emscripten" id="output" rows="12"></textarea>
+                </p>
+                </div>
+            </td>
+        </tr>
+    </table>
+
+    <script type='text/javascript'>
+
+      window.addEventListener("load",function() {
+          const url = new URL(window.location.href);
+          const no_title = url.searchParams.get("no-title");
+          document.querySelector(".title").hidden = no_title && no_title === "1";
+      });
+
+      function onParamChangedInt(name, value) {
+          console.log("param change int: ", name, "value: ", value);
+          Module.set_param_value_int32(name, parseInt(value));
+      }
+      function onParamChangedFloat(name, value) {
+          console.log("param change float: ", name, "value: ", value);
+          Module.set_param_value_float(name, parseFloat(value));
+      }
+
+      var user_override_requested = false;
+      function moveRCSticks() {
+          user_override_requested = true;
+      }
+
+      var state = null;
+      function runFailsafeUpdate() {
+          if (state == null) {
+              state = new Module.state();
+
+              // get the used params & download json metadata
+              fetch("parameters.json")
+                  .then(response => response.json())
+                  .then(json => {
+                  var used_params = Module.get_used_params();
+                  var parameters_html = "<table>";
+                  var parameter_meta = json["parameters"];
+                  for (var i = 0; i < used_params.size(); i++) {
+                      var param_name = used_params.get(i);
+
+                      var meta = parameter_meta.filter(item => item.name === param_name);
+                      if (meta.length > 0) {
+                          meta = meta[0];
+                          var param_type_is_int32 = true;
+                          if (meta["type"] == "Int32") {
+                              var param_value = Module.get_param_value_int32(param_name);
+                          } else if (meta["type"] == "Float") {
+                              param_type_is_int32 = false;
+                              var param_value = Module.get_param_value_float(param_name);
+                          } else {
+                              console.log("Error: unknown param type: ", param_name, meta["type"]);
+                              continue;
+                          }
+                          console.log("param: ", param_name, param_value);
+                          parameters_html += "<tr>";
+                          parameters_html += "<td><label for=\""+param_name+"\">"+param_name+":&nbsp;</label></td>";
+                          onChange = "onChange=\"onParamChangedInt('"+param_name+"', this.options[this.selectedIndex].value)\"";
+                          if (meta.hasOwnProperty("values")) {
+                              parameters_html += "<td><select "+onChange+" id=\""+param_name+"\">";
+                              var enum_values = meta["values"];
+                              for (var k = 0; k < enum_values.length; ++k) {
+                                  var selected = enum_values[k]["value"] == param_value;
+                                  var selected_str = selected ? "selected" : "";
+                                  parameters_html += "<option value=\""+enum_values[k]["value"].toString()+"\" "+selected_str+">"+enum_values[k]["description"]+"</option>";
+                              }
+                              parameters_html += "</select>";
+                          } else {
+                              var unit = "";
+                              if (meta.hasOwnProperty("units")) {
+                                  unit = "&nbsp;"+meta["units"];
+                              }
+                              if (param_type_is_int32) {
+                                  onChange = "onChange=\"onParamChangedInt('"+param_name+"', this.value)\"";
+                              } else {
+                                  onChange = "onChange=\"onParamChangedFloat('"+param_name+"', this.value)\"";
+                              }
+                              parameters_html += "<td><input "+onChange+" size='8' type='text' id=\""+param_name+"\" name=\""+param_name+"\" value=\""+param_value+"\">"+unit;
+                          }
+                          parameters_html += "</td>";
+
+                          var param_description = meta["shortDesc"];
+                          parameters_html += "<td><div class='tooltip'>?<span class='tooltiptext'>"+param_description+"</span></div></td>";
+
+                          parameters_html += "</tr>";
+                      } else {
+                          console.log("no metadata for ", param_name);
+                      }
+                  }
+                  parameters_html += "</table>";
+                  document.getElementById("parameters").innerHTML = parameters_html;
+              });
+          }
+
+          {{{ JS_STATE_CODE }}}
+
+          var armed = document.querySelector('input[id="armed"]').checked;
+          var vtol_in_transition_mode = document.querySelector('input[id="vtol_in_transition_mode"]').checked;
+          var mission_finished = document.querySelector('input[id="mission_finished"]').checked;
+          var intended_nav_state = document.querySelector('select[id="intended_nav_state"]').value;
+          var vehicle_type = document.querySelector('select[id="vehicle_type"]').value;
+          var updated_user_intended_mode = Module.failsafe_update(armed, vtol_in_transition_mode, mission_finished, user_override_requested, parseInt(intended_nav_state),
+              parseInt(vehicle_type), state);
+          user_override_requested = false;
+          var action = Module.selected_action();
+          action_str = Module.action_str(action);
+          if (action_str == "Disarm") {
+              document.querySelector('input[id="armed"]').checked = false;
+          }
+          if (action != 0) {
+              action_str = "<font color='crimson'>"+action_str+"</font>";
+          }
+          document.getElementById("action").innerHTML = action_str;
+          var user_takeover_active = Module.user_takeover_active();
+          document.getElementById("user_takeover_active").innerHTML = user_takeover_active ? "<b>Yes</b>" : "No";
+          if (intended_nav_state != updated_user_intended_mode) {
+              document.querySelector('select[id="intended_nav_state"]').value = updated_user_intended_mode;
+          }
+      }
+
+
+
+      // emscripten template code (from shell_minimal.html)
+
+      var statusElement = document.getElementById('status');
+      var progressElement = document.getElementById('progress');
+      var spinnerElement = document.getElementById('spinner');
+
+      var Module = {
+        preRun: [],
+        postRun: [],
+        print: (function() {
+          var element = document.getElementById('output');
+          if (element) element.value = ''; // clear browser cache
+          return function(text) {
+            if (arguments.length > 1) text = Array.prototype.slice.call(arguments).join(' ');
+            // These replacements are necessary if you render to raw HTML
+            //text = text.replace(/&/g, "&amp;");
+            //text = text.replace(/</g, "&lt;");
+            //text = text.replace(/>/g, "&gt;");
+            //text = text.replace('\n', '<br>', 'g');
+            console.log(text);
+            if (element) {
+              element.value += text + "\n";
+              element.scrollTop = element.scrollHeight; // focus on bottom
+            }
+          };
+        })(),
+        setStatus: function(text) {
+          if (!Module.setStatus.last) Module.setStatus.last = { time: Date.now(), text: '' };
+          if (text === Module.setStatus.last.text) return;
+          var m = text.match(/([^(]+)\((\d+(\.\d+)?)\/(\d+)\)/);
+          var now = Date.now();
+          if (m && now - Module.setStatus.last.time < 30) return; // if this is a progress update, skip it if too soon
+          Module.setStatus.last.time = now;
+          Module.setStatus.last.text = text;
+          if (m) {
+            text = m[1];
+            progressElement.value = parseInt(m[2])*100;
+            progressElement.max = parseInt(m[4])*100;
+            progressElement.hidden = false;
+            spinnerElement.hidden = false;
+          } else {
+            progressElement.value = null;
+            progressElement.max = null;
+            progressElement.hidden = true;
+            if (!text) spinnerElement.hidden = true;
+          }
+          statusElement.innerHTML = text;
+        },
+        totalDependencies: 0,
+        monitorRunDependencies: function(left) {
+          this.totalDependencies = Math.max(this.totalDependencies, left);
+          Module.setStatus(left ? 'Preparing... (' + (this.totalDependencies-left) + '/' + this.totalDependencies + ')' : 'All downloads complete.');
+        },
+        onRuntimeInitialized: function() {
+            setInterval(runFailsafeUpdate, 100);
+        }
+      };
+      Module.setStatus('Downloading...');
+      window.onerror = function() {
+        Module.setStatus('Exception thrown, see JavaScript console');
+        spinnerElement.style.display = 'none';
+        Module.setStatus = function(text) {
+          if (text) console.error('[post-exception status] ' + text);
+        };
+      };
+    </script>
+    {{{ SCRIPT }}}
+  </body>
+</html>

src/modules/commander/failsafe/failsafe.cpp

@@ -0,0 +1,531 @@
+/****************************************************************************
+ *
+ *   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 "failsafe.h"
+
+#include <px4_platform_common/log.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/battery_status.h>
+#include <lib/circuit_breaker/circuit_breaker.h>
+
+using namespace time_literals;
+
+FailsafeBase::ActionOptions Failsafe::fromNavDllOrRclActParam(int param_value)
+{
+	ActionOptions options{};
+
+	switch (param_value) {
+	case 0:
+		options.action = Action::None;
+		break;
+
+	case 1:
+		options.action = Action::Hold;
+		break;
+
+	case 2:
+		options.action = Action::RTL;
+		options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
+		break;
+
+	case 3:
+		options.action = Action::Land;
+		break;
+
+	case 5:
+		options.allow_user_takeover = UserTakeoverAllowed::Never;
+		options.action = Action::Terminate;
+		options.clear_condition = ClearCondition::Never;
+		break;
+
+	case 6: // Lockdown
+		options.allow_user_takeover = UserTakeoverAllowed::Never;
+		options.action = Action::Disarm;
+		break;
+
+	default:
+		options.action = Action::None;
+		break;
+	}
+
+	return options;
+}
+
+FailsafeBase::ActionOptions Failsafe::fromGfActParam(int param_value)
+{
+	ActionOptions options{};
+
+	switch (param_value) {
+	case 0:
+		options.action = Action::None;
+		break;
+
+	case 1:
+		options.action = Action::Warn;
+		break;
+
+	case 2:
+		options.allow_user_takeover = UserTakeoverAllowed::AlwaysModeSwitchOnly; // ensure the user can escape again
+		options.action = Action::Hold;
+		options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
+		break;
+
+	case 3:
+		options.action = Action::RTL;
+		options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
+		break;
+
+	case 4:
+		options.allow_user_takeover = UserTakeoverAllowed::Never;
+		options.action = Action::Terminate;
+		options.clear_condition = ClearCondition::Never;
+		break;
+
+	case 5:
+		options.action = Action::Land;
+		break;
+
+	default:
+		options.action = Action::Warn;
+		break;
+	}
+
+	return options;
+}
+
+FailsafeBase::ActionOptions Failsafe::fromImbalancedPropActParam(int param_value)
+{
+	ActionOptions options{};
+
+	switch (param_value) {
+	case -1:
+	default:
+		options.action = Action::None;
+		break;
+
+	case 0:
+		options.action = Action::Warn;
+		break;
+
+	case 1:
+		options.action = Action::RTL;
+		options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
+		break;
+
+	case 2:
+		options.action = Action::Land;
+		options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
+		break;
+	}
+
+	return options;
+}
+
+FailsafeBase::ActionOptions Failsafe::fromActuatorFailureActParam(int param_value)
+{
+	ActionOptions options{};
+
+	switch (param_value) {
+	case 0:
+	default:
+		options.action = Action::Warn;
+		break;
+
+	case 1:
+		options.action = Action::Hold;
+		break;
+
+	case 2:
+		options.action = Action::Land;
+		options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
+		break;
+
+	case 3:
+		options.action = Action::RTL;
+		options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
+		break;
+
+	case 4:
+		options.action = Action::Terminate;
+		options.clear_condition = ClearCondition::Never;
+		break;
+	}
+
+	return options;
+}
+
+FailsafeBase::ActionOptions Failsafe::fromBatteryWarningActParam(int param_value, uint8_t battery_warning)
+{
+	ActionOptions options{};
+
+	switch (battery_warning) {
+	default:
+	case battery_status_s::BATTERY_WARNING_NONE:
+		options.action = Action::None;
+		break;
+
+	case battery_status_s::BATTERY_WARNING_LOW:
+		options.action = Action::Warn;
+		options.cause = Cause::BatteryLow;
+		break;
+
+	case battery_status_s::BATTERY_WARNING_CRITICAL:
+		options.action = Action::Warn;
+		options.cause = Cause::BatteryCritical;
+
+		switch ((LowBatteryAction)param_value) {
+		case LowBatteryAction::Return:
+		case LowBatteryAction::ReturnOrLand:
+			options.action = Action::RTL;
+			break;
+
+		case LowBatteryAction::Land:
+			options.action = Action::Land;
+			break;
+
+		case LowBatteryAction::Warning:
+			options.action = Action::Warn;
+			break;
+		}
+
+		break;
+
+	case battery_status_s::BATTERY_WARNING_EMERGENCY:
+		options.action = Action::Warn;
+		options.cause = Cause::BatteryEmergency;
+
+		switch ((LowBatteryAction)param_value) {
+		case LowBatteryAction::Return:
+			options.action = Action::RTL;
+			break;
+
+		case LowBatteryAction::ReturnOrLand:
+		case LowBatteryAction::Land:
+			options.action = Action::Land;
+			break;
+
+		case LowBatteryAction::Warning:
+			options.action = Action::Warn;
+			break;
+		}
+
+		break;
+	}
+
+	return options;
+}
+
+FailsafeBase::ActionOptions Failsafe::fromQuadchuteActParam(int param_value)
+{
+	ActionOptions options{};
+
+	switch (param_value) {
+	case -1:
+	default:
+		options.action = Action::Warn;
+		break;
+
+	case 0:
+		options.action = Action::RTL;
+		options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
+		break;
+
+	case 1:
+		options.action = Action::Land;
+		options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
+		break;
+
+	case 2:
+		options.action = Action::Hold;
+		options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
+		break;
+	}
+
+	return options;
+}
+
+FailsafeBase::Action Failsafe::fromOffboardLossActParam(int param_value, uint8_t &user_intended_mode)
+{
+	Action action{Action::None};
+
+	switch (param_value) {
+	case 0:
+	default:
+		action = Action::FallbackPosCtrl;
+		user_intended_mode = vehicle_status_s::NAVIGATION_STATE_POSCTL;
+		break;
+
+	case 1:
+		action = Action::FallbackAltCtrl;
+		user_intended_mode = vehicle_status_s::NAVIGATION_STATE_ALTCTL;
+		break;
+
+	case 2:
+		action = Action::FallbackStab;
+		user_intended_mode = vehicle_status_s::NAVIGATION_STATE_STAB;
+		break;
+
+	case 3:
+		action = Action::RTL;
+		user_intended_mode = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
+		break;
+
+	case 4:
+		action = Action::Land;
+		user_intended_mode = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
+		break;
+
+	case 5:
+		action = Action::Hold;
+		user_intended_mode = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
+		break;
+
+	case 6:
+		action = Action::Terminate;
+		user_intended_mode = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
+		break;
+
+	case 7:
+		action = Action::Disarm;
+		break;
+	}
+
+	return action;
+}
+
+void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
+				 const failsafe_flags_s &status_flags)
+{
+	updateArmingState(time_us, state.armed, status_flags);
+
+	const bool in_forward_flight = state.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING
+				       || state.vtol_in_transition_mode;
+
+	// Do not enter failsafe while doing a vtol takeoff after the vehicle has started a transition and before it reaches the loiter
+	// altitude. The vtol takeoff navigaton mode will set mission_finished to true as soon as the loiter is established
+	const bool ignore_link_failsafe = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF
+					  && in_forward_flight && !state.mission_finished;
+
+	// Manual control (RC) loss
+	if (!status_flags.manual_control_signal_lost) {
+		// If manual control was lost and arming was allowed, consider it optional until we regain manual control
+		_manual_control_lost_at_arming = false;
+	}
+
+	const bool rc_loss_ignored_mission = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION
+					     && (_param_com_rcl_except.get() & (int)ManualControlLossExceptionBits::Mission);
+	const bool rc_loss_ignored_loiter = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER
+					    && (_param_com_rcl_except.get() & (int)ManualControlLossExceptionBits::Hold);
+	const bool rc_loss_ignored_offboard = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_OFFBOARD
+					      && (_param_com_rcl_except.get() & (int)ManualControlLossExceptionBits::Offboard);
+	const bool rc_loss_ignored_takeoff = (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF ||
+					      state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF)
+					     && (_param_com_rcl_except.get() & (int)ManualControlLossExceptionBits::Hold);
+	const bool rc_loss_ignored = rc_loss_ignored_mission || rc_loss_ignored_loiter || rc_loss_ignored_offboard ||
+				     rc_loss_ignored_takeoff || ignore_link_failsafe || _manual_control_lost_at_arming;
+
+	if (_param_com_rc_in_mode.get() != 4 && !rc_loss_ignored) {
+		CHECK_FAILSAFE(status_flags, manual_control_signal_lost,
+			       fromNavDllOrRclActParam(_param_nav_rcl_act.get()).causedBy(Cause::ManualControlLoss));
+	}
+
+	// GCS connection loss
+	const bool gcs_connection_loss_ignored = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND ||
+			state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND || ignore_link_failsafe;
+
+	if (_param_nav_dll_act.get() != 0 && !gcs_connection_loss_ignored) {
+		CHECK_FAILSAFE(status_flags, gcs_connection_lost,
+			       fromNavDllOrRclActParam(_param_nav_dll_act.get()).causedBy(Cause::GCSConnectionLoss));
+	}
+
+	// VTOL transition failure (quadchute)
+	if (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION ||
+	    state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER ||
+	    state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF ||
+	    state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF) {
+		CHECK_FAILSAFE(status_flags, vtol_transition_failure, fromQuadchuteActParam(_param_com_qc_act.get()));
+	}
+
+	// Mission
+	if (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) {
+		CHECK_FAILSAFE(status_flags, mission_failure, Action::RTL);
+
+		// If manual control loss and GCS connection loss are disabled and we lose both command links and the mission finished,
+		// trigger RTL to avoid losing the vehicle
+		if ((_param_com_rc_in_mode.get() == 4 || rc_loss_ignored_mission) && _param_nav_dll_act.get() == 0
+		    && state.mission_finished) {
+			_last_state_mission_control_lost = checkFailsafe(_caller_id_mission_control_lost, _last_state_mission_control_lost,
+							   status_flags.gcs_connection_lost, Action::RTL);
+		}
+	}
+
+
+	CHECK_FAILSAFE(status_flags, wind_limit_exceeded,
+		       ActionOptions(Action::RTL).clearOn(ClearCondition::OnModeChangeOrDisarm));
+	CHECK_FAILSAFE(status_flags, flight_time_limit_exceeded,
+		       ActionOptions(Action::RTL).allowUserTakeover(UserTakeoverAllowed::Never));
+
+	CHECK_FAILSAFE(status_flags, primary_geofence_breached, fromGfActParam(_param_gf_action.get()));
+
+	// Battery
+	CHECK_FAILSAFE(status_flags, battery_low_remaining_time, ActionOptions(Action::RTL).causedBy(Cause::BatteryLow));
+	CHECK_FAILSAFE(status_flags, battery_unhealthy, Action::Warn);
+
+	if (status_flags.battery_warning == battery_status_s::BATTERY_WARNING_LOW) {
+		_last_state_battery_warning_low = checkFailsafe(_caller_id_battery_warning_low, _last_state_battery_warning_low,
+						  true, fromBatteryWarningActParam(_param_com_low_bat_act.get(), battery_status_s::BATTERY_WARNING_LOW));
+
+	} else if (status_flags.battery_warning == battery_status_s::BATTERY_WARNING_CRITICAL) {
+		_last_state_battery_warning_critical = checkFailsafe(_caller_id_battery_warning_critical,
+						       _last_state_battery_warning_critical,
+						       true, fromBatteryWarningActParam(_param_com_low_bat_act.get(), battery_status_s::BATTERY_WARNING_CRITICAL));
+
+	} else if (status_flags.battery_warning == battery_status_s::BATTERY_WARNING_EMERGENCY) {
+		_last_state_battery_warning_emergency = checkFailsafe(_caller_id_battery_warning_emergency,
+							_last_state_battery_warning_emergency,
+							true, fromBatteryWarningActParam(_param_com_low_bat_act.get(), battery_status_s::BATTERY_WARNING_EMERGENCY));
+	}
+
+
+	// Failure detector
+	if (_armed_time != 0 && time_us - _armed_time < _param_com_spoolup_time.get() * 1_s) {
+		CHECK_FAILSAFE(status_flags, fd_esc_arming_failure, Action::Disarm);
+	}
+
+	if (_armed_time != 0 && time_us - _armed_time < (_param_com_lkdown_tko.get() + _param_com_spoolup_time.get()) * 1_s) {
+		// This handles the case where something fails during the early takeoff phase
+		CHECK_FAILSAFE(status_flags, fd_critical_failure, Action::Disarm);
+
+	} else if (!circuit_breaker_enabled_by_val(_param_cbrk_flightterm.get(), CBRK_FLIGHTTERM_KEY)) {
+		CHECK_FAILSAFE(status_flags, fd_critical_failure, Action::Terminate);
+
+	} else {
+		CHECK_FAILSAFE(status_flags, fd_critical_failure, Action::Warn);
+	}
+
+	CHECK_FAILSAFE(status_flags, fd_imbalanced_prop, fromImbalancedPropActParam(_param_com_imb_prop_act.get()));
+	CHECK_FAILSAFE(status_flags, fd_motor_failure, fromActuatorFailureActParam(_param_com_actuator_failure_act.get()));
+
+
+
+	// Mode fallback (last)
+	Action mode_fallback_action = checkModeFallback(status_flags, state.user_intended_mode);
+	_last_state_mode_fallback = checkFailsafe(_caller_id_mode_fallback, _last_state_mode_fallback,
+				    mode_fallback_action != Action::None,
+				    ActionOptions(mode_fallback_action).allowUserTakeover(UserTakeoverAllowed::Always));
+}
+
+void Failsafe::updateArmingState(const hrt_abstime &time_us, bool armed, const failsafe_flags_s &status_flags)
+{
+	if (!_was_armed && armed) {
+		_armed_time = time_us;
+		_manual_control_lost_at_arming = status_flags.manual_control_signal_lost;
+
+	} else if (!armed) {
+		_manual_control_lost_at_arming = status_flags.manual_control_signal_lost; // ensure action isn't added while disarmed
+		_armed_time = 0;
+	}
+
+	_was_armed = armed;
+}
+
+FailsafeBase::Action Failsafe::checkModeFallback(const failsafe_flags_s &status_flags,
+		uint8_t user_intended_mode) const
+{
+	Action action = Action::None;
+
+	// offboard signal
+	if (status_flags.offboard_control_signal_lost && (status_flags.mode_req_offboard_signal & (1u << user_intended_mode))) {
+		action = fromOffboardLossActParam(_param_com_obl_rc_act.get(), user_intended_mode);
+	}
+
+	// posctrl
+	switch (_param_com_posctl_navl.get()) {
+	case 0: // AltCtrl/Manual
+
+		// PosCtrl -> AltCtrl
+		if (user_intended_mode == vehicle_status_s::NAVIGATION_STATE_POSCTL
+		    && !modeCanRun(status_flags, user_intended_mode)) {
+			action = Action::FallbackAltCtrl;
+			user_intended_mode = vehicle_status_s::NAVIGATION_STATE_ALTCTL;
+		}
+
+		// AltCtrl -> Stabilized
+		if (user_intended_mode == vehicle_status_s::NAVIGATION_STATE_ALTCTL
+		    && !modeCanRun(status_flags, user_intended_mode)) {
+			action = Action::FallbackStab;
+			user_intended_mode = vehicle_status_s::NAVIGATION_STATE_STAB;
+		}
+
+		break;
+
+	case 1: // Land/Terminate
+
+		// PosCtrl -> Land
+		if (user_intended_mode == vehicle_status_s::NAVIGATION_STATE_POSCTL
+		    && !modeCanRun(status_flags, user_intended_mode)) {
+			action = Action::Land;
+			user_intended_mode = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
+
+			// Land -> Descend
+			if (!modeCanRun(status_flags, user_intended_mode)) {
+				action = Action::Descend;
+				user_intended_mode = vehicle_status_s::NAVIGATION_STATE_DESCEND;
+			}
+		}
+
+		break;
+	}
+
+
+	// Last, check can_run for intended mode
+	if (!modeCanRun(status_flags, user_intended_mode)) {
+		action = Action::RTL;
+		user_intended_mode = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
+	}
+
+	return action;
+}
+
+uint8_t Failsafe::modifyUserIntendedMode(Action previous_action, Action current_action,
+		uint8_t user_intended_mode) const
+{
+	// If we switch from a failsafe back into orbit, switch to loiter instead
+	if ((int)previous_action > (int)Action::Warn
+	    && modeFromAction(current_action, user_intended_mode) == vehicle_status_s::NAVIGATION_STATE_ORBIT) {
+		PX4_DEBUG("Failsafe cleared, switching from ORBIT to LOITER");
+		return vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
+	}
+
+	return user_intended_mode;
+}

src/modules/commander/failsafe/failsafe.h

@@ -0,0 +1,113 @@
+/****************************************************************************
+ *
+ *   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 "framework.h"
+
+
+class Failsafe : public FailsafeBase
+{
+public:
+	Failsafe(ModuleParams *parent) : FailsafeBase(parent) {}
+
+protected:
+
+	void checkStateAndMode(const hrt_abstime &time_us, const State &state,
+			       const failsafe_flags_s &status_flags) override;
+	Action checkModeFallback(const failsafe_flags_s &status_flags, uint8_t user_intended_mode) const override;
+
+	uint8_t modifyUserIntendedMode(Action previous_action, Action current_action,
+				       uint8_t user_intended_mode) const override;
+
+private:
+	void updateArmingState(const hrt_abstime &time_us, bool armed, const failsafe_flags_s &status_flags);
+
+	enum class ManualControlLossExceptionBits : int32_t {
+		Mission = (1 << 0),
+		Hold = (1 << 1),
+		Offboard = (1 << 2)
+	};
+
+	// COM_LOW_BAT_ACT parameter values
+	enum class LowBatteryAction : int32_t {
+		Warning = 0,        // Warning
+		Return = 1,         // Return mode (deprecated)
+		Land = 2,           // Land mode
+		ReturnOrLand = 3    // Return mode at critically low level, Land mode at current position if reaching dangerously low levels
+	};
+
+	static ActionOptions fromNavDllOrRclActParam(int param_value);
+
+	static ActionOptions fromGfActParam(int param_value);
+	static ActionOptions fromImbalancedPropActParam(int param_value);
+	static ActionOptions fromActuatorFailureActParam(int param_value);
+	static ActionOptions fromBatteryWarningActParam(int param_value, uint8_t battery_warning);
+	static ActionOptions fromQuadchuteActParam(int param_value);
+	static Action fromOffboardLossActParam(int param_value, uint8_t &user_intended_mode);
+
+	const int _caller_id_mode_fallback{genCallerId()};
+	bool _last_state_mode_fallback{false};
+	const int _caller_id_mission_control_lost{genCallerId()};
+	bool _last_state_mission_control_lost{false};
+
+	const int _caller_id_battery_warning_low{genCallerId()}; ///< battery warning caller ID's: use different ID's as they have different actions
+	bool _last_state_battery_warning_low{false};
+	const int _caller_id_battery_warning_critical{genCallerId()};
+	bool _last_state_battery_warning_critical{false};
+	const int _caller_id_battery_warning_emergency{genCallerId()};
+	bool _last_state_battery_warning_emergency{false};
+
+	hrt_abstime _armed_time{0};
+	bool _was_armed{false};
+	bool _manual_control_lost_at_arming{false}; ///< true if manual control was lost at arming time
+
+	DEFINE_PARAMETERS_CUSTOM_PARENT(FailsafeBase,
+					(ParamInt<px4::params::NAV_DLL_ACT>) 	_param_nav_dll_act,
+					(ParamInt<px4::params::NAV_RCL_ACT>) 	_param_nav_rcl_act,
+					(ParamInt<px4::params::COM_RCL_EXCEPT>) _param_com_rcl_except,
+					(ParamInt<px4::params::COM_RC_IN_MODE>) _param_com_rc_in_mode,
+					(ParamInt<px4::params::COM_POSCTL_NAVL>) _param_com_posctl_navl,
+					(ParamInt<px4::params::GF_ACTION>)  	_param_gf_action,
+					(ParamFloat<px4::params::COM_SPOOLUP_TIME>) _param_com_spoolup_time,
+					(ParamInt<px4::params::COM_IMB_PROP_ACT>) _param_com_imb_prop_act,
+					(ParamFloat<px4::params::COM_LKDOWN_TKO>) _param_com_lkdown_tko,
+					(ParamInt<px4::params::CBRK_FLIGHTTERM>) _param_cbrk_flightterm,
+					(ParamInt<px4::params::COM_ACT_FAIL_ACT>) _param_com_actuator_failure_act,
+					(ParamInt<px4::params::COM_LOW_BAT_ACT>) _param_com_low_bat_act,
+					(ParamInt<px4::params::COM_OBL_RC_ACT>) _param_com_obl_rc_act,
+					(ParamInt<px4::params::COM_QC_ACT>) _param_com_qc_act
+				       );
+
+};
+

src/modules/commander/failsafe/failsafe_test.cpp

@@ -0,0 +1,260 @@
+/****************************************************************************
+ *
+ *   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 <gtest/gtest.h>
+
+#include "framework.h"
+#include <uORB/topics/vehicle_status.h>
+
+// to run: make tests TESTFILTER=failsafe_test
+
+using namespace time_literals;
+
+class FailsafeTester : public FailsafeBase
+{
+public:
+	FailsafeTester(ModuleParams *parent) : FailsafeBase(parent) {}
+
+protected:
+
+	void checkStateAndMode(const hrt_abstime &time_us, const State &state,
+			       const failsafe_flags_s &status_flags) override
+	{
+		CHECK_FAILSAFE(status_flags, manual_control_signal_lost,
+			       ActionOptions(Action::RTL).clearOn(ClearCondition::OnModeChangeOrDisarm));
+		CHECK_FAILSAFE(status_flags, gcs_connection_lost, Action::Descend);
+
+		if (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) {
+			CHECK_FAILSAFE(status_flags, mission_failure, Action::Descend);
+		}
+
+		CHECK_FAILSAFE(status_flags, wind_limit_exceeded,
+			       ActionOptions(Action::RTL).allowUserTakeover(UserTakeoverAllowed::Never));
+
+		_last_state_test = checkFailsafe(_caller_id_test, _last_state_test, status_flags.fd_motor_failure
+						 && status_flags.fd_critical_failure, Action::Terminate);
+	}
+
+	Action checkModeFallback(const failsafe_flags_s &status_flags, uint8_t user_intended_mode) const override
+	{
+		return Action::None;
+	}
+
+private:
+	const int _caller_id_test{genCallerId()};
+	bool _last_state_test{false};
+};
+
+class FailsafeTest : public ::testing::Test
+
+{
+public:
+
+	void SetUp() override
+	{
+		param_control_autosave(false);
+		param_t param = param_handle(px4::params::COM_FAIL_ACT_T);
+
+		float hold_delay = 5.f;
+		param_set(param, &hold_delay);
+	}
+
+};
+
+
+TEST_F(FailsafeTest, general)
+{
+	FailsafeTester failsafe(nullptr);
+
+	failsafe_flags_s failsafe_flags{};
+	FailsafeBase::State state{};
+	state.armed = true;
+	state.user_intended_mode = vehicle_status_s::NAVIGATION_STATE_POSCTL;
+	state.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
+	hrt_abstime time = 5_s;
+	bool stick_override_request = false;
+
+	uint8_t updated_user_intented_mode = failsafe.update(time, state, false, stick_override_request, failsafe_flags);
+
+	ASSERT_EQ(updated_user_intented_mode, state.user_intended_mode);
+	ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::None);
+
+	// RC lost -> Hold, then RTL
+	time += 10_ms;
+	failsafe_flags.manual_control_signal_lost = true;
+	updated_user_intented_mode = failsafe.update(time, state, false, stick_override_request, failsafe_flags);
+	ASSERT_EQ(updated_user_intented_mode, state.user_intended_mode);
+	ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::Hold);
+	time += 6_s;
+	updated_user_intented_mode = failsafe.update(time, state, false, stick_override_request, failsafe_flags);
+	ASSERT_EQ(updated_user_intented_mode, state.user_intended_mode);
+	ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::RTL);
+
+	// DL link lost -> Descend
+	time += 10_ms;
+	failsafe_flags.gcs_connection_lost = true;
+	updated_user_intented_mode = failsafe.update(time, state, false, stick_override_request, failsafe_flags);
+	ASSERT_EQ(updated_user_intented_mode, state.user_intended_mode);
+	ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::Descend);
+
+	// DL link regained -> RTL (RC still lost)
+	time += 10_ms;
+	failsafe_flags.gcs_connection_lost = false;
+	updated_user_intented_mode = failsafe.update(time, state, false, stick_override_request, failsafe_flags);
+	ASSERT_EQ(updated_user_intented_mode, state.user_intended_mode);
+	ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::RTL);
+
+	// RC lost cleared -> keep RTL
+	time += 10_ms;
+	failsafe_flags.manual_control_signal_lost = false;
+	updated_user_intented_mode = failsafe.update(time, state, false, stick_override_request, failsafe_flags);
+	ASSERT_EQ(updated_user_intented_mode, state.user_intended_mode);
+	ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::RTL);
+
+	// Mode change -> clear failsafe
+	time += 10_ms;
+	state.user_intended_mode = vehicle_status_s::NAVIGATION_STATE_ALTCTL;
+	updated_user_intented_mode = failsafe.update(time, state, false, stick_override_request, failsafe_flags);
+	ASSERT_EQ(updated_user_intented_mode, state.user_intended_mode);
+	ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::None);
+}
+
+TEST_F(FailsafeTest, takeover)
+{
+	FailsafeTester failsafe(nullptr);
+
+	failsafe_flags_s failsafe_flags{};
+	FailsafeBase::State state{};
+	state.armed = true;
+	state.user_intended_mode = vehicle_status_s::NAVIGATION_STATE_POSCTL;
+	state.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
+	hrt_abstime time = 3847124342;
+	bool stick_override_request = false;
+
+	uint8_t updated_user_intented_mode = failsafe.update(time, state, false, stick_override_request, failsafe_flags);
+
+	// Mission failure -> no failsafe
+	time += 10_ms;
+	failsafe_flags.mission_failure = true;
+	updated_user_intented_mode = failsafe.update(time, state, false, stick_override_request, failsafe_flags);
+	ASSERT_EQ(updated_user_intented_mode, state.user_intended_mode);
+	ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::None);
+
+	// Change to mission -> Hold, then Descend
+	time += 10_ms;
+	state.user_intended_mode = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION;
+	updated_user_intented_mode = failsafe.update(time, state, false, stick_override_request, failsafe_flags);
+	ASSERT_EQ(updated_user_intented_mode, state.user_intended_mode);
+	ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::Hold);
+	// Try stick movement during hold -> must be denied
+	time += 3_s;
+	stick_override_request = true;
+	updated_user_intented_mode = failsafe.update(time, state, false, stick_override_request, failsafe_flags);
+	stick_override_request = false;
+	ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::Hold);
+	// Now Descend
+	time += 3_s;
+	updated_user_intented_mode = failsafe.update(time, state, false, stick_override_request, failsafe_flags);
+	ASSERT_EQ(updated_user_intented_mode, state.user_intended_mode);
+	ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::Descend);
+
+	// Move sticks -> user takeover active
+	time += 10_ms;
+	stick_override_request = true;
+	updated_user_intented_mode = failsafe.update(time, state, false, stick_override_request, failsafe_flags);
+	state.user_intended_mode = vehicle_status_s::NAVIGATION_STATE_POSCTL;
+	ASSERT_EQ(updated_user_intented_mode, state.user_intended_mode);
+	ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::Warn);
+	ASSERT_TRUE(failsafe.userTakeoverActive());
+	stick_override_request = false;
+
+	// Now the failsafe must clear as we switched mode
+	time += 10_ms;
+	updated_user_intented_mode = failsafe.update(time, state, false, stick_override_request, failsafe_flags);
+	ASSERT_EQ(updated_user_intented_mode, state.user_intended_mode);
+	ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::None);
+	ASSERT_FALSE(failsafe.userTakeoverActive());
+}
+
+TEST_F(FailsafeTest, takeover_denied)
+{
+	FailsafeTester failsafe(nullptr);
+
+	failsafe_flags_s failsafe_flags{};
+	FailsafeBase::State state{};
+	state.armed = true;
+	state.user_intended_mode = vehicle_status_s::NAVIGATION_STATE_POSCTL;
+	state.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
+	hrt_abstime time = 3847124342;
+	bool stick_override_request = false;
+
+	uint8_t updated_user_intented_mode = failsafe.update(time, state, false, stick_override_request, failsafe_flags);
+
+	// Wind limit exceeded -> RTL w/o delay and denying takeover
+	time += 10_ms;
+	failsafe_flags.wind_limit_exceeded = true;
+	updated_user_intented_mode = failsafe.update(time, state, false, stick_override_request, failsafe_flags);
+	ASSERT_EQ(updated_user_intented_mode, state.user_intended_mode);
+	ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::RTL);
+	// Try takeover (mode switch + stick movements)
+	time += 10_ms;
+	stick_override_request = true;
+	state.user_intended_mode = vehicle_status_s::NAVIGATION_STATE_STAB;
+	updated_user_intented_mode = failsafe.update(time, state, false, stick_override_request, failsafe_flags);
+	stick_override_request = false;
+	ASSERT_EQ(updated_user_intented_mode, state.user_intended_mode);
+	ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::RTL);
+
+	// Test termination
+	failsafe_flags.fd_motor_failure = true;
+	failsafe_flags.fd_critical_failure = true;
+	time += 10_ms;
+	stick_override_request = true;
+	updated_user_intented_mode = failsafe.update(time, state, false, stick_override_request, failsafe_flags);
+	stick_override_request = false;
+	ASSERT_EQ(updated_user_intented_mode, state.user_intended_mode);
+	ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::Terminate);
+
+	// Termination is final
+	failsafe_flags.wind_limit_exceeded = false;
+	failsafe_flags.fd_motor_failure = false;
+	failsafe_flags.fd_critical_failure = false;
+	state.armed = false;
+	time += 10_ms;
+	stick_override_request = true;
+	updated_user_intented_mode = failsafe.update(time, state, false, stick_override_request, failsafe_flags);
+	stick_override_request = false;
+	ASSERT_EQ(updated_user_intented_mode, state.user_intended_mode);
+	ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::Terminate);
+}
+

src/modules/commander/failsafe/framework.cpp

@@ -0,0 +1,628 @@
+/****************************************************************************
+ *
+ *   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 "framework.h"
+#define DEFINE_GET_PX4_CUSTOM_MODE
+#include "../px4_custom_mode.h"
+
+#include <uORB/topics/vehicle_status.h>
+#include <px4_platform_common/events.h>
+#include <px4_platform_common/log.h>
+#include <systemlib/mavlink_log.h>
+
+using failsafe_action_t = events::px4::enums::failsafe_action_t;
+using failsafe_cause_t = events::px4::enums::failsafe_cause_t;
+
+using namespace time_literals;
+
+FailsafeBase::FailsafeBase(ModuleParams *parent) : ModuleParams(parent)
+{
+	_current_start_delay = _param_com_fail_act_t.get() * 1_s;
+}
+
+uint8_t FailsafeBase::update(const hrt_abstime &time_us, const State &state, bool user_intended_mode_updated,
+			     bool rc_sticks_takeover_request,
+			     const failsafe_flags_s &status_flags)
+{
+	if (_last_update == 0) {
+		_last_update = time_us;
+	}
+
+	if ((_last_armed && !state.armed) || (!_last_armed && state.armed)) { // Disarming or Arming
+		removeActions(ClearCondition::OnDisarm);
+		removeActions(ClearCondition::OnModeChangeOrDisarm);
+		_user_takeover_active = false;
+	}
+
+	user_intended_mode_updated |= _last_user_intended_mode != state.user_intended_mode;
+
+	if (user_intended_mode_updated || _user_takeover_active) {
+		removeActions(ClearCondition::OnModeChangeOrDisarm);
+	}
+
+	updateDelay(time_us - _last_update);
+
+	checkStateAndMode(time_us, state, status_flags);
+	removeNonActivatedActions();
+	clearDelayIfNeeded(state, status_flags);
+
+	SelectedActionState action_state{};
+	getSelectedAction(state, status_flags, user_intended_mode_updated, rc_sticks_takeover_request, action_state);
+
+	updateDelay(time_us - _last_update, action_state.delayed_action != Action::None);
+
+	// Notify user if the action is worse than before, or a new action got added
+	if (action_state.action > _selected_action || (action_state.action != Action::None && _notification_required)) {
+		notifyUser(state.user_intended_mode, action_state.action, action_state.delayed_action, action_state.cause);
+	}
+
+	_notification_required = false;
+
+	_last_user_intended_mode = modifyUserIntendedMode(_selected_action, action_state.action,
+				   action_state.updated_user_intended_mode);
+	_user_takeover_active = action_state.user_takeover;
+	_selected_action = action_state.action;
+	_last_update = time_us;
+	_last_status_flags = status_flags;
+	_last_armed = state.armed;
+	return _last_user_intended_mode;
+}
+
+void FailsafeBase::updateDelay(const hrt_abstime &dt, bool delay_active)
+{
+	// Ensure that even with a toggling state the delayed action is executed at some point.
+	// This is done by increasing the delay slower than reducing it.
+	if (delay_active) {
+		if (dt < _current_start_delay) {
+			_current_start_delay -= dt;
+
+		} else {
+			_current_start_delay = 0;
+		}
+
+	} else {
+		_current_start_delay += dt / 4;
+		hrt_abstime configured_delay = _param_com_fail_act_t.get() * 1_s;
+
+		if (_current_start_delay > configured_delay) {
+			_current_start_delay = configured_delay;
+		}
+	}
+}
+
+void FailsafeBase::updateParams()
+{
+	ModuleParams::updateParams();
+	_current_start_delay = _param_com_fail_act_t.get() * 1_s;
+}
+
+void FailsafeBase::updateDelay(const hrt_abstime &elapsed_us)
+{
+	if (_current_delay < elapsed_us) {
+		_current_delay = 0;
+
+	} else {
+		_current_delay -= elapsed_us;
+	}
+}
+
+void FailsafeBase::removeActions(ClearCondition condition)
+{
+	for (int action_idx = 0; action_idx < max_num_actions; ++action_idx) {
+		ActionOptions &cur_action = _actions[action_idx];
+
+		if (cur_action.valid() && !cur_action.state_failure && cur_action.clear_condition == condition) {
+			PX4_DEBUG("Caller %i: clear condition triggered, removing", cur_action.id);
+			cur_action.setInvalid();
+		}
+	}
+}
+
+void FailsafeBase::notifyUser(uint8_t user_intended_mode, Action action, Action delayed_action, Cause cause)
+{
+	int delay_s = (_current_delay + 500_ms) / 1_s;
+	PX4_DEBUG("User notification: failsafe triggered (action=%i, delayed_action=%i, cause=%i, delay=%is)", (int)action,
+		  (int)delayed_action, (int)cause, delay_s);
+
+#ifdef EMSCRIPTEN_BUILD
+	(void)_mavlink_log_pub;
+#else
+
+	px4_custom_mode custom_mode = get_px4_custom_mode(user_intended_mode);
+	uint32_t mavlink_mode = custom_mode.data;
+
+	static_assert((int)failsafe_cause_t::_max + 1 == (int)Cause::Count, "Enum needs to be extended");
+	static_assert((int)failsafe_action_t::_max + 1 == (int)Action::Count, "Enum needs to be extended");
+	failsafe_cause_t failsafe_cause = (failsafe_cause_t)cause;
+
+	if (action == Action::Hold && delayed_action != Action::None) {
+		failsafe_action_t failsafe_action = (failsafe_action_t)delayed_action;
+
+		if (cause == Cause::Generic) {
+			/* EVENT
+			* @type append_health_and_arming_messages
+			*/
+			events::send<uint32_t, events::px4::enums::failsafe_action_t, uint16_t>(
+				events::ID("commander_failsafe_enter_generic_hold"),
+			{events::Log::Critical, events::LogInternal::Warning},
+			"Failsafe activated, triggering {2} in {3} seconds", mavlink_mode, failsafe_action,
+			(uint16_t) delay_s);
+
+		} else {
+			/* EVENT
+			*/
+			events::send<uint32_t, events::px4::enums::failsafe_action_t, uint16_t, events::px4::enums::failsafe_cause_t>(
+				events::ID("commander_failsafe_enter_hold"),
+			{events::Log::Critical, events::LogInternal::Warning},
+			"Failsafe activated due to {4}, triggering {2} in {3} seconds", mavlink_mode, failsafe_action,
+			(uint16_t) delay_s, failsafe_cause);
+		}
+
+		mavlink_log_critical(&_mavlink_log_pub, "Failsafe activated, entering Hold for %i seconds\t", delay_s);
+
+	} else { // no delay
+		failsafe_action_t failsafe_action = (failsafe_action_t)action;
+
+		if (cause == Cause::Generic) {
+			if (action == Action::Warn) {
+				/* EVENT
+				* @description No action is triggered.
+				* @type append_health_and_arming_messages
+				*/
+				events::send<uint32_t>(
+					events::ID("commander_failsafe_enter_generic_warn"),
+				{events::Log::Warning, events::LogInternal::Warning},
+				"Failsafe warning triggered", mavlink_mode);
+
+			} else {
+				/* EVENT
+				* @type append_health_and_arming_messages
+				*/
+				events::send<uint32_t, events::px4::enums::failsafe_action_t>(
+					events::ID("commander_failsafe_enter_generic"),
+				{events::Log::Critical, events::LogInternal::Warning},
+				"Failsafe activated, triggering {2}", mavlink_mode, failsafe_action);
+			}
+
+		} else {
+			if (action == Action::Warn) {
+				if (cause == Cause::BatteryLow) {
+					events::send(events::ID("commander_failsafe_enter_low_bat"),
+					{events::Log::Warning, events::LogInternal::Info},
+					"Low battery level, return advised");
+
+				} else if (cause == Cause::BatteryCritical) {
+					events::send(events::ID("commander_failsafe_enter_crit_bat_warn"),
+					{events::Log::Critical, events::LogInternal::Info},
+					"Critical battery level, land now");
+
+				} else if (cause == Cause::BatteryEmergency) {
+					events::send(events::ID("commander_failsafe_enter_crit_low_bat_warn"), {events::Log::Emergency, events::LogInternal::Info},
+						     "Emergency battery level, land immediately");
+
+				} else {
+					/* EVENT
+					* @description No action is triggered.
+					*/
+					events::send<uint32_t, events::px4::enums::failsafe_cause_t>(
+						events::ID("commander_failsafe_enter_warn"),
+					{events::Log::Warning, events::LogInternal::Warning},
+					"Failsafe warning triggered due to {2}", mavlink_mode, failsafe_cause);
+
+				}
+
+			} else { // action != Warn
+				/* EVENT
+				*/
+				events::send<uint32_t, events::px4::enums::failsafe_action_t, events::px4::enums::failsafe_cause_t>(
+					events::ID("commander_failsafe_enter"),
+				{events::Log::Critical, events::LogInternal::Warning},
+				"Failsafe activated due to {3}, triggering {2}", mavlink_mode, failsafe_action, failsafe_cause);
+			}
+		}
+
+		mavlink_log_critical(&_mavlink_log_pub, "Failsafe activated\t");
+	}
+
+#endif /* EMSCRIPTEN_BUILD */
+}
+
+bool FailsafeBase::checkFailsafe(int caller_id, bool last_state_failure, bool cur_state_failure,
+				 const ActionOptions &options)
+{
+	if (cur_state_failure) {
+		// Invalid state: find or add action
+		int free_idx = -1;
+		int found_idx = -1;
+
+		for (int i = 0; i < max_num_actions; ++i) {
+			if (!_actions[i].valid()) {
+				free_idx = i;
+
+			} else if (_actions[i].id == caller_id) {
+				found_idx = i;
+				break;
+			}
+		}
+
+		if (found_idx != -1) {
+			if (_actions[found_idx].activated && !_duplicate_reported_once) {
+				PX4_ERR("BUG: duplicate check for caller_id %i", caller_id);
+				_duplicate_reported_once = true;
+			}
+
+			_actions[found_idx].state_failure = true;
+			_actions[found_idx].activated = true;
+			_actions[found_idx].action = options.action; // Allow action to be updated, but keep the rest
+
+			if (!last_state_failure) {
+				PX4_DEBUG("Caller %i: state changed to failed, action already active", caller_id);
+			}
+
+		} else {
+			if (free_idx == -1) {
+				PX4_ERR("No free failsafe action idx");
+
+				// replace based on action severity
+				for (int i = 0; i < max_num_actions; ++i) {
+					if (options.action > _actions[i].action) {
+						free_idx = i;
+					}
+				}
+			}
+
+			if (free_idx != -1) {
+				_actions[free_idx] = options;
+				_actions[free_idx].id = caller_id;
+				_actions[free_idx].state_failure = true;
+				_actions[free_idx].activated = true;
+
+				if (options.allow_user_takeover == UserTakeoverAllowed::Auto) {
+					if (_param_com_fail_act_t.get() > 0.1f) {
+						if (options.action != Action::Warn && _current_delay == 0) {
+							_current_delay = _current_start_delay;
+						}
+
+						_actions[free_idx].allow_user_takeover = UserTakeoverAllowed::Always;
+
+					} else {
+						_actions[free_idx].allow_user_takeover = UserTakeoverAllowed::AlwaysModeSwitchOnly;
+					}
+				}
+
+				if (options.action == Action::Warn) {
+					_notification_required = true;
+				}
+
+				if (options.action >= Action::Hold) { // If not a Fallback
+					_user_takeover_active = false; // Clear takeover
+				}
+
+				PX4_DEBUG("Caller %i: state changed to failed, adding action", caller_id);
+			}
+		}
+
+	} else if (last_state_failure && !cur_state_failure) {
+		// Invalid -> valid transition: remove action
+		bool found = false;
+
+		for (int i = 0; i < max_num_actions; ++i) {
+			if (_actions[i].id == caller_id) {
+				if (found) {
+					PX4_ERR("Dup action with ID %i", caller_id);
+				}
+
+				removeAction(_actions[i]);
+				found = true;
+			}
+		}
+
+		// It's ok if we did not find the action, it might already have been removed due to not being activated
+	}
+
+	return cur_state_failure;
+}
+
+void FailsafeBase::removeAction(ActionOptions &action) const
+{
+	if (action.clear_condition == ClearCondition::WhenConditionClears) {
+		// Remove action
+		PX4_DEBUG("Caller %i: state changed to valid, removing action", action.id);
+		action.setInvalid();
+
+	} else {
+		if (action.state_failure) {
+			PX4_DEBUG("Caller %i: state changed to valid, keeping action", action.id);
+		}
+
+		// Keep action, just flag the state
+		action.state_failure = false;
+	}
+}
+
+void FailsafeBase::removeNonActivatedActions()
+{
+	// A non-activated action means the check was not called during the last update:
+	// treat the state as valid and remove the action depending on the clear_condition
+	for (int action_idx = 0; action_idx < max_num_actions; ++action_idx) {
+		ActionOptions &cur_action = _actions[action_idx];
+
+		if (cur_action.valid() && !cur_action.activated) {
+			if (_actions[action_idx].state_failure) {
+				PX4_DEBUG("Caller %i: action not activated", cur_action.id);
+			}
+
+			removeAction(cur_action);
+		}
+
+		cur_action.activated = false;
+	}
+}
+
+
+void FailsafeBase::getSelectedAction(const State &state, const failsafe_flags_s &status_flags,
+				     bool user_intended_mode_updated,
+				     bool rc_sticks_takeover_request,
+				     SelectedActionState &returned_state) const
+{
+	returned_state.updated_user_intended_mode = state.user_intended_mode;
+	returned_state.cause = Cause::Generic;
+
+	if (_selected_action == Action::Terminate) { // Terminate never clears
+		returned_state.action = Action::Terminate;
+		return;
+	}
+
+	if (!state.armed) {
+		returned_state.action = Action::None;
+		return;
+	}
+
+	returned_state.action = Action::None;
+	Action &selected_action = returned_state.action;
+	UserTakeoverAllowed allow_user_takeover = UserTakeoverAllowed::Always;
+
+	// Select the worst action based on the current active actions
+	for (int action_idx = 0; action_idx < max_num_actions; ++action_idx) {
+		const ActionOptions &cur_action = _actions[action_idx];
+
+		if (cur_action.valid()) {
+			if (cur_action.allow_user_takeover > allow_user_takeover) {
+				// Use the most restrictive setting among all active actions
+				allow_user_takeover = cur_action.allow_user_takeover;
+			}
+
+			if (cur_action.action > selected_action) {
+				selected_action = cur_action.action;
+				returned_state.cause = cur_action.cause;
+			}
+		}
+	}
+
+	// Check if we should enter delayed Hold
+	if (_current_delay > 0 && !_user_takeover_active && allow_user_takeover <= UserTakeoverAllowed::AlwaysModeSwitchOnly
+	    && selected_action != Action::Disarm && selected_action != Action::Terminate) {
+		returned_state.delayed_action = selected_action;
+		selected_action = Action::Hold;
+		allow_user_takeover = UserTakeoverAllowed::AlwaysModeSwitchOnly;
+	}
+
+	// User takeover is activated on user intented mode update (w/o action change, so takeover is not immediately
+	// requested when entering failsafe) or rc stick movements
+	bool want_user_takeover_mode_switch = user_intended_mode_updated && _selected_action == selected_action;
+	bool want_user_takeover = want_user_takeover_mode_switch || rc_sticks_takeover_request;
+	bool takeover_allowed =
+		(allow_user_takeover == UserTakeoverAllowed::Always && (_user_takeover_active || want_user_takeover))
+		|| (allow_user_takeover == UserTakeoverAllowed::AlwaysModeSwitchOnly && (_user_takeover_active
+				|| want_user_takeover_mode_switch));
+
+	if (actionAllowsUserTakeover(selected_action) && takeover_allowed) {
+		if (!_user_takeover_active && rc_sticks_takeover_request) {
+			// TODO: if the user intended mode is a stick-controlled mode, switch back to that instead
+			returned_state.updated_user_intended_mode = vehicle_status_s::NAVIGATION_STATE_POSCTL;
+		}
+
+		selected_action = Action::Warn;
+		returned_state.user_takeover = true;
+		returned_state.delayed_action = Action::None;
+
+		if (!_user_takeover_active) {
+			PX4_DEBUG("Activating user takeover");
+#ifndef EMSCRIPTEN_BUILD
+			events::send(events::ID("failsafe_rc_override"), events::Log::Info, "Pilot took over using sticks");
+#endif // EMSCRIPTEN_BUILD
+		}
+
+		// We must check for mode fallback again here
+		Action mode_fallback = checkModeFallback(status_flags, modeFromAction(selected_action,
+				       returned_state.updated_user_intended_mode));
+
+		if (mode_fallback > selected_action) {
+			selected_action = mode_fallback;
+		}
+	}
+
+	// Check if the selected action is possible, and fall back if needed
+	switch (selected_action) {
+
+	case Action::FallbackPosCtrl:
+		if (modeCanRun(status_flags, vehicle_status_s::NAVIGATION_STATE_POSCTL)) {
+			selected_action = Action::FallbackPosCtrl;
+			break;
+		}
+
+	// fallthrough
+	case Action::FallbackAltCtrl:
+		if (modeCanRun(status_flags, vehicle_status_s::NAVIGATION_STATE_ALTCTL)) {
+			selected_action = Action::FallbackAltCtrl;
+			break;
+		}
+
+	// fallthrough
+	case Action::FallbackStab:
+		if (modeCanRun(status_flags, vehicle_status_s::NAVIGATION_STATE_STAB)) {
+			selected_action = Action::FallbackStab;
+			break;
+		} // else: fall through here as well. If stabilized isn't available, we most certainly end up in Terminate
+
+	// fallthrough
+	case Action::Hold:
+		if (modeCanRun(status_flags, vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER)) {
+			selected_action = Action::Hold;
+			break;
+		}
+
+	// fallthrough
+	case Action::RTL:
+		if (modeCanRun(status_flags, vehicle_status_s::NAVIGATION_STATE_AUTO_RTL)) {
+			selected_action = Action::RTL;
+			break;
+		}
+
+	// fallthrough
+	case Action::Land:
+		if (modeCanRun(status_flags, vehicle_status_s::NAVIGATION_STATE_AUTO_LAND)) {
+			selected_action = Action::Land;
+			break;
+		}
+
+	// fallthrough
+	case Action::Descend:
+		if (modeCanRun(status_flags, vehicle_status_s::NAVIGATION_STATE_DESCEND)) {
+			selected_action = Action::Descend;
+			break;
+		}
+
+	// fallthrough
+	case Action::Terminate:
+		selected_action = Action::Terminate;
+		break;
+
+	case Action::Disarm:
+		selected_action = Action::Disarm;
+		break;
+
+	case Action::None:
+	case Action::Warn:
+	case Action::Count:
+		break;
+	}
+
+	// UX improvement (this is optional for safety): change failsafe to a warning in certain situations.
+	// If already landing, do not go into RTL
+	if (returned_state.updated_user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND) {
+		if ((selected_action == Action::RTL || returned_state.delayed_action == Action::RTL)
+		    && modeCanRun(status_flags, vehicle_status_s::NAVIGATION_STATE_AUTO_LAND)) {
+			selected_action = Action::Warn;
+			returned_state.delayed_action = Action::None;
+		}
+	}
+
+	// If already precision landing, do not go into RTL or Land
+	if (returned_state.updated_user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND) {
+		if ((selected_action == Action::RTL || selected_action == Action::Land ||
+		     returned_state.delayed_action == Action::RTL || returned_state.delayed_action == Action::Land)
+		    && modeCanRun(status_flags, vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND)) {
+			selected_action = Action::Warn;
+			returned_state.delayed_action = Action::None;
+		}
+	}
+}
+
+bool FailsafeBase::actionAllowsUserTakeover(Action action) const
+{
+	// Stick-controlled modes do not need user takeover
+	return action == Action::Hold || action == Action::RTL || action == Action::Land || action == Action::Descend;
+}
+
+void FailsafeBase::clearDelayIfNeeded(const State &state,
+				      const failsafe_flags_s &status_flags)
+{
+	// Clear delay if one of the following is true:
+	// - Already in a failsafe
+	// - Hold not available
+	// - Takeover is active (due to a mode switch during the delay)
+	if (_selected_action > Action::Hold || !modeCanRun(status_flags, vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER)
+	    || _user_takeover_active) {
+		if (_current_delay > 0) {
+			PX4_DEBUG("Clearing delay, Hold not available, already in failsafe or taken over");
+		}
+
+		_current_delay = 0;
+	}
+}
+
+uint8_t FailsafeBase::modeFromAction(const Action &action, uint8_t user_intended_mode)
+{
+	switch (action) {
+
+	case Action::FallbackPosCtrl: return vehicle_status_s::NAVIGATION_STATE_POSCTL;
+
+	case Action::FallbackAltCtrl: return vehicle_status_s::NAVIGATION_STATE_ALTCTL;
+
+	case Action::FallbackStab: return vehicle_status_s::NAVIGATION_STATE_STAB;
+
+	case Action::Hold: return vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
+
+	case Action::RTL: return vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
+
+	case Action::Land: return vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
+
+	case Action::Descend: return vehicle_status_s::NAVIGATION_STATE_DESCEND;
+
+	case Action::Terminate:
+	case Action::Disarm:
+	case Action::None:
+	case Action::Warn:
+	case Action::Count:
+		break;
+	}
+
+	return user_intended_mode;
+}
+
+bool FailsafeBase::modeCanRun(const failsafe_flags_s &status_flags, uint8_t mode)
+{
+	uint32_t mode_mask = 1u << mode;
+	return
+		(!status_flags.angular_velocity_invalid || ((status_flags.mode_req_angular_velocity & mode_mask) == 0)) &&
+		(!status_flags.attitude_invalid || ((status_flags.mode_req_attitude & mode_mask) == 0)) &&
+		(!status_flags.local_position_invalid || ((status_flags.mode_req_local_position & mode_mask) == 0)) &&
+		(!status_flags.local_position_invalid_relaxed || ((status_flags.mode_req_local_position_relaxed & mode_mask) == 0)) &&
+		(!status_flags.global_position_invalid || ((status_flags.mode_req_global_position & mode_mask) == 0)) &&
+		(!status_flags.local_altitude_invalid || ((status_flags.mode_req_local_alt & mode_mask) == 0)) &&
+		(!status_flags.auto_mission_missing || ((status_flags.mode_req_mission & mode_mask) == 0)) &&
+		(!status_flags.offboard_control_signal_lost || ((status_flags.mode_req_offboard_signal & mode_mask) == 0)) &&
+		(!status_flags.home_position_invalid || ((status_flags.mode_req_home_position & mode_mask) == 0)) &&
+		((status_flags.mode_req_other & mode_mask) == 0);
+}

src/modules/commander/failsafe/framework.h

@@ -0,0 +1,261 @@
+/****************************************************************************
+ *
+ *   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 <uORB/topics/failsafe_flags.h>
+#include <drivers/drv_hrt.h>
+#include <px4_platform_common/module_params.h>
+
+#include <cstddef>
+
+#define CHECK_FAILSAFE(status_flags, flag_name, options) \
+	checkFailsafe((int)offsetof(failsafe_flags_s, flag_name), lastStatusFlags().flag_name, status_flags.flag_name, options)
+
+class FailsafeBase: public ModuleParams
+{
+public:
+	enum class Action : uint8_t {
+		// Actions further down take precedence
+		None,
+		Warn,
+
+		// Fallbacks
+		FallbackPosCtrl,
+		FallbackAltCtrl,
+		FallbackStab,
+
+		Hold,
+		RTL,
+		Land,
+		Descend,
+		Disarm,
+		Terminate,
+
+		Count
+	};
+
+	enum class ClearCondition : uint8_t {
+		WhenConditionClears,  ///< Disable action when condition that triggered it cleared
+		OnModeChangeOrDisarm, ///< Disable after condition cleared AND (mode switch OR disarm happens)
+		OnDisarm,             ///< Disable after condition cleared AND disarm happens
+		Never,                ///< Never clear the condition, require reboot to clear
+	};
+
+	enum class Cause : uint8_t {
+		Generic,
+		ManualControlLoss,
+		GCSConnectionLoss,
+		BatteryLow,
+		BatteryCritical,
+		BatteryEmergency,
+
+		Count
+	};
+
+	static const char *actionStr(Action action)
+	{
+		switch (action) {
+		case Action::None: return "None";
+
+		case Action::Warn: return "Warn";
+
+		case Action::FallbackPosCtrl: return "Fallback to PosCtrl";
+
+		case Action::FallbackAltCtrl: return "Fallback to AltCtrl";
+
+		case Action::FallbackStab: return "Fallback to Stabilized";
+
+		case Action::Hold: return "Hold";
+
+		case Action::RTL: return "RTL";
+
+		case Action::Land: return "Land";
+
+		case Action::Descend: return "Descend";
+
+		case Action::Disarm: return "Disarm";
+
+		case Action::Terminate: return "Terminate";
+
+		case Action::Count: return "(invalid)";
+		}
+	}
+
+	enum class RcOverrideBits : int32_t {
+		AUTO_MODE_BIT = (1 << 0),
+		OFFBOARD_MODE_BIT = (1 << 1),
+	};
+
+	struct State {
+		bool armed{false};
+		uint8_t user_intended_mode{0};
+		uint8_t vehicle_type;
+		bool vtol_in_transition_mode{false};
+		bool mission_finished{false};
+	};
+
+	FailsafeBase(ModuleParams *parent);
+
+	/**
+	 * update the state machine
+	 *
+	 * @param time_us current time
+	 * @param state vehicle state
+	 * @param user_intended_mode_updated true if state.user_intended_mode got set (it may not have changed)
+	 * @param rc_sticks_takeover_request true if sticks are moved, and therefore requesting user takeover if in failsafe
+	 * @param status_flags condition flags
+	 * @return uint8_t updated user intended mode (changes when user wants to take over)
+	 */
+	uint8_t update(const hrt_abstime &time_us, const State &state, bool user_intended_mode_updated,
+		       bool rc_sticks_takeover_request,
+		       const failsafe_flags_s &status_flags);
+
+	bool inFailsafe() const { return _selected_action != Action::None; }
+
+	Action selectedAction() const { return _selected_action; }
+
+	static uint8_t modeFromAction(const Action &action, uint8_t user_intended_mode);
+
+	bool userTakeoverActive() const { return _user_takeover_active; }
+
+protected:
+	enum class UserTakeoverAllowed {
+		Always, ///< allow takeover (immediately)
+		AlwaysModeSwitchOnly, ///<  allow takeover (immediately), but not via stick movements, only by mode switch
+		Auto,   ///< allow takeover depending on delay parameter: if >0 go into hold first (w/o stick takeover possible), then take action
+		Never,  ///< never allow takeover
+	};
+
+	struct ActionOptions {
+		ActionOptions(Action action_ = Action::None) : action(action_) {}
+		ActionOptions &allowUserTakeover(UserTakeoverAllowed allow = UserTakeoverAllowed::Auto) { allow_user_takeover = allow; return *this; }
+		ActionOptions &clearOn(ClearCondition clear_condition_) { clear_condition = clear_condition_; return *this; }
+		ActionOptions &causedBy(Cause cause_) { cause = cause_; return *this; }
+
+		bool valid() const { return id != -1; }
+		void setInvalid() { id = -1; }
+
+		Action action{Action::None};
+		ClearCondition clear_condition{ClearCondition::WhenConditionClears};
+		Cause cause{Cause::Generic};
+		UserTakeoverAllowed allow_user_takeover{UserTakeoverAllowed::Auto};
+
+		bool state_failure{false}; ///< used when the clear_condition isn't set to clear immediately
+		bool activated{false}; ///< true if checkFailsafe was called during current update
+		int id{-1}; ///< unique caller id
+	};
+
+	struct SelectedActionState {
+		Action action{Action::None};
+		Action delayed_action{Action::None};
+		Cause cause{Cause::Generic};
+		uint8_t updated_user_intended_mode{};
+		bool user_takeover{false};
+	};
+
+	virtual void checkStateAndMode(const hrt_abstime &time_us, const State &state,
+				       const failsafe_flags_s &status_flags) = 0;
+	virtual Action checkModeFallback(const failsafe_flags_s &status_flags, uint8_t user_intended_mode) const = 0;
+
+	const failsafe_flags_s &lastStatusFlags() const { return _last_status_flags; }
+
+	bool checkFailsafe(int caller_id, bool last_state_failure, bool cur_state_failure, const ActionOptions &options);
+
+	virtual void getSelectedAction(const State &state, const failsafe_flags_s &status_flags,
+				       bool user_intended_mode_updated,
+				       bool rc_sticks_takeover_request,
+				       SelectedActionState &returned_state) const;
+
+	int genCallerId() { return ++_next_caller_id; }
+
+	static bool modeCanRun(const failsafe_flags_s &status_flags, uint8_t mode);
+
+	/**
+	 * Allows to modify the user intended mode. Use only in limited cases.
+	 *
+	 * @param previous_action previous action
+	 * @param current_action current action
+	 * @param user_intended_mode current user intended mode
+	 * @return uint8_t new user intended mode
+	 */
+	virtual uint8_t modifyUserIntendedMode(Action previous_action, Action current_action,
+					       uint8_t user_intended_mode) const { return user_intended_mode; }
+
+	void updateParams() override;
+
+private:
+	/**
+	 * Remove actions matching a condition
+	 */
+	void removeActions(ClearCondition condition);
+	/**
+	 * Try to remove an action, depending on the clear_condition
+	 */
+	void removeAction(ActionOptions &action) const;
+	void removeNonActivatedActions();
+
+	void updateDelay(const hrt_abstime &elapsed_us);
+	void notifyUser(uint8_t user_intended_mode, Action action, Action delayed_action, Cause cause);
+
+	void clearDelayIfNeeded(const State &state, const failsafe_flags_s &status_flags);
+
+	bool actionAllowsUserTakeover(Action action) const;
+
+	void updateDelay(const hrt_abstime &dt, bool delay_active);
+
+	static constexpr int max_num_actions{8};
+	ActionOptions _actions[max_num_actions]; ///< currently active actions
+
+	hrt_abstime _last_update{};
+	bool _last_armed{false};
+	uint8_t _last_user_intended_mode{0};
+	failsafe_flags_s _last_status_flags{};
+	Action _selected_action{Action::None};
+	bool _user_takeover_active{false};
+	bool _notification_required{false};
+
+	hrt_abstime _current_start_delay{0}; ///< _current_delay is set to this value when starting the delay
+	hrt_abstime _current_delay{0}; ///< If > 0, stay in Hold, and take action once delay reaches 0
+
+	int _next_caller_id{sizeof(failsafe_flags_s) + 1};
+	bool _duplicate_reported_once{false};
+
+	orb_advert_t _mavlink_log_pub{nullptr};
+
+	DEFINE_PARAMETERS_CUSTOM_PARENT(ModuleParams,
+					(ParamFloat<px4::params::COM_FAIL_ACT_T>) 	_param_com_fail_act_t
+				       );
+
+};
+

src/modules/commander/failsafe/parse_flags_from_msg.py

@@ -0,0 +1,140 @@
+#!/usr/bin/env python3
+
+"""
+Extract fields from .msg for failsafe state machine simulation
+"""
+import math
+import os
+import argparse
+import sys
+import re
+
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser(description='Extract fields from .msg files')
+    parser.add_argument('msg_file', help='failsafe_flags.msg file')
+    parser.add_argument('header_file', help='generated_uorb_struct_field_mapping.h')
+    parser.add_argument('html_template', help='HTML template input file')
+    parser.add_argument('html_output', help='HTML output file')
+    args = parser.parse_args()
+
+    msg_filename = args.msg_file
+    header_file = args.header_file
+    html_template_file = args.html_template
+    html_output_file = args.html_output
+    msg_name = os.path.splitext(os.path.basename(msg_filename))[0]
+
+    groups = []
+    class Group:
+        def __init__(self, group_name):
+            self.name = group_name
+            self.fields = [] # tuples of (field_name, comment)
+            self.new_row = False
+
+    group_name = ''
+    new_group = True
+    num_fields = 0
+
+    with open(msg_filename, 'r') as lineparser:
+        while True:
+            line = lineparser.readline()
+            if not line:
+                break
+            line = line.strip()
+            if line == '':
+                new_group = True
+                continue
+            if line.startswith('#'):
+                # use comment & empty lines for grouping and group names
+                group_name = line[1:].strip()
+                continue
+            if not line.startswith('bool') and not line.startswith('uint8'):
+                # only booleans & uint8 supported
+                continue
+            field_search = re.search('^(\w+)\s+(\w+)', line)
+            field_name = field_search.group(2)
+            if field_name.startswith('mode_req_') or field_name == 'timestamp':
+                continue
+
+            field_search = re.search('^(\w+)\s+(\w+)\s+#(.*)$', line)
+            if not field_search:
+                raise Exception("{:}:\nFailed to extract fields from '{:}' (missing comment?)".format(msg_filename, line))
+
+            if new_group:
+                groups.append(Group(group_name))
+                new_group = False
+                group_name = ''
+            groups[-1].fields.append((field_search.group(1), field_search.group(2), field_search.group(3).strip()))
+            num_fields += 1
+
+    # Split conditions into rows
+    num_rows = 2
+    num_fields_per_row = math.ceil(num_fields / num_rows)
+    current_num_fields = 0
+    for group in groups:
+        current_num_fields += len(group.fields)
+        if current_num_fields > num_fields_per_row:
+            group.new_row = True
+            current_num_fields = 0
+
+    # Header file
+    macro_lines = ''
+    for group in groups:
+        for field_type, field_name, comment in group.fields:
+            macro_lines += '   .property("{0}", &{1}_s::{0}) \\\n'.format(field_name, msg_name)
+
+    cpp_emscription_macro = '#define UORB_STRUCT_FIELD_MAPPING \\\n{}\n'.format(macro_lines)
+
+    with open(header_file, 'w') as file:
+        file.write(cpp_emscription_macro)
+
+    # JS + Html
+    js_tag = '{{{ JS_STATE_CODE }}}'
+    html_tag = '{{{ HTML_CONDITIONS }}}'
+    js_code = ''
+    html_conditions = ''
+    html_conditions += '<table><tr><td>'
+    for group in groups:
+        if group.new_row:
+            html_conditions += '</td><td>'
+        html_conditions += '<p>'
+        if group.name != '':
+            html_conditions += '<h5>' + group.name + '</h5>'
+        for field_type, field_name, comment in group.fields:
+            if field_type == 'bool':
+                js_code += "    state.{0} = document.querySelector('input[id=\"{0}\"]').checked;\n".format(field_name)
+                html_conditions += '''
+                            <div class="checkbox-field">
+                                <input type="checkbox" id="{0}" name="{0}">
+                                <label for="{0}">{1}</label>
+                            </div>
+    '''.format(field_name, comment)
+
+            elif field_type == 'uint8':
+                assert field_name == 'battery_warning', "only battery_warning supported for uint8 type"
+                js_code += "    state.{0} = document.querySelector('select[id=\"{0}\"]').value;\n".format(field_name)
+                html_conditions += '''
+                            <div>
+                                <label for="{0}">{1}:&nbsp;</label>
+                                <select id="{0}" name="{0}">
+                                    <option value="0">None</option>
+                                    <option value="1">Low</option>
+                                    <option value="2">Critical</option>
+                                    <option value="3">Emergency</option>
+                                </select>
+                            </div>
+    '''.format(field_name, comment)
+            else:
+                raise Exception("Unsupported type: {:}".format(field_type))
+        html_conditions += '</p>'
+    html_conditions += '</td></tr></table>'
+
+    with open(html_template_file, 'r') as file:
+        html_template_content = file.read()
+    assert js_tag in html_template_content, "js_tag not found in {}".format(html_template_file)
+    assert html_tag in html_template_content, "html_tag not found in {}".format(html_template_file)
+    html_template_content = html_template_content.replace(js_tag, js_code)
+    html_template_content = html_template_content.replace(html_tag, html_conditions)
+    with open(html_output_file, 'w') as file:
+        file.write(html_template_content)
+

src/modules/commander/px4_custom_mode.h

@@ -37,8 +37,7 @@
  *
  */
 
-#ifndef PX4_CUSTOM_MODE_H_
-#define PX4_CUSTOM_MODE_H_
+#pragma once
 
 #include <stdint.h>
 
@@ -86,4 +85,96 @@ union px4_custom_mode {
 	};
 };
 
-#endif /* PX4_CUSTOM_MODE_H_ */
+#ifdef DEFINE_GET_PX4_CUSTOM_MODE
+#include <uORB/topics/vehicle_status.h>
+
+static inline union px4_custom_mode get_px4_custom_mode(uint8_t nav_state)
+{
+	union px4_custom_mode custom_mode;
+	custom_mode.data = 0;
+
+	switch (nav_state) {
+	case vehicle_status_s::NAVIGATION_STATE_MANUAL:
+		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
+		break;
+
+	case vehicle_status_s::NAVIGATION_STATE_ALTCTL:
+		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTL;
+		break;
+
+	case vehicle_status_s::NAVIGATION_STATE_POSCTL:
+		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTL;
+		break;
+
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
+		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
+		custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
+		break;
+
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
+		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
+		custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
+		break;
+
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
+		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
+		custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL;
+		break;
+
+	case vehicle_status_s::NAVIGATION_STATE_ACRO:
+		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ACRO;
+		break;
+
+	case vehicle_status_s::NAVIGATION_STATE_DESCEND:
+		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
+		custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND;
+		break;
+
+	case vehicle_status_s::NAVIGATION_STATE_TERMINATION:
+		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
+		break;
+
+	case vehicle_status_s::NAVIGATION_STATE_OFFBOARD:
+		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_OFFBOARD;
+		break;
+
+	case vehicle_status_s::NAVIGATION_STATE_STAB:
+		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_STABILIZED;
+		break;
+
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF:
+		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
+		custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF;
+		break;
+
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND:
+		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
+		custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND;
+		break;
+
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET:
+		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
+		custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET;
+		break;
+
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND:
+		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
+		custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND;
+		break;
+
+	case vehicle_status_s::NAVIGATION_STATE_ORBIT:
+		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTL;
+		custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_POSCTL_ORBIT;
+		break;
+
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF:
+		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
+		custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_VTOL_TAKEOFF;
+		break;
+	}
+
+	return custom_mode;
+}
+
+#endif /* DEFINE_GET_PX4_CUSTOM_MODE */
+

src/modules/commander/state_machine_helper.h

@@ -1,158 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2013, 2014 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.
- *
- ****************************************************************************/
-
-/**
- * @file state_machine_helper.h
- * State machine helper functions definitions
- *
- * @author Thomas Gubler <thomasgubler@student.ethz.ch>
- * @author Julian Oes <julian@oes.ch>
- */
-
-#ifndef STATE_MACHINE_HELPER_H_
-#define STATE_MACHINE_HELPER_H_
-
-#include <drivers/drv_hrt.h>
-
-#include <uORB/uORB.h>
-#include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/battery_status.h>
-#include <uORB/topics/actuator_armed.h>
-#include <uORB/topics/commander_state.h>
-#include <uORB/topics/vehicle_status_flags.h>
-#include <px4_platform_common/events.h>
-
-typedef enum {
-	TRANSITION_DENIED = -1,
-	TRANSITION_NOT_CHANGED = 0,
-	TRANSITION_CHANGED
-} transition_result_t;
-
-enum class link_loss_actions_t {
-	DISABLED = 0,
-	AUTO_LOITER = 1,	// Hold mode
-	AUTO_RTL = 2,		// Return mode
-	AUTO_LAND = 3,		// Land mode
-	TERMINATE = 5,		// Terminate flight (set actuator outputs to failsafe values, and stop controllers)
-	LOCKDOWN = 6,		// Lock actuators (set actuator outputs to disarmed values)
-};
-
-enum class quadchute_actions_t {
-	NO_ACTION = -1,
-	AUTO_RTL = 0,		// Return mode
-	AUTO_LAND = 1,		// Land mode
-	AUTO_LOITER = 2,	// Hold mode
-};
-
-enum class offboard_loss_actions_t {
-	DISABLED = -1,
-	AUTO_LAND = 0,		// Land mode
-	AUTO_LOITER = 1,	// Hold mode
-	AUTO_RTL = 2,		// Return mode
-	TERMINATE = 3,		// Terminate flight (set actuator outputs to failsafe values, and stop controllers)
-	LOCKDOWN = 4,		// Lock actuators (set actuator outputs to disarmed values)
-};
-
-enum class offboard_loss_rc_actions_t {
-	DISABLED = -1, 		// Disabled
-	MANUAL_POSITION = 0, 	// Position mode
-	MANUAL_ALTITUDE = 1, 	// Altitude mode
-	MANUAL_ATTITUDE = 2, 	// Manual
-	AUTO_RTL = 3, 		// Return mode
-	AUTO_LAND = 4, 		// Land mode
-	AUTO_LOITER = 5, 	// Hold mode
-	TERMINATE = 6, 		// Terminate flight (set actuator outputs to failsafe values, and stop controllers)
-	LOCKDOWN = 7, 		// Lock actuators (set actuator outputs to disarmed values)
-};
-
-enum class position_nav_loss_actions_t {
-	ALTITUDE_MANUAL = 0,	// Altitude/Manual. Assume use of remote control after fallback. Switch to Altitude mode if a height estimate is available, else switch to MANUAL.
-	LAND_TERMINATE = 1,	// Land/Terminate.  Assume no use of remote control after fallback. Switch to Land mode if a height estimate is available, else switch to TERMINATION.
-};
-
-extern const char *const nav_state_names[];
-
-enum RCLossExceptionBits {
-	RCL_EXCEPT_MISSION = (1 << 0),
-	RCL_EXCEPT_HOLD = (1 << 1),
-	RCL_EXCEPT_OFFBOARD = (1 << 2)
-};
-
-transition_result_t
-main_state_transition(const vehicle_status_s &status, const main_state_t new_main_state,
-		      const vehicle_status_flags_s &status_flags, commander_state_s &internal_state);
-
-bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_state_s &internal_state,
-		   orb_advert_t *mavlink_log_pub, const link_loss_actions_t data_link_loss_act, const bool mission_finished,
-		   const bool stay_in_failsafe, const vehicle_status_flags_s &status_flags, bool landed,
-		   const link_loss_actions_t rc_loss_act, const offboard_loss_actions_t offb_loss_act,
-		   const quadchute_actions_t quadchute_act,
-		   const offboard_loss_rc_actions_t offb_loss_rc_act,
-		   const position_nav_loss_actions_t posctl_nav_loss_act,
-		   const float param_com_rcl_act_t, const int param_com_rcl_except);
-
-/*
- * Checks the validty of position data against the requirements of the current navigation
- * mode and switches mode if position data required is not available.
- */
-bool check_invalid_pos_nav_state(vehicle_status_s &status, bool old_failsafe, orb_advert_t *mavlink_log_pub,
-				 const vehicle_status_flags_s &status_flags, const bool use_rc, const bool using_global_pos);
-
-
-// COM_LOW_BAT_ACT parameter values
-typedef enum LOW_BAT_ACTION {
-	WARNING = 0,		// Warning
-	RETURN = 1,			// Return mode
-	LAND = 2,			// Land mode
-	RETURN_OR_LAND = 3	// Return mode at critically low level, Land mode at current position if reaching dangerously low levels
-} low_battery_action_t;
-
-void warn_user_about_battery(orb_advert_t *mavlink_log_pub, const uint8_t battery_warning,
-			     const uint8_t failsafe_action, const float param_com_bat_act_t,
-			     const char *failsafe_action_string, const events::px4::enums::navigation_mode_t failsafe_action_navigation_mode);
-uint8_t get_battery_failsafe_action(const commander_state_s &internal_state, const uint8_t battery_warning,
-				    const low_battery_action_t param_com_low_bat_act);
-
-// COM_IMB_PROP_ACT parameter values
-enum class imbalanced_propeller_action_t {
-	DISABLED = -1,
-	WARNING = 0,
-	RETURN = 1,
-	LAND = 2
-};
-
-void imbalanced_prop_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status,
-			      const vehicle_status_flags_s &status_flags, commander_state_s *internal_state,
-			      const imbalanced_propeller_action_t failsafe_action);
-
-#endif /* STATE_MACHINE_HELPER_H_ */

src/modules/ekf2/CMakeLists.txt

@@ -33,35 +33,38 @@
 
 add_subdirectory(Utility)
 
-# Symforce code generation
-execute_process(
-    COMMAND ${PYTHON_EXECUTABLE} -m symforce.symbolic
-    RESULT_VARIABLE PYTHON_SYMFORCE_EXIT_CODE
-    OUTPUT_QUIET
-)
-if(${PYTHON_SYMFORCE_EXIT_CODE} EQUAL 0)
+# Symforce code generation TODO:fixme
+# execute_process(
+#     COMMAND ${PYTHON_EXECUTABLE} -m symforce.symbolic
+#     RESULT_VARIABLE PYTHON_SYMFORCE_EXIT_CODE
+#     OUTPUT_QUIET
+# )
+# if(${PYTHON_SYMFORCE_EXIT_CODE} EQUAL 0)
 
-	set(EKF_DERIVATION_DIR ${CMAKE_CURRENT_SOURCE_DIR}/EKF/python/ekf_derivation)
+# 	set(EKF_DERIVATION_DIR ${CMAKE_CURRENT_SOURCE_DIR}/EKF/python/ekf_derivation)
 
-	set(EKF_GENERATED_SRC_FILES
-		${EKF_DERIVATION_DIR}/generated/compute_airspeed_h_and_k.h
-		${EKF_DERIVATION_DIR}/generated/compute_airspeed_innov_and_innov_var.h
-	)
+# 	set(EKF_GENERATED_SRC_FILES
+# 		${EKF_DERIVATION_DIR}/generated/compute_airspeed_h_and_k.h
+# 		${EKF_DERIVATION_DIR}/generated/compute_airspeed_innov_and_innov_var.h
+# 		${EKF_DERIVATION_DIR}/generated/compute_sideslip_h_and_k.h
+# 		${EKF_DERIVATION_DIR}/generated/compute_sideslip_innov_and_innov_var.h
+# 		${EKF_DERIVATION_DIR}/generated/covariance_prediction.h
+# 	)
 
-	add_custom_command(
-		OUTPUT ${EKF_GENERATED_SRC_FILES}
-		COMMAND ${PYTHON_EXECUTABLE} derivation.py
-		DEPENDS
-			${EKF_DERIVATION_DIR}/derivation.py
-			${EKF_DERIVATION_DIR}/derivation_utils.py
+# 	add_custom_command(
+# 		OUTPUT ${EKF_GENERATED_SRC_FILES}
+# 		COMMAND ${PYTHON_EXECUTABLE} derivation.py
+# 		DEPENDS
+# 			${EKF_DERIVATION_DIR}/derivation.py
+# 			${EKF_DERIVATION_DIR}/derivation_utils.py
 
-		WORKING_DIRECTORY ${EKF_DERIVATION_DIR}
-		COMMENT "Symforce code generation"
-		USES_TERMINAL
-	)
+# 		WORKING_DIRECTORY ${EKF_DERIVATION_DIR}
+# 		COMMENT "Symforce code generation"
+# 		USES_TERMINAL
+# 	)
 
-	add_custom_target(ekf2_symforce_generate DEPENDS ${EKF_GENERATED_SRC_FILES})
-endif()
+# 	add_custom_target(ekf2_symforce_generate DEPENDS ${EKF_GENERATED_SRC_FILES})
+# endif()
 
 px4_add_module(
 	MODULE modules__ekf2

src/modules/ekf2/EKF/airspeed_fusion.cpp

@@ -52,7 +52,7 @@
 void Ekf::updateAirspeed(const airspeedSample &airspeed_sample, estimator_aid_source_1d_s &airspeed) const
 {
 	// reset flags
-	resetEstimatorAidStatusFlags(airspeed);
+	resetEstimatorAidStatus(airspeed);
 
 	// Variance for true airspeed measurement - (m/sec)^2
 	const float R = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) *