diff --git a/boards/agam/fmu-v6xrt/nuttx-config/scripts/itcm_functions_includes.ld b/boards/agam/fmu-v6xrt/nuttx-config/scripts/itcm_functions_includes.ld index 47240adc864..1ae6137e1a5 100644 --- a/boards/agam/fmu-v6xrt/nuttx-config/scripts/itcm_functions_includes.ld +++ b/boards/agam/fmu-v6xrt/nuttx-config/scripts/itcm_functions_includes.ld @@ -458,7 +458,8 @@ *(.text._ZN24ManualVelocitySmoothingZC1Ev) /* itcm-check-ignore */ *(.text._ZN3ADC6sampleEj) *(.text._ZNK3Ekf22isTerrainEstimateValidEv) -*(.text._ZN15EstimatorChecks23setModeRequirementFlagsERK7ContextbbbRK24vehicle_local_position_sRK14vehicle_gnss_sR16failsafe_flags_sR6Report) +*(.text._ZN15EstimatorChecks23setModeRequirementFlagsERK7ContextbbRK24vehicle_local_position_sRK14vehicle_gnss_sR16failsafe_flags_sR6Report) +*(.text._ZN15EstimatorChecks29warnOfImminentPositionFailureER6ReportRKyRK25vehicle_global_position_sfRK16failsafe_flags_s) *(.text._ZN11ControlMath11addIfNotNanERff) *(.text._ZN9Commander21checkForMissionUpdateEv) *(.text._Z8set_tunei) @@ -601,6 +602,12 @@ *(.text._ZN3GPS8callbackE15GPSCallbackTypePviS1_) *(.text._ZN13AnalogBattery19get_current_channelEv) *(.text._ZN15EstimatorChecks20checkEstimatorStatusERK7ContextR6ReportRK18estimator_status_s8NavModes) +*(.text._ZN15EstimatorChecks25checkInnovationsPreflightERK7ContextR6ReportRK18estimator_status_s8NavModes) +*(.text._ZN15EstimatorChecks34checkMagneticInterferencePreflightERK7ContextR6ReportRK18estimator_status_s8NavModes) +*(.text._ZN15EstimatorChecks15checkGnssFusionERK7ContextR6ReportRK18estimator_status_s) +*(.text._ZN15EstimatorChecks22reportGnssFusionChangeERK7ContextR6Reportb) +*(.text._ZN15EstimatorChecks22reportGnssInterferenceER6Reportt) +*(.text._ZN15EstimatorChecks30reportFailedGnssCheckPreflightER6ReportRK18estimator_status_sb) *(.text._ZN12FailsafeBase11updateDelayERKy) *(.text._ZN10FlightTask25_evaluateDistanceToGroundEv) *(.text._ZN4EKF218PublishGnssHgtBiasERKy) diff --git a/boards/ark/fmu-v6xrt/nuttx-config/scripts/itcm_functions_includes.ld b/boards/ark/fmu-v6xrt/nuttx-config/scripts/itcm_functions_includes.ld index 72de799c019..528787aabc4 100644 --- a/boards/ark/fmu-v6xrt/nuttx-config/scripts/itcm_functions_includes.ld +++ b/boards/ark/fmu-v6xrt/nuttx-config/scripts/itcm_functions_includes.ld @@ -445,7 +445,8 @@ *(.text._ZN24ManualVelocitySmoothingZC1Ev) /* itcm-check-ignore */ *(.text._ZN3ADC6sampleEj) *(.text._ZNK3Ekf22isTerrainEstimateValidEv) -*(.text._ZN15EstimatorChecks23setModeRequirementFlagsERK7ContextbbbRK24vehicle_local_position_sRK14vehicle_gnss_sR16failsafe_flags_sR6Report) +*(.text._ZN15EstimatorChecks23setModeRequirementFlagsERK7ContextbbRK24vehicle_local_position_sRK14vehicle_gnss_sR16failsafe_flags_sR6Report) +*(.text._ZN15EstimatorChecks29warnOfImminentPositionFailureER6ReportRKyRK25vehicle_global_position_sfRK16failsafe_flags_s) *(.text._ZN11ControlMath11addIfNotNanERff) *(.text._ZN9Commander21checkForMissionUpdateEv) *(.text._Z8set_tunei) @@ -586,6 +587,12 @@ *(.text._ZN12SafetyButton3RunEv) *(.text._ZN3GPS8callbackE15GPSCallbackTypePviS1_) *(.text._ZN15EstimatorChecks20checkEstimatorStatusERK7ContextR6ReportRK18estimator_status_s8NavModes) +*(.text._ZN15EstimatorChecks25checkInnovationsPreflightERK7ContextR6ReportRK18estimator_status_s8NavModes) +*(.text._ZN15EstimatorChecks34checkMagneticInterferencePreflightERK7ContextR6ReportRK18estimator_status_s8NavModes) +*(.text._ZN15EstimatorChecks15checkGnssFusionERK7ContextR6ReportRK18estimator_status_s) +*(.text._ZN15EstimatorChecks22reportGnssFusionChangeERK7ContextR6Reportb) +*(.text._ZN15EstimatorChecks22reportGnssInterferenceER6Reportt) +*(.text._ZN15EstimatorChecks30reportFailedGnssCheckPreflightER6ReportRK18estimator_status_sb) *(.text._ZN12FailsafeBase11updateDelayERKy) *(.text._ZN10FlightTask25_evaluateDistanceToGroundEv) *(.text._ZN4EKF218PublishGnssHgtBiasERKy) diff --git a/boards/nxp/mr-tropic/nuttx-config/scripts/itcm_functions_includes.ld b/boards/nxp/mr-tropic/nuttx-config/scripts/itcm_functions_includes.ld index e0054802a9a..01da17b4d6e 100644 --- a/boards/nxp/mr-tropic/nuttx-config/scripts/itcm_functions_includes.ld +++ b/boards/nxp/mr-tropic/nuttx-config/scripts/itcm_functions_includes.ld @@ -450,7 +450,8 @@ *(.text._ZN24ManualVelocitySmoothingZC1Ev) /* itcm-check-ignore */ *(.text._ZN3ADC6sampleEj) *(.text._ZNK3Ekf22isTerrainEstimateValidEv) -*(.text._ZN15EstimatorChecks23setModeRequirementFlagsERK7ContextbbbRK24vehicle_local_position_sRK14vehicle_gnss_sR16failsafe_flags_sR6Report) +*(.text._ZN15EstimatorChecks23setModeRequirementFlagsERK7ContextbbRK24vehicle_local_position_sRK14vehicle_gnss_sR16failsafe_flags_sR6Report) +*(.text._ZN15EstimatorChecks29warnOfImminentPositionFailureER6ReportRKyRK25vehicle_global_position_sfRK16failsafe_flags_s) *(.text._ZN11ControlMath11addIfNotNanERff) *(.text._ZN9Commander21checkForMissionUpdateEv) *(.text._Z8set_tunei) @@ -589,6 +590,12 @@ *(.text._ZN3GPS8callbackE15GPSCallbackTypePviS1_) *(.text._ZN13AnalogBattery19get_current_channelEv) *(.text._ZN15EstimatorChecks20checkEstimatorStatusERK7ContextR6ReportRK18estimator_status_s8NavModes) +*(.text._ZN15EstimatorChecks25checkInnovationsPreflightERK7ContextR6ReportRK18estimator_status_s8NavModes) +*(.text._ZN15EstimatorChecks34checkMagneticInterferencePreflightERK7ContextR6ReportRK18estimator_status_s8NavModes) +*(.text._ZN15EstimatorChecks15checkGnssFusionERK7ContextR6ReportRK18estimator_status_s) +*(.text._ZN15EstimatorChecks22reportGnssFusionChangeERK7ContextR6Reportb) +*(.text._ZN15EstimatorChecks22reportGnssInterferenceER6Reportt) +*(.text._ZN15EstimatorChecks30reportFailedGnssCheckPreflightER6ReportRK18estimator_status_sb) *(.text._ZN12FailsafeBase11updateDelayERKy) *(.text._ZN10FlightTask25_evaluateDistanceToGroundEv) *(.text._ZN4EKF218PublishGnssHgtBiasERKy) diff --git a/boards/nxp/tropic-community/nuttx-config/scripts/itcm_functions_includes.ld b/boards/nxp/tropic-community/nuttx-config/scripts/itcm_functions_includes.ld index bb1ce6c978b..38d1c2a84e4 100644 --- a/boards/nxp/tropic-community/nuttx-config/scripts/itcm_functions_includes.ld +++ b/boards/nxp/tropic-community/nuttx-config/scripts/itcm_functions_includes.ld @@ -450,7 +450,8 @@ *(.text._ZN24ManualVelocitySmoothingZC1Ev) /* itcm-check-ignore */ *(.text._ZN3ADC6sampleEj) *(.text._ZNK3Ekf22isTerrainEstimateValidEv) -*(.text._ZN15EstimatorChecks23setModeRequirementFlagsERK7ContextbbbRK24vehicle_local_position_sRK14vehicle_gnss_sR16failsafe_flags_sR6Report) +*(.text._ZN15EstimatorChecks23setModeRequirementFlagsERK7ContextbbRK24vehicle_local_position_sRK14vehicle_gnss_sR16failsafe_flags_sR6Report) +*(.text._ZN15EstimatorChecks29warnOfImminentPositionFailureER6ReportRKyRK25vehicle_global_position_sfRK16failsafe_flags_s) *(.text._ZN11ControlMath11addIfNotNanERff) *(.text._ZN9Commander21checkForMissionUpdateEv) *(.text._Z8set_tunei) @@ -588,6 +589,12 @@ *(.text._ZN3GPS8callbackE15GPSCallbackTypePviS1_) *(.text._ZN13AnalogBattery19get_current_channelEv) *(.text._ZN15EstimatorChecks20checkEstimatorStatusERK7ContextR6ReportRK18estimator_status_s8NavModes) +*(.text._ZN15EstimatorChecks25checkInnovationsPreflightERK7ContextR6ReportRK18estimator_status_s8NavModes) +*(.text._ZN15EstimatorChecks34checkMagneticInterferencePreflightERK7ContextR6ReportRK18estimator_status_s8NavModes) +*(.text._ZN15EstimatorChecks15checkGnssFusionERK7ContextR6ReportRK18estimator_status_s) +*(.text._ZN15EstimatorChecks22reportGnssFusionChangeERK7ContextR6Reportb) +*(.text._ZN15EstimatorChecks22reportGnssInterferenceER6Reportt) +*(.text._ZN15EstimatorChecks30reportFailedGnssCheckPreflightER6ReportRK18estimator_status_sb) *(.text._ZN12FailsafeBase11updateDelayERKy) *(.text._ZN10FlightTask25_evaluateDistanceToGroundEv) *(.text._ZN4EKF218PublishGnssHgtBiasERKy) diff --git a/boards/px4/fmu-v6xrt/nuttx-config/scripts/itcm_functions_includes.ld b/boards/px4/fmu-v6xrt/nuttx-config/scripts/itcm_functions_includes.ld index a1157e3030f..06ac8e4ca13 100644 --- a/boards/px4/fmu-v6xrt/nuttx-config/scripts/itcm_functions_includes.ld +++ b/boards/px4/fmu-v6xrt/nuttx-config/scripts/itcm_functions_includes.ld @@ -457,7 +457,8 @@ *(.text._ZN24ManualVelocitySmoothingZC1Ev) /* itcm-check-ignore */ *(.text._ZN3ADC6sampleEj) *(.text._ZNK3Ekf22isTerrainEstimateValidEv) -*(.text._ZN15EstimatorChecks23setModeRequirementFlagsERK7ContextbbbRK24vehicle_local_position_sRK14vehicle_gnss_sR16failsafe_flags_sR6Report) +*(.text._ZN15EstimatorChecks23setModeRequirementFlagsERK7ContextbbRK24vehicle_local_position_sRK14vehicle_gnss_sR16failsafe_flags_sR6Report) +*(.text._ZN15EstimatorChecks29warnOfImminentPositionFailureER6ReportRKyRK25vehicle_global_position_sfRK16failsafe_flags_s) *(.text._ZN11ControlMath11addIfNotNanERff) *(.text._ZN9Commander21checkForMissionUpdateEv) *(.text._Z8set_tunei) @@ -600,6 +601,12 @@ *(.text._ZN3GPS8callbackE15GPSCallbackTypePviS1_) *(.text._ZN13AnalogBattery19get_current_channelEv) *(.text._ZN15EstimatorChecks20checkEstimatorStatusERK7ContextR6ReportRK18estimator_status_s8NavModes) +*(.text._ZN15EstimatorChecks25checkInnovationsPreflightERK7ContextR6ReportRK18estimator_status_s8NavModes) +*(.text._ZN15EstimatorChecks34checkMagneticInterferencePreflightERK7ContextR6ReportRK18estimator_status_s8NavModes) +*(.text._ZN15EstimatorChecks15checkGnssFusionERK7ContextR6ReportRK18estimator_status_s) +*(.text._ZN15EstimatorChecks22reportGnssFusionChangeERK7ContextR6Reportb) +*(.text._ZN15EstimatorChecks22reportGnssInterferenceER6Reportt) +*(.text._ZN15EstimatorChecks30reportFailedGnssCheckPreflightER6ReportRK18estimator_status_sb) *(.text._ZN12FailsafeBase11updateDelayERKy) *(.text._ZN10FlightTask25_evaluateDistanceToGroundEv) *(.text._ZN4EKF218PublishGnssHgtBiasERKy) diff --git a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp index 36cea75bbab..e3464aff757 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp @@ -61,7 +61,6 @@ void EstimatorChecks::checkAndReport(const Context &context, Report &reporter) lpos = {}; } - bool pre_flt_fail_innov_heading = false; bool pre_flt_fail_innov_vel_horiz = false; bool pre_flt_fail_innov_pos_horiz = false; bool missing_data = false; @@ -89,7 +88,6 @@ void EstimatorChecks::checkAndReport(const Context &context, Report &reporter) estimator_status_s estimator_status; if (_estimator_status_sub.copy(&estimator_status)) { - pre_flt_fail_innov_heading = estimator_status.pre_flt_fail_innov_heading; pre_flt_fail_innov_vel_horiz = estimator_status.pre_flt_fail_innov_vel_horiz; pre_flt_fail_innov_pos_horiz = estimator_status.pre_flt_fail_innov_pos_horiz; @@ -123,10 +121,8 @@ void EstimatorChecks::checkAndReport(const Context &context, Report &reporter) } // set mode requirements - setModeRequirementFlags(context, pre_flt_fail_innov_heading, pre_flt_fail_innov_vel_horiz, pre_flt_fail_innov_pos_horiz, - lpos, vehicle_gnss, - reporter.failsafeFlags(), reporter); - + setModeRequirementFlags(context, pre_flt_fail_innov_vel_horiz, pre_flt_fail_innov_pos_horiz, lpos, + vehicle_gnss, reporter.failsafeFlags(), reporter); lowPositionAccuracy(context, reporter, lpos); } @@ -134,16 +130,31 @@ void EstimatorChecks::checkAndReport(const Context &context, Report &reporter) void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &reporter, const estimator_status_s &estimator_status, NavModes required_groups) { + checkInnovationsPreflight(context, reporter, estimator_status, required_groups); + checkMagneticInterferencePreflight(context, reporter, estimator_status, required_groups); + + // If GPS aiding is required, declare fault condition if the required GPS quality checks are failing + if (_param_sys_has_gps.get()) { + checkGnssFusion(context, reporter, estimator_status); + } +} + +void EstimatorChecks::checkInnovationsPreflight(const Context &context, Report &reporter, + const estimator_status_s &estimator_status, NavModes required_groups) +{ + // Skip the checks to avoid warnings during calibration (they recover once the vehicle is still again) + if (context.isArmed() || context.status().calibration_enabled) { + return; + } + // Heading is required to arm for all modes that need any form of local position, plus FW AUTO_TAKEOFF const NavModes heading_required_groups = (NavModes)( reporter.failsafeFlags().mode_req_local_position | reporter.failsafeFlags().mode_req_local_position_relaxed | (1u << vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF)); - // Skip the checks to avoid warnings during calibration (they recover once the vehicle is still again) - const bool report_innovation_failures = !context.isArmed() && !context.status().calibration_enabled; - - if (report_innovation_failures && estimator_status.pre_flt_fail_innov_heading) { + // Only the first failing innovation is reported + if (estimator_status.pre_flt_fail_innov_heading) { /* EVENT * @description * Recalibrate compass or perform manual heading reset. @@ -156,7 +167,7 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: heading estimate invalid"); } - } else if (report_innovation_failures && estimator_status.pre_flt_fail_innov_vel_horiz) { + } else if (estimator_status.pre_flt_fail_innov_vel_horiz) { /* EVENT */ reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate, @@ -167,7 +178,7 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: horizontal velocity unstable"); } - } else if (report_innovation_failures && estimator_status.pre_flt_fail_innov_vel_vert) { + } else if (estimator_status.pre_flt_fail_innov_vel_vert) { /* EVENT */ reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate, @@ -178,7 +189,7 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: vertical velocity unstable"); } - } else if (report_innovation_failures && estimator_status.pre_flt_fail_innov_pos_horiz) { + } else if (estimator_status.pre_flt_fail_innov_pos_horiz) { /* EVENT */ reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate, @@ -189,7 +200,7 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: horizontal position unstable"); } - } else if (report_innovation_failures && estimator_status.pre_flt_fail_innov_height) { + } else if (estimator_status.pre_flt_fail_innov_height) { /* EVENT */ reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate, @@ -200,341 +211,337 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: height estimate not stable"); } } +} +void EstimatorChecks::checkMagneticInterferencePreflight(const Context &context, Report &reporter, + const estimator_status_s &estimator_status, NavModes required_groups) +{ + if (!_param_com_arm_mag_str.get() || context.isArmed() || !estimator_status.pre_flt_fail_mag_field_disturbed) { + return; + } - if (_param_com_arm_mag_str.get() - && (!context.isArmed() && estimator_status.pre_flt_fail_mag_field_disturbed)) { + const MagArmingCheck mag_arming_check = static_cast(_param_com_arm_mag_str.get()); - const MagArmingCheck mag_arming_check = static_cast(_param_com_arm_mag_str.get()); + // optional unless arming is denied + const NavModes required_groups_mag = (mag_arming_check == MagArmingCheck::DenyArming) ? required_groups : NavModes::None; - NavModes required_groups_mag = required_groups; + /* EVENT + * @description + * + * Measured strength: {1:.3}, expected: {2:.3} ± EKF2_MAG_CHK_STR + * Measured inclination: {3:.3}, expected: {4:.3} ± EKF2_MAG_CHK_INC + * This check can be configured via COM_ARM_MAG_STR and EKF2_MAG_CHECK parameters. + * + */ + reporter.armingCheckFailure(required_groups_mag, + health_component_t::local_position_estimate, + events::ID("check_estimator_mag_interference"), + events::Log::Warning, "Strong magnetic interference", + estimator_status.mag_strength_gs, estimator_status.mag_strength_ref_gs, + estimator_status.mag_inclination_deg, estimator_status.mag_inclination_ref_deg); - if (mag_arming_check != MagArmingCheck::DenyArming) { - required_groups_mag = NavModes::None; // optional + if (reporter.mavlink_log_pub()) { + const char *message = "Preflight%s: Strong magnetic interference"; + + if (mag_arming_check == MagArmingCheck::DenyArming) { + mavlink_log_critical(reporter.mavlink_log_pub(), message, " Fail"); + + } else { + mavlink_log_warning(reporter.mavlink_log_pub(), message, ""); + } + } +} + +void EstimatorChecks::checkGnssFusion(const Context &context, Report &reporter, const estimator_status_s &estimator_status) +{ + const bool gnss_fused = estimator_status.control_mode_flags & (1 << estimator_status_s::CS_GNSS_POS); + + // The flags describe only the newest sample, while EKF2 keeps rejecting samples for a while + // after one failed and keeps fusing for longer still, so the check that kept GNSS out may + // have passed again by the time the position goes. Each check is remembered for a while after + // it last failed, on its own, so one that keeps failing doesn't keep the others alive. + for (int i = 0; i < kNumGnssChecks; i++) { + if (estimator_status.gps_check_fail_flags & (1 << i)) { + _last_gnss_check_fail_time_us[i] = estimator_status.timestamp; + } + } + + if (gnss_fused) { + reporter.setIsPresent(health_component_t::gps); // should be based on the sensor data directly + _last_gnss_fusion_time_us = hrt_absolute_time(); + } + + reportGnssFusionChange(context, reporter, gnss_fused); + reportGnssInterference(reporter, estimator_status.gps_check_fail_flags); + + if (!context.isArmed() && (estimator_status.gps_check_fail_flags > 0)) { + reportFailedGnssCheckPreflight(reporter, estimator_status, gnss_fused); + } +} + +void EstimatorChecks::reportGnssFusionChange(const Context &context, Report &reporter, bool gnss_fused) +{ + if (context.isArmed()) { + + if (_gps_was_fused && !gnss_fused) { + if (reporter.mavlink_log_pub()) { + mavlink_log_warning(reporter.mavlink_log_pub(), "GNSS data fusion stopped\t"); + } + + // only report this failure as critical if not already in a local position invalid state + events::Log log_level = reporter.failsafeFlags().local_position_invalid ? events::Log::Info : events::Log::Error; + events::send(events::ID("check_estimator_gnss_fusion_stopped"), {log_level, events::LogInternal::Info}, + "GNSS data fusion stopped"); + + } else if (!_gps_was_fused && gnss_fused) { + + 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_started"), {events::Log::Info, events::LogInternal::Info}, + "GNSS data fusion started"); + } + } + + _gps_was_fused = gnss_fused; +} + +void EstimatorChecks::reportGnssInterference(Report &reporter, uint16_t gps_check_fail_flags) +{ + // Each is reported once, when it starts + const bool spoofed = gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_SPOOFED); + + if (spoofed && !_gnss_spoofed) { + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "GNSS signal spoofed\t"); } + events::send(events::ID("check_estimator_gnss_warning_spoofing"), {events::Log::Alert, events::LogInternal::Info}, + "GNSS signal spoofed"); + } + + _gnss_spoofed = spoofed; + + const bool jammed = gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_JAMMED); + + if (jammed && !_gnss_jammed) { + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "GNSS signal jammed\t"); + } + + events::send(events::ID("check_estimator_gnss_warning_jamming"), {events::Log::Alert, events::LogInternal::Info}, + "GNSS signal jammed"); + } + + _gnss_jammed = jammed; +} + +void EstimatorChecks::reportFailedGnssCheckPreflight(Report &reporter, const estimator_status_s &estimator_status, + bool gnss_fused) +{ + // What COM_ARM_WO_GPS makes of a failing check: the modes it blocks and how loudly it is reported + NavModesMessageFail required_modes; + events::Log log_level; + + switch (static_cast(_param_com_arm_wo_gps.get())) { + default: + + /* FALLTHROUGH */ + case GnssArmingCheck::DenyArming: + required_modes.message_modes = required_modes.fail_modes = NavModes::All; + log_level = events::Log::Error; + break; + + case GnssArmingCheck::WarningOnly: + 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_modes.message_modes = required_modes.fail_modes = NavModes::None; + log_level = events::Log::Disabled; + break; + } + + // Only report the first failure to avoid spamming + const char *message = nullptr; + + if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_GPS_FIX)) { + message = "Preflight%s: GPS fix too low"; /* EVENT * @description * - * Measured strength: {1:.3}, expected: {2:.3} ± EKF2_MAG_CHK_STR - * Measured inclination: {3:.3}, expected: {4:.3} ± EKF2_MAG_CHK_INC - * This check can be configured via COM_ARM_MAG_STR and EKF2_MAG_CHECK parameters. + * Can be configured with EKF2_GPS_CHECK and COM_ARM_WO_GPS. * */ - reporter.armingCheckFailure(required_groups_mag, - health_component_t::local_position_estimate, - events::ID("check_estimator_mag_interference"), - events::Log::Warning, "Strong magnetic interference", - estimator_status.mag_strength_gs, estimator_status.mag_strength_ref_gs, - estimator_status.mag_inclination_deg, estimator_status.mag_inclination_ref_deg); + reporter.armingCheckFailure(required_modes, health_component_t::gps, + events::ID("check_estimator_gps_fix_too_low"), + log_level, "GPS fix too low"); - if (reporter.mavlink_log_pub()) { - const char *message = "Preflight%s: Strong magnetic interference"; + } else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MIN_SAT_COUNT)) { + message = "Preflight%s: not enough GPS Satellites"; + /* EVENT + * @description + * + * Can be configured with EKF2_GPS_CHECK and COM_ARM_WO_GPS. + * + */ + reporter.armingCheckFailure(required_modes, health_component_t::gps, + events::ID("check_estimator_gps_num_sats_too_low"), + log_level, "Not enough GPS Satellites"); - if (mag_arming_check == MagArmingCheck::DenyArming) { - mavlink_log_critical(reporter.mavlink_log_pub(), message, " Fail"); + } else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_PDOP)) { + message = "Preflight%s: GPS PDOP too high"; + /* EVENT + * @description + * + * Can be configured with EKF2_GPS_CHECK and COM_ARM_WO_GPS. + * + */ + reporter.armingCheckFailure(required_modes, health_component_t::gps, + events::ID("check_estimator_gps_pdop_too_high"), + log_level, "GPS PDOP too high"); - } else { - mavlink_log_warning(reporter.mavlink_log_pub(), message, ""); - } - } + } else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_ERR)) { + message = "Preflight%s: GPS Horizontal Pos Error too high"; + /* EVENT + * @description + * + * Can be configured with EKF2_GPS_CHECK and COM_ARM_WO_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"); + + } else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_ERR)) { + message = "Preflight%s: GPS Vertical Pos Error too high"; + /* EVENT + * @description + * + * Can be configured with EKF2_GPS_CHECK and COM_ARM_WO_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"); + + } else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_SPD_ERR)) { + message = "Preflight%s: GPS Speed Accuracy too low"; + /* EVENT + * @description + * + * Can be configured with EKF2_GPS_CHECK and COM_ARM_WO_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"); + + } else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_DRIFT)) { + message = "Preflight%s: GPS Horizontal Pos Drift too high"; + /* EVENT + * @description + * + * Can be configured with EKF2_GPS_CHECK and COM_ARM_WO_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"); + + } else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_DRIFT)) { + message = "Preflight%s: GPS Vertical Pos Drift too high"; + /* EVENT + * @description + * + * Can be configured with EKF2_GPS_CHECK and COM_ARM_WO_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"); + + } else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_SPD_ERR)) { + message = "Preflight%s: GPS Hor Speed Drift too high"; + /* EVENT + * @description + * + * Can be configured with EKF2_GPS_CHECK and COM_ARM_WO_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"); + + } else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_SPD_ERR)) { + message = "Preflight%s: GPS Vert Speed Drift too high"; + /* EVENT + * @description + * + * Can be configured with EKF2_GPS_CHECK and COM_ARM_WO_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"); + + } else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_SPOOFED)) { + message = "Preflight%s: GPS signal spoofed"; + /* EVENT + * @description + * + * Can be configured with EKF2_GPS_CHECK and COM_ARM_WO_GPS. + * + */ + reporter.armingCheckFailure(required_modes, health_component_t::gps, + events::ID("check_estimator_gps_spoofed"), + log_level, "GPS signal spoofed"); + + } else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_JAMMED)) { + message = "Preflight%s: GPS signal jammed"; + /* EVENT + * @description + * + * Can be configured with EKF2_GPS_CHECK and COM_ARM_WO_GPS. + * + */ + reporter.armingCheckFailure(required_modes, health_component_t::gps, + events::ID("check_estimator_gps_jammed"), + log_level, "GPS signal jammed"); + + } else if (!gnss_fused) { + // Likely cause unknown + message = "Preflight%s: Estimator not using GPS"; + /* EVENT + */ + reporter.armingCheckFailure(required_modes, health_component_t::gps, + events::ID("check_estimator_gps_not_fusing"), + log_level, "Estimator not using GPS"); + + } else { + // if we land here there was a new flag added and the code not updated. Show a generic message. + message = "Preflight%s: Poor GPS Quality"; + /* EVENT + */ + reporter.armingCheckFailure(required_modes, health_component_t::gps, + events::ID("check_estimator_gps_generic"), + log_level, "Poor GPS Quality"); } - // If GPS aiding is required, declare fault condition if the required GPS quality checks are failing - if (_param_sys_has_gps.get()) { - const bool ekf_gps_fusion = estimator_status.control_mode_flags & (1 << estimator_status_s::CS_GNSS_POS); - const bool ekf_gps_check_fail = estimator_status.gps_check_fail_flags > 0; + if (reporter.mavlink_log_pub()) { + if (log_level == events::Log::Error) { + mavlink_log_critical(reporter.mavlink_log_pub(), message, " Fail"); - const hrt_abstime now = hrt_absolute_time(); - - // The flags describe only the newest sample, while EKF2 keeps rejecting samples for a while - // after one failed and keeps fusing for longer still, so the check that kept GNSS out may - // have passed again by the time the position goes. Each check is remembered for a while after - // it last failed, on its own, so one that keeps failing doesn't keep the others alive. - for (int i = 0; i < kNumGnssChecks; i++) { - if (estimator_status.gps_check_fail_flags & (1 << i)) { - _last_gnss_check_fail_time_us[i] = estimator_status.timestamp; - } - } - - if (ekf_gps_fusion) { - reporter.setIsPresent(health_component_t::gps); // should be based on the sensor data directly - _last_gnss_fusion_time_us = now; - } - - 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"); - } - - // only report this failure as critical if not already in a local position invalid state - events::Log log_level = reporter.failsafeFlags().local_position_invalid ? events::Log::Info : events::Log::Error; - events::send(events::ID("check_estimator_gnss_fusion_stopped"), {log_level, 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_started"), {events::Log::Info, events::LogInternal::Info}, - "GNSS data fusion started"); - } - } - - _gps_was_fused = ekf_gps_fusion; - - if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_SPOOFED)) { - if (!_gnss_spoofed) { - _gnss_spoofed = true; - - if (reporter.mavlink_log_pub()) { - mavlink_log_critical(reporter.mavlink_log_pub(), "GNSS signal spoofed\t"); - } - - events::send(events::ID("check_estimator_gnss_warning_spoofing"), {events::Log::Alert, events::LogInternal::Info}, - "GNSS signal spoofed"); - } - - } else { - _gnss_spoofed = false; - } - - if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_JAMMED)) { - if (!_gnss_jammed) { - _gnss_jammed = true; - - if (reporter.mavlink_log_pub()) { - mavlink_log_critical(reporter.mavlink_log_pub(), "GNSS signal jammed\t"); - } - - events::send(events::ID("check_estimator_gnss_warning_jamming"), {events::Log::Alert, events::LogInternal::Info}, - "GNSS signal jammed"); - } - - } else { - _gnss_jammed = false; - } - - if (!context.isArmed() && ekf_gps_check_fail) { - NavModesMessageFail required_modes; - events::Log log_level; - - switch (static_cast(_param_com_arm_wo_gps.get())) { - default: - - /* FALLTHROUGH */ - case GnssArmingCheck::DenyArming: - required_modes.message_modes = required_modes.fail_modes = NavModes::All; - log_level = events::Log::Error; - break; - - case GnssArmingCheck::WarningOnly: - 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_modes.message_modes = required_modes.fail_modes = NavModes::None; - log_level = events::Log::Disabled; - break; - } - - // Only report the first failure to avoid spamming - const char *message = nullptr; - - if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_GPS_FIX)) { - message = "Preflight%s: GPS fix too low"; - /* EVENT - * @description - * - * Can be configured with EKF2_GPS_CHECK and COM_ARM_WO_GPS. - * - */ - reporter.armingCheckFailure(required_modes, health_component_t::gps, - events::ID("check_estimator_gps_fix_too_low"), - log_level, "GPS fix too low"); - - } else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MIN_SAT_COUNT)) { - message = "Preflight%s: not enough GPS Satellites"; - /* EVENT - * @description - * - * Can be configured with EKF2_GPS_CHECK and COM_ARM_WO_GPS. - * - */ - reporter.armingCheckFailure(required_modes, health_component_t::gps, - events::ID("check_estimator_gps_num_sats_too_low"), - log_level, "Not enough GPS Satellites"); - - } else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_PDOP)) { - message = "Preflight%s: GPS PDOP too high"; - /* EVENT - * @description - * - * Can be configured with EKF2_GPS_CHECK and COM_ARM_WO_GPS. - * - */ - reporter.armingCheckFailure(required_modes, health_component_t::gps, - events::ID("check_estimator_gps_pdop_too_high"), - log_level, "GPS PDOP too high"); - - } else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_ERR)) { - message = "Preflight%s: GPS Horizontal Pos Error too high"; - /* EVENT - * @description - * - * Can be configured with EKF2_GPS_CHECK and COM_ARM_WO_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"); - - } else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_ERR)) { - message = "Preflight%s: GPS Vertical Pos Error too high"; - /* EVENT - * @description - * - * Can be configured with EKF2_GPS_CHECK and COM_ARM_WO_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"); - - } else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_SPD_ERR)) { - message = "Preflight%s: GPS Speed Accuracy too low"; - /* EVENT - * @description - * - * Can be configured with EKF2_GPS_CHECK and COM_ARM_WO_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"); - - } else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_DRIFT)) { - message = "Preflight%s: GPS Horizontal Pos Drift too high"; - /* EVENT - * @description - * - * Can be configured with EKF2_GPS_CHECK and COM_ARM_WO_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"); - - } else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_DRIFT)) { - message = "Preflight%s: GPS Vertical Pos Drift too high"; - /* EVENT - * @description - * - * Can be configured with EKF2_GPS_CHECK and COM_ARM_WO_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"); - - } else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_SPD_ERR)) { - message = "Preflight%s: GPS Hor Speed Drift too high"; - /* EVENT - * @description - * - * Can be configured with EKF2_GPS_CHECK and COM_ARM_WO_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"); - - } else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_SPD_ERR)) { - message = "Preflight%s: GPS Vert Speed Drift too high"; - /* EVENT - * @description - * - * Can be configured with EKF2_GPS_CHECK and COM_ARM_WO_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"); - - } else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_SPOOFED)) { - message = "Preflight%s: GPS signal spoofed"; - /* EVENT - * @description - * - * Can be configured with EKF2_GPS_CHECK and COM_ARM_WO_GPS. - * - */ - reporter.armingCheckFailure(required_modes, health_component_t::gps, - events::ID("check_estimator_gps_spoofed"), - log_level, "GPS signal spoofed"); - - } else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_JAMMED)) { - message = "Preflight%s: GPS signal jammed"; - /* EVENT - * @description - * - * Can be configured with EKF2_GPS_CHECK and COM_ARM_WO_GPS. - * - */ - reporter.armingCheckFailure(required_modes, health_component_t::gps, - events::ID("check_estimator_gps_jammed"), - log_level, "GPS signal jammed"); - - } else { - if (!ekf_gps_fusion) { - // Likely cause unknown - message = "Preflight%s: Estimator not using GPS"; - /* EVENT - */ - reporter.armingCheckFailure(required_modes, health_component_t::gps, - events::ID("check_estimator_gps_not_fusing"), - log_level, "Estimator not using GPS"); - - } else { - // if we land here there was a new flag added and the code not updated. Show a generic message. - message = "Preflight%s: Poor GPS Quality"; - /* EVENT - */ - reporter.armingCheckFailure(required_modes, health_component_t::gps, - events::ID("check_estimator_gps_generic"), - log_level, "Poor GPS Quality"); - } - } - - if (message && reporter.mavlink_log_pub()) { - switch (static_cast(_param_com_arm_wo_gps.get())) { - default: - - /* 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; - } - } + } else if (log_level == events::Log::Warning) { + mavlink_log_warning(reporter.mavlink_log_pub(), message, ""); } } - } void EstimatorChecks::checkSensorBias(const Context &context, Report &reporter, NavModes required_groups) @@ -547,73 +554,74 @@ void EstimatorChecks::checkSensorBias(const Context &context, Report &reporter, // _estimator_sensor_bias_sub instance got changed above already estimator_sensor_bias_s bias; - if (_estimator_sensor_bias_sub.copy(&bias) && hrt_elapsed_time(&bias.timestamp) < 30_s) { + if (!_estimator_sensor_bias_sub.copy(&bias) || (hrt_elapsed_time(&bias.timestamp) >= 30_s)) { + return; + } - // check accelerometer bias estimates - if (bias.accel_bias_valid) { - const float ekf_ab_test_limit = 0.75f * bias.accel_bias_limit; + // check accelerometer bias estimates + if (bias.accel_bias_valid) { + const float ekf_ab_test_limit = 0.75f * bias.accel_bias_limit; - for (int axis_index = 0; axis_index < 3; axis_index++) { - // allow for higher uncertainty in estimates for axes that are less observable to prevent false positives - // adjust test threshold by 3-sigma - const float test_uncertainty = 3.0f * sqrtf(fmaxf(bias.accel_bias_variance[axis_index], 0.0f)); + for (int axis_index = 0; axis_index < 3; axis_index++) { + // allow for higher uncertainty in estimates for axes that are less observable to prevent false positives + // adjust test threshold by 3-sigma + const float test_uncertainty = 3.0f * sqrtf(fmaxf(bias.accel_bias_variance[axis_index], 0.0f)); - if (fabsf(bias.accel_bias[axis_index]) > ekf_ab_test_limit + test_uncertainty) { - /* EVENT - * @description - * An accelerometer recalibration might help. - * - * - * Axis {1}: |{2:.8}| \> {3:.8} + {4:.8} - * - * This check can be configured via EKF2_ABL_LIM parameter. - * - */ - reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate, - events::ID("check_estimator_high_accel_bias"), - events::Log::Error, "High Accelerometer Bias", axis_index, - bias.accel_bias[axis_index], ekf_ab_test_limit, test_uncertainty); + if (fabsf(bias.accel_bias[axis_index]) > ekf_ab_test_limit + test_uncertainty) { + /* EVENT + * @description + * An accelerometer recalibration might help. + * + * + * Axis {1}: |{2:.8}| \> {3:.8} + {4:.8} + * + * This check can be configured via EKF2_ABL_LIM parameter. + * + */ + reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate, + events::ID("check_estimator_high_accel_bias"), + events::Log::Error, "High Accelerometer Bias", axis_index, + bias.accel_bias[axis_index], ekf_ab_test_limit, test_uncertainty); - if (reporter.mavlink_log_pub()) { - mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: High Accelerometer Bias"); - } - - return; // avoid showing more than one error + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: High Accelerometer Bias"); } + + return; // avoid showing more than one error } } + } - // check gyro bias estimates - if (bias.gyro_bias_valid) { - const float ekf_gb_test_limit = 0.75f * bias.gyro_bias_limit; + // check gyro bias estimates + if (bias.gyro_bias_valid) { + const float ekf_gb_test_limit = 0.75f * bias.gyro_bias_limit; - for (int axis_index = 0; axis_index < 3; axis_index++) { - // allow for higher uncertainty in estimates for axes that are less observable to prevent false positives - // adjust test threshold by 3-sigma - const float test_uncertainty = 3.0f * sqrtf(fmaxf(bias.gyro_bias_variance[axis_index], 0.0f)); + for (int axis_index = 0; axis_index < 3; axis_index++) { + // allow for higher uncertainty in estimates for axes that are less observable to prevent false positives + // adjust test threshold by 3-sigma + const float test_uncertainty = 3.0f * sqrtf(fmaxf(bias.gyro_bias_variance[axis_index], 0.0f)); - if (fabsf(bias.gyro_bias[axis_index]) > ekf_gb_test_limit + test_uncertainty) { - /* EVENT - * @description - * A Gyro recalibration might help. - * - * - * Axis {1}: |{2:.8}| \> {3:.8} + {4:.8} - * - * This check can be configured via EKF2_ABL_GYRLIM parameter. - * - */ - reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate, - events::ID("check_estimator_high_gyro_bias"), - events::Log::Error, "High Gyro Bias", axis_index, - bias.gyro_bias[axis_index], ekf_gb_test_limit, test_uncertainty); + if (fabsf(bias.gyro_bias[axis_index]) > ekf_gb_test_limit + test_uncertainty) { + /* EVENT + * @description + * A Gyro recalibration might help. + * + * + * Axis {1}: |{2:.8}| \> {3:.8} + {4:.8} + * + * This check can be configured via EKF2_ABL_GYRLIM parameter. + * + */ + reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate, + events::ID("check_estimator_high_gyro_bias"), + events::Log::Error, "High Gyro Bias", axis_index, + bias.gyro_bias[axis_index], ekf_gb_test_limit, test_uncertainty); - if (reporter.mavlink_log_pub()) { - mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: High Gyro Bias"); - } - - return; // avoid showing more than one error + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: High Gyro Bias"); } + + return; // avoid showing more than one error } } } @@ -624,58 +632,60 @@ void EstimatorChecks::checkEstimatorStatusFlags(const Context &context, Report & { estimator_status_flags_s estimator_status_flags; - if (_estimator_status_flags_sub.copy(&estimator_status_flags)) { - // Check for a magnetometer fault and notify the user - if (estimator_status_flags.cs_mag_fault) { - /* EVENT - * @description - * Land and calibrate the compass. - */ - reporter.armingCheckFailure(NavModes::All, health_component_t::local_position_estimate, - events::ID("check_estimator_mag_fault"), - events::Log::Critical, "Stopping compass use"); + if (!_estimator_status_flags_sub.copy(&estimator_status_flags)) { + return; + } - if (reporter.mavlink_log_pub()) { - mavlink_log_critical(reporter.mavlink_log_pub(), "Compass needs calibration - Land now!\t"); - } + // Check for a magnetometer fault and notify the user + if (estimator_status_flags.cs_mag_fault) { + /* EVENT + * @description + * Land and calibrate the compass. + */ + reporter.armingCheckFailure(NavModes::All, health_component_t::local_position_estimate, + events::ID("check_estimator_mag_fault"), + events::Log::Critical, "Stopping compass use"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Compass needs calibration - Land now!\t"); } + } - if (estimator_status_flags.cs_gnss_yaw_fault) { - /* EVENT - * @description - * Land now - */ - reporter.armingCheckFailure(NavModes::All, health_component_t::local_position_estimate, - events::ID("check_estimator_gnss_fault"), - events::Log::Critical, "GNSS heading not reliable"); + if (estimator_status_flags.cs_gnss_yaw_fault) { + /* EVENT + * @description + * Land now + */ + reporter.armingCheckFailure(NavModes::All, health_component_t::local_position_estimate, + events::ID("check_estimator_gnss_fault"), + events::Log::Critical, "GNSS heading not reliable"); - if (reporter.mavlink_log_pub()) { - mavlink_log_critical(reporter.mavlink_log_pub(), "GNSS heading not reliable - Land now!\t"); - } + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "GNSS heading not reliable - Land now!\t"); } + } - // Only require a heading reference when a global origin is set (i.e. global ops are intended) - if (!context.isArmed() - && (hrt_absolute_time() - estimator_status_flags.timestamp < 5_s) - && !estimator_status_flags.cs_yaw_align - && lpos.xy_global) { + // Only require a heading reference when a global origin is set (i.e. global ops are intended) + if (!context.isArmed() + && (hrt_absolute_time() - estimator_status_flags.timestamp < 5_s) + && !estimator_status_flags.cs_yaw_align + && lpos.xy_global) { - const NavModes heading_required_groups = (NavModes)( - reporter.failsafeFlags().mode_req_local_position | - reporter.failsafeFlags().mode_req_local_position_relaxed | - (1u << vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF)); + const NavModes heading_required_groups = (NavModes)( + reporter.failsafeFlags().mode_req_local_position | + reporter.failsafeFlags().mode_req_local_position_relaxed | + (1u << vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF)); - /* EVENT - * @description - * No heading source has aligned the EKF yaw - */ - reporter.armingCheckFailure(heading_required_groups, health_component_t::local_position_estimate, - events::ID("check_estimator_heading_no_source"), - events::Log::Error, "No heading reference"); + /* EVENT + * @description + * No heading source has aligned the EKF yaw + */ + reporter.armingCheckFailure(heading_required_groups, health_component_t::local_position_estimate, + events::ID("check_estimator_heading_no_source"), + events::Log::Error, "No heading reference"); - if (reporter.mavlink_log_pub()) { - mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: no heading reference"); - } + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: no heading reference"); } } } @@ -772,10 +782,9 @@ void EstimatorChecks::lowPositionAccuracy(const Context &context, Report &report reporter.failsafeFlags().position_accuracy_low = position_valid_but_low_accuracy; } -void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_flt_fail_innov_heading, - bool pre_flt_fail_innov_vel_horiz, bool pre_flt_fail_innov_pos_horiz, - const vehicle_local_position_s &lpos, const vehicle_gnss_s &vehicle_gnss, failsafe_flags_s &failsafe_flags, - Report &reporter) +void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_flt_fail_innov_vel_horiz, + bool pre_flt_fail_innov_pos_horiz, const vehicle_local_position_s &lpos, const vehicle_gnss_s &vehicle_gnss, + failsafe_flags_s &failsafe_flags, Report &reporter) { // The following flags correspond to mode requirements, and are reported in the corresponding mode checks vehicle_global_position_s gpos; @@ -803,6 +812,7 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f } } + // global position const bool global_pos_valid = gpos.lat_lon_valid && gpos.alt_valid; failsafe_flags.global_position_invalid = @@ -815,48 +825,9 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f pos_eph_relaxed_treshold, gpos.timestamp, _last_gpos_relaxed_fail_time_us, !failsafe_flags.global_position_invalid_relaxed); - // Additional warning if the system is about to enter position-loss failsafe after dead-reckoning period - const float eph_critical = 2.5f * lpos_eph_threshold; // threshold used to trigger the navigation failsafe - const float gpos_critical_warning_thrld = math::max(0.9f * eph_critical, math::max(eph_critical - 10.f, 0.f)); - - estimator_status_flags_s estimator_status_flags; - - if (_estimator_status_flags_sub.copy(&estimator_status_flags)) { - - // only do the following if the estimator status flags are recent (less than 5 seconds old) - if (now - estimator_status_flags.timestamp < 5_s) { - const bool dead_reckoning = estimator_status_flags.cs_inertial_dead_reckoning - || estimator_status_flags.cs_wind_dead_reckoning; - - if (!failsafe_flags.global_position_invalid - && failsafe_flags.mode_req_global_position - && !_nav_failure_imminent_warned - && gpos.eph > gpos_critical_warning_thrld - && dead_reckoning) { - /* EVENT - * @description - * Switch to manual mode recommended. - * - * - * This warning is triggered when the position error estimate is 90% of (or only 10m below) COM_POS_FS_EPH parameter. - * - */ - events::send(events::ID("check_estimator_position_failure_imminent"), {events::Log::Error, events::LogInternal::Info}, - "Estimated position error is approaching the failsafe threshold"); - - if (reporter.mavlink_log_pub()) { - mavlink_log_critical(reporter.mavlink_log_pub(), - "Estimated position error is approaching the failsafe threshold\t"); - } - - _nav_failure_imminent_warned = true; - - } else if (!dead_reckoning) { - _nav_failure_imminent_warned = false; - } - } - } + warnOfImminentPositionFailure(reporter, now, gpos, lpos_eph_threshold, failsafe_flags); + // local position const bool local_position_was_valid = !failsafe_flags.local_position_invalid; failsafe_flags.local_position_invalid = @@ -867,7 +838,6 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f reportGnssReasonForPositionLoss(context, reporter, now, vehicle_gnss); } - // In some modes we assume that the operator will compensate for the drift so we do not need to check the position error const float lpos_eph_threshold_relaxed = INFINITY; @@ -879,11 +849,9 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f !checkPosVelValidity(now, v_xy_valid, lpos.evh, _param_com_vel_fs_evh.get(), lpos.timestamp, _last_lvel_fail_time_us, !failsafe_flags.local_velocity_invalid); - // altitude failsafe_flags.local_altitude_invalid = !lpos.z_valid || (now > lpos.timestamp + 1_s); - // attitude vehicle_attitude_s attitude; @@ -926,6 +894,52 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f failsafe_flags.angular_velocity_invalid = angular_velocity_invalid; } +void EstimatorChecks::warnOfImminentPositionFailure(Report &reporter, const hrt_abstime &now, + const vehicle_global_position_s &gpos, float lpos_eph_threshold, const failsafe_flags_s &failsafe_flags) +{ + // Additional warning if the system is about to enter position-loss failsafe after dead-reckoning period, + // only with recent estimator status flags (less than 5 seconds old) + estimator_status_flags_s estimator_status_flags; + + if (!_estimator_status_flags_sub.copy(&estimator_status_flags) || (now - estimator_status_flags.timestamp >= 5_s)) { + return; + } + + const bool dead_reckoning = estimator_status_flags.cs_inertial_dead_reckoning + || estimator_status_flags.cs_wind_dead_reckoning; + + if (!dead_reckoning) { + _nav_failure_imminent_warned = false; + return; + } + + const float eph_critical = 2.5f * lpos_eph_threshold; // threshold used to trigger the navigation failsafe + const float gpos_critical_warning_thrld = math::max(0.9f * eph_critical, math::max(eph_critical - 10.f, 0.f)); + + if (!failsafe_flags.global_position_invalid + && failsafe_flags.mode_req_global_position + && !_nav_failure_imminent_warned + && gpos.eph > gpos_critical_warning_thrld) { + /* EVENT + * @description + * Switch to manual mode recommended. + * + * + * This warning is triggered when the position error estimate is 90% of (or only 10m below) COM_POS_FS_EPH parameter. + * + */ + events::send(events::ID("check_estimator_position_failure_imminent"), {events::Log::Error, events::LogInternal::Info}, + "Estimated position error is approaching the failsafe threshold"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), + "Estimated position error is approaching the failsafe threshold\t"); + } + + _nav_failure_imminent_warned = true; + } +} + bool EstimatorChecks::checkPosVelValidity(const hrt_abstime &now, const bool data_valid, const float data_accuracy, const float required_accuracy, const hrt_abstime &data_timestamp_us, hrt_abstime &last_fail_time_us, diff --git a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp index 8fa69d20cd6..213e63a5ebe 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp @@ -71,8 +71,19 @@ private: WarningOnly = 2 }; + // The checks on the estimator status: the preflight innovation and magnetic interference checks, and + // what the estimator makes of GNSS void checkEstimatorStatus(const Context &context, Report &reporter, const estimator_status_s &estimator_status, NavModes required_groups); + void checkInnovationsPreflight(const Context &context, Report &reporter, const estimator_status_s &estimator_status, + NavModes required_groups); + void checkMagneticInterferencePreflight(const Context &context, Report &reporter, + const estimator_status_s &estimator_status, NavModes required_groups); + void checkGnssFusion(const Context &context, Report &reporter, const estimator_status_s &estimator_status); + void reportGnssFusionChange(const Context &context, Report &reporter, bool gnss_fused); + void reportGnssInterference(Report &reporter, uint16_t gps_check_fail_flags); + void reportFailedGnssCheckPreflight(Report &reporter, const estimator_status_s &estimator_status, bool gnss_fused); + void checkSensorBias(const Context &context, Report &reporter, NavModes required_groups); void checkEstimatorStatusFlags(const Context &context, Report &reporter, const estimator_status_s &estimator_status, const vehicle_local_position_s &lpos); @@ -84,10 +95,12 @@ private: void reportGnssReasonForPositionLoss(const Context &context, Report &reporter, const hrt_abstime &now, const vehicle_gnss_s &vehicle_gnss) const; - void setModeRequirementFlags(const Context &context, bool pre_flt_fail_innov_heading, - bool pre_flt_fail_innov_vel_horiz, bool pre_flt_fail_innov_pos_horiz, - const vehicle_local_position_s &lpos, const vehicle_gnss_s &vehicle_gnss, - failsafe_flags_s &failsafe_flags, Report &reporter); + // The mode requirement flags, with the warning of an imminent position failure on its own + void setModeRequirementFlags(const Context &context, bool pre_flt_fail_innov_vel_horiz, + bool pre_flt_fail_innov_pos_horiz, const vehicle_local_position_s &lpos, + const vehicle_gnss_s &vehicle_gnss, failsafe_flags_s &failsafe_flags, Report &reporter); + void warnOfImminentPositionFailure(Report &reporter, const hrt_abstime &now, const vehicle_global_position_s &gpos, + float lpos_eph_threshold, const failsafe_flags_s &failsafe_flags); bool checkPosVelValidity(const hrt_abstime &now, const bool data_valid, const float data_accuracy, const float required_accuracy, diff --git a/src/modules/commander/HealthAndArmingChecks/estimatorChecksTest.cpp b/src/modules/commander/HealthAndArmingChecks/estimatorChecksTest.cpp index 760913ce679..c51fa001a6b 100644 --- a/src/modules/commander/HealthAndArmingChecks/estimatorChecksTest.cpp +++ b/src/modules/commander/HealthAndArmingChecks/estimatorChecksTest.cpp @@ -41,9 +41,15 @@ #include #include #include +#include #include +#include +#include #include #include +#include +#include +#include #include #include @@ -63,17 +69,57 @@ public: { param_control_autosave(false); - int32_t one = 1; - param_set(param_find("SYS_HAS_GPS"), &one); - param_set(param_find("SENS_IMU_MODE"), &one); + setParam("SYS_HAS_GPS", 1); + setParam("SENS_IMU_MODE", 1); + + // back to their defaults, the cases below change them + param_reset(param_find("COM_ARM_MAG_STR")); + param_reset(param_find("COM_ARM_WO_GPS")); + param_reset(param_find("COM_POS_LOW_EPH")); + param_reset(param_find("COM_POS_LOW_ACT")); // the event topic has to exist before the first send, or the queued events are lost orb_advertise(ORB_ID(event), nullptr); _failsafe_flags = {}; + // every mode requires attitude and a local and global position, so a failed check shows on every mode + _failsafe_flags.mode_req_attitude = ~0u; + _failsafe_flags.mode_req_local_position = ~0u; + _failsafe_flags.mode_req_global_position = ~0u; + + // the topics keep their last sample across cases, so each case starts from the same clean state + publishEstimatorStatus(false, 0); + estimator_status_flags_s flags{}; + flags.timestamp = hrt_absolute_time(); + flags.cs_yaw_align = true; + publishStatusFlags(flags); + estimator_sensor_bias_s bias{}; + bias.timestamp = hrt_absolute_time(); + publishSensorBias(bias); + vehicle_global_position_s gpos{}; + gpos.timestamp = hrt_absolute_time(); + publishGlobalPosition(gpos); + vehicle_attitude_s attitude{}; + attitude.timestamp = hrt_absolute_time(); + attitude.q[0] = 1.f; + publishAttitude(attitude); + vehicle_angular_velocity_s rates{}; + rates.timestamp = hrt_absolute_time(); + publishAngularVelocity(rates); + drainEvents(); } + void setParam(const char *name, int32_t value) { param_set(param_find(name), &value); } + void setParam(const char *name, float value) { param_set(param_find(name), &value); } + + void publishEstimatorStatus(const estimator_status_s &status) { _estimator_status_pub.publish(status); } + void publishStatusFlags(const estimator_status_flags_s &flags) { _status_flags_pub.publish(flags); } + void publishSensorBias(const estimator_sensor_bias_s &bias) { _sensor_bias_pub.publish(bias); } + void publishGlobalPosition(const vehicle_global_position_s &gpos) { _global_position_pub.publish(gpos); } + void publishAttitude(const vehicle_attitude_s &attitude) { _attitude_pub.publish(attitude); } + void publishAngularVelocity(const vehicle_angular_velocity_s &rates) { _angular_velocity_pub.publish(rates); } + // what the estimator reports: whether it fuses GNSS position, and which receiver checks fail. // An age stands in for a report seen that long ago. void publishEstimatorStatus(bool gnss_fused, uint16_t gps_check_fail_flags, hrt_abstime age = 0) @@ -98,7 +144,7 @@ public: _receiver_pub.publish(gnss); } - void publishLocalPosition(bool valid) + void publishLocalPosition(bool valid, float eph = 0.5f, bool global_origin = false) { vehicle_local_position_s lpos{}; lpos.timestamp = hrt_absolute_time(); @@ -107,15 +153,17 @@ public: lpos.v_xy_valid = valid; lpos.z_valid = true; lpos.v_z_valid = true; - lpos.eph = 0.5f; + lpos.xy_global = global_origin; + lpos.eph = eph; lpos.epv = 0.5f; lpos.evh = 0.2f; lpos.evv = 0.2f; _local_position_pub.publish(lpos); } - // one commander cycle. The failsafe flags persist across cycles as they do in commander. - void runCheck(bool armed) + // one commander cycle. The failsafe flags persist across cycles as they do in commander, and the + // health report of the cycle is kept for the assertions. + void runCheck(bool armed, bool calibrating = false) { vehicle_status_s status{}; @@ -123,10 +171,28 @@ public: status.arming_state = vehicle_status_s::ARMING_STATE_ARMED; } + status.calibration_enabled = calibrating; + _check.updateParams(); Context context{status}; Report reporter{_failsafe_flags, 0}; _check.checkAndReport(context, reporter); + reporter.getHealthReport(_report); + } + + bool armingError(health_component_t component) const + { + return _report.arming_check_error_flags & (uint64_t)component; + } + + bool armingWarning(health_component_t component) const + { + return _report.arming_check_warning_flags & (uint64_t)component; + } + + bool isPresent(health_component_t component) const + { + return _report.health_is_present_flags & (uint64_t)component; } // everything published since the last drain is kept, so one test can count several ids @@ -178,14 +244,27 @@ public: static constexpr uint32_t kReasonEvent = events::ID("check_estimator_position_lost_gnss_reason"); static constexpr uint32_t kNoDataEvent = events::ID("check_estimator_position_lost_gnss_no_data"); + static constexpr uint32_t kFusionStartedEvent = events::ID("check_estimator_gnss_fusion_started"); + static constexpr uint32_t kFusionStoppedEvent = events::ID("check_estimator_gnss_fusion_stopped"); + static constexpr uint32_t kSpoofingEvent = events::ID("check_estimator_gnss_warning_spoofing"); + static constexpr uint32_t kJammingEvent = events::ID("check_estimator_gnss_warning_jamming"); + static constexpr uint32_t kFailureImminentEvent = events::ID("check_estimator_position_failure_imminent"); static constexpr uint16_t kSpeedAccuracy = 1 << estimator_status_s::GPS_CHECK_FAIL_MAX_SPD_ERR; static constexpr uint16_t kSpoofed = 1 << estimator_status_s::GPS_CHECK_FAIL_SPOOFED; + static constexpr uint16_t kJammed = 1 << estimator_status_s::GPS_CHECK_FAIL_JAMMED; + static constexpr uint16_t kFixTooLow = 1 << estimator_status_s::GPS_CHECK_FAIL_GPS_FIX; uORB::PublicationMulti _estimator_status_pub{ORB_ID(estimator_status)}; + uORB::PublicationMulti _status_flags_pub{ORB_ID(estimator_status_flags)}; + uORB::PublicationMulti _sensor_bias_pub{ORB_ID(estimator_sensor_bias)}; uORB::Publication _local_position_pub{ORB_ID(vehicle_local_position)}; + uORB::Publication _global_position_pub{ORB_ID(vehicle_global_position)}; + uORB::Publication _attitude_pub{ORB_ID(vehicle_attitude)}; + uORB::Publication _angular_velocity_pub{ORB_ID(vehicle_angular_velocity)}; uORB::Publication _receiver_pub{ORB_ID(vehicle_gnss)}; uORB::Subscription _event_sub{ORB_ID(event)}; + health_report_s _report{}; failsafe_flags_s _failsafe_flags{}; std::vector _events; EstimatorChecks _check; @@ -400,3 +479,353 @@ TEST_F(EstimatorChecksTest, NothingWhileDisarmed) ASSERT_TRUE(_failsafe_flags.local_position_invalid); EXPECT_EQ(countEvents(kReasonEvent), 0); } + +// The rest of the checks in the file, pinned as they behave so a reorganisation of the file +// can be held against them + +TEST_F(EstimatorChecksTest, PreflightInnovationFailureIsReportedOnTheGround) +{ + estimator_status_s status{}; + status.timestamp = hrt_absolute_time(); + status.pre_flt_fail_innov_heading = true; + publishEstimatorStatus(status); + + runCheck(false); + EXPECT_TRUE(armingError(health_component_t::local_position_estimate)); + + runCheck(true); + EXPECT_FALSE(armingError(health_component_t::local_position_estimate)) << "the innovations are not judged in flight"; + + runCheck(false, true); + EXPECT_FALSE(armingError(health_component_t::local_position_estimate)) << "nor during calibration"; + + // the last innovation in the chain is reported like the first + status.pre_flt_fail_innov_heading = false; + status.pre_flt_fail_innov_height = true; + publishEstimatorStatus(status); + runCheck(false); + EXPECT_TRUE(armingError(health_component_t::local_position_estimate)); + + status.pre_flt_fail_innov_height = false; + publishEstimatorStatus(status); + runCheck(false); + EXPECT_FALSE(armingError(health_component_t::local_position_estimate)); +} + +TEST_F(EstimatorChecksTest, MagneticInterferenceIsReportedAsConfigured) +{ + estimator_status_s status{}; + status.timestamp = hrt_absolute_time(); + status.pre_flt_fail_mag_field_disturbed = true; + publishEstimatorStatus(status); + + setParam("COM_ARM_MAG_STR", 1); // deny arming + runCheck(false); + EXPECT_TRUE(armingWarning(health_component_t::local_position_estimate)); + EXPECT_EQ(_report.can_arm_mode_flags, 0u) << "every mode needs the attitude, so none can arm"; + + setParam("COM_ARM_MAG_STR", 2); // warn only + runCheck(false); + EXPECT_TRUE(armingWarning(health_component_t::local_position_estimate)); + EXPECT_NE(_report.can_arm_mode_flags, 0u) << "the warning alone does not block arming"; + + setParam("COM_ARM_MAG_STR", 0); + runCheck(false); + EXPECT_FALSE(armingWarning(health_component_t::local_position_estimate)); + + setParam("COM_ARM_MAG_STR", 1); + runCheck(true); + EXPECT_FALSE(armingWarning(health_component_t::local_position_estimate)) << "only judged on the ground"; +} + +TEST_F(EstimatorChecksTest, GnssFusionStartAndStopAreReportedInFlight) +{ + publishLocalPosition(true); + runCheck(true); + drainEvents(); + + publishEstimatorStatus(true, 0); + runCheck(true); + EXPECT_EQ(countEvents(kFusionStartedEvent), 1); + + runCheck(true); + EXPECT_EQ(countEvents(kFusionStartedEvent), 1) << "reported on the change, not every cycle"; + + event_s event{}; + publishEstimatorStatus(false, 0); + runCheck(true); + ASSERT_EQ(countEvents(kFusionStoppedEvent, &event), 1); + EXPECT_EQ(event.log_levels & 0x0f, (uint8_t)events::Log::Error) << "an error while the position estimate is still valid"; +} + +TEST_F(EstimatorChecksTest, GnssFusionStopIsOnlyInformationOncePositionIsLost) +{ + flyOnGnss(); + + publishLocalPosition(false); + runCheck(true); + ASSERT_TRUE(_failsafe_flags.local_position_invalid); + drainEvents(); + + event_s event{}; + publishEstimatorStatus(false, 0); + runCheck(true); + ASSERT_EQ(countEvents(kFusionStoppedEvent, &event), 1); + EXPECT_EQ(event.log_levels & 0x0f, (uint8_t)events::Log::Info); +} + +TEST_F(EstimatorChecksTest, GnssFusionChangesAreNotReportedOnTheGround) +{ + publishEstimatorStatus(true, 0); + runCheck(false); + publishEstimatorStatus(false, 0); + runCheck(false); + EXPECT_EQ(countEvents(kFusionStartedEvent), 0); + EXPECT_EQ(countEvents(kFusionStoppedEvent), 0); + + // the state is still followed, so arming does not report a change that happened on the ground + publishEstimatorStatus(true, 0); + runCheck(false); + runCheck(true); + EXPECT_EQ(countEvents(kFusionStartedEvent), 0); +} + +TEST_F(EstimatorChecksTest, SpoofingAndJammingAreReportedOnceUntilTheyClear) +{ + publishEstimatorStatus(true, kSpoofed); + runCheck(false); + runCheck(false); + EXPECT_EQ(countEvents(kSpoofingEvent), 1); + + publishEstimatorStatus(true, 0); + runCheck(false); + publishEstimatorStatus(true, kSpoofed); + runCheck(false); + EXPECT_EQ(countEvents(kSpoofingEvent), 2) << "reported again once it cleared in between"; + + publishEstimatorStatus(true, kJammed); + runCheck(false); + runCheck(false); + EXPECT_EQ(countEvents(kJammingEvent), 1); +} + +TEST_F(EstimatorChecksTest, AFailingGnssCheckBlocksArmingAsConfigured) +{ + publishEstimatorStatus(true, 0); + runCheck(false); + const uint64_t can_arm_with_good_gnss = _report.can_arm_mode_flags; + + publishEstimatorStatus(true, kFixTooLow); + + setParam("COM_ARM_WO_GPS", 0); // deny arming + runCheck(false); + EXPECT_TRUE(armingError(health_component_t::gps)); + EXPECT_EQ(_report.can_arm_mode_flags, 0u); + + setParam("COM_ARM_WO_GPS", 1); // warn only + runCheck(false); + EXPECT_FALSE(armingError(health_component_t::gps)); + EXPECT_TRUE(armingWarning(health_component_t::gps)); + EXPECT_EQ(_report.can_arm_mode_flags, can_arm_with_good_gnss); + + setParam("COM_ARM_WO_GPS", 2); // disabled + runCheck(false); + EXPECT_FALSE(armingError(health_component_t::gps)); + EXPECT_FALSE(armingWarning(health_component_t::gps)); + + setParam("COM_ARM_WO_GPS", 0); + runCheck(true); + EXPECT_FALSE(armingError(health_component_t::gps)) << "the quality checks are only judged on the ground"; +} + +TEST_F(EstimatorChecksTest, GnssIsPresentWhileTheEstimatorFusesIt) +{ + publishEstimatorStatus(true, 0); + runCheck(false); + EXPECT_TRUE(isPresent(health_component_t::gps)); + + publishEstimatorStatus(false, 0); + runCheck(false); + EXPECT_FALSE(isPresent(health_component_t::gps)); +} + +TEST_F(EstimatorChecksTest, HighSensorBiasBlocksArming) +{ + publishEstimatorStatus(true, 0); + + estimator_sensor_bias_s bias{}; + bias.timestamp = hrt_absolute_time(); + bias.accel_bias_valid = true; + bias.accel_bias_limit = 0.4f; + bias.accel_bias[1] = 0.35f; // above three quarters of the limit + publishSensorBias(bias); + + runCheck(false); + EXPECT_TRUE(armingError(health_component_t::local_position_estimate)); + + runCheck(false, true); + EXPECT_FALSE(armingError(health_component_t::local_position_estimate)) << "not judged during calibration"; + + bias.timestamp = hrt_absolute_time() - 31_s; + publishSensorBias(bias); + runCheck(false); + EXPECT_FALSE(armingError(health_component_t::local_position_estimate)) << "a stale estimate is not judged"; + + bias = {}; + bias.timestamp = hrt_absolute_time(); + bias.gyro_bias_valid = true; + bias.gyro_bias_limit = 0.1f; + bias.gyro_bias[2] = 0.09f; + publishSensorBias(bias); + runCheck(false); + EXPECT_TRUE(armingError(health_component_t::local_position_estimate)); +} + +TEST_F(EstimatorChecksTest, CompassFaultBlocksEveryMode) +{ + publishEstimatorStatus(true, 0); + + estimator_status_flags_s flags{}; + flags.timestamp = hrt_absolute_time(); + flags.cs_yaw_align = true; + flags.cs_mag_fault = true; + publishStatusFlags(flags); + + runCheck(true); + EXPECT_TRUE(armingError(health_component_t::local_position_estimate)); + EXPECT_EQ(_report.can_arm_mode_flags, 0u); +} + +TEST_F(EstimatorChecksTest, NoHeadingReferenceBlocksArmingOnlyWithAGlobalOrigin) +{ + publishEstimatorStatus(true, 0); + + estimator_status_flags_s flags{}; + flags.timestamp = hrt_absolute_time(); + flags.cs_yaw_align = false; + publishStatusFlags(flags); + + publishLocalPosition(true, 0.5f, true); + runCheck(false); + EXPECT_TRUE(armingError(health_component_t::local_position_estimate)); + + runCheck(true); + EXPECT_FALSE(armingError(health_component_t::local_position_estimate)) << "only judged on the ground"; + + publishLocalPosition(true, 0.5f, false); + runCheck(false); + EXPECT_FALSE(armingError(health_component_t::local_position_estimate)) << "no heading reference is needed without a global origin"; +} + +TEST_F(EstimatorChecksTest, PositionFailureImminentIsWarnedOnceWhileDeadReckoning) +{ + publishEstimatorStatus(true, 0); + publishLocalPosition(true); + + // a global position that is still valid but whose error is close to COM_POS_FS_EPH + vehicle_global_position_s gpos{}; + gpos.timestamp = hrt_absolute_time(); + gpos.lat_lon_valid = true; + gpos.alt_valid = true; + gpos.eph = 12.f; + publishGlobalPosition(gpos); + + estimator_status_flags_s flags{}; + flags.timestamp = hrt_absolute_time(); + flags.cs_yaw_align = true; + flags.cs_inertial_dead_reckoning = true; + publishStatusFlags(flags); + + runCheck(true); + ASSERT_FALSE(_failsafe_flags.global_position_invalid); + EXPECT_EQ(countEvents(kFailureImminentEvent), 1); + + runCheck(true); + EXPECT_EQ(countEvents(kFailureImminentEvent), 1) << "warned once"; + + flags.cs_inertial_dead_reckoning = false; + publishStatusFlags(flags); + runCheck(true); + flags.cs_inertial_dead_reckoning = true; + publishStatusFlags(flags); + runCheck(true); + EXPECT_EQ(countEvents(kFailureImminentEvent), 2) << "warned again after dead reckoning ended in between"; +} + +TEST_F(EstimatorChecksTest, LowPositionAccuracyIsFlaggedAndReportedInFlight) +{ + setParam("COM_POS_LOW_EPH", 1.f); + setParam("COM_POS_LOW_ACT", 1); + publishEstimatorStatus(true, 0); + + publishLocalPosition(true, 2.f); + runCheck(true); + ASSERT_FALSE(_failsafe_flags.local_position_invalid); + EXPECT_TRUE(_failsafe_flags.position_accuracy_low); + EXPECT_TRUE(armingError(health_component_t::local_position_estimate)); + + runCheck(false); + EXPECT_TRUE(_failsafe_flags.position_accuracy_low); + EXPECT_FALSE(armingError(health_component_t::local_position_estimate)) << "flagged but not reported on the ground"; + + publishLocalPosition(true, 0.5f); + runCheck(true); + EXPECT_FALSE(_failsafe_flags.position_accuracy_low); +} + +TEST_F(EstimatorChecksTest, AttitudeValidityFollowsTheQuaternion) +{ + vehicle_attitude_s attitude{}; + attitude.timestamp = hrt_absolute_time(); + attitude.q[0] = 1.f; + publishAttitude(attitude); + runCheck(true); + EXPECT_FALSE(_failsafe_flags.attitude_invalid); + + attitude.q[0] = 1.1f; + publishAttitude(attitude); + runCheck(true); + EXPECT_TRUE(_failsafe_flags.attitude_invalid) << "not a unit quaternion"; + + attitude.q[0] = 1.f; + attitude.timestamp = hrt_absolute_time() - 2_s; + publishAttitude(attitude); + runCheck(true); + EXPECT_TRUE(_failsafe_flags.attitude_invalid) << "stale"; +} + +TEST_F(EstimatorChecksTest, AngularVelocityValidityFollowsTheSamples) +{ + vehicle_angular_velocity_s rates{}; + rates.timestamp = hrt_absolute_time(); + publishAngularVelocity(rates); + runCheck(true); + EXPECT_FALSE(_failsafe_flags.angular_velocity_invalid); + + rates.xyz[0] = NAN; + publishAngularVelocity(rates); + runCheck(true); + EXPECT_TRUE(_failsafe_flags.angular_velocity_invalid) << "not finite"; + + rates.xyz[0] = 0.f; + rates.timestamp = hrt_absolute_time() - 2_s; + publishAngularVelocity(rates); + runCheck(true); + EXPECT_TRUE(_failsafe_flags.angular_velocity_invalid) << "stale"; +} + +TEST_F(EstimatorChecksTest, LocalAltitudeIsInvalidWithoutAValidHeight) +{ + publishLocalPosition(true); + runCheck(true); + EXPECT_FALSE(_failsafe_flags.local_altitude_invalid); + + vehicle_local_position_s lpos{}; + lpos.timestamp = hrt_absolute_time(); + lpos.xy_valid = true; + lpos.v_xy_valid = true; + lpos.z_valid = false; + _local_position_pub.publish(lpos); + runCheck(true); + EXPECT_TRUE(_failsafe_flags.local_altitude_invalid); +}