Set feedforward heightrate setpoint in offboard mode

This commit fixes feedforward acceleration setpoints for fixedwing offboard position control.

Previously when acceleration feedforward inputs were sent, negative curvature accelerations were not being computed properly

.ci/Jenkinsfile-hardware

@@ -937,7 +937,7 @@ void printTopics() {
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_odometry" || true'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_rates_setpoint" || true'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_status" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_status_flags" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener failsafe_flags" || true'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vtol_vehicle_status" || true'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener yaw_estimator_status" || true'
 }

.github/workflows/failsafe_sim.yml

@@ -0,0 +1,44 @@
+name: Failsafe Simulator Build
+
+on:
+  push:
+    branches:
+    - 'main'
+  pull_request:
+    branches:
+    - '*'
+
+jobs:
+  build:
+    runs-on: ubuntu-latest
+    defaults:
+      run:
+        shell: bash
+    strategy:
+      fail-fast: false
+      matrix:
+        check: [
+          "failsafe_web",
+        ]
+    container:
+      image: px4io/px4-dev-nuttx-focal:2021-09-08
+      options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined
+    steps:
+    - uses: actions/checkout@v1
+      with:
+        token: ${{ secrets.ACCESS_TOKEN }}
+
+    - name: check environment
+      run: |
+          export
+          ulimit -a
+    - name: install emscripten
+      run: |
+        git clone https://github.com/emscripten-core/emsdk.git _emscripten_sdk
+        cd _emscripten_sdk
+        ./emsdk install latest
+        ./emsdk activate latest
+    - name: ${{matrix.check}}
+      run: |
+        . ./_emscripten_sdk/emsdk_env.sh
+        make ${{matrix.check}}

Jenkinsfile

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

Makefile

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

README.md

