From c0d9e2ac7a53eec7722ab4ace4a76fe4de9b9396 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Thu, 27 Jul 2023 20:12:04 +0200 Subject: [PATCH] commander: add warning for imminent navigation failure Some system are able to dead-reckon for a while after losing GPS or other sources providing positional feedback. If the estimated position error grows above the failsafe threshold, the system enters a failsafe mode. As the position error estimate is growing linerly over time, and it is recommended to take action before entering the failsafe, we here warn the user about the imminent failsafe and propose to take manual control. --------- Signed-off-by: Silvan Fuhrer --- .../checks/estimatorCheck.cpp | 46 ++++++++++++++++++- .../checks/estimatorCheck.hpp | 4 +- 2 files changed, 47 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp index 44b5fa91c9..9bb33f2bd2 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp @@ -117,7 +117,7 @@ void EstimatorChecks::checkAndReport(const Context &context, Report &reporter) // set mode requirements setModeRequirementFlags(context, pre_flt_fail_innov_heading, pre_flt_fail_innov_vel_horiz, lpos, vehicle_gps_position, - reporter.failsafeFlags()); + reporter.failsafeFlags(), reporter); lowPositionAccuracy(context, reporter, lpos); @@ -731,7 +731,8 @@ void EstimatorChecks::lowPositionAccuracy(const Context &context, Report &report 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) + const vehicle_local_position_s &lpos, const sensor_gps_s &vehicle_gps_position, failsafe_flags_s &failsafe_flags, + Report &reporter) { // The following flags correspond to mode requirements, and are reported in the corresponding mode checks vehicle_global_position_s gpos; @@ -772,6 +773,47 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f !checkPosVelValidity(now, xy_valid, gpos.eph, lpos_eph_threshold, gpos.timestamp, _last_gpos_fail_time_us, !failsafe_flags.global_position_invalid); + // Additional warning if the system is about to enter position-loss failsafe after dead-reckoning period + const float eph_critical = 2.5f * _param_com_pos_fs_eph.get(); // threshold used to trigger the navigation failsafe + const float gpos_critical_warning_thrld = math::max(0.9f * eph_critical, math::max(eph_critical - 10.f, 0.f)); + + estimator_status_flags_s estimator_status_flags; + + if (_estimator_status_flags_sub.copy(&estimator_status_flags)) { + + // only do the following if the estimator status flags are recent (less than 5 seconds old) + if (now - estimator_status_flags.timestamp < 5_s) { + const bool dead_reckoning = estimator_status_flags.cs_inertial_dead_reckoning + || estimator_status_flags.cs_wind_dead_reckoning; + + if (!failsafe_flags.global_position_invalid + && !_nav_failure_imminent_warned + && gpos.eph > gpos_critical_warning_thrld + && dead_reckoning) { + /* EVENT + * @description + * Switch to manual mode recommended. + * + * + * This warning is triggered when the position error estimate is 90% of (or only 10m below) COM_POS_FS_EPH parameter. + * + */ + events::send(events::ID("check_estimator_position_failure_imminent"), {events::Log::Error, events::LogInternal::Info}, + "Estimated position error is approaching the failsafe threshold"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), + "Estimated position error is approaching the failsafe threshold\t"); + } + + _nav_failure_imminent_warned = true; + + } else if (!dead_reckoning) { + _nav_failure_imminent_warned = false; + } + } + } + failsafe_flags.local_position_invalid = !checkPosVelValidity(now, xy_valid, lpos.eph, lpos_eph_threshold, lpos.timestamp, _last_lpos_fail_time_us, !failsafe_flags.local_position_invalid); diff --git a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp index 4dd45cd70e..f5b8073524 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp @@ -69,7 +69,7 @@ private: 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); + failsafe_flags_s &failsafe_flags, Report &reporter); bool checkPosVelValidity(const hrt_abstime &now, const bool data_valid, const float data_accuracy, const float required_accuracy, @@ -103,6 +103,8 @@ private: bool _gps_was_fused{false}; + bool _nav_failure_imminent_warned{false}; + DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase, (ParamInt) _param_sys_mc_est_group, (ParamInt) _param_sens_imu_mode,