Add camera capture topic advertising to constructor

.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 failsafe_flags" || 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 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

@@ -1,44 +0,0 @@
-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,35 +162,6 @@ 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 {
@@ -232,7 +203,6 @@ 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')
@@ -241,7 +211,6 @@ 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,13 +575,3 @@ 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)
 
-[![Discord Shield](https://discordapp.com/api/guilds/1022170275984457759/widget.png?style=shield)](https://discord.gg/dronecode)
+[![Slack](/.github/slack.svg)](https://join.slack.com/t/px4/shared_invite/zt-si4xo5qs-R4baYFmMjlrT4rQK5yUnaA)
 
 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

@@ -1,75 +0,0 @@
-#!/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,7 +118,6 @@ 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), 'generic'
+        return search.group(1)
 
     # nxp rt1062 format: initIOPWM(PWM::FlexPWM2),
     search = re.search('PWM::Flex([0-9a-zA-Z_]+)[,\)]', line, re.IGNORECASE)
     if search:
-        return search.group(1), 'imxrt'
+        return search.group(1)
 
-    return None, 'unknown'
+    return None
 
-def extract_timer_from_channel(line, num_channels_already_found):
+def extract_timer_from_channel(line):
     # 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,8 +50,7 @@ def extract_timer_from_channel(line, num_channels_already_found):
     # 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:
-        # imxrt uses a 1:1 timer group to channel association
-        return str(num_channels_already_found)
+        return search.group(1)
 
     return None
 
@@ -73,14 +72,7 @@ def get_timer_groups(timer_config_file, verbose=False):
         line = line.strip()
         if len(line) == 0 or line.startswith('//'):
             continue
-        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
+        timer = extract_timer(line)
 
         if timer:
             if verbose: print('found timer def: {:}'.format(timer))
@@ -109,7 +101,7 @@ def get_timer_groups(timer_config_file, verbose=False):
             continue
 
         if verbose: print('--'+line+'--')
-        timer = extract_timer_from_channel(line, len(channel_timers))
+        timer = extract_timer_from_channel(line)
 
         if timer:
             if verbose: print('Found timer: {:} in channel line {:}'.format(timer, line))

Tools/px4events/srcparser.py

@@ -211,11 +211,6 @@ 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":
@@ -233,7 +228,8 @@ class SourceParser(object):
                             else:
                                 raise Exception("Could not extract event ID from {:}".format(args_split[0]))
                             event.name = event_name
-                            event.message = parse_message(args_split[2])
+                            # 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')
                     elif call in ['reporter.healthFailure', 'reporter.armingCheckFailure']:
                         assert len(args_split) == num_args + 5, \
                             "Unexpected Number of arguments for: {:}, {:}".format(args_split, num_args)
@@ -243,7 +239,7 @@ class SourceParser(object):
                         else:
                             raise Exception("Could not extract event ID from {:}".format(args_split[2]))
                         event.name = event_name
-                        event.message = parse_message(args_split[4])
+                        event.message = args_split[4][1:-1]
                         if 'health' in call:
                             event.group = "health"
                         else:

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

@@ -236,7 +236,6 @@
  */
 
 #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,14 +76,9 @@
 #define rENBL         REG(IMXRT_TMR_ENBL_OFFSET)
 
 constexpr io_timers_t io_timers[MAX_IO_TIMERS] = {
-	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),
+	initIOPWM(PWM::FlexPWM2),
+	initIOPWM(PWM::FlexPWM3),
+	initIOPWM(PWM::FlexPWM4),
 };
 
 constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = {

boards/px4/sitl/default.px4board

@@ -29,7 +29,6 @@ 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,6 +59,7 @@ set(msg_files
 	cellular_status.msg
 	collision_constraints.msg
 	collision_report.msg
+	commander_state.msg
 	control_allocator_status.msg
 	cpuload.msg
 	differential_pressure.msg
@@ -80,7 +81,6 @@ 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,6 +202,7 @@ 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 vehicle_status_s::NAVIGATION_STATE_*
+uint8 mode # for ACTION_SWITCH_MODE what mode is requested according to commander_state.MAIN_STATE_

msg/commander_state.msg

@@ -0,0 +1,24 @@
+# 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,7 +19,6 @@ 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_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_baro_hgt estimator_aid_src_rng_hgt estimator_aid_src_airspeed
 # 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 time_last_fuse
+uint64[2] time_last_fuse
 
 float32[2] observation
 float32[2] observation_variance
@@ -14,10 +14,9 @@ float32[2] innovation
 float32[2] innovation_variance
 float32[2] test_ratio
 
-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
+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
 
 # TOPICS estimator_aid_source_2d
-# 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
+# TOPICS estimator_aid_src_fake_pos

msg/estimator_aid_source_3d.msg

@@ -5,7 +5,7 @@ uint8 estimator_instance
 
 uint32 device_id
 
-uint64 time_last_fuse
+uint64[3] time_last_fuse
 
 float32[3] observation
 float32[3] observation_variance
@@ -14,11 +14,10 @@ float32[3] innovation
 float32[3] innovation_variance
 float32[3] test_ratio
 
-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
+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
 
 # TOPICS estimator_aid_source_3d
-# TOPICS estimator_aid_src_ev_vel
-# TOPICS estimator_aid_src_gnss_vel
-# TOPICS estimator_aid_src_mag
+# TOPICS estimator_aid_src_gnss_pos estimator_aid_src_gnss_vel
+# TOPICS estimator_aid_src_mag estimator_aid_src_aux_vel

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 device_id		# unique device ID for the sensor that does not change between power cycles
+uint32 baro_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

@@ -1,57 +0,0 @@
-# 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,14 +1,12 @@
-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
 
-uint8 geofence_violation_reason         # one of geofence_violation_reason_t::*
+bool geofence_violated          # true if the geofence is violated
+uint8 geofence_action           # action to take when geofence is violated
 
-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
+bool home_required              # true if the geofence requires a valid home position

msg/mission_result.msg

@@ -14,6 +14,9 @@ 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,12 +9,18 @@ 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,9 +33,8 @@ uint8 ARM_DISARM_REASON_UNIT_TEST = 13
 
 uint64 nav_state_timestamp # time when current nav_state activated
 
-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
+# Navigation state "what should vehicle do"
+uint8 nav_state
 uint8 NAVIGATION_STATE_MANUAL = 0               # Manual mode
 uint8 NAVIGATION_STATE_ALTCTL = 1               # Altitude control mode
 uint8 NAVIGATION_STATE_POSCTL = 2               # Position control mode
@@ -84,11 +83,12 @@ uint8 VEHICLE_TYPE_ROVER = 3
 uint8 VEHICLE_TYPE_AIRSHIP = 4
 
 bool failsafe # true if system is in failsafe state (e.g.:RTL, Hover, Terminate, ...)
-bool failsafe_and_user_took_over # true if system is in failsafe state but the user took over control
+uint64 failsafe_timestamp # time when failsafe was activated
 
 # Link loss
-bool gcs_connection_lost              # datalink to GCS lost
-uint8 gcs_connection_lost_counter     # counts unique GCS connection lost events
+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 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,6 +97,9 @@ 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
@@ -104,6 +107,7 @@ 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
@@ -116,9 +120,3 @@ 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

@@ -0,0 +1,43 @@
+# 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%08" PRIx32 ": %s\n", id, str)
+#define CONSOLE_PRINT_EVENT(log_level, id, str) PX4_INFO_RAW("Event 0x%08x: %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,11 +45,7 @@
 #pragma once
 __BEGIN_DECLS
 /* configuration limits */
-#ifdef BOARD_NUM_IO_TIMERS
-#define MAX_IO_TIMERS     BOARD_NUM_IO_TIMERS
-#else
-#define MAX_IO_TIMERS     4
-#endif
+#define MAX_IO_TIMERS			4
 #define MAX_TIMER_IO_CHANNELS	16
 
 #define MAX_LED_TIMERS			2
@@ -82,7 +78,6 @@ 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,6 +56,7 @@ 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
@@ -590,7 +591,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 && io_timers_conf[i].submodle == ret.sub_module) {
+		if (io_timers_conf[i].base == timer_base) {
 			ret.timer_index = i;
 			break;
 		}
@@ -601,11 +602,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, PWM::FlexPWMSubmodule sub)
+static inline constexpr io_timers_t initIOPWM(PWM::FlexPWM pwm)
 {
 	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,69 +431,46 @@ 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;
 
-	// Yes do triggering
-	if (oneshots) {
-		struct {
-			uint32_t base;
-			uint16_t triggers;
-		} action_cache[MAX_IO_TIMERS] = {0};
+	/* Pre-calculate the list of channels to Trigger */
+	int mask;
 
-		int actions = 0;
+	for (int timer = 0; timer < MAX_IO_TIMERS; timer++) {
+		action_cache[actions].base = io_timers[timer].base;
 
-		// 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;
-				}
+		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;
 
+			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)
@@ -708,71 +685,44 @@ int io_timer_set_enable(bool state, io_timer_channel_mode_t mode, io_timer_chann
 
 	}
 
-	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];
+	struct {
+		uint32_t sm_ens;
+		uint32_t base;
+		uint32_t io_index;
+		uint32_t gpios[MAX_TIMER_IO_CHANNELS];
+	} action_cache[MAX_IO_TIMERS];
 
-		unsigned int actions = 0;
-		memset(action_cache, 0, sizeof(action_cache));
+	memset(action_cache, 0, sizeof(action_cache));
 
-		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;
+	for (int chan_index = 0; masks != 0 && chan_index < MAX_TIMER_IO_CHANNELS; chan_index++) {
+		if (masks & (1 << chan_index)) {
+			masks &= ~(1 << chan_index);
 
-			} else if (action_cache[actions].base != io_timers[timer].base) {
-				actions++;
-				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;
 			}
 		}
+	}
 
-		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;
-			}
+	irqstate_t flags = px4_enter_critical_section();
 
+	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,6 +47,7 @@ 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: AVG_CELL_VOLTAGE
+                19: (unused) AVG_CELL_VOLTAGE
                 20: (unused) HORIZON_SIDEBARS
-                21: POWER
+                21: (unused) POWER
             default: 16383
 
         # OSD Log Level

src/drivers/osd/msp_osd/msp_osd.cpp

@@ -71,44 +71,25 @@
 
 // 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
-// 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;
+const uint16_t osd_craft_name_pos = 2543;
+const uint16_t osd_disarmed_pos = 2125;
 
 // Right
 const uint16_t osd_main_batt_voltage_pos = 2073;
 const uint16_t osd_current_draw_pos = 2103;
-
-
-const uint16_t osd_numerical_vario_pos = LOCATION_HIDDEN;
+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;
 
 MspOsd::MspOsd(const char *device) :
 	ModuleParams(nullptr),
@@ -166,18 +147,15 @@ 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,9 +277,10 @@ 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.link_quality * 1023.0f) / 100.0f);
+	analog.rssi = (uint16_t)((input_rc.rssi * 1023.0f) / 100.0f);
 	analog.amperage = battery_status.current_a * 100; // main amperage
 	analog.mAhDrawn = battery_status.discharged_mah; // unused
