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

estimatorCheck: only warn about GPS in modes that require a position but fail all modes if GPS required by configuration

Browse Source
This commit is contained in:
Matthias Grob
2025-01-30 18:45:10 +01:00
parent f142363575
commit 4c2e69b2e6
1 changed files with 32 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp
+32 -28
View File
@@ -273,7 +273,7 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
}
if (!context.isArmed() && ekf_gps_check_fail) {
NavModes required_groups_gps;
NavModesMessageFail required_modes;
events::Log log_level;
switch (static_cast<GnssArmingCheck>(_param_com_arm_wo_gps.get())) {
@@ -281,17 +281,21 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
/* FALLTHROUGH */
case GnssArmingCheck::DenyArming:
required_groups_gps = required_groups;
required_modes.message_modes = required_modes.fail_modes = NavModes::All;
log_level = events::Log::Error;
break;
case GnssArmingCheck::WarningOnly:
required_groups_gps = NavModes::None; // optional
required_modes.message_modes = (NavModes)(reporter.failsafeFlags().mode_req_local_position
| reporter.failsafeFlags().mode_req_local_position_relaxed
| reporter.failsafeFlags().mode_req_global_position);
// Only warn and don't block arming because there could still be a valid position estimate from another source e.g. optical flow, VIO
required_modes.fail_modes = NavModes::None;
log_level = events::Log::Warning;
break;
case GnssArmingCheck::Disabled:
required_groups_gps = NavModes::None;
required_modes.message_modes = required_modes.fail_modes = NavModes::None;
log_level = events::Log::Disabled;
break;
}
@@ -304,10 +308,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
/* EVENT
* @description
* <profile name="dev">
* This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
* Can be configured with <param>EKF2_GPS_CHECK</param> and <param>COM_ARM_WO_GPS</param>.
* </profile>
*/
reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
reporter.armingCheckFailure(required_modes, health_component_t::gps,
events::ID("check_estimator_gps_fix_too_low"),
log_level, "GPS fix too low");
@@ -316,10 +320,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
/* EVENT
* @description
* <profile name="dev">
* This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
* Can be configured with <param>EKF2_GPS_CHECK</param> and <param>COM_ARM_WO_GPS</param>.
* </profile>
*/
reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
reporter.armingCheckFailure(required_modes, health_component_t::gps,
events::ID("check_estimator_gps_num_sats_too_low"),
log_level, "Not enough GPS Satellites");
@@ -328,10 +332,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
/* EVENT
* @description
* <profile name="dev">
* This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
* Can be configured with <param>EKF2_GPS_CHECK</param> and <param>COM_ARM_WO_GPS</param>.
* </profile>
*/
reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
reporter.armingCheckFailure(required_modes, health_component_t::gps,
events::ID("check_estimator_gps_pdop_too_high"),
log_level, "GPS PDOP too high");
@@ -340,10 +344,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
/* EVENT
* @description
* <profile name="dev">
* This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
* Can be configured with <param>EKF2_GPS_CHECK</param> and <param>COM_ARM_WO_GPS</param>.
* </profile>
*/
reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
reporter.armingCheckFailure(required_modes, health_component_t::gps,
events::ID("check_estimator_gps_hor_pos_err_too_high"),
log_level, "GPS Horizontal Position Error too high");
@@ -352,10 +356,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
/* EVENT
* @description
* <profile name="dev">
* This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
* Can be configured with <param>EKF2_GPS_CHECK</param> and <param>COM_ARM_WO_GPS</param>.
* </profile>
*/
reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
reporter.armingCheckFailure(required_modes, health_component_t::gps,
events::ID("check_estimator_gps_vert_pos_err_too_high"),
log_level, "GPS Vertical Position Error too high");
@@ -364,10 +368,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
/* EVENT
* @description
* <profile name="dev">
* This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
* Can be configured with <param>EKF2_GPS_CHECK</param> and <param>COM_ARM_WO_GPS</param>.
* </profile>
*/
reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
reporter.armingCheckFailure(required_modes, health_component_t::gps,
events::ID("check_estimator_gps_speed_acc_too_low"),
log_level, "GPS Speed Accuracy too low");
@@ -376,10 +380,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
/* EVENT
* @description
* <profile name="dev">
* This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
* Can be configured with <param>EKF2_GPS_CHECK</param> and <param>COM_ARM_WO_GPS</param>.
* </profile>
*/
reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
reporter.armingCheckFailure(required_modes, health_component_t::gps,
events::ID("check_estimator_gps_hor_pos_drift_too_high"),
log_level, "GPS Horizontal Position Drift too high");
@@ -388,10 +392,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
/* EVENT
* @description
* <profile name="dev">
* This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
* Can be configured with <param>EKF2_GPS_CHECK</param> and <param>COM_ARM_WO_GPS</param>.
* </profile>
*/
reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
reporter.armingCheckFailure(required_modes, health_component_t::gps,
events::ID("check_estimator_gps_vert_pos_drift_too_high"),
log_level, "GPS Vertical Position Drift too high");
@@ -400,10 +404,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
/* EVENT
* @description
* <profile name="dev">
* This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
* Can be configured with <param>EKF2_GPS_CHECK</param> and <param>COM_ARM_WO_GPS</param>.
* </profile>
*/
reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
reporter.armingCheckFailure(required_modes, health_component_t::gps,
events::ID("check_estimator_gps_hor_speed_drift_too_high"),
log_level, "GPS Horizontal Speed Drift too high");
@@ -412,10 +416,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
/* EVENT
* @description
* <profile name="dev">
* This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
* Can be configured with <param>EKF2_GPS_CHECK</param> and <param>COM_ARM_WO_GPS</param>.
* </profile>
*/
reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
reporter.armingCheckFailure(required_modes, health_component_t::gps,
events::ID("check_estimator_gps_vert_speed_drift_too_high"),
log_level, "GPS Vertical Speed Drift too high");
@@ -424,10 +428,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
/* EVENT
* @description
* <profile name="dev">
* This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
* Can be configured with <param>EKF2_GPS_CHECK</param> and <param>COM_ARM_WO_GPS</param>.
* </profile>
*/
reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
reporter.armingCheckFailure(required_modes, health_component_t::gps,
events::ID("check_estimator_gps_spoofed"),
log_level, "GPS signal spoofed");
@@ -437,7 +441,7 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
message = "Preflight%s: Estimator not using GPS";
/* EVENT
*/
reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
reporter.armingCheckFailure(required_modes, health_component_t::gps,
events::ID("check_estimator_gps_not_fusing"),
log_level, "Estimator not using GPS");
@@ -446,7 +450,7 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
message = "Preflight%s: Poor GPS Quality";
/* EVENT
*/
reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
reporter.armingCheckFailure(required_modes, health_component_t::gps,
events::ID("check_estimator_gps_generic"),
log_level, "Poor GPS Quality");
}