forked from Archive/PX4-Autopilot
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
parent
82911e48be
commit
a04230faa1
10
Makefile
10
Makefile
|
|
@ -575,3 +575,13 @@ update_px4_ros_com:
|
|||
|
||||
update_px4_msgs:
|
||||
@Tools/update_px4_ros2_bridge.sh --ws_dir ${ROS2_WS_DIR} --px4_msgs
|
||||
|
||||
.PHONY: failsafe_web run_failsafe_web_server
|
||||
failsafe_web:
|
||||
@if ! command -v emcc; then echo -e "Install emscripten first: https://emscripten.org/docs/getting_started/downloads.html\nAnd source the env: source <path>/emsdk_env.sh"; exit 1; fi
|
||||
@$(MAKE) --no-print-directory px4_sitl_default failsafe_test parameters_xml \
|
||||
PX4_CMAKE_BUILD_TYPE=Release BUILD_DIR_SUFFIX=_failsafe_web \
|
||||
CMAKE_ARGS="-DCMAKE_CXX_COMPILER=em++ -DCMAKE_C_COMPILER=emcc"
|
||||
run_failsafe_web_server: failsafe_web
|
||||
@cd build/px4_sitl_default_failsafe_web && \
|
||||
python3 -m http.server
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
);
|
||||
|
||||
};
|
||||
|
||||
|
|
@ -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 <uORB/topics/vehicle_status.h>
|
||||
#include <px4_platform_common/events.h>
|
||||
#include <px4_platform_common/log.h>
|
||||
#include <systemlib/mavlink_log.h>
|
||||
|
||||
using failsafe_action_t = events::px4::enums::failsafe_action_t;
|
||||
using failsafe_cause_t = events::px4::enums::failsafe_cause_t;
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
FailsafeBase::FailsafeBase(ModuleParams *parent) : ModuleParams(parent)
|
||||
{
|
||||
_current_start_delay = _param_com_fail_act_t.get() * 1_s;
|
||||
}
|
||||
|
||||
uint8_t FailsafeBase::update(const hrt_abstime &time_us, const State &state, bool user_intended_mode_updated,
|
||||
bool rc_sticks_takeover_request,
|
||||
const 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<uint32_t, events::px4::enums::failsafe_action_t, uint16_t>(
|
||||
events::ID("commander_failsafe_enter_generic_hold"),
|
||||
{events::Log::Critical, events::LogInternal::Warning},
|
||||
"Failsafe activated, triggering {2} in {3} seconds", mavlink_mode, failsafe_action,
|
||||
(uint16_t) delay_s);
|
||||
|
||||
} else {
|
||||
/* EVENT
|
||||
*/
|
||||
events::send<uint32_t, events::px4::enums::failsafe_action_t, uint16_t, events::px4::enums::failsafe_cause_t>(
|
||||
events::ID("commander_failsafe_enter_hold"),
|
||||
{events::Log::Critical, events::LogInternal::Warning},
|
||||
"Failsafe activated due to {4}, triggering {2} in {3} seconds", mavlink_mode, failsafe_action,
|
||||
(uint16_t) delay_s, failsafe_cause);
|
||||
}
|
||||
|
||||
mavlink_log_critical(&_mavlink_log_pub, "Failsafe activated, entering Hold for %i seconds\t", delay_s);
|
||||
|
||||
} else { // no delay
|
||||
failsafe_action_t failsafe_action = (failsafe_action_t)action;
|
||||
|
||||
if (cause == Cause::Generic) {
|
||||
if (action == Action::Warn) {
|
||||
/* EVENT
|
||||
* @description No action is triggered.
|
||||
* @type append_health_and_arming_messages
|
||||
*/
|
||||
events::send<uint32_t>(
|
||||
events::ID("commander_failsafe_enter_generic_warn"),
|
||||
{events::Log::Warning, events::LogInternal::Warning},
|
||||
"Failsafe warning triggered", mavlink_mode);
|
||||
|
||||
} else {
|
||||
/* EVENT
|
||||
* @type append_health_and_arming_messages
|
||||
*/
|
||||
events::send<uint32_t, events::px4::enums::failsafe_action_t>(
|
||||
events::ID("commander_failsafe_enter_generic"),
|
||||
{events::Log::Critical, events::LogInternal::Warning},
|
||||
"Failsafe activated, triggering {2}", mavlink_mode, failsafe_action);
|
||||
}
|
||||
|
||||
} else {
|
||||
if (action == Action::Warn) {
|
||||
if (cause == Cause::BatteryLow) {
|
||||
events::send(events::ID("commander_failsafe_enter_low_bat"),
|
||||
{events::Log::Warning, events::LogInternal::Info},
|
||||
"Low battery level, return advised");
|
||||
|
||||
} else if (cause == Cause::BatteryCritical) {
|
||||
events::send(events::ID("commander_failsafe_enter_crit_bat_warn"),
|
||||
{events::Log::Critical, events::LogInternal::Info},
|
||||
"Critical battery level, land now");
|
||||
|
||||
} else if (cause == Cause::BatteryEmergency) {
|
||||
events::send(events::ID("commander_failsafe_enter_crit_low_bat_warn"), {events::Log::Emergency, events::LogInternal::Info},
|
||||
"Emergency battery level, land immediately");
|
||||
|
||||
} else {
|
||||
/* EVENT
|
||||
* @description No action is triggered.
|
||||
*/
|
||||
events::send<uint32_t, events::px4::enums::failsafe_cause_t>(
|
||||
events::ID("commander_failsafe_enter_warn"),
|
||||
{events::Log::Warning, events::LogInternal::Warning},
|
||||
"Failsafe warning triggered due to {2}", mavlink_mode, failsafe_cause);
|
||||
|
||||
}
|
||||
|
||||
} else { // action != Warn
|
||||
/* EVENT
|
||||
*/
|
||||
events::send<uint32_t, events::px4::enums::failsafe_action_t, events::px4::enums::failsafe_cause_t>(
|
||||
events::ID("commander_failsafe_enter"),
|
||||
{events::Log::Critical, events::LogInternal::Warning},
|
||||
"Failsafe activated due to {3}, triggering {2}", mavlink_mode, failsafe_action, failsafe_cause);
|
||||
}
|
||||
}
|
||||
|
||||
mavlink_log_critical(&_mavlink_log_pub, "Failsafe activated\t");
|
||||
}
|
||||
|
||||
#endif /* EMSCRIPTEN_BUILD */
|
||||
}
|
||||
|
||||
bool FailsafeBase::checkFailsafe(int caller_id, bool last_state_failure, bool cur_state_failure,
|
||||
const ActionOptions &options)
|
||||
{
|
||||
if (cur_state_failure) {
|
||||
// Invalid state: find or add action
|
||||
int free_idx = -1;
|
||||
int found_idx = -1;
|
||||
|
||||
for (int i = 0; i < max_num_actions; ++i) {
|
||||
if (!_actions[i].valid()) {
|
||||
free_idx = i;
|
||||
|
||||
} else if (_actions[i].id == caller_id) {
|
||||
found_idx = i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (found_idx != -1) {
|
||||
if (_actions[found_idx].activated && !_duplicate_reported_once) {
|
||||
PX4_ERR("BUG: duplicate check for caller_id %i", caller_id);
|
||||
_duplicate_reported_once = true;
|
||||
}
|
||||
|
||||
_actions[found_idx].state_failure = true;
|
||||
_actions[found_idx].activated = true;
|
||||
_actions[found_idx].action = options.action; // Allow action to be updated, but keep the rest
|
||||
|
||||
if (!last_state_failure) {
|
||||
PX4_DEBUG("Caller %i: state changed to failed, action already active", caller_id);
|
||||
}
|
||||
|
||||
} else {
|
||||
if (free_idx == -1) {
|
||||
PX4_ERR("No free failsafe action idx");
|
||||
|
||||
// replace based on action severity
|
||||
for (int i = 0; i < max_num_actions; ++i) {
|
||||
if (options.action > _actions[i].action) {
|
||||
free_idx = i;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (free_idx != -1) {
|
||||
_actions[free_idx] = options;
|
||||
_actions[free_idx].id = caller_id;
|
||||
_actions[free_idx].state_failure = true;
|
||||
_actions[free_idx].activated = true;
|
||||
|
||||
if (options.allow_user_takeover == UserTakeoverAllowed::Auto) {
|
||||
if (_param_com_fail_act_t.get() > 0.1f) {
|
||||
if (options.action != Action::Warn && _current_delay == 0) {
|
||||
_current_delay = _current_start_delay;
|
||||
}
|
||||
|
||||
_actions[free_idx].allow_user_takeover = UserTakeoverAllowed::Always;
|
||||
|
||||
} else {
|
||||
_actions[free_idx].allow_user_takeover = UserTakeoverAllowed::AlwaysModeSwitchOnly;
|
||||
}
|
||||
}
|
||||
|
||||
if (options.action == Action::Warn) {
|
||||
_notification_required = true;
|
||||
}
|
||||
|
||||
if (options.action >= Action::Hold) { // If not a Fallback
|
||||
_user_takeover_active = false; // Clear takeover
|
||||
}
|
||||
|
||||
PX4_DEBUG("Caller %i: state changed to failed, adding action", caller_id);
|
||||
}
|
||||
}
|
||||
|
||||
} else if (last_state_failure && !cur_state_failure) {
|
||||
// Invalid -> valid transition: remove action
|
||||
bool found = false;
|
||||
|
||||
for (int i = 0; i < max_num_actions; ++i) {
|
||||
if (_actions[i].id == caller_id) {
|
||||
if (found) {
|
||||
PX4_ERR("Dup action with ID %i", caller_id);
|
||||
}
|
||||
|
||||
removeAction(_actions[i]);
|
||||
found = true;
|
||||
}
|
||||
}
|
||||
|
||||
// It's ok if we did not find the action, it might already have been removed due to not being activated
|
||||
}
|
||||
|
||||
return cur_state_failure;
|
||||
}
|
||||
|
||||
void FailsafeBase::removeAction(ActionOptions &action) const
|
||||
{
|
||||
if (action.clear_condition == ClearCondition::WhenConditionClears) {
|
||||
// Remove action
|
||||
PX4_DEBUG("Caller %i: state changed to valid, removing action", action.id);
|
||||
action.setInvalid();
|
||||
|
||||
} else {
|
||||
if (action.state_failure) {
|
||||
PX4_DEBUG("Caller %i: state changed to valid, keeping action", action.id);
|
||||
}
|
||||
|
||||
// Keep action, just flag the state
|
||||
action.state_failure = false;
|
||||
}
|
||||
}
|
||||
|
||||
void FailsafeBase::removeNonActivatedActions()
|
||||
{
|
||||
// A non-activated action means the check was not called during the last update:
|
||||
// treat the state as valid and remove the action depending on the clear_condition
|
||||
for (int action_idx = 0; action_idx < max_num_actions; ++action_idx) {
|
||||
ActionOptions &cur_action = _actions[action_idx];
|
||||
|
||||
if (cur_action.valid() && !cur_action.activated) {
|
||||
if (_actions[action_idx].state_failure) {
|
||||
PX4_DEBUG("Caller %i: action not activated", cur_action.id);
|
||||
}
|
||||
|
||||
removeAction(cur_action);
|
||||
}
|
||||
|
||||
cur_action.activated = false;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void FailsafeBase::getSelectedAction(const State &state, const 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);
|
||||
}
|
||||
|
|
@ -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
|
||||
);
|
||||
|
||||
};
|
||||
|
||||
|
|
@ -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)
|
||||
|
||||
Loading…
Reference in New Issue