@@ -4,7 +4,7 @@
 
 [![Nuttx Targets](https://github.com/PX4/PX4-Autopilot/workflows/Nuttx%20Targets/badge.svg)](https://github.com/PX4/PX4-Autopilot/actions?query=workflow%3A%22Nuttx+Targets%22?branch=master) [![SITL Tests](https://github.com/PX4/PX4-Autopilot/workflows/SITL%20Tests/badge.svg?branch=master)](https://github.com/PX4/PX4-Autopilot/actions?query=workflow%3A%22SITL+Tests%22)
 
-[![Slack](/.github/slack.svg)](https://join.slack.com/t/px4/shared_invite/zt-si4xo5qs-R4baYFmMjlrT4rQK5yUnaA)
+[![Discord Shield](https://discordapp.com/api/guilds/1022170275984457759/widget.png?style=shield)](https://discord.gg/dronecode)
 
 This repository holds the [PX4](http://px4.io) flight control solution for drones, with the main applications located in the [src/modules](https://github.com/PX4/PX4-Autopilot/tree/main/src/modules) directory. It also contains the PX4 Drone Middleware Platform, which provides drivers and middleware to run drones.
 

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

@@ -0,0 +1,75 @@
+#!/bin/sh
+#
+# @name Aion Robotics R1 UGV
+#
+# @url https://www.aionrobotics.com/r1
+#
+# @type Rover
+# @class Rover
+#
+# @board px4_fmu-v2 exclude
+# @board bitcraze_crazyflie exclude
+#
+
+. ${R}etc/init.d/rc.rover_defaults
+
+param set-default BAT1_N_CELLS 4
+
+param set-default EKF2_GBIAS_INIT 0.01
+param set-default EKF2_ANGERR_INIT 0.01
+param set-default EKF2_MAG_TYPE 1
+
+param set-default FW_AIRSPD_MIN 0
+param set-default FW_AIRSPD_TRIM 1
+param set-default FW_AIRSPD_MAX 3
+
+param set-default GND_SP_CTRL_MODE 1
+param set-default GND_L1_DIST 5
+param set-default GND_L1_PERIOD 3
+param set-default GND_THR_CRUISE 0.7
+param set-default GND_THR_MAX 0.5
+
+# Because this is differential drive, it can make a turn with radius 0.
+# This corresponds to a turn angle of pi radians.
+# If a special case is made for differential-drive, this will need to change.
+param set-default GND_MAX_ANG 3.142
+param set-default GND_WHEEL_BASE 0.3
+
+# TODO: Set to -1.0, to allow reversing. This will require many changes in the codebase
+# to support negative throttle.
+param set-default GND_THR_MIN 0
+param set-default GND_SPEED_P 0.25
+param set-default GND_SPEED_I 3
+param set-default GND_SPEED_D 0.001
+param set-default GND_SPEED_IMAX 0.125
+param set-default GND_SPEED_THR_SC 1
+
+param set-default MIS_LTRMIN_ALT 0.01
+param set-default MIS_TAKEOFF_ALT 0.01
+
+param set-default NAV_ACC_RAD 0.5
+
+# Enable Airspeed check circuit breaker because Rovers will have no airspeed sensor
+param set-default CBRK_AIRSPD_CHK 162128
+
+# Differential drive acts like ackermann steering with a maximum turn angle of 180 degrees, or pi radians
+param set-default GND_MAX_ANG 3.1415
+
+param set-default RBCLW_BAUD 8
+param set-default RBCLW_COUNTS_REV 1200
+param set-default RBCLW_ADDRESS 128
+# 104 corresponds to Telem 4
+param set-default RBCLW_SER_CFG 104
+# Start this driver after setting parameters, because the driver uses some of those parameters.
+# roboclaw start /dev/ttyS3
+
+# Set geometry & output configration
+param set-default CA_AIRFRAME 6
+param set-default CA_R_REV 3
+param set-default PWM_MAIN_FUNC1 101
+param set-default PWM_MAIN_FUNC2 102
+param set-default PWM_MAIN_DIS1 1500
+param set-default PWM_MAIN_DIS2 1500
+param set-default PWM_MAIN_TIM0 50
+param set-default PWM_MAIN_TIM1 50
+

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

@@ -118,6 +118,7 @@ px4_add_romfs_files(
 
 	50000_generic_ground_vehicle
 	50004_nxpcup_car_dfrobot_gpx
+	50003_aion_robotics_r1_rover
 
 	# [60000, 61000] (Unmanned) Underwater Robots
 	60000_uuv_generic

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

boards/px4/sitl/default.px4board

@@ -29,6 +29,7 @@ CONFIG_MODULES_LOGGER=y
 CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
+CONFIG_MAVLINK_DIALECT="development"
 CONFIG_MODULES_MC_ATT_CONTROL=y
 CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y

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 estimator_aid_src_optical_flow

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/position_setpoint.msg

@@ -9,18 +9,12 @@ uint8 SETPOINT_TYPE_TAKEOFF=3	# takeoff setpoint
 uint8 SETPOINT_TYPE_LAND=4	# land setpoint, altitude must be ignored, descend until landing
 uint8 SETPOINT_TYPE_IDLE=5	# do nothing, switch off motors or keep at idle speed (MC)
 
-uint8 VELOCITY_FRAME_LOCAL_NED = 1 # MAV_FRAME_LOCAL_NED
-uint8 VELOCITY_FRAME_BODY_NED = 8 # MAV_FRAME_BODY_NED
-
 bool valid			# true if setpoint is valid
 uint8 type			# setpoint type to adjust behavior of position controller
 
 float32 vx			# local velocity setpoint in m/s in NED
 float32 vy			# local velocity setpoint in m/s in NED
 float32 vz			# local velocity setpoint in m/s in NED
-bool velocity_valid		# true if local velocity setpoint valid
-uint8 velocity_frame		# to set velocity setpoints in NED or body
-bool alt_valid		# do not set for 3D position control. Set to true if you want z-position control while doing vx,vy velocity control.
 
 float64 lat			# latitude, in deg
 float64 lon			# longitude, in deg

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
@@ -101,6 +104,7 @@ px4_add_module(
 		EKF/imu_down_sampler.cpp
 		EKF/mag_control.cpp
 		EKF/mag_fusion.cpp
+		EKF/optical_flow_control.cpp
 		EKF/optflow_fusion.cpp
 		EKF/range_finder_consistency_check.cpp
 		EKF/range_height_control.cpp

src/modules/ekf2/EKF/CMakeLists.txt

@@ -55,6 +55,7 @@ add_library(ecl_EKF
 	imu_down_sampler.cpp
 	mag_control.cpp
 	mag_fusion.cpp
+	optical_flow_control.cpp
 	optflow_fusion.cpp
 	range_finder_consistency_check.cpp
 	range_height_control.cpp

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