diff --git a/Makefile b/Makefile index 856e7bb883..34aa1c1273 100644 --- a/Makefile +++ b/Makefile @@ -575,3 +575,13 @@ update_px4_ros_com: update_px4_msgs: @Tools/update_px4_ros2_bridge.sh --ws_dir ${ROS2_WS_DIR} --px4_msgs + +.PHONY: failsafe_web run_failsafe_web_server +failsafe_web: + @if ! command -v emcc; then echo -e "Install emscripten first: https://emscripten.org/docs/getting_started/downloads.html\nAnd source the env: source /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 diff --git a/msg/vehicle_status_flags.msg b/msg/vehicle_status_flags.msg index f6eb4df696..88fb851f29 100644 --- a/msg/vehicle_status_flags.msg +++ b/msg/vehicle_status_flags.msg @@ -1,5 +1,8 @@ # TODO: rename to failsafe_flags (will be input to failsafe state machine) # +# Input flags for the failsafe state machine. +# 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) @@ -19,22 +22,26 @@ uint32 mode_req_other # other requirements, not covered above (for e 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 auto_mission_available # Mission available +bool angular_velocity_valid # Angular velocity valid +bool attitude_valid # Attitude valid +bool local_altitude_valid # Local altitude valid +bool local_position_valid # Local position estimate valid 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 global_position_valid # Global position estimate valid bool gps_position_valid -bool home_position_valid # indicates a valid home position (a valid home position is not always a valid launch) +bool home_position_valid # Home position valid -bool offboard_control_signal_lost +bool offboard_control_signal_lost # Offboard signal lost +bool geofence_violated # Geofence violated bool rc_signal_found_once +bool rc_signal_lost # RC signal lost +bool data_link_lost # Data link lost +bool mission_failure # Mission failure bool rc_calibration_in_progress -bool vtol_transition_failure # Set to true if vtol transition failed +bool vtol_transition_failure # VTOL transition failed bool battery_low_remaining_time bool battery_unhealthy diff --git a/src/lib/parameters/CMakeLists.txt b/src/lib/parameters/CMakeLists.txt index 609e0a2b4f..b64a265177 100644 --- a/src/lib/parameters/CMakeLists.txt +++ b/src/lib/parameters/CMakeLists.txt @@ -147,6 +147,7 @@ add_custom_command(OUTPUT px4_parameters.hpp px_generate_params.py templates/px4_parameters.hpp.jinja ) +add_custom_target(parameters_header DEPENDS px4_parameters.hpp) set(SRCS) diff --git a/src/modules/commander/CMakeLists.txt b/src/modules/commander/CMakeLists.txt index 9006b7992d..df8168f06e 100644 --- a/src/modules/commander/CMakeLists.txt +++ b/src/modules/commander/CMakeLists.txt @@ -33,6 +33,7 @@ add_subdirectory(failure_detector) add_subdirectory(HealthAndArmingChecks) +add_subdirectory(failsafe) add_subdirectory(Arming) add_subdirectory(ModeUtil) @@ -69,6 +70,7 @@ px4_add_module( sensor_calibration world_magnetic_model mode_util + failsafe ) px4_add_unit_gtest(SRC mag_calibration_test.cpp LINKLIBS modules__commander) diff --git a/src/modules/commander/ModeUtil/CMakeLists.txt b/src/modules/commander/ModeUtil/CMakeLists.txt index dab3d3ba2e..8713e53f6b 100644 --- a/src/modules/commander/ModeUtil/CMakeLists.txt +++ b/src/modules/commander/ModeUtil/CMakeLists.txt @@ -31,7 +31,9 @@ # ############################################################################ -px4_add_library(mode_util +add_library(mode_util control_mode.cpp mode_requirements.cpp ) +add_dependencies(mode_util uorb_headers prebuild_targets) + diff --git a/src/modules/commander/failsafe/CMakeLists.txt b/src/modules/commander/failsafe/CMakeLists.txt new file mode 100644 index 0000000000..01c9e7c2bb --- /dev/null +++ b/src/modules/commander/failsafe/CMakeLists.txt @@ -0,0 +1,79 @@ +############################################################################ +# +# Copyright (c) 2022 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# Extract information from status msg file +set(status_msg_file ${PX4_SOURCE_DIR}/msg/vehicle_status_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 + ${status_msg_file} ${generated_uorb_struct_field_mapping_header} + ${html_template_file} ${html_output_file} + DEPENDS + ${status_msg_file} + ${html_template_file} + ${CMAKE_CURRENT_SOURCE_DIR}/parse_flags_from_msg.py + COMMENT "Extracting info from status 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 "$<$:-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 +) + diff --git a/src/modules/commander/failsafe/emscripten.cpp b/src/modules/commander/failsafe/emscripten.cpp new file mode 100644 index 0000000000..d078b64630 --- /dev/null +++ b/src/modules/commander/failsafe/emscripten.cpp @@ -0,0 +1,221 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#define PARAM_IMPLEMENTATION +#include + +#include "failsafe.h" +#include "../ModeUtil/mode_requirements.hpp" +#include + +#include +#include +#include +#include +#include +#include +#include + +using namespace emscripten; + +#include + +// parameter storage +struct Param { + union param_value_u val; +}; + +class FailsafeTest : public ModuleParams +{ +public: + FailsafeTest() : ModuleParams(nullptr) {} + + void updateParams() override + { + ModuleParams::updateParams(); + } + + FailsafeBase ¤t() { return _failsafe; } + std::map ¶ms() { return _used_params; } +private: + std::map _used_params; + Failsafe _failsafe{this}; +}; + +static FailsafeTest failsafe_instance; + + +int param_get(param_t param, void *val) +{ + std::map &used_params = failsafe_instance.params(); + auto param_iter = used_params.find(param); + + if (param_iter != used_params.end()) { + memcpy(val, ¶m_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 &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 get_used_params() +{ + std::vector ret; + std::map &used_params = failsafe_instance.params(); + + for (const auto ¶m_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 &used_params = failsafe_instance.params(); + + for (const auto ¶m_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 &used_params = failsafe_instance.params(); + + for (auto ¶m_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, + vehicle_status_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_("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("vector"); +} diff --git a/src/modules/commander/failsafe/emscripten_template.html b/src/modules/commander/failsafe/emscripten_template.html new file mode 100644 index 0000000000..a7f417cd40 --- /dev/null +++ b/src/modules/commander/failsafe/emscripten_template.html @@ -0,0 +1,411 @@ + + + + + + + Failsafe State Machine + + + +
emscripten
+
Downloading...
+
+ +
+

