Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-21 21:55:34 +08:00
Code Issues Actions 5 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-13 10:40:15 +01: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/HealthAndArmingChecks/checks/estimatorCheck.cpp
+3 -2
View File
@@ -692,7 +692,8 @@ void EstimatorChecks::lowPositionAccuracy(const Context &context, Report &report
const bool local_position_valid_but_low_accuracy = !reporter.failsafeFlags().local_position_invalid
&& (_param_com_low_eph.get() > FLT_EPSILON && lpos.eph > _param_com_low_eph.get());
if (!reporter.failsafeFlags().local_position_accuracy_low && local_position_valid_but_low_accuracy) {
if (!reporter.failsafeFlags().local_position_accuracy_low && local_position_valid_but_low_accuracy
&& _param_com_pos_low_act.get()) {
// only report if armed
if (context.isArmed()) {
@@ -700,7 +701,7 @@ void EstimatorChecks::lowPositionAccuracy(const Context &context, Report &report
* @description Local position estimate valid but has low accuracy. Warn user.
*
* <profile name="dev">
* This check can be configured via <param>COM_POS_LOW_EPH</param> parameter.
* This check can be configured via <param>COM_POS_LOW_EPH</param> and <param>COM_POS_LOW_ACT</param> parameters.
* </profile>
*/
events::send(events::ID("check_estimator_low_position_accuracy"), {events::Log::Error, events::LogInternal::Info},
src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp
+2 -1
View File
@@ -119,6 +119,7 @@ private:
(ParamFloat<px4::params::COM_POS_FS_EPH>) _param_com_pos_fs_eph,
(ParamFloat<px4::params::COM_VEL_FS_EVH>) _param_com_vel_fs_evh,
(ParamInt<px4::params::COM_POS_FS_DELAY>) _param_com_pos_fs_delay,
(ParamFloat<px4::params::COM_POS_LOW_EPH>) _param_com_low_eph
(ParamFloat<px4::params::COM_POS_LOW_EPH>) _param_com_low_eph,
(ParamInt<px4::params::COM_POS_LOW_ACT>) _param_com_pos_low_act
)
};
src/modules/commander/commander_params.c
+24 -6
View File
@@ -928,13 +928,11 @@ PARAM_DEFINE_FLOAT(COM_WIND_MAX, -1.f);
PARAM_DEFINE_INT32(COM_WIND_MAX_ACT, 0);
/**
* EPH threshold for RTL
* Low position accuracy failsafe threshold
*
* Specify the threshold for triggering a warning for low local position accuracy. Additionally triggers
* a RTL if currently in Mission or Loiter mode.
* Local position has to be still declared valid, which is most of all depending on COM_POS_FS_EPH.
* Use this feature on systems with dead-reckoning capabilites (e.g. fixed-wing vehicles with airspeed sensor)
* to improve the user notification and failure mitigation when flying in GNSS-denied areas.
* This triggers the action specified in COM_POS_LOW_ACT if the estimated position accuracy is below this threshold.
* Local position has to be still declared valid, which requires some kind of velocity aiding or large dead-reckoning time (EKF2_NOAID_TOUT),
* and a high failsafe threshold (COM_POS_FS_EPH).
*
* Set to -1 to disable.
*
@@ -945,6 +943,26 @@ PARAM_DEFINE_INT32(COM_WIND_MAX_ACT, 0);
*/
PARAM_DEFINE_FLOAT(COM_POS_LOW_EPH, -1.0f);
/**
* Low position accuracy action
*
* Action the system takes when the estimated position has an accuracy below the specified threshold.
* See COM_POS_LOW_EPH to set the failsafe threshold.
* The failsafe action is only executed if the vehicle is in auto mission or auto loiter mode,
* otherwise it is only a warning.
*
* @group Commander
*
* @value 0 None
* @value 1 Warning
* @value 2 Hold
* @value 3 Return
* @value 4 Terminate
* @value 5 Land
* @increment 1
*/
PARAM_DEFINE_INT32(COM_POS_LOW_ACT, 3);
/**
* Flag to allow arming
*
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
);
};