+
 	return analog;
 }
 
@@ -289,8 +290,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 * 100.0f; // Used for power element
-	battery_state.batteryVoltage = (uint16_t)((battery_status.voltage_v / battery_status.cell_count) * 400.0f);  // OK
+	battery_state.amperage = battery_status.current_a; // not used?
+	battery_state.batteryVoltage = (uint16_t)(battery_status.voltage_v * 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?
@@ -317,24 +318,14 @@ 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;
-
-		float course = math::degrees(vehicle_gps_position.cog_rad);
-
-		if (course < 0) {
-			course += 360.0f;
-		}
-
-		raw_gps.groundCourse = course * 100.0f; // centidegrees
+		//raw_gps.groundCourse = vehicle_gps_position_struct
 
 	} 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;
@@ -376,24 +367,20 @@ 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 = 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 bearing_to_home = get_bearing_to_next_waypoint(vehicle_global_position.lat,
+					vehicle_global_position.lon,
+					home_position.lat, home_position.lon);
 
 		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;
+		comp_gps.directionToHome = bearing_to_home; // degrees
 
 	} else {
 		comp_gps.distanceToHome = 0; // meters
-		comp_gps.directionToHome = 0;
+		comp_gps.directionToHome = 0; // degrees
 	}
 
 	comp_gps.heartbeat = heartbeat;
