fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 04:23:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Commander: put action for low position estimation accuracy into parameter

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2024-11-15 15:32:53 -05:00
committed by Daniel Agar
parent 2ce390a780
commit 3093c593a5
5 changed files with 86 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/failsafe/failsafe.cpp
+45 -2
View File
@@ -368,6 +368,49 @@ FailsafeBase::ActionOptions Failsafe::fromHighWindLimitActParam(int param_value)
return options;
}
FailsafeBase::ActionOptions Failsafe::fromPosLowActParam(int param_value)
{
ActionOptions options{};
options.allow_user_takeover = UserTakeoverAllowed::AlwaysModeSwitchOnly; // ensure the user can escape again
switch (command_after_pos_low_failsafe(param_value)) {
case command_after_pos_low_failsafe::None:
options.action = Action::None;
break;
case command_after_pos_low_failsafe::Warning:
options.action = Action::Warn;
break;
case command_after_pos_low_failsafe::Hold_mode:
options.action = Action::Hold;
options.clear_condition = ClearCondition::WhenConditionClears;
break;
case command_after_pos_low_failsafe::Return_mode:
options.action = Action::RTL;
options.clear_condition = ClearCondition::WhenConditionClears;
break;
case command_after_pos_low_failsafe::Terminate:
options.allow_user_takeover = UserTakeoverAllowed::Never;
options.action = Action::Terminate;
options.clear_condition = ClearCondition::Never;
break;
case command_after_pos_low_failsafe::Land_mode:
options.action = Action::Land;
options.clear_condition = ClearCondition::WhenConditionClears;
break;
default:
options.action = Action::Warn;
break;
}
return options;
}
FailsafeBase::ActionOptions Failsafe::fromRemainingFlightTimeLowActParam(int param_value)
{
ActionOptions options{};
@@ -469,10 +512,10 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
ActionOptions(fromHighWindLimitActParam(_param_com_wind_max_act.get()).cannotBeDeferred()));
CHECK_FAILSAFE(status_flags, flight_time_limit_exceeded, ActionOptions(Action::RTL).cannotBeDeferred());
// trigger RTL if low position accurancy is detected
// trigger Low Position Accuracy Failsafe (only in auto mission and auto loiter)
if (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION ||
state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER) {
CHECK_FAILSAFE(status_flags, local_position_accuracy_low, ActionOptions(Action::RTL));
CHECK_FAILSAFE(status_flags, local_position_accuracy_low, fromPosLowActParam(_param_com_pos_low_act.get()));
}
if (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF ||
src/modules/commander/failsafe/failsafe.h
+12 -1
View File
@@ -141,6 +141,15 @@ private:
Land_mode = 5
};
enum class command_after_pos_low_failsafe : int32_t {
None = 0,
Warning = 1,
Hold_mode = 2,
Return_mode = 3,
Terminate = 4,
Land_mode = 5
};
enum class command_after_remaining_flight_time_low : int32_t {
None = 0,
Warning = 1,
@@ -156,6 +165,7 @@ private:
static ActionOptions fromQuadchuteActParam(int param_value);
static Action fromOffboardLossActParam(int param_value, uint8_t &user_intended_mode);
static ActionOptions fromHighWindLimitActParam(int param_value);
static ActionOptions fromPosLowActParam(int param_value);
static ActionOptions fromRemainingFlightTimeLowActParam(int param_value);
const int _caller_id_mode_fallback{genCallerId()};
@@ -191,7 +201,8 @@ private:
(ParamInt<px4::params::COM_OBL_RC_ACT>) _param_com_obl_rc_act,
(ParamInt<px4::params::COM_QC_ACT>) _param_com_qc_act,
(ParamInt<px4::params::COM_WIND_MAX_ACT>) _param_com_wind_max_act,
(ParamInt<px4::params::COM_FLTT_LOW_ACT>) _param_com_fltt_low_act
(ParamInt<px4::params::COM_FLTT_LOW_ACT>) _param_com_fltt_low_act,
(ParamInt<px4::params::COM_POS_LOW_ACT>) _param_com_pos_low_act
);
};