From 526e066d9ad6bee45c5716f7a1dde29bff3791a0 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Tue, 21 Feb 2023 17:35:46 +0100 Subject: [PATCH] Commander: rework GPS invalid warning to use estimator feedback instead of separate GPS quality thresholds Signed-off-by: Silvan Fuhrer --- msg/FailsafeFlags.msg | 1 - .../HealthAndArmingChecks.cpp | 3 +- .../checks/estimatorCheck.cpp | 69 +++++++------------ .../checks/estimatorCheck.hpp | 6 +- src/modules/mavlink/streams/HIGH_LATENCY2.hpp | 8 +-- 5 files changed, 33 insertions(+), 54 deletions(-) diff --git a/msg/FailsafeFlags.msg b/msg/FailsafeFlags.msg index cf83ec08cf..e67041a57a 100644 --- a/msg/FailsafeFlags.msg +++ b/msg/FailsafeFlags.msg @@ -28,7 +28,6 @@ bool local_position_invalid # Local position estimate invalid bool local_position_invalid_relaxed # Local position with reduced accuracy requirements invalid (e.g. flying with optical flow) bool local_velocity_invalid # Local velocity estimate invalid bool global_position_invalid # Global position estimate invalid -bool gps_position_invalid # GPS position invalid bool auto_mission_missing # No mission available bool offboard_control_signal_lost # Offboard signal lost bool home_position_invalid # No home position available diff --git a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp index 2428d94943..a638148513 100644 --- a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp +++ b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2023 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -45,7 +45,6 @@ HealthAndArmingChecks::HealthAndArmingChecks(ModuleParams *parent, vehicle_statu _failsafe_flags.local_position_invalid_relaxed = true; _failsafe_flags.local_velocity_invalid = true; _failsafe_flags.global_position_invalid = true; - _failsafe_flags.gps_position_invalid = true; _failsafe_flags.auto_mission_missing = true; _failsafe_flags.offboard_control_signal_lost = true; _failsafe_flags.home_position_invalid = true; diff --git a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp index e11cef237b..8b6fc5156c 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-2023 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -37,9 +37,6 @@ using namespace time_literals; EstimatorChecks::EstimatorChecks() { - _vehicle_gps_position_valid.set_hysteresis_time_from(false, GPS_VALID_TIME); - _vehicle_gps_position_valid.set_hysteresis_time_from(true, 0); - // initially set to failed _last_lpos_fail_time_us = hrt_absolute_time(); _last_lpos_relaxed_fail_time_us = _last_lpos_fail_time_us; @@ -119,13 +116,9 @@ void EstimatorChecks::checkAndReport(const Context &context, Report &reporter) } // set mode requirements - const bool condition_gps_position_was_valid = !reporter.failsafeFlags().gps_position_invalid; setModeRequirementFlags(context, pre_flt_fail_innov_heading, pre_flt_fail_innov_vel_horiz, lpos, vehicle_gps_position, reporter.failsafeFlags()); - if (condition_gps_position_was_valid && reporter.failsafeFlags().gps_position_invalid) { - gpsNoLongerValid(context, reporter); - } lowPositionAccuracy(context, reporter, lpos); } @@ -280,7 +273,7 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor } // If GPS aiding is required, declare fault condition if the required GPS quality checks are failing - if (!context.isArmed() && _param_sys_has_gps.get()) { + if (_param_sys_has_gps.get()) { const bool ekf_gps_fusion = estimator_status.control_mode_flags & (1 << estimator_status_s::CS_GPS); const bool ekf_gps_check_fail = estimator_status.gps_check_fail_flags > 0; @@ -288,7 +281,30 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor reporter.setIsPresent(health_component_t::gps); // should be based on the sensor data directly } - if (ekf_gps_check_fail) { + if (context.isArmed()) { + + if (_gps_was_fused && !ekf_gps_fusion) { + if (reporter.mavlink_log_pub()) { + mavlink_log_warning(reporter.mavlink_log_pub(), "GNSS data fusion stopped\t"); + } + + events::send(events::ID("check_estimator_gnss_fusion_stopped"), {events::Log::Error, events::LogInternal::Info}, + "GNSS data fusion stopped"); + + } else if (!_gps_was_fused && ekf_gps_fusion) { + + if (reporter.mavlink_log_pub()) { + mavlink_log_info(reporter.mavlink_log_pub(), "GNSS data fusion started\t"); + } + + events::send(events::ID("check_estimator_gnss_fusion_stated"), {events::Log::Info, events::LogInternal::Info}, + "GNSS data fusion stated"); + } + } + + _gps_was_fused = ekf_gps_fusion; + + if (!context.isArmed() && ekf_gps_check_fail) { NavModes required_groups_gps = required_groups; events::Log log_level = events::Log::Error; @@ -656,21 +672,6 @@ void EstimatorChecks::checkGps(const Context &context, Report &reporter, const s } } -void EstimatorChecks::gpsNoLongerValid(const Context &context, Report &reporter) const -{ - PX4_DEBUG("GPS no longer valid"); - - // report GPS failure if armed and the global position estimate is still valid - if (context.isArmed() && !reporter.failsafeFlags().global_position_invalid) { - if (reporter.mavlink_log_pub()) { - mavlink_log_warning(reporter.mavlink_log_pub(), "GPS no longer valid\t"); - } - - events::send(events::ID("check_estimator_gps_lost"), {events::Log::Error, events::LogInternal::Info}, - "GPS no longer valid"); - } -} - void EstimatorChecks::lowPositionAccuracy(const Context &context, Report &reporter, const vehicle_local_position_s &lpos) const { @@ -798,24 +799,6 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f } failsafe_flags.angular_velocity_invalid = angular_velocity_invalid; - - - // gps - if (vehicle_gps_position.timestamp != 0) { - bool time = (now < vehicle_gps_position.timestamp + 1_s); - - bool fix = vehicle_gps_position.fix_type >= 2; - bool eph = vehicle_gps_position.eph < _param_com_pos_fs_eph.get(); - bool epv = vehicle_gps_position.epv < _param_com_pos_fs_epv.get(); - bool evh = vehicle_gps_position.s_variance_m_s < _param_com_vel_fs_evh.get(); - bool no_jamming = (vehicle_gps_position.jamming_state != sensor_gps_s::JAMMING_STATE_CRITICAL); - - _vehicle_gps_position_valid.set_state_and_update(time && fix && eph && epv && evh && no_jamming, now); - failsafe_flags.gps_position_invalid = !_vehicle_gps_position_valid.get_state(); - - } else { - failsafe_flags.gps_position_invalid = true; - } } bool EstimatorChecks::checkPosVelValidity(const hrt_abstime &now, const bool data_valid, const float data_accuracy, diff --git a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp index 0bd2502fcd..44520da00e 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp @@ -66,7 +66,6 @@ private: const vehicle_local_position_s &lpos); 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, @@ -100,11 +99,10 @@ private: bool _nav_test_passed{false}; ///< true if the post takeoff navigation test has passed bool _nav_test_failed{false}; ///< true if the post takeoff navigation test has failed - static constexpr hrt_abstime GPS_VALID_TIME{3_s}; - systemlib::Hysteresis _vehicle_gps_position_valid{false}; - bool _position_reliant_on_optical_flow{false}; + bool _gps_was_fused{false}; + DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase, (ParamInt) _param_sys_mc_est_group, (ParamInt) _param_sens_imu_mode, diff --git a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp index 50d97fd9d4..ca6bf41231 100644 --- a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp +++ b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp @@ -310,6 +310,10 @@ private: msg->failure_flags |= HL_FAILURE_FLAG_ESTIMATOR; } + if (estimator_status.gps_check_fail_flags > 0) { + msg->failure_flags |= HL_FAILURE_FLAG_GPS; + } + return true; } @@ -448,10 +452,6 @@ private: failsafe_flags_s failsafe_flags; if (_failsafe_flags_sub.update(&failsafe_flags)) { - if (failsafe_flags.gps_position_invalid) { - msg->failure_flags |= HL_FAILURE_FLAG_GPS; - } - if (failsafe_flags.offboard_control_signal_lost) { msg->failure_flags |= HL_FAILURE_FLAG_OFFBOARD_LINK; }