From 9b3a28dff54c52d5a82420e78aab418da3f7f173 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Fri, 27 Jan 2023 16:04:55 +0100 Subject: [PATCH] Commander: add local_position_accuracy_low flag, incl. warning and RTL Set this flag to true if local position is valid but accuracy low, such that the operator can be warned before system switches to position-failure failsafe. Additionally, switch to RTL if currently in Mission or Loiter to try to reach home or fly out of GNSS-denied area. Set low accuracy threshold to 50m by default for FW and VTOL. Signed-off-by: Silvan Fuhrer --- ROMFS/px4fmu_common/init.d/rc.fw_defaults | 3 ++ ROMFS/px4fmu_common/init.d/rc.vtol_defaults | 2 ++ msg/FailsafeFlags.msg | 1 + .../checks/estimatorCheck.cpp | 28 +++++++++++++++++++ .../checks/estimatorCheck.hpp | 4 ++- src/modules/commander/commander_params.c | 17 +++++++++++ src/modules/commander/failsafe/failsafe.cpp | 6 ++++ 7 files changed, 60 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_defaults b/ROMFS/px4fmu_common/init.d/rc.fw_defaults index e896756107..eafe1cd017 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.fw_defaults @@ -19,6 +19,9 @@ param set-default COM_POS_FS_DELAY 5 param set-default COM_POS_FS_EPH 50 param set-default COM_POS_FS_EPV 30 param set-default COM_VEL_FS_EVH 5 + +param set-default COM_POS_LOW_EPH 50 + # Disable preflight disarm to not interfere with external launching param set-default COM_DISARM_PRFLT -1 diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_defaults b/ROMFS/px4fmu_common/init.d/rc.vtol_defaults index 2f4cb66bb9..6a0bc29952 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vtol_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.vtol_defaults @@ -13,6 +13,8 @@ param set-default MAV_TYPE 22 # there is a 2.5 factor applied on COM_POS_FS_EPH if for invalidation param set-default COM_POS_FS_EPH 50 +param set-default COM_POS_LOW_EPH 50 + param set-default MIS_TAKEOFF_ALT 20 param set-default MIS_YAW_TMT 10 diff --git a/msg/FailsafeFlags.msg b/msg/FailsafeFlags.msg index 8271298f11..cf83ec08cf 100644 --- a/msg/FailsafeFlags.msg +++ b/msg/FailsafeFlags.msg @@ -48,6 +48,7 @@ bool mission_failure # Mission failure bool vtol_fixed_wing_system_failure # vehicle in fixed-wing system failure failsafe mode (after quad-chute) bool wind_limit_exceeded # Wind limit exceeded bool flight_time_limit_exceeded # Maximum flight time exceeded +bool local_position_accuracy_low # Local position estimate has dropped below threshold, but is currently still declared valid # Failure detector bool fd_critical_failure # Critical failure (attitude/altitude limit exceeded, or external ATS) diff --git a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp index 9828dcb920..deb424653a 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp @@ -126,6 +126,8 @@ void EstimatorChecks::checkAndReport(const Context &context, Report &reporter) if (condition_gps_position_was_valid && reporter.failsafeFlags().gps_position_invalid) { gpsNoLongerValid(context, reporter); } + + lowPositionAccuracy(context, reporter, lpos); } void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &reporter, @@ -669,6 +671,32 @@ void EstimatorChecks::gpsNoLongerValid(const Context &context, Report &reporter) } } +void EstimatorChecks::lowPositionAccuracy(const Context &context, Report &reporter, + const vehicle_local_position_s &lpos) const +{ + 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) { + + // only report if armed + if (context.isArmed()) { + /* EVENT + * @description Local position estimate valid but has low accuracy. Warn user. + * This check can be configured via COM_POS_LOW_EPH parameter. + */ + events::send(events::ID("check_estimator_low_position_accuracy"), {events::Log::Error, events::LogInternal::Info}, + "Local position estimate has low accuracy"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_warning(reporter.mavlink_log_pub(), "Local position estimate has low accuracy\t"); + } + } + } + + reporter.failsafeFlags().local_position_accuracy_low = local_position_valid_but_low_accuracy; +} + void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_flt_fail_innov_heading, bool pre_flt_fail_innov_vel_horiz, const vehicle_local_position_s &lpos, const sensor_gps_s &vehicle_gps_position, failsafe_flags_s &failsafe_flags) diff --git a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp index f591cf03da..0bd2502fcd 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp @@ -67,6 +67,7 @@ private: void checkGps(const Context &context, Report &reporter, const sensor_gps_s &vehicle_gps_position) const; void gpsNoLongerValid(const Context &context, Report &reporter) const; + void lowPositionAccuracy(const Context &context, Report &reporter, const vehicle_local_position_s &lpos) const; void setModeRequirementFlags(const Context &context, bool pre_flt_fail_innov_heading, bool pre_flt_fail_innov_vel_horiz, const vehicle_local_position_s &lpos, const sensor_gps_s &vehicle_gps_position, failsafe_flags_s &failsafe_flags); @@ -117,6 +118,7 @@ private: (ParamFloat) _param_com_pos_fs_eph, (ParamFloat) _param_com_pos_fs_epv, (ParamFloat) _param_com_vel_fs_evh, - (ParamInt) _param_com_pos_fs_delay + (ParamInt) _param_com_pos_fs_delay, + (ParamFloat) _param_com_low_eph ) }; diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 43b8606c34..5d17953644 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -1088,3 +1088,20 @@ PARAM_DEFINE_INT32(COM_FLT_TIME_MAX, -1); * @unit m/s */ PARAM_DEFINE_FLOAT(COM_WIND_MAX, -1.f); + +/** + * EPH threshold for RTL + * + * 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. + * + * Set to -1 to disable. + * + * @group Commander + * @min -1 + * @unit m + */ +PARAM_DEFINE_FLOAT(COM_POS_LOW_EPH, -1.0f); diff --git a/src/modules/commander/failsafe/failsafe.cpp b/src/modules/commander/failsafe/failsafe.cpp index 69747868f1..4d26276ef2 100644 --- a/src/modules/commander/failsafe/failsafe.cpp +++ b/src/modules/commander/failsafe/failsafe.cpp @@ -395,6 +395,12 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state, ActionOptions(Action::RTL).clearOn(ClearCondition::OnModeChangeOrDisarm)); CHECK_FAILSAFE(status_flags, flight_time_limit_exceeded, ActionOptions(Action::RTL)); + // trigger RTL if low position accurancy is detected + 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, primary_geofence_breached, fromGfActParam(_param_gf_action.get())); // Battery