mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-20 19:33:34 +08:00
Commander: unify RC loss and data link exception options
This commit is contained in:
Matthias Grob
committed by
Silvan Fuhrer
Silvan Fuhrer
parent
ec6dd286fc
commit
b5846fd8c2
@@ -607,7 +607,7 @@ PARAM_DEFINE_INT32(NAV_RCL_ACT, 2);
|
||||
/**
|
||||
* Manual control loss exceptions
|
||||
*
|
||||
* Specify modes where manual control loss is ignored and no failsafe is triggered.
|
||||
* Specify modes in which stick input is ignored and no failsafe action is triggered.
|
||||
* External modes requiring stick input will still failsafe.
|
||||
* Auto modes are: Hold, Takeoff, Land, RTL, Descend, Follow Target, Precland, Orbit.
|
||||
*
|
||||
@@ -625,13 +625,16 @@ PARAM_DEFINE_INT32(COM_RCL_EXCEPT, 0);
|
||||
/**
|
||||
* Datalink loss exceptions
|
||||
*
|
||||
* Specify modes in which datalink loss is ignored and the failsafe action not triggered.
|
||||
* Specify modes in which ground control station connection loss is ignored and no failsafe action is triggered.
|
||||
* See also COM_RCL_EXCEPT.
|
||||
*
|
||||
* @min 0
|
||||
* @max 7
|
||||
* @max 31
|
||||
* @bit 0 Mission
|
||||
* @bit 1 Hold
|
||||
* @bit 1 Auto modes
|
||||
* @bit 2 Offboard
|
||||
* @bit 3 External Mode
|
||||
* @bit 4 Altitude Cruise
|
||||
* @group Commander
|
||||
*/
|
||||
PARAM_DEFINE_INT32(COM_DLL_EXCEPT, 0);
|
||||
|
||||
@@ -442,18 +442,53 @@ FailsafeBase::ActionOptions Failsafe::fromRemainingFlightTimeLowActParam(int par
|
||||
return options;
|
||||
}
|
||||
|
||||
bool Failsafe::isFailsafeIgnored(uint8_t user_intended_mode, int32_t exception_mask_parameter)
|
||||
{
|
||||
switch (user_intended_mode) {
|
||||
case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
|
||||
return exception_mask_parameter & (int)LinkLossExceptionBits::Mission;
|
||||
|
||||
case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
|
||||
case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF:
|
||||
case vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF:
|
||||
case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND:
|
||||
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
|
||||
case vehicle_status_s::NAVIGATION_STATE_DESCEND:
|
||||
case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET:
|
||||
case vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND:
|
||||
case vehicle_status_s::NAVIGATION_STATE_ORBIT:
|
||||
return exception_mask_parameter & (int)LinkLossExceptionBits::AutoModes;
|
||||
|
||||
case vehicle_status_s::NAVIGATION_STATE_OFFBOARD:
|
||||
return exception_mask_parameter & (int)LinkLossExceptionBits::Offboard;
|
||||
|
||||
case vehicle_status_s::NAVIGATION_STATE_EXTERNAL1:
|
||||
case vehicle_status_s::NAVIGATION_STATE_EXTERNAL2:
|
||||
case vehicle_status_s::NAVIGATION_STATE_EXTERNAL3:
|
||||
case vehicle_status_s::NAVIGATION_STATE_EXTERNAL4:
|
||||
case vehicle_status_s::NAVIGATION_STATE_EXTERNAL5:
|
||||
case vehicle_status_s::NAVIGATION_STATE_EXTERNAL6:
|
||||
case vehicle_status_s::NAVIGATION_STATE_EXTERNAL7:
|
||||
case vehicle_status_s::NAVIGATION_STATE_EXTERNAL8:
|
||||
return exception_mask_parameter & (int)LinkLossExceptionBits::ExternalMode;
|
||||
|
||||
case vehicle_status_s::NAVIGATION_STATE_ALTITUDE_CRUISE:
|
||||
return exception_mask_parameter & (int)LinkLossExceptionBits::AltitudeCruise;
|
||||
|
||||
default:
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
|
||||
const failsafe_flags_s &status_flags)
|
||||
{
|
||||
updateArmingState(time_us, state.armed, status_flags);
|
||||
|
||||
const bool in_forward_flight = state.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING
|
||||
|| state.vtol_in_transition_mode;
|
||||
|
||||
// Do not enter failsafe while doing a vtol takeoff after the vehicle has started a transition and before it reaches the loiter
|
||||
// altitude. The vtol takeoff navigaton mode will set mission_finished to true as soon as the loiter is established
|
||||
const bool ignore_any_link_loss_vtol_takeoff_fixedwing = state.user_intended_mode ==
|
||||
vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF
|
||||
const bool in_forward_flight = (state.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) || state.vtol_in_transition_mode;
|
||||
const bool ignore_any_link_loss_vtol_takeoff_fixedwing = (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF)
|
||||
&& in_forward_flight && !state.mission_finished;
|
||||
|
||||
// Manual control (RC or joystick) loss
|
||||
@@ -462,67 +497,23 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
|
||||
_manual_control_lost_at_arming = false;
|
||||
}
|
||||
|
||||
const bool rc_loss_ignored_mission = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION
|
||||
&& (_param_com_rcl_except.get() & (int)ManualControlLossExceptionBits::Mission);
|
||||
|
||||
const bool rc_loss_ignored_auto_modes = (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 ||
|
||||
state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND ||
|
||||
state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL ||
|
||||
state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_DESCEND ||
|
||||
state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET ||
|
||||
state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND ||
|
||||
state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_ORBIT)
|
||||
&& (_param_com_rcl_except.get() & (int)ManualControlLossExceptionBits::AutoModes);
|
||||
|
||||
const bool rc_loss_ignored_offboard = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_OFFBOARD
|
||||
&& (_param_com_rcl_except.get() & (int)ManualControlLossExceptionBits::Offboard);
|
||||
|
||||
const bool rc_loss_ignored_altitude_cruise = (state.user_intended_mode ==
|
||||
vehicle_status_s::NAVIGATION_STATE_ALTITUDE_CRUISE
|
||||
&& (_param_com_rcl_except.get() & (int)ManualControlLossExceptionBits::AltitudeCruise));
|
||||
|
||||
const bool rc_loss_ignored_external_mode =
|
||||
(state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_EXTERNAL1 ||
|
||||
state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_EXTERNAL2 ||
|
||||
state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_EXTERNAL3 ||
|
||||
state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_EXTERNAL4 ||
|
||||
state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_EXTERNAL5 ||
|
||||
state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_EXTERNAL6 ||
|
||||
state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_EXTERNAL7 ||
|
||||
state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_EXTERNAL8)
|
||||
&& (_param_com_rcl_except.get() & (int)ManualControlLossExceptionBits::ExternalMode);
|
||||
|
||||
const bool rc_loss_ignored = rc_loss_ignored_mission || rc_loss_ignored_auto_modes || rc_loss_ignored_offboard ||
|
||||
rc_loss_ignored_external_mode || ignore_any_link_loss_vtol_takeoff_fixedwing
|
||||
|| _manual_control_lost_at_arming || rc_loss_ignored_altitude_cruise;
|
||||
const bool rc_loss_ignored = isFailsafeIgnored(state.user_intended_mode, _param_com_rcl_except.get())
|
||||
|| ignore_any_link_loss_vtol_takeoff_fixedwing || _manual_control_lost_at_arming;
|
||||
|
||||
if (_param_com_rc_in_mode.get() != int32_t(RcInMode::DisableManualControl) && !rc_loss_ignored) {
|
||||
CHECK_FAILSAFE(status_flags, manual_control_signal_lost,
|
||||
fromNavDllOrRclActParam(_param_nav_rcl_act.get()).causedBy(Cause::ManualControlLoss));
|
||||
}
|
||||
|
||||
// GCS connection loss
|
||||
// Ground control station connection loss
|
||||
const bool dll_loss_ignored_land = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND
|
||||
|| state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND;
|
||||
|
||||
const bool dll_loss_ignored_mission = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION
|
||||
&& (_param_com_dll_except.get() & (int)DatalinkLossExceptionBits::Mission);
|
||||
const bool dll_loss_ignored_loiter = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER
|
||||
&& (_param_com_dll_except.get() & (int)DatalinkLossExceptionBits::Hold);
|
||||
const bool dll_loss_ignored_offboard = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_OFFBOARD
|
||||
&& (_param_com_dll_except.get() & (int)DatalinkLossExceptionBits::Offboard);
|
||||
const bool dll_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_dll_except.get() & (int)DatalinkLossExceptionBits::Hold);
|
||||
|
||||
const bool dll_loss_ignored = dll_loss_ignored_mission || dll_loss_ignored_loiter || dll_loss_ignored_offboard ||
|
||||
dll_loss_ignored_takeoff || ignore_any_link_loss_vtol_takeoff_fixedwing || dll_loss_ignored_land;
|
||||
const bool dll_loss_ignored = isFailsafeIgnored(state.user_intended_mode, _param_com_dll_except.get())
|
||||
|| ignore_any_link_loss_vtol_takeoff_fixedwing || dll_loss_ignored_land;
|
||||
|
||||
if (_param_nav_dll_act.get() != int32_t(gcs_connection_loss_failsafe_mode::Disabled) && !dll_loss_ignored) {
|
||||
CHECK_FAILSAFE(status_flags, gcs_connection_lost,
|
||||
fromNavDllOrRclActParam(_param_nav_dll_act.get()).causedBy(Cause::GCSConnectionLoss));
|
||||
CHECK_FAILSAFE(status_flags, gcs_connection_lost, fromNavDllOrRclActParam(_param_nav_dll_act.get()).causedBy(Cause::GCSConnectionLoss));
|
||||
}
|
||||
|
||||
// VTOL transition failure (quadchute)
|
||||
@@ -539,7 +530,8 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
|
||||
|
||||
// If manual control loss and GCS connection loss are disabled and we lose both command links and the mission finished,
|
||||
// trigger RTL to avoid losing the vehicle
|
||||
if ((_param_com_rc_in_mode.get() == int32_t(RcInMode::DisableManualControl) || rc_loss_ignored_mission)
|
||||
if ((_param_com_rc_in_mode.get() == int32_t(RcInMode::DisableManualControl)
|
||||
|| isFailsafeIgnored(state.user_intended_mode, _param_com_rcl_except.get()))
|
||||
&& _param_nav_dll_act.get() == int32_t(gcs_connection_loss_failsafe_mode::Disabled)
|
||||
&& state.mission_finished) {
|
||||
_last_state_mission_control_lost = checkFailsafe(_caller_id_mission_control_lost, _last_state_mission_control_lost,
|
||||
|
||||
@@ -53,7 +53,7 @@ protected:
|
||||
private:
|
||||
void updateArmingState(const hrt_abstime &time_us, bool armed, const failsafe_flags_s &status_flags);
|
||||
|
||||
enum class ManualControlLossExceptionBits : int32_t {
|
||||
enum class LinkLossExceptionBits : int32_t {
|
||||
Mission = (1 << 0),
|
||||
AutoModes = (1 << 1),
|
||||
Offboard = (1 << 2),
|
||||
@@ -61,12 +61,6 @@ private:
|
||||
AltitudeCruise = (1 << 4)
|
||||
};
|
||||
|
||||
enum class DatalinkLossExceptionBits : int32_t {
|
||||
Mission = (1 << 0),
|
||||
Hold = (1 << 1),
|
||||
Offboard = (1 << 2)
|
||||
};
|
||||
|
||||
// COM_LOW_BAT_ACT parameter values
|
||||
enum class LowBatteryAction : int32_t {
|
||||
Warning = 0, // Warning
|
||||
@@ -175,6 +169,8 @@ private:
|
||||
static ActionOptions fromPosLowActParam(int param_value);
|
||||
static ActionOptions fromRemainingFlightTimeLowActParam(int param_value);
|
||||
|
||||
static bool isFailsafeIgnored(uint8_t user_intended_mode, int32_t exception_mask_parameter);
|
||||
|
||||
const int _caller_id_mode_fallback{genCallerId()};
|
||||
bool _last_state_mode_fallback{false};
|
||||
const int _caller_id_mission_control_lost{genCallerId()};
|
||||
|
||||
Reference in New Issue
Block a user