mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-28 02:36:37 +08:00
commander: add failsafe state machine with webassembly simulation
to run the simulation: install sdk: https://emscripten.org/docs/getting_started/downloads.html make run_failsafe_web_server open http://0.0.0.0:8000/ Co-authored-by: Konrad <konrad@auterion.com>
This commit is contained in:
Beat Küng
Daniel Agar
@@ -575,3 +575,13 @@ update_px4_ros_com:
|
|||||||
|
|
||||||
update_px4_msgs:
|
update_px4_msgs:
|
||||||
@Tools/update_px4_ros2_bridge.sh --ws_dir ${ROS2_WS_DIR} --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
|
||||||
|
|||||||
@@ -1,5 +1,8 @@
|
|||||||
# TODO: rename to failsafe_flags (will be input to failsafe state machine)
|
# 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)
|
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 calibration_enabled
|
||||||
bool pre_flight_checks_pass # true if all checks necessary to arm pass
|
bool pre_flight_checks_pass # true if all checks necessary to arm pass
|
||||||
bool auto_mission_available
|
bool auto_mission_available # Mission available
|
||||||
bool angular_velocity_valid
|
bool angular_velocity_valid # Angular velocity valid
|
||||||
bool attitude_valid
|
bool attitude_valid # Attitude valid
|
||||||
bool local_altitude_valid
|
bool local_altitude_valid # 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 # Local position estimate valid
|
||||||
bool local_position_valid_relaxed # Local position with reduced accuracy requirements (e.g. flying with optical flow)
|
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 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 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_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 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_low_remaining_time
|
||||||
bool battery_unhealthy
|
bool battery_unhealthy
|
||||||
|
|||||||
@@ -147,6 +147,7 @@ add_custom_command(OUTPUT px4_parameters.hpp
|
|||||||
px_generate_params.py
|
px_generate_params.py
|
||||||
templates/px4_parameters.hpp.jinja
|
templates/px4_parameters.hpp.jinja
|
||||||
)
|
)
|
||||||
|
add_custom_target(parameters_header DEPENDS px4_parameters.hpp)
|
||||||
|
|
||||||
set(SRCS)
|
set(SRCS)
|
||||||
|
|
||||||
|
|||||||
@@ -33,6 +33,7 @@
|
|||||||
|
|
||||||
add_subdirectory(failure_detector)
|
add_subdirectory(failure_detector)
|
||||||
add_subdirectory(HealthAndArmingChecks)
|
add_subdirectory(HealthAndArmingChecks)
|
||||||
|
add_subdirectory(failsafe)
|
||||||
add_subdirectory(Arming)
|
add_subdirectory(Arming)
|
||||||
add_subdirectory(ModeUtil)
|
add_subdirectory(ModeUtil)
|
||||||
|
|
||||||
@@ -69,6 +70,7 @@ px4_add_module(
|
|||||||
sensor_calibration
|
sensor_calibration
|
||||||
world_magnetic_model
|
world_magnetic_model
|
||||||
mode_util
|
mode_util
|
||||||
|
failsafe
|
||||||
)
|
)
|
||||||
|
|
||||||
px4_add_unit_gtest(SRC mag_calibration_test.cpp LINKLIBS modules__commander)
|
px4_add_unit_gtest(SRC mag_calibration_test.cpp LINKLIBS modules__commander)
|
||||||
|
|||||||
@@ -31,7 +31,9 @@
|
|||||||
#
|
#
|
||||||
############################################################################
|
############################################################################
|
||||||
|
|
||||||
px4_add_library(mode_util
|
add_library(mode_util
|
||||||
control_mode.cpp
|
control_mode.cpp
|
||||||
mode_requirements.cpp
|
mode_requirements.cpp
|
||||||
)
|
)
|
||||||
|
add_dependencies(mode_util uorb_headers prebuild_targets)
|
||||||
|
|
||||||
|
|||||||
@@ -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 "$<$<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
|
||||||
|
)
|
||||||
|
|
||||||
@@ -0,0 +1,221 @@
|
|||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
|
||||||
|
*
|
||||||
|
* Redistribution and use in source and binary forms, with or without
|
||||||
|
* modification, are permitted provided that the following conditions
|
||||||
|
* are met:
|
||||||
|
*
|
||||||
|
* 1. Redistributions of source code must retain the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer.
|
||||||
|
* 2. Redistributions in binary form must reproduce the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer in
|
||||||
|
* the documentation and/or other materials provided with the
|
||||||
|
* distribution.
|
||||||
|
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||||
|
* used to endorse or promote products derived from this software
|
||||||
|
* without specific prior written permission.
|
||||||
|
*
|
||||||
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||||
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||||
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||||
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||||
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||||
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||||
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||||
|
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||||
|
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||||
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||||
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||||
|
* POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
*
|
||||||
|
****************************************************************************/
|
||||||
|
|
||||||
|
#define PARAM_IMPLEMENTATION
|
||||||
|
#include <parameters/param.h>
|
||||||
|
|
||||||
|
#include "failsafe.h"
|
||||||
|
#include "../ModeUtil/mode_requirements.hpp"
|
||||||
|
#include <uORB/topics/vehicle_status.h>
|
||||||
|
|
||||||
|
#include <emscripten/emscripten.h>
|
||||||
|
#include <emscripten/bind.h>
|
||||||
|
#include <emscripten/html5.h>
|
||||||
|
#include <cstdio>
|
||||||
|
#include <string>
|
||||||
|
#include <map>
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
|
using namespace emscripten;
|
||||||
|
|
||||||
|
#include <generated_uorb_struct_field_mapping.h>
|
||||||
|
|
||||||
|
// parameter storage
|
||||||
|
struct Param {
|
||||||
|
union param_value_u val;
|
||||||
|
};
|
||||||
|
|
||||||
|
class FailsafeTest : public ModuleParams
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
FailsafeTest() : ModuleParams(nullptr) {}
|
||||||
|
|
||||||
|
void updateParams() override
|
||||||
|
{
|
||||||
|
ModuleParams::updateParams();
|
||||||
|
}
|
||||||
|
|
||||||
|
FailsafeBase ¤t() { return _failsafe; }
|
||||||
|
std::map<param_t, Param> ¶ms() { 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, ¶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<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 ¶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<param_t, Param> &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<param_t, Param> &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_<vehicle_status_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>");
|
||||||
|
}
|
||||||
@@ -0,0 +1,411 @@
|
|||||||
|
<!doctype html>
|
||||||
|
<html lang="en-us">
|
||||||
|
<head>
|
||||||
|
<meta charset="utf-8">
|
||||||
|
<link rel="icon" href="data:;base64,iVBORw0KGgo=">
|
||||||
|
<meta http-equiv="Content-Type" content="text/html; charset=utf-8">
|
||||||
|
<title>Failsafe State Machine</title>
|
||||||
|
<style>
|
||||||
|
html * {
|
||||||
|
font-family: Helvetica, sans-serif;
|
||||||
|
}
|
||||||
|
.title {
|
||||||
|
margin-left: 10px;
|
||||||
|
margin-bottom: 0px;
|
||||||
|
}
|
||||||
|
h3 {
|
||||||
|
margin-bottom: 5px;
|
||||||
|
}
|
||||||
|
h5 {
|
||||||
|
margin-top: 5px;
|
||||||
|
margin-bottom: 0px;
|
||||||
|
}
|
||||||
|
input[type=text] {
|
||||||
|
text-align: right;
|
||||||
|
}
|
||||||
|
p {
|
||||||
|
margin-top: 8px;
|
||||||
|
margin-bottom: 8px;
|
||||||
|
}
|
||||||
|
button {
|
||||||
|
margin-top: 6px;
|
||||||
|
}
|
||||||
|
.checkbox-field {
|
||||||
|
display: flex;
|
||||||
|
flex-direction: row;
|
||||||
|
}
|
||||||
|
.box {
|
||||||
|
background-color: rgb(231, 233, 235);
|
||||||
|
border-bottom-left-radius: 5px;
|
||||||
|
border-bottom-right-radius: 5px;
|
||||||
|
border-top-left-radius: 5px;
|
||||||
|
border-top-right-radius: 5px;
|
||||||
|
box-shadow: none;
|
||||||
|
box-sizing: border-box;
|
||||||
|
line-height: 22.5px;
|
||||||
|
margin-bottom: 10px;
|
||||||
|
margin-left: 10px;
|
||||||
|
margin-right: 10px;
|
||||||
|
margin-top: 20px;
|
||||||
|
padding-bottom: 16px;
|
||||||
|
padding-left: 20px;
|
||||||
|
padding-right: 20px;
|
||||||
|
padding-top: 8px
|
||||||
|
}
|
||||||
|
table td {
|
||||||
|
vertical-align: top;
|
||||||
|
}
|
||||||
|
|
||||||
|
.tooltip {
|
||||||
|
position: relative;
|
||||||
|
background: rgba(0,0,0,0.2);
|
||||||
|
padding: 0px 5px;
|
||||||
|
border-radius: 100%;
|
||||||
|
font-size: 90%;
|
||||||
|
width: 11px;
|
||||||
|
display: inline-block;
|
||||||
|
margin-left: 8px;
|
||||||
|
text-align: center;
|
||||||
|
color: white;
|
||||||
|
}
|
||||||
|
|
||||||
|
.tooltip .tooltiptext {
|
||||||
|
visibility: hidden;
|
||||||
|
width: 200px;
|
||||||
|
background-color: #555;
|
||||||
|
color: #fff;
|
||||||
|
text-align: center;
|
||||||
|
border-radius: 6px;
|
||||||
|
padding: 5px 0;
|
||||||
|
position: absolute;
|
||||||
|
z-index: 1;
|
||||||
|
top: 125%;
|
||||||
|
left: 50%;
|
||||||
|
margin-left: -100px;
|
||||||
|
opacity: 0;
|
||||||
|
transition: opacity 0.3s;
|
||||||
|
}
|
||||||
|
|
||||||
|
.tooltip:hover .tooltiptext {
|
||||||
|
visibility: visible;
|
||||||
|
opacity: 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
.emscripten { padding-right: 0; display: block; }
|
||||||
|
textarea.emscripten {
|
||||||
|
font-family: monospace;
|
||||||
|
width: 100%;
|
||||||
|
white-space: pre;
|
||||||
|
overflow-wrap: normal;
|
||||||
|
overflow-x: scroll;
|
||||||
|
}
|
||||||
|
div.emscripten { text-align: center; }
|
||||||
|
div.emscripten_border { border: 1px solid black; }
|
||||||
|
/* the canvas *must not* have any border or padding, or mouse coords will be wrong */
|
||||||
|
canvas.emscripten { border: 0px none; background-color: black; }
|
||||||
|
|
||||||
|
.spinner {
|
||||||
|
height: 50px;
|
||||||
|
width: 50px;
|
||||||
|
margin: 0px auto;
|
||||||
|
-webkit-animation: rotation .8s linear infinite;
|
||||||
|
-moz-animation: rotation .8s linear infinite;
|
||||||
|
-o-animation: rotation .8s linear infinite;
|
||||||
|
animation: rotation 0.8s linear infinite;
|
||||||
|
border-left: 10px solid rgb(0,150,240);
|
||||||
|
border-right: 10px solid rgb(0,150,240);
|
||||||
|
border-bottom: 10px solid rgb(0,150,240);
|
||||||
|
border-top: 10px solid rgb(100,0,200);
|
||||||
|
border-radius: 100%;
|
||||||
|
background-color: rgb(200,100,250);
|
||||||
|
}
|
||||||
|
@-webkit-keyframes rotation {
|
||||||
|
from {-webkit-transform: rotate(0deg);}
|
||||||
|
to {-webkit-transform: rotate(360deg);}
|
||||||
|
}
|
||||||
|
@-moz-keyframes rotation {
|
||||||
|
from {-moz-transform: rotate(0deg);}
|
||||||
|
to {-moz-transform: rotate(360deg);}
|
||||||
|
}
|
||||||
|
@-o-keyframes rotation {
|
||||||
|
from {-o-transform: rotate(0deg);}
|
||||||
|
to {-o-transform: rotate(360deg);}
|
||||||
|
}
|
||||||
|
@keyframes rotation {
|
||||||
|
from {transform: rotate(0deg);}
|
||||||
|
to {transform: rotate(360deg);}
|
||||||
|
}
|
||||||
|
|
||||||
|
</style>
|
||||||
|
</head>
|
||||||
|
<body>
|
||||||
|
<figure style="overflow:visible;" id="spinner"><div class="spinner"></div><center style="margin-top:0.5em"><strong>emscripten</strong></center></figure>
|
||||||
|
<div class="emscripten" id="status">Downloading...</div>
|
||||||
|
<div class="emscripten">
|
||||||
|
<progress value="0" max="100" id="progress" hidden=1></progress>
|
||||||
|
</div>
|
||||||
|
<h1 class="title" id="title">Failsafe State Machine Simulation</h1>
|
||||||
|
<table>
|
||||||
|
<tr>
|
||||||
|
<td><div class="box"><h3>State</h3>
|
||||||
|
<table>
|
||||||
|
<tr>
|
||||||
|
<td><label for="vehicle_type">Vehicle Type: </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: </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: <b id="action"></b></p>
|
||||||
|
<p>User takeover active: <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+": </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 = " "+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, "&");
|
||||||
|
//text = text.replace(/</g, "<");
|
||||||
|
//text = text.replace(/>/g, ">");
|
||||||
|
//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>
|
||||||
@@ -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 <px4_platform_common/log.h>
|
||||||
|
#include <uORB/topics/vehicle_status.h>
|
||||||
|
|
||||||
|
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;
|
||||||
|
}
|
||||||
@@ -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<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
|
||||||
|
);
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,261 @@
|
|||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
|
||||||
|
*
|
||||||
|
* Redistribution and use in source and binary forms, with or without
|
||||||
|
* modification, are permitted provided that the following conditions
|
||||||
|
* are met:
|
||||||
|
*
|
||||||
|
* 1. Redistributions of source code must retain the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer.
|
||||||
|
* 2. Redistributions in binary form must reproduce the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer in
|
||||||
|
* the documentation and/or other materials provided with the
|
||||||
|
* distribution.
|
||||||
|
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||||
|
* used to endorse or promote products derived from this software
|
||||||
|
* without specific prior written permission.
|
||||||
|
*
|
||||||
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||||
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||||
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||||
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||||
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||||
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||||
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||||
|
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||||
|
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||||
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||||
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||||
|
* POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
*
|
||||||
|
****************************************************************************/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <uORB/topics/vehicle_status_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(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<px4::params::COM_FAIL_ACT_T>) _param_com_fail_act_t
|
||||||
|
);
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
+140
@@ -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 += '<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}: </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)
|
||||||
|
|
||||||
Reference in New Issue
Block a user