dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Commander: allow disabling RC loss failsafe for mission, hold, offboard independently

This commit is contained in:
Matthias Grob 2021-06-18 18:17:19 +02:00
parent 933d31b476
commit 11556d4e9a
5 changed files with 65 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/modules/commander/Commander.cpp
View File

@ -2575,7 +2575,8 @@ Commander::run()
static_cast<offboard_loss_actions_t>(_param_com_obl_act.get()),
static_cast<offboard_loss_rc_actions_t>(_param_com_obl_rc_act.get()),
static_cast<position_nav_loss_actions_t>(_param_com_posctl_navl.get()),
_param_com_rcl_act_t.get());
_param_com_rcl_act_t.get(),
_param_com_rcl_except.get());
if (nav_state_changed) {
_status.nav_state_timestamp = hrt_absolute_time();

1
src/modules/commander/Commander.hpp
View File

@ -198,6 +198,7 @@ private:
(ParamInt<px4::params::NAV_RCL_ACT>) _param_nav_rcl_act,
(ParamFloat<px4::params::COM_RCL_ACT_T>) _param_com_rcl_act_t,
(ParamInt<px4::params::COM_RCL_EXCEPT>) _param_com_rcl_except,
(ParamFloat<px4::params::COM_HOME_H_T>) _param_com_home_h_t,
(ParamFloat<px4::params::COM_HOME_V_T>) _param_com_home_v_t,

19
src/modules/commander/commander_params.c
View File

@ -850,6 +850,8 @@ PARAM_DEFINE_INT32(COM_TAKEOFF_ACT, 0);
* @value 3 Land mode
* @value 5 Terminate
* @value 6 Lockdown
* @min 0
* @max 6
*
* @group Commander
*/
@ -862,17 +864,32 @@ PARAM_DEFINE_INT32(NAV_DLL_ACT, 0);
* set by COM_RC_LOSS_T in seconds. If RC input checks have been disabled
* by setting the COM_RC_IN_MODE param it will not be triggered.
*
* @value 0 Disabled
* @value 1 Hold mode
* @value 2 Return mode
* @value 3 Land mode
* @value 5 Terminate
* @value 6 Lockdown
* @min 1
* @max 6
*
* @group Commander
*/
PARAM_DEFINE_INT32(NAV_RCL_ACT, 2);
/**
* RC loss exceptions
*
* Specify modes in which RC loss is ignored and the failsafe action not triggered.
*
* @min 0
* @max 31
* @bit 0 Mission
* @bit 1 Hold
* @bit 2 Offboard
* @group Commander
*/
PARAM_DEFINE_INT32(COM_RCL_EXCEPT, 0);
/**
* Flag to enable obstacle avoidance.
*

67
src/modules/commander/state_machine_helper.cpp
View File

@ -436,15 +436,13 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
const link_loss_actions_t rc_loss_act, const offboard_loss_actions_t offb_loss_act,
const offboard_loss_rc_actions_t offb_loss_rc_act,
const position_nav_loss_actions_t posctl_nav_loss_act,
const float param_com_rcl_act_t)
const float param_com_rcl_act_t, const int param_com_rcl_except)
{
const navigation_state_t nav_state_old = status.nav_state;
const bool data_link_loss_act_configured = data_link_loss_act > link_loss_actions_t::DISABLED;
const bool rc_loss_act_configured = rc_loss_act > link_loss_actions_t::DISABLED;
const bool rc_lost = rc_loss_act_configured && (status.rc_signal_lost);
const bool is_armed = (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
const bool data_link_loss_act_configured = (data_link_loss_act > link_loss_actions_t::DISABLED);
bool is_armed = (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
bool old_failsafe = status.failsafe;
status.failsafe = false;
@ -453,17 +451,16 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
reset_link_loss_globals(armed, old_failsafe, data_link_loss_act);
reset_offboard_loss_globals(armed, old_failsafe, offb_loss_act, offb_loss_rc_act);
/* evaluate main state to decide in normal (non-failsafe) mode */
// Failsafe decision logic for every normal non-failsafe mode
switch (internal_state.main_state) {
case commander_state_s::MAIN_STATE_ACRO:
case commander_state_s::MAIN_STATE_MANUAL:
case commander_state_s::MAIN_STATE_STAB:
case commander_state_s::MAIN_STATE_ALTCTL:
/* require RC for all manual modes */
if (rc_lost && is_armed) {
// Require RC for all manual modes
if (status.rc_signal_lost && is_armed) {
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc);
set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act, param_com_rcl_act_t);
} else {
@ -496,7 +493,7 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
const bool rc_fallback_allowed = (posctl_nav_loss_act != position_nav_loss_actions_t::LAND_TERMINATE) || !is_armed;
if (rc_lost && is_armed) {
if (status.rc_signal_lost && is_armed) {
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc);
set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act, param_com_rcl_act_t);
@ -539,27 +536,27 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink);
set_link_loss_nav_state(status, armed, status_flags, internal_state, data_link_loss_act, 0);
} else if (status.rc_signal_lost && rc_loss_act_configured && status_flags.rc_signal_found_once
&& is_armed && !landed) {
// RC link lost, rc loss reaction configured, RC was used before -> RC loss reaction after delay
} else if (status.rc_signal_lost && !(param_com_rcl_except & RCLossExceptionBits::RCL_EXCEPT_MISSION)
&& status_flags.rc_signal_found_once && is_armed && !landed) {
// RC link lost, rc loss not disabled in mission, RC was used before -> RC loss reaction after delay
// Safety pilot expects to be able to take over by RC in case anything unexpected happens
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc);
set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act, param_com_rcl_act_t);
} else if (status.rc_signal_lost && rc_loss_act_configured
} else if (status.rc_signal_lost && !(param_com_rcl_except & RCLossExceptionBits::RCL_EXCEPT_MISSION)
&& status.data_link_lost && !data_link_loss_act_configured
&& is_armed && !landed) {
// All links lost, no data link loss reaction configured -> immediately do RC loss reaction
// Lost all communication, by default it's considered unsafe to continue the mission
// Note this case is reached after the previous one when flying mission completely without RC
// This is only reached when flying mission completely without RC (it was not present since boot)
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink);
set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act, 0);
} else if (status.rc_signal_lost && !rc_loss_act_configured
} else if (status.rc_signal_lost && (param_com_rcl_except & RCLossExceptionBits::RCL_EXCEPT_MISSION)
&& status.data_link_lost && !data_link_loss_act_configured
&& is_armed && !landed
&& mission_finished) {
// All links lost, all link loss reactions disabled -> return after mission
// All links lost, all link loss reactions disabled -> return after mission finished
// Pilot disabled all reactions, finish mission but then return to avoid lost vehicle
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink);
set_link_loss_nav_state(status, armed, status_flags, internal_state, link_loss_actions_t::AUTO_RTL, 0);
@ -580,25 +577,35 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
} else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) {
// nothing to do - everything done in check_invalid_pos_nav_state
} else if (status.data_link_lost && data_link_loss_act_configured && !landed && is_armed) {
/* also go into failsafe if just datalink is lost, and we're actually in air */
// Data link lost, data link loss reaction configured -> do configured reaction
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink);
set_link_loss_nav_state(status, armed, status_flags, internal_state, data_link_loss_act, 0);
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink);
} else if (rc_lost && !data_link_loss_act_configured && status.data_link_lost && is_armed) {
/* go into failsafe if RC is lost and datalink is lost and datalink loss is not set up */
} else if (status.rc_signal_lost && !(param_com_rcl_except & RCLossExceptionBits::RCL_EXCEPT_HOLD)
&& status_flags.rc_signal_found_once && is_armed && !landed) {
// RC link lost, rc loss not disabled in loiter, RC was used before -> RC loss reaction after delay
// Safety pilot expects to be able to take over by RC in case anything unexpected happens
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc);
set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act, param_com_rcl_act_t);
} else if (status.rc_signal_lost && !(param_com_rcl_except & RCLossExceptionBits::RCL_EXCEPT_HOLD)
&& status.data_link_lost && !data_link_loss_act_configured
&& is_armed && !landed) {
// All links lost, no data link loss reaction configured -> immediately do RC loss reaction
// Lost all communication, by default it's considered unsafe to continue the mission
// This is only reached when flying mission completely without RC (it was not present since boot)
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink);
set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act, 0);
} else if (status.rc_signal_lost) {
/* don't bother if RC is lost if datalink is connected */
/* this mode is ok, we don't need RC for LOITERing */
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
} else if (status.rc_signal_lost && (param_com_rcl_except & RCLossExceptionBits::RCL_EXCEPT_HOLD)
&& status.data_link_lost && !data_link_loss_act_configured
&& is_armed && !landed) {
// All links lost, all link loss reactions disabled -> return
// Pilot disabled all reactions, return to avoid lost vehicle
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink);
set_link_loss_nav_state(status, armed, status_flags, internal_state, link_loss_actions_t::AUTO_RTL, 0);
} else {
/* everything is perfect */
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
}
@ -661,7 +668,7 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
// is not possible and therefore the internal_state needs to be adjusted.
internal_state.main_state = commander_state_s::MAIN_STATE_POSCTL;
} else if (rc_lost && status.data_link_lost && !data_link_loss_act_configured && is_armed) {
} else if (status.rc_signal_lost && status.data_link_lost && !data_link_loss_act_configured && is_armed) {
// Orbit does not depend on RC but while armed & all links lost & when datalink loss is not set up, we failsafe
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc);
@ -729,7 +736,7 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
case commander_state_s::MAIN_STATE_OFFBOARD:
if (status_flags.offboard_control_signal_lost) {
if (status.rc_signal_lost) {
if (status.rc_signal_lost && !(param_com_rcl_except & RCLossExceptionBits::RCL_EXCEPT_OFFBOARD)) {
// Offboard and RC are lost
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc_and_no_offboard);
set_offboard_loss_nav_state(status, armed, status_flags, offb_loss_act);

8
src/modules/commander/state_machine_helper.h
View File

@ -115,6 +115,12 @@ enum class arm_disarm_reason_t {
UNIT_TEST = 13
};
enum RCLossExceptionBits {
RCL_EXCEPT_MISSION = (1 << 0),
RCL_EXCEPT_HOLD = (1 << 1),
RCL_EXCEPT_OFFBOARD = (1 << 2)
};
transition_result_t
arming_state_transition(vehicle_status_s &status, const safety_s &safety, const arming_state_t new_arming_state,
actuator_armed_s &armed, const bool fRunPreArmChecks, orb_advert_t *mavlink_log_pub,
@ -133,7 +139,7 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
const link_loss_actions_t rc_loss_act, const offboard_loss_actions_t offb_loss_act,
const offboard_loss_rc_actions_t offb_loss_rc_act,
const position_nav_loss_actions_t posctl_nav_loss_act,
const float param_com_rcl_act_t);
const float param_com_rcl_act_t, const int param_com_rcl_except);
/*
* Checks the validty of position data against the requirements of the current navigation