Skip to content

Commit

Permalink
estimatorCheck: only warn about GPS in modes that require a position …
Browse files Browse the repository at this point in the history 
…but fail all modes if GPS required by configuration
  • Loading branch information
@MaEtUgR
MaEtUgR committed Jan 30, 2025
1 parent f142363 commit 4c2e69b
Showing 1 changed file with 32 additions and 28 deletions.
60 changes: 32 additions & 28 deletions src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -273,25 +273,29 @@ 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())) {
default:

/* 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;
}
Expand All @@ -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");

Expand All @@ -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");

Expand All @@ -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");

Expand All @@ -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");

Expand All @@ -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");

Expand All @@ -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");

Expand All @@ -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");

Expand All @@ -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");

Expand All @@ -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");

Expand All @@ -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");

Expand All @@ -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");

Expand All @@ -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");

Expand All @@ -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");
}
Expand Down

0 comments on commit 4c2e69b

Please sign in to comment.