diff --git a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp index 2b7c453183..aa400e27c1 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp @@ -273,12 +273,27 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor } if (!context.isArmed() && ekf_gps_check_fail) { - NavModes required_groups_gps = required_groups; - events::Log log_level = events::Log::Error; + NavModes required_groups_gps; + events::Log log_level; - if (_param_com_arm_wo_gps.get()) { + switch (static_cast(_param_com_arm_wo_gps.get())) { + default: + + /* FALLTHROUGH */ + case GnssArmingCheck::DenyArming: + required_groups_gps = required_groups; + log_level = events::Log::Error; + break; + + case GnssArmingCheck::WarningOnly: required_groups_gps = NavModes::None; // optional log_level = events::Log::Warning; + break; + + case GnssArmingCheck::Disabled: + required_groups_gps = NavModes::None; + log_level = events::Log::Disabled; + break; } // Only report the first failure to avoid spamming @@ -438,11 +453,20 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor } if (message && reporter.mavlink_log_pub()) { - if (!_param_com_arm_wo_gps.get()) { - mavlink_log_critical(reporter.mavlink_log_pub(), message, " Fail"); + switch (static_cast(_param_com_arm_wo_gps.get())) { + default: - } else { - mavlink_log_critical(reporter.mavlink_log_pub(), message, ""); + /* FALLTHROUGH */ + case GnssArmingCheck::DenyArming: + mavlink_log_critical(reporter.mavlink_log_pub(), message, " Fail"); + break; + + case GnssArmingCheck::WarningOnly: + mavlink_log_warning(reporter.mavlink_log_pub(), message, ""); + break; + + case GnssArmingCheck::Disabled: + break; } } } diff --git a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp index 4f61df8867..d5cb363e11 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp @@ -59,6 +59,12 @@ public: void checkAndReport(const Context &context, Report &reporter) override; private: + enum class GnssArmingCheck : uint8_t { + DenyArming = 0, + WarningOnly = 1, + Disabled = 2 + }; + void checkEstimatorStatus(const Context &context, Report &reporter, const estimator_status_s &estimator_status, NavModes required_groups); void checkSensorBias(const Context &context, Report &reporter, NavModes required_groups); @@ -108,7 +114,7 @@ private: DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase, (ParamInt) _param_sens_imu_mode, (ParamInt) _param_com_arm_mag_str, - (ParamBool) _param_com_arm_wo_gps, + (ParamInt) _param_com_arm_wo_gps, (ParamBool) _param_sys_has_gps, (ParamFloat) _param_com_pos_fs_eph, (ParamFloat) _param_com_vel_fs_evh, diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index a462c8ae89..0b20831a6e 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -225,11 +225,14 @@ PARAM_DEFINE_FLOAT(COM_DISARM_LAND, 2.0f); PARAM_DEFINE_FLOAT(COM_DISARM_PRFLT, 10.0f); /** - * Allow arming without GPS + * GPS preflight check + * + * Measures taken when a check defined by EKF2_GPS_CHECK is failing. * * @group Commander - * @value 0 Require GPS lock to arm - * @value 1 Allow arming without GPS + * @value 0 Deny arming + * @value 1 Warning only + * @value 2 Disabled */ PARAM_DEFINE_INT32(COM_ARM_WO_GPS, 1);