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

Commander: rework GPS invalid warning to use estimator feedback instead of separate GPS quality thresholds

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2023-02-24 13:32:38 +01:00
committed by Beat Küng
parent e6af8b9aa6
commit 526e066d9a
5 changed files with 33 additions and 54 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/FailsafeFlags.msg
-1
View File
@@ -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
src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp
+1 -2
View File
@@ -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;
src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp
+26 -43
View File
@@ -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,
src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp
+2 -4
View File
@@ -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<px4::params::SYS_MC_EST_GROUP>) _param_sys_mc_est_group,
(ParamInt<px4::params::SENS_IMU_MODE>) _param_sens_imu_mode,
src/modules/mavlink/streams/HIGH_LATENCY2.hpp
+4 -4
View File
@@ -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;
}