Failsafe State Machine Simulation

+ + + + + +

State

+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
?For VTOLs the type switches between Multirotor and Fixed Wing
+
+ +
+
+

Parameters

+
+

Conditions

+ {{{ HTML_CONDITIONS }}} +
+

Output

+

Failsafe action: 

+

User takeover active: 

+

Console output (debug):
+ +

+
+
+ + + {{{ SCRIPT }}} + + diff --git a/src/modules/commander/failsafe/failsafe.cpp b/src/modules/commander/failsafe/failsafe.cpp new file mode 100644 index 0000000000..5d80dfa2fa --- /dev/null +++ b/src/modules/commander/failsafe/failsafe.cpp @@ -0,0 +1,267 @@ +/**************************************************************************** + * + * 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 +#include + +FailsafeBase::ActionOptions Failsafe::fromNavDllOrRclActParam(int param_value) const +{ + 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) const +{ + 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; +} + +void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state, + const vehicle_status_flags_s &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; + + // RC loss + const bool rc_loss_ignored_mission = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION + && (_param_com_rcl_except.get() & (int)RCLossExceptionBits::Mission); + const bool rc_loss_ignored_loiter = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER + && (_param_com_rcl_except.get() & (int)RCLossExceptionBits::Hold); + const bool rc_loss_ignored_offboard = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_OFFBOARD + && (_param_com_rcl_except.get() & (int)RCLossExceptionBits::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)RCLossExceptionBits::Hold); + const bool rc_loss_ignored = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL || + state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND || + state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND || + rc_loss_ignored_mission || rc_loss_ignored_loiter || rc_loss_ignored_offboard || rc_loss_ignored_takeoff || + ignore_link_failsafe; + + if (_param_com_rc_in_mode.get() != 4 && !rc_loss_ignored) { + CHECK_FAILSAFE(status_flags, rc_signal_lost, + fromNavDllOrRclActParam(_param_nav_rcl_act.get()).causedBy(Cause::RCLoss)); + } + + // Datalink loss + const bool data_link_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 && !data_link_loss_ignored) { + CHECK_FAILSAFE(status_flags, data_link_lost, + fromNavDllOrRclActParam(_param_nav_dll_act.get()).causedBy(Cause::DatalinkLoss)); + } + + // VTOL transition failure + 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, Action::RTL); + } + + // Mission + if (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) { + CHECK_FAILSAFE(status_flags, mission_failure, Action::RTL); + + // If RC loss and datalink 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.data_link_lost, Action::RTL); + } + } + + + + + CHECK_FAILSAFE(status_flags, geofence_violated, fromGfActParam(_param_gf_action.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)); +} + +FailsafeBase::Action Failsafe::checkModeFallback(const vehicle_status_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 = Action::FallbackPosCtrl; // TODO: use param + user_intended_mode = vehicle_status_s::NAVIGATION_STATE_POSCTL; + } + + // 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; +} diff --git a/src/modules/commander/failsafe/failsafe.h b/src/modules/commander/failsafe/failsafe.h new file mode 100644 index 0000000000..240755f687 --- /dev/null +++ b/src/modules/commander/failsafe/failsafe.h @@ -0,0 +1,79 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#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 vehicle_status_flags_s &status_flags) override; + Action checkModeFallback(const vehicle_status_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: + enum class RCLossExceptionBits : int32_t { + Mission = (1 << 0), + Hold = (1 << 1), + Offboard = (1 << 2) + }; + + ActionOptions fromNavDllOrRclActParam(int param_value) const; + + ActionOptions fromGfActParam(int param_value) const; + + 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}; + + DEFINE_PARAMETERS_CUSTOM_PARENT(FailsafeBase, + (ParamInt) _param_nav_dll_act, + (ParamInt) _param_nav_rcl_act, + (ParamInt) _param_com_rcl_except, + (ParamInt) _param_com_rc_in_mode, + (ParamInt) _param_com_posctl_navl, + (ParamInt) _param_gf_action + ); + +}; + diff --git a/src/modules/commander/failsafe/framework.cpp b/src/modules/commander/failsafe/framework.cpp new file mode 100644 index 0000000000..06d45c9398 --- /dev/null +++ b/src/modules/commander/failsafe/framework.cpp @@ -0,0 +1,625 @@ +/**************************************************************************** + * + * 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 +#include +#include +#include + +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 vehicle_status_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( + 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( + 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( + 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( + 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( + 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( + 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 vehicle_status_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"); + } + + // 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 vehicle_status_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 vehicle_status_flags_s &status_flags, uint8_t mode) +{ + uint32_t mode_mask = 1u << mode; + return + (status_flags.angular_velocity_valid || ((status_flags.mode_req_angular_velocity & mode_mask) == 0)) && + (status_flags.attitude_valid || ((status_flags.mode_req_attitude & mode_mask) == 0)) && + (status_flags.local_position_valid || ((status_flags.mode_req_local_position & mode_mask) == 0)) && + (status_flags.local_position_valid_relaxed || ((status_flags.mode_req_local_position_relaxed & mode_mask) == 0)) && + (status_flags.global_position_valid || ((status_flags.mode_req_global_position & mode_mask) == 0)) && + (status_flags.local_altitude_valid || ((status_flags.mode_req_local_alt & mode_mask) == 0)) && + (status_flags.auto_mission_available || ((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_valid || ((status_flags.mode_req_home_position & mode_mask) == 0)) && + ((status_flags.mode_req_other & mode_mask) == 0); +} diff --git a/src/modules/commander/failsafe/framework.h b/src/modules/commander/failsafe/framework.h new file mode 100644 index 0000000000..cc73d3e64c --- /dev/null +++ b/src/modules/commander/failsafe/framework.h @@ -0,0 +1,261 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include + +#include + +#define CHECK_FAILSAFE(status_flags, flag_name, options) \ + checkFailsafe((int)offsetof(vehicle_status_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, + RCLoss, + DatalinkLoss, + 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 vehicle_status_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 vehicle_status_flags_s &status_flags) = 0; + virtual Action checkModeFallback(const vehicle_status_flags_s &status_flags, uint8_t user_intended_mode) const = 0; + + const vehicle_status_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 vehicle_status_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 vehicle_status_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 vehicle_status_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}; + vehicle_status_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(vehicle_status_flags_s) + 1}; + bool _duplicate_reported_once{false}; + + orb_advert_t _mavlink_log_pub{nullptr}; + + DEFINE_PARAMETERS_CUSTOM_PARENT(ModuleParams, + (ParamFloat) _param_com_fail_act_t + ); + +}; + diff --git a/src/modules/commander/failsafe/parse_flags_from_msg.py b/src/modules/commander/failsafe/parse_flags_from_msg.py new file mode 100755 index 0000000000..2f6a0114bd --- /dev/null +++ b/src/modules/commander/failsafe/parse_flags_from_msg.py @@ -0,0 +1,140 @@ +#!/usr/bin/env python3 + +""" +Extract fields from .msg for failsafe state machine simulation +""" +import math +import os +import argparse +import sys +import re + + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description='Extract fields from .msg files') + parser.add_argument('msg_file', help='vehicle_status_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 += '
' + for group in groups: + if group.new_row: + html_conditions += '' + html_conditions += '

' + if group.name != '': + html_conditions += '

' + group.name + '
' + 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 += ''' +
+ + +
+ '''.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 += ''' +
+ + +
+ '''.format(field_name, comment) + else: + raise Exception("Unsupported type: {:}".format(field_type)) + html_conditions += '

' + html_conditions += '
' + + 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) +