@@ -409,17 +396,7 @@ 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;
-
-	float yaw_fixed = math::degrees(euler_attitude.psi());
-
-	if (yaw_fixed < 0) {
-		yaw_fixed += 360.0f;
-	}
-
-	attitude.yaw = yaw_fixed;
-
-	//attitude.yaw = 360;
+	attitude.yaw = math::degrees(euler_attitude.psi()) * 10;
 
 	return attitude;
 }

src/lib/drivers/device/Device.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2017 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{0};
+		uint32_t devid;
 	};
 
 	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);
-		set_device_bus_type(bus_type);
-		set_device_bus(bus);
+		_device_id.devid_s.bus_type = bus_type;
+		_device_id.devid_s.bus = bus;
 		set_device_address(address);
 	}
 

src/lib/events/enums.json

@@ -276,83 +276,41 @@
                         }
                     }
                 },
-                "failsafe_cause_t": {
+                "failsafe_reason_t": {
                     "type": "uint8_t",
-                    "description": "Cause for entering failsafe",
+                    "description": "Reason for entering failsafe",
                     "entries": {
                         "0": {
-                            "name": "generic",
-                            "description": ""
+                            "name": "no_rc",
+                            "description": "No manual control stick input"
                         },
                         "1": {
-                            "name": "manual_control_loss",
-                            "description": "manual control loss"
+                            "name": "no_offboard",
+                            "description": "No offboard control inputs"
                         },
                         "2": {
-                            "name": "gcs_connection_loss",
-                            "description": "GCS connection loss"
+                            "name": "no_rc_and_no_offboard",
+                            "description": "No manual control stick and no offboard control inputs"
                         },
                         "3": {
-                            "name": "low_battery_level",
-                            "description": "low battery level"
+                            "name": "no_local_position",
+                            "description": "No local position estimate"
                         },
                         "4": {
-                            "name": "critical_battery_level",
-                            "description": "critical battery level"
+                            "name": "no_global_position",
+                            "description": "No global position estimate"
                         },
                         "5": {
-                            "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"
+                            "name": "no_datalink",
+                            "description": "No datalink"
                         },
                         "6": {
-                            "name": "rtl",
-                            "description": "RTL"
+                            "name": "no_rc_and_no_datalink",
+                            "description": "No RC and no datalink"
                         },
                         "7": {
-                            "name": "land",
-                            "description": "land"
-                        },
-                        "8": {
-                            "name": "descend",
-                            "description": "descend"
-                        },
-                        "9": {
-                            "name": "disarm",
-                            "description": "disarm"
-                        },
-                        "10": {
-                            "name": "terminate",
-                            "description": "terminate"
+                            "name": "no_gps",
+                            "description": "No valid GPS"
                         }
                     }
                 },
@@ -415,10 +373,6 @@
                         "13": {
                             "name": "rc_button",
                             "description": "RC (button)"
-                        },
-                        "14": {
-                            "name": "failsafe",
-                            "description": "failsafe"
                         }
                     }
                 },
@@ -623,24 +577,6 @@
                             "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,7 +147,6 @@ 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/camera_feedback/CameraFeedback.cpp

@@ -42,6 +42,8 @@ CameraFeedback::CameraFeedback() :
 	if (_p_cam_cap_fback != PARAM_INVALID) {
 		param_get(_p_cam_cap_fback, (int32_t *)&_cam_cap_fback);
 	}
+
+	_capture_pub.advertise();
 }
 
 bool
@@ -52,8 +54,6 @@ CameraFeedback::init()
 		return false;
 	}
 
-	_capture_pub.advertise();
-
 	return true;
 }
 

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

@@ -39,12 +39,9 @@
 #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>
 
-typedef enum {
-	TRANSITION_DENIED = -1,
-	TRANSITION_NOT_CHANGED = 0,
-	TRANSITION_CHANGED
-} transition_result_t;
+#include "../../state_machine_helper.h" // TODO: get independent of transition_result_t
 
 using arm_disarm_reason_t = events::px4::enums::arm_disarm_reason_t;
 

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

@@ -242,6 +242,7 @@ 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]);
@@ -253,7 +254,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);
+		HealthAndArmingChecks health_and_arming_checks(nullptr, status_flags, status);
 
 		// Attempt transition
 		transition_result_t result = arm_state_machine.arming_state_transition(

src/modules/commander/CMakeLists.txt

@@ -33,7 +33,6 @@
 
 add_subdirectory(failure_detector)
 add_subdirectory(HealthAndArmingChecks)
-add_subdirectory(failsafe)
 add_subdirectory(Arming)
 add_subdirectory(ModeUtil)
 
@@ -52,12 +51,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
@@ -70,7 +69,10 @@ 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,12 +36,11 @@
 /*   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>
@@ -58,6 +57,7 @@
 #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,6 +68,7 @@
 #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>
@@ -83,6 +84,7 @@
 #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;
@@ -115,11 +117,12 @@ 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();
@@ -129,11 +132,9 @@ private:
 	/**
 	 * Checks the status of all available data links and handles switching between different system telemetry states.
 	 */
-	void dataLinkCheck();
+	void data_link_check();
 
-	void manualControlCheck();
-
-	void offboardControlCheck();
+	void manual_control_check();
 
 	/**
 	 * @brief Handle incoming vehicle command relavant to Commander
@@ -145,43 +146,110 @@ private:
 	 */
 	bool handle_command(const vehicle_command_s &cmd);
 
-	unsigned handleCommandActuatorTest(const vehicle_command_s &cmd);
+	unsigned handle_command_actuator_test(const vehicle_command_s &cmd);
 
 	void executeActionRequest(const action_request_s &action_request);
 
-	void printRejectMode(uint8_t nav_state);
+	void offboard_control_update();
 
-	void updateControlMode();
+	void print_reject_mode(uint8_t main_state);
 
-	bool shutdownIfAllowed();
+	void update_control_mode();
+
+	bool shutdown_if_allowed();
 
 	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 checkAndInformReadyForTakeoff();
+	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};
 
 	enum class PrearmedMode {
 		DISABLED = 0,
@@ -194,91 +262,112 @@ 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};
 
-	vehicle_status_s        _vehicle_status{};
+	ArmStateMachine _arm_state_machine{};
 
-	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		_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};
 
-	const failsafe_flags_s &_failsafe_flags{_health_and_arming_checks.failsafeFlags()};
-	HomePosition 		_home_position{_failsafe_flags};
+	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};
 
 
-	Hysteresis _auto_disarm_landed{false};
-	Hysteresis _auto_disarm_killed{false};
+	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};
 
-	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};
+	hrt_abstime	_high_latency_datalink_heartbeat{0};
+	hrt_abstime	_high_latency_datalink_lost{0};
 
-	hrt_abstime _last_print_mode_reject_time{0};	///< To remember when last notification was sent
+	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 _high_latency_datalink_heartbeat{0};
-	hrt_abstime _high_latency_datalink_lost{0};
+	hrt_abstime	_last_print_mode_reject_time{0};	///< To remember when last notification was sent
+	bool            _mode_switch_mapped{false};
 
-	hrt_abstime _boot_timestamp{0};
-	hrt_abstime _last_disarmed_timestamp{0};
-	hrt_abstime _overload_start{0};		///< time when CPU overload started
+	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 _led_armed_state_toggle{0};
 	hrt_abstime _led_overload_toggle{0};
 
-	hrt_abstime _last_health_and_arming_check{0};
+	hrt_abstime _last_termination_message_sent{0};
 
-	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};
+	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};
 
+	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{};
-	vtol_vehicle_status_s	_vtol_vehicle_status{};
+	vehicle_status_s        _vehicle_status{};
+	vehicle_status_flags_s  _vehicle_status_flags{};
+
+	Safety _safety;
+
+	WorkerThread _worker_thread;
 
 	// 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)};
@@ -289,41 +378,19 @@ private:
 
 	// Publications
 	uORB::Publication<actuator_armed_s>			_actuator_armed_pub{ORB_ID(actuator_armed)};
-	uORB::Publication<actuator_test_s>			_actuator_test_pub{ORB_ID(actuator_test)};
+	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<vehicle_command_ack_s>		_vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)};
+	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<vehicle_command_ack_s>		_vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)};
+
 	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")};
-
-	// 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
-	)
+	HealthAndArmingChecks _health_and_arming_checks;
+	HomePosition _home_position{_vehicle_status_flags};
 };

src/modules/commander/HealthAndArmingChecks/CMakeLists.txt

@@ -53,14 +53,6 @@ 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,13 +172,12 @@ 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, _failsafe_flags);
+	mode_util::getModeRequirements(vehicle_type, _status_flags);
 }
 
 NavModes Report::getModeGroup(uint8_t nav_state) const
@@ -187,18 +186,16 @@ NavModes Report::getModeGroup(uint8_t nav_state) const
 	return (NavModes)(1u << nav_state);
 }
 
-bool Report::finalize()
+void 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_changed;
+	const bool has_difference = _had_unreported_difference || _results[0] != _results[1];
 
 	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/failsafe_flags.h>
+#include <uORB/topics/vehicle_status_flags.h>
 #include <systemlib/mavlink_log.h>
 
 #include <stdint.h>
@@ -188,11 +188,11 @@ public:
 		}
 	};
 
-	Report(failsafe_flags_s &failsafe_flags, hrt_abstime min_reporting_interval = 2_s)
-		: _min_reporting_interval(min_reporting_interval), _failsafe_flags(failsafe_flags) { }
+	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() = default;
 
-	failsafe_flags_s &failsafeFlags() { return _failsafe_flags; }
+	vehicle_status_flags_s &failsafeFlags() { return _status_flags; }
 
 	orb_advert_t *mavlink_log_pub() { return _mavlink_log_pub; }
 
@@ -254,8 +254,6 @@ 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:
 
 	/**
@@ -323,10 +321,7 @@ private:
 	 */
 	void reset();
 	void prepare(uint8_t vehicle_type);
-	/**
-	 * Called after all checks are run. Returns true if the results changed
-	 */
-	bool finalize();
+	void finalize();
 
 	bool report(bool is_armed, bool force);
 
@@ -340,13 +335,12 @@ 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};
 
-	failsafe_flags_s &_failsafe_flags;
+	vehicle_status_flags_s &_status_flags;
 
 	orb_advert_t *_mavlink_log_pub{nullptr}; ///< mavlink log publication for legacy reporting
 };

src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp

@@ -33,23 +33,12 @@
 
 #include "HealthAndArmingChecks.hpp"
 
-HealthAndArmingChecks::HealthAndArmingChecks(ModuleParams *parent, vehicle_status_s &status)
+HealthAndArmingChecks::HealthAndArmingChecks(ModuleParams *parent, vehicle_status_flags_s &status_flags,
+		vehicle_status_s &status)
 	: ModuleParams(parent),
 	  _context(status),
-	  _reporter(_failsafe_flags)
+	  _reporter(status_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)
@@ -66,10 +55,9 @@ bool HealthAndArmingChecks::update(bool force_reporting)
 		_checks[i]->checkAndReport(_context, _reporter);
 	}
 
-	const bool results_changed = _reporter.finalize();
-	const bool reported = _reporter.report(_context.isArmed(), force_reporting);
+	_reporter.finalize();
 
-	if (reported) {
+	if (_reporter.report(_context.isArmed(), force_reporting)) {
 
 		// LEGACY start
 		// Run the checks again, this time with the mavlink publication set.
@@ -97,17 +85,10 @@ bool HealthAndArmingChecks::update(bool force_reporting)
 		_reporter.getHealthReport(health_report);
 		health_report.timestamp = hrt_absolute_time();
 		_health_report_pub.publish(health_report);
+		return true;
 	}
 
-	// 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;
+	return false;
 }
 
 void HealthAndArmingChecks::updateParams()

src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp

@@ -38,7 +38,6 @@
 #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"
@@ -52,7 +51,6 @@
 #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"
@@ -60,19 +58,12 @@
 #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_s &status);
+	HealthAndArmingChecks(ModuleParams *parent, vehicle_status_flags_s &status_flags, vehicle_status_s &status);
 	~HealthAndArmingChecks() = default;
 
 	/**
@@ -93,13 +84,6 @@ 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:
@@ -107,10 +91,7 @@ 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;
@@ -125,7 +106,6 @@ 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;
@@ -133,13 +113,6 @@ 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,
@@ -154,21 +127,13 @@ private:
 		&_imu_consistency_checks,
 		&_magnetometer_checks,
 		&_manual_control_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
+		&_mode_checks, // must be after _estimator_checks
 		&_parachute_checks,
 		&_power_checks,
 		&_rc_calibration_checks,
 		&_sd_card_checks,
-		&_system_checks, // must be after _estimator_checks & _home_position_checks
+		&_system_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)
 {
-	failsafe_flags_s failsafe_flags{};
-	Report reporter{failsafe_flags, 0_s};
+	vehicle_status_flags_s status_flags{};
+	Report reporter{status_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)
 {
-	failsafe_flags_s failsafe_flags{};
-	Report reporter{failsafe_flags, 0_s};
+	vehicle_status_flags_s status_flags{};
+	Report reporter{status_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)
 {
-	failsafe_flags_s failsafe_flags{};
-	Report reporter{failsafe_flags, 0_s};
+	vehicle_status_flags_s status_flags{};
+	Report reporter{status_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)
 {
-	failsafe_flags_s failsafe_flags{};
-	Report reporter{failsafe_flags, 0_s};
+	vehicle_status_flags_s status_flags{};
+	Report reporter{status_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)
 {
-	failsafe_flags_s failsafe_flags{};
-	Report reporter{failsafe_flags, 0_s};
+	vehicle_status_flags_s status_flags{};
+	Report reporter{status_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)
 {
-	failsafe_flags_s failsafe_flags{};
-	Report reporter{failsafe_flags, 0_s};
+	vehicle_status_flags_s status_flags{};
+	Report reporter{status_flags, 0_s};
 
 	uORB::Subscription event_sub{ORB_ID(event)};
 	event_sub.subscribe();

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

@@ -113,13 +113,9 @@ 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 && _battery_connected_at_arming[index]) { // If disconnected after arming
+			if (!battery.connected) {
 				/* EVENT
 				 */
 				reporter.healthFailure<uint8_t>(NavModes::All, health_component_t::battery, events::ID("check_battery_disconnected"),
@@ -203,8 +199,7 @@ 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
-	    && reporter.failsafeFlags().battery_warning < battery_status_s::BATTERY_WARNING_FAILED) {
+	if (reporter.failsafeFlags().battery_warning > battery_status_s::BATTERY_WARNING_NONE) {
 		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;
@@ -227,13 +222,12 @@ 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
-		|| reporter.failsafeFlags().battery_warning == battery_status_s::BATTERY_WARNING_FAILED;
+		|| battery_has_fault;
 
 	if (reporter.failsafeFlags().battery_unhealthy && !battery_has_fault) { // faults are reported above already
 		/* EVENT
 		 * @description
-		 * Make sure all batteries are connected and operational.
+		 * Make sure all batteries are connected.
 		 */
 		reporter.healthFailure(NavModes::All, health_component_t::battery, events::ID("check_battery_unhealthy"),
 				       events::Log::Error, "Battery unhealthy");
@@ -247,7 +241,6 @@ 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,7 +53,5 @@ 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 && _param_sys_mc_est_group.get() == 2) {
+	if (missing_data) {
 		/* 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_invalid;
+	const bool condition_gps_position_was_valid = reporter.failsafeFlags().gps_position_valid;
 	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_invalid) {
+	if (condition_gps_position_was_valid && !reporter.failsafeFlags().gps_position_valid) {
 		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_invalid) {
+	if (context.isArmed() && reporter.failsafeFlags().global_position_valid) {
 		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, failsafe_flags_s &failsafe_flags)
+		const vehicle_local_position_s &lpos, const sensor_gps_s &vehicle_gps_position, vehicle_status_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_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.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.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 =
+		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_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_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_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);
+	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);
 
 
 	// altitude
-	failsafe_flags.local_altitude_invalid = !lpos.z_valid
-						|| (now - lpos.timestamp > (_param_com_pos_fs_delay.get() * 1_s));
+	failsafe_flags.local_altitude_valid = lpos.z_valid
+					      && (now - lpos.timestamp < (_param_com_pos_fs_delay.get() * 1_s));
 
 
 	// attitude
@@ -740,11 +740,10 @@ 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_invalid = now > attitude.timestamp + 1_s || !norm_in_tolerance
-						  || !no_element_larger_than_one;
+		failsafe_flags.attitude_valid = now - attitude.timestamp < 1_s && norm_in_tolerance && no_element_larger_than_one;
 
 	} else {
-		failsafe_flags.attitude_invalid = true;
+		failsafe_flags.attitude_valid = false;
 	}
 
 	// angular velocity
@@ -754,13 +753,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_invalid = !condition_angular_velocity_time_valid
-					      || !condition_angular_velocity_finite;
+	const bool angular_velocity_valid = condition_angular_velocity_time_valid
+					    && condition_angular_velocity_finite;
 
-	if (!failsafe_flags.angular_velocity_invalid && angular_velocity_invalid) {
+	if (failsafe_flags.angular_velocity_valid && !angular_velocity_valid) {
 		const char err_str[] {"angular velocity no longer valid"};
 
-		if (!condition_angular_velocity_time_valid && angular_velocity.timestamp != 0) {
+		if (!condition_angular_velocity_time_valid) {
 			PX4_ERR("%s (timeout)", err_str);
 
 		} else if (!condition_angular_velocity_finite) {
@@ -768,7 +767,7 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f
 		}
 	}
 
-	failsafe_flags.angular_velocity_invalid = angular_velocity_invalid;
+	failsafe_flags.angular_velocity_valid = angular_velocity_valid;
 
 
 	// gps
@@ -782,10 +781,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_invalid = !_vehicle_gps_position_valid.get_state();
+		failsafe_flags.gps_position_valid = _vehicle_gps_position_valid.get_state();
 
 	} else {
-		failsafe_flags.gps_position_invalid = true;
+		failsafe_flags.gps_position_valid = false;
 	}
 }
 

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,
-				     failsafe_flags_s &failsafe_flags);
+				     vehicle_status_flags_s &failsafe_flags);
 
 	bool checkPosVelValidity(const hrt_abstime &now, const bool data_valid, const float data_accuracy,
 				 const float required_accuracy,
@@ -105,7 +105,6 @@ 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,125 +35,81 @@
 
 void FailureDetectorChecks::checkAndReport(const Context &context, Report &reporter)
 {
-	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_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 (reporter.mavlink_log_pub()) {
-			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Attitude failure (roll)");
+			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)");
+			}
 		}
 
-	} 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 (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: Attitude failure (pitch)");
+			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 (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

@@ -1,58 +0,0 @@
-/****************************************************************************
- *
- *   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

@@ -1,50 +0,0 @@
-/****************************************************************************
- *
- *   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

@@ -1,78 +0,0 @@
-/****************************************************************************
- *
- *   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,51 +0,0 @@
-/****************************************************************************
- *
- *   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/geofence_result.h>
-
-class GeofenceChecks : public HealthAndArmingCheckBase
-{
-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

@@ -1,48 +0,0 @@
-/****************************************************************************
- *
- *   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

@@ -1,51 +0,0 @@
-/****************************************************************************
- *
- *   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

@@ -1,61 +0,0 @@
-/****************************************************************************
- *
- *   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/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_invalid && reporter.failsafeFlags().mode_req_angular_velocity != 0) {
+	if (!reporter.failsafeFlags().angular_velocity_valid && 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_invalid && reporter.failsafeFlags().mode_req_attitude != 0) {
+	if (!reporter.failsafeFlags().attitude_valid && 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_invalid && reporter.failsafeFlags().mode_req_local_position != 0) {
+	if (!reporter.failsafeFlags().local_position_valid && reporter.failsafeFlags().mode_req_local_position != 0) {
 		local_position_modes = (NavModes)reporter.failsafeFlags().mode_req_local_position;
 	}
 
-	if (reporter.failsafeFlags().local_position_invalid_relaxed
+	if (!reporter.failsafeFlags().local_position_valid_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_invalid && reporter.failsafeFlags().mode_req_global_position != 0) {
+	if (!reporter.failsafeFlags().global_position_valid && 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_invalid && reporter.failsafeFlags().mode_req_local_alt != 0) {
+	if (!reporter.failsafeFlags().local_altitude_valid && reporter.failsafeFlags().mode_req_local_alt != 0) {
 		/* EVENT
 		 */
 		reporter.armingCheckFailure((NavModes)reporter.failsafeFlags().mode_req_local_alt, health_component_t::system,
@@ -102,31 +102,21 @@ void ModeChecks::checkAndReport(const Context &context, Report &reporter)
 		reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_local_alt);
 	}
 
-	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) {
+	if (!reporter.failsafeFlags().auto_mission_available && reporter.failsafeFlags().mode_req_mission != 0) {
 		/* 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(mission_required_modes, health_component_t::system,
+		reporter.armingCheckFailure((NavModes)reporter.failsafeFlags().mode_req_mission, health_component_t::system,
 					    events::ID("check_modes_mission"),
-					    events::Log::Info, "No valid mission available");
+					    events::Log::Info, "No 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 or the required estimate (e.g. position) is missing.
+		 * The offboard component is not sending setpoints.
 		 */
 		reporter.armingCheckFailure((NavModes)reporter.failsafeFlags().mode_req_offboard_signal, health_component_t::system,
 					    events::ID("check_modes_offboard_signal"),
@@ -134,7 +124,7 @@ void ModeChecks::checkAndReport(const Context &context, Report &reporter)
 		reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_offboard_signal);
 	}
 
-	if (reporter.failsafeFlags().home_position_invalid && reporter.failsafeFlags().mode_req_home_position != 0) {
+	if (!reporter.failsafeFlags().home_position_valid && 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,7 +48,4 @@ 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

@@ -1,70 +0,0 @@
-/****************************************************************************
- *
- *   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

@@ -1,57 +0,0 @@
-/****************************************************************************
- *
- *   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

@@ -1,129 +0,0 @@
-/****************************************************************************
- *
- *   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

@@ -1,59 +0,0 @@
-/****************************************************************************
- *
- *   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,11 +62,13 @@ 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)
@@ -80,10 +82,18 @@ void RcCalibrationChecks::checkAndReport(const Context &context, Report &reporte
 	}
 
 	for (unsigned i = 0; i < input_rc_s::RC_INPUT_MAX_CHANNELS; i++) {
-		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;
+		/* 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);
 
 		/* assert min..center..max ordering */
 		if (param_min < RC_INPUT_LOWEST_MIN_US) {
@@ -165,21 +175,3 @@ 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,23 +47,14 @@ 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,9 +72,29 @@ 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_invalid) {
+		if (!reporter.failsafeFlags().global_position_valid) {
 			/* EVENT
 			 * @description
 			 * <profile name="dev">
@@ -88,7 +108,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_invalid) {
+		} else if (!reporter.failsafeFlags().home_position_valid) {
 			/* EVENT
 			 * @description
 			 * <profile name="dev">
@@ -171,6 +191,21 @@ 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) {
@@ -180,4 +215,8 @@ 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,6 +52,7 @@ 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

@@ -1,56 +0,0 @@
-/****************************************************************************
- *
- *   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/windCheck.cpp

@@ -1,79 +0,0 @@
-/****************************************************************************
- *
- *   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

@@ -1,58 +0,0 @@
-/****************************************************************************
- *
- *   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 failsafe_flags_s &failsafe_flags)
-	: _failsafe_flags(failsafe_flags)
+HomePosition::HomePosition(const vehicle_status_flags_s &vehicle_status_flags)
+	: _vehicle_status_flags(vehicle_status_flags)
 {
 }
 
@@ -69,7 +69,7 @@ bool HomePosition::hasMovedFromCurrentHomeLocation()
 			eph = gpos.eph;
 			epv = gpos.epv;
 
-		} else if (!_failsafe_flags.gps_position_invalid) {
+		} else if (_vehicle_status_flags.gps_position_valid) {
 			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 (!_failsafe_flags.local_position_invalid) {
+	if (_vehicle_status_flags.local_position_valid) {
 		// 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 (!_failsafe_flags.global_position_invalid) {
+	if (_vehicle_status_flags.global_position_valid) {
 		// 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 (!_failsafe_flags.gps_position_invalid) {
+	} else if (_vehicle_status_flags.gps_position_valid) {
 		// 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 (!_failsafe_flags.local_position_invalid && !_failsafe_flags.global_position_invalid) {
+		if (_vehicle_status_flags.local_position_valid && _vehicle_status_flags.global_position_valid) {
 			// 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 (!_failsafe_flags.local_position_invalid && !_failsafe_flags.gps_position_invalid) {
+		} else if (_vehicle_status_flags.local_position_valid && _vehicle_status_flags.gps_position_valid) {
 			// 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 (!_failsafe_flags.local_position_invalid && lpos.xy_global && lpos.z_global) {
+		if (_vehicle_status_flags.local_position_valid && 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 && !_failsafe_flags.local_position_invalid;
+		const bool can_set_home_lpos_first_time = !home.valid_lpos && _vehicle_status_flags.local_position_valid;
 		const bool can_set_home_gpos_first_time = ((!home.valid_hpos || !home.valid_alt)
-				&& (!_failsafe_flags.global_position_invalid || !_failsafe_flags.gps_position_invalid));
+				&& (_vehicle_status_flags.global_position_valid || _vehicle_status_flags.gps_position_valid));
 		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/failsafe_flags.h>
+#include <uORB/topics/vehicle_status_flags.h>
 
 class HomePosition
 {
 public:
-	HomePosition(const failsafe_flags_s &failsafe_flags);
+	HomePosition(const vehicle_status_flags_s &vehicle_status_flags);
 	~HomePosition() = default;
 
 	bool setHomePosition(bool force = false);
@@ -74,5 +74,5 @@ private:
 
 	uint8_t							_heading_reset_counter{0};
 	bool							_valid{false};
-	const failsafe_flags_s					&_failsafe_flags;
+	const vehicle_status_flags_s				&_vehicle_status_flags;
 };

src/modules/commander/ModeUtil/CMakeLists.txt

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

src/modules/commander/ModeUtil/conversions.hpp

@@ -1,110 +0,0 @@
-/****************************************************************************
- *
- *   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, failsafe_flags_s &flags)
+void getModeRequirements(uint8_t vehicle_type, vehicle_status_flags_s &flags)
 {
 	flags.mode_req_angular_velocity = 0;
 	flags.mode_req_attitude = 0;
@@ -130,7 +130,6 @@ void getModeRequirements(uint8_t vehicle_type, failsafe_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/failsafe_flags.h>
+#include <uORB/topics/vehicle_status_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, failsafe_flags_s &flags);
+void getModeRequirements(uint8_t vehicle_type, vehicle_status_flags_s &flags);
 
 
 } // namespace mode_util

src/modules/commander/UserModeIntention.cpp

@@ -1,83 +0,0 @@
-/****************************************************************************
- *
- *   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

@@ -1,85 +0,0 @@
-/****************************************************************************
- *
- *   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);
 
 /**
- * GCS connection loss time threshold
+ * Datalink loss time threshold
  *
- * After this amount of seconds without datalink, the GCS connection lost mode triggers
+ * After this amount of seconds without datalink the data link lost mode triggers
  *
  * @group Commander
  * @unit s
@@ -283,14 +283,13 @@ PARAM_DEFINE_INT32(COM_ARM_SWISBTN, 0);
 PARAM_DEFINE_INT32(COM_LOW_BAT_ACT, 0);
 
 /**
- * Delay between failsafe condition triggered and failsafe reaction
+ * Delay between battery state change and failsafe reaction
  *
- * 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.
+ * 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
  *
- * A zero value disables the delay and the user cannot take over via stick movements (switching modes is still allowed).
+ * A zero value disables the delay.
  *
  * @group Commander
  * @unit s
@@ -298,7 +297,7 @@ PARAM_DEFINE_INT32(COM_LOW_BAT_ACT, 0);
  * @max 25.0
  * @decimal 3
  */
-PARAM_DEFINE_FLOAT(COM_FAIL_ACT_T, 5.f);
+PARAM_DEFINE_FLOAT(COM_BAT_ACT_T, 5.f);
 
 /**
  * Imbalanced propeller failsafe mode
@@ -320,7 +319,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_RC_ACT to configure action.
+ * See COM_OBL_ACT and COM_OBL_RC_ACT to configure action.
  *
  * @group Commander
  * @unit s
@@ -330,10 +329,27 @@ 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 Warning only
+ * @value -1 No action: stay in current flight mode
  * @value  0 Return mode
  * @value  1 Land mode
  * @value  2 Hold mode
@@ -342,11 +358,12 @@ PARAM_DEFINE_FLOAT(COM_OF_LOSS_T, 1.0f);
 PARAM_DEFINE_INT32(COM_QC_ACT, 0);
 
 /**
- * Set offboard loss failsafe mode
+ * Set offboard loss failsafe mode when RC is available
  *
  * 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
@@ -354,7 +371,7 @@ PARAM_DEFINE_INT32(COM_QC_ACT, 0);
  * @value  4 Land mode
  * @value  5 Hold mode
  * @value  6 Terminate
- * @value  7 Disarm
+ * @value  7 Lockdown
  * @group Commander
  */
 PARAM_DEFINE_INT32(COM_OBL_RC_ACT, 0);
@@ -592,7 +609,7 @@ PARAM_DEFINE_FLOAT(COM_ARM_IMU_GYR, 0.25f);
  * @min 3
  * @max 180
  */
-PARAM_DEFINE_INT32(COM_ARM_MAG_ANG, 60);
+PARAM_DEFINE_INT32(COM_ARM_MAG_ANG, 45);
 
 /**
  * Enable mag strength preflight check
@@ -659,10 +676,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/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.
+ * 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.
  *
  * @value 0 Altitude/Manual
- * @value 1 Land/Descend
+ * @value 1 Land/Terminate
  *
  * @group Commander
  */
@@ -785,9 +802,9 @@ PARAM_DEFINE_INT32(COM_FLIGHT_UUID, 0);
 PARAM_DEFINE_INT32(COM_TAKEOFF_ACT, 0);
 
 /**
- * Set GCS connection loss failsafe mode
+ * Set data link loss failsafe mode
  *
- * The GCS connection loss failsafe will only be entered after a timeout,
+ * The data link 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.
  *
@@ -796,7 +813,7 @@ PARAM_DEFINE_INT32(COM_TAKEOFF_ACT, 0);
  * @value 2 Return mode
  * @value 3 Land mode
  * @value 5 Terminate
- * @value 6 Disarm
+ * @value 6 Lockdown
  * @min 0
  * @max 6
  *
@@ -815,7 +832,7 @@ PARAM_DEFINE_INT32(NAV_DLL_ACT, 0);
  * @value 2 Return mode
  * @value 3 Land mode
  * @value 5 Terminate
- * @value 6 Disarm
+ * @value 6 Lockdown
  * @min 1
  * @max 6
  *
@@ -845,7 +862,7 @@ PARAM_DEFINE_INT32(COM_RCL_EXCEPT, 0);
  *
  * @min 0
  * @max 3
- * @value 0 Warning only
+ * @value 0 Disabled
  * @value 1 Hold mode
  * @value 2 Land mode
  * @value 3 Return mode
@@ -971,8 +988,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 disarm the vehicle
- * 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 put the vehicle into
+ * a lockdown state 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

@@ -0,0 +1,41 @@
+############################################################################
+#
+#   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/commander_tests.cpp

@@ -1,6 +1,7 @@
 /****************************************************************************
  *
- *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *   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
@@ -31,21 +32,21 @@
  *
  ****************************************************************************/
 
-#pragma once
+/**
+ * @file commander_tests.cpp
+ * Commander unit tests. Run the tests as follows:
+ *   nsh> commander_tests
+ *
+ */
 
-#include "../Common.hpp"
+#include <systemlib/err.h>
 
-#include <uORB/Subscription.hpp>
-#include <uORB/topics/vtol_vehicle_status.h>
+#include "state_machine_helper_test.h"
 
-class VtolChecks : public HealthAndArmingCheckBase
+extern "C" __EXPORT int commander_tests_main(int argc, char *argv[]);
+
+
+int commander_tests_main(int argc, char *argv[])
 {
-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)};
-};
+	return stateMachineHelperTest() ? 0 : -1;
+}

src/modules/commander/commander_tests/state_machine_helper_test.cpp

@@ -0,0 +1,283 @@
+/****************************************************************************
+ *
+ *   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/commander_tests/state_machine_helper_test.h

@@ -1,6 +1,7 @@
 /****************************************************************************
  *
- *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *   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
@@ -31,21 +32,11 @@
  *
  ****************************************************************************/
 
+/**
+ * @file state_machine_helper_test.h
+ */
+
 #pragma once
 
-#include "../Common.hpp"
+bool stateMachineHelperTest(void);
 
-#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/failsafe/CMakeLists.txt

@@ -1,82 +0,0 @@
-############################################################################
-#
-#   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

@@ -1,221 +0,0 @@
-/****************************************************************************
- *
- *   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

@@ -1,411 +0,0 @@
-<!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

@@ -1,531 +0,0 @@
-/****************************************************************************
- *
- *   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

@@ -1,113 +0,0 @@
-/****************************************************************************
- *
- *   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

@@ -1,260 +0,0 @@
-/****************************************************************************
- *
- *   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

@@ -1,628 +0,0 @@
-/****************************************************************************
- *
- *   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

@@ -1,261 +0,0 @@
-/****************************************************************************
- *
- *   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

@@ -1,140 +0,0 @@
-#!/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,7 +37,8 @@
  *
  */
 
-#pragma once
+#ifndef PX4_CUSTOM_MODE_H_
+#define PX4_CUSTOM_MODE_H_
 
 #include <stdint.h>
 
@@ -85,96 +86,4 @@ union px4_custom_mode {
 	};
 };
 
-#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 */
-
+#endif /* PX4_CUSTOM_MODE_H_ */

src/modules/commander/state_machine_helper.h

@@ -0,0 +1,158 @@
+/****************************************************************************
+ *
+ *   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,38 +33,35 @@
 
 add_subdirectory(Utility)
 
-# 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)
+# 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)
 
-# 	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
-# 		${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
-# 	)
+	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
+	)
 
-# 	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
@@ -104,7 +101,6 @@ 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,7 +55,6 @@ 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