diff --git a/msg/EstimatorStatus.msg b/msg/EstimatorStatus.msg index 55c996621e..937fa34f63 100644 --- a/msg/EstimatorStatus.msg +++ b/msg/EstimatorStatus.msg @@ -113,12 +113,6 @@ uint8 reset_count_quat # number of quaternion reset events (allow to wrap if c float32 time_slip # cumulative amount of time in seconds that the EKF inertial calculation has slipped relative to system time -bool pre_flt_fail_innov_heading -bool pre_flt_fail_innov_vel_horiz -bool pre_flt_fail_innov_vel_vert -bool pre_flt_fail_innov_height -bool pre_flt_fail_mag_field_disturbed - uint32 accel_device_id uint32 gyro_device_id uint32 baro_device_id diff --git a/msg/EstimatorStatusFlags.msg b/msg/EstimatorStatusFlags.msg index 2905011582..a613f9e3e6 100644 --- a/msg/EstimatorStatusFlags.msg +++ b/msg/EstimatorStatusFlags.msg @@ -73,3 +73,10 @@ bool reject_sideslip # 9 - true if the synthetic sideslip bool reject_hagl # 10 - true if the height above ground observation has been rejected bool reject_optflow_x # 11 - true if the X optical flow observation has been rejected bool reject_optflow_y # 12 - true if the Y optical flow observation has been rejected + +# preflight failure checks +bool pre_flt_fail_innov_heading +bool pre_flt_fail_innov_vel_horiz +bool pre_flt_fail_innov_vel_vert +bool pre_flt_fail_innov_pos_horiz +bool pre_flt_fail_innov_height diff --git a/src/lib/mathlib/math/filter/AlphaFilter.hpp b/src/lib/mathlib/math/filter/AlphaFilter.hpp index 0635ae438c..86f4a8060d 100644 --- a/src/lib/mathlib/math/filter/AlphaFilter.hpp +++ b/src/lib/mathlib/math/filter/AlphaFilter.hpp @@ -52,7 +52,7 @@ class AlphaFilter { public: AlphaFilter() = default; - explicit AlphaFilter(float alpha) : _alpha(alpha) {} + AlphaFilter(float alpha) : _alpha(alpha) {} ~AlphaFilter() = default; @@ -99,7 +99,7 @@ public: * * @param sample new initial value */ - void reset(const T &sample) { _filter_state = sample; } + void reset(const T &sample = {}) { _filter_state = sample; } /** * Add a new raw value to the filter diff --git a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp index 53a155feb5..5e5246cf61 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp @@ -88,14 +88,14 @@ void EstimatorChecks::checkAndReport(const Context &context, Report &reporter) } if (!missing_data) { - estimator_status_s estimator_status; + estimator_status_flags_s estimator_status_flags; - 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; + if (_estimator_status_flags_sub.copy(&estimator_status_flags)) { + pre_flt_fail_innov_heading = estimator_status_flags.pre_flt_fail_innov_heading; + pre_flt_fail_innov_vel_horiz = estimator_status_flags.pre_flt_fail_innov_vel_horiz; - checkEstimatorStatus(context, reporter, estimator_status, required_groups); - checkEstimatorStatusFlags(context, reporter, estimator_status, lpos); + checkEstimatorStatus(context, reporter, lpos, required_groups); + checkEstimatorStatusFlags(context, reporter, estimator_status_flags, required_groups); } else { missing_data = true; @@ -129,327 +129,261 @@ 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) + const vehicle_local_position_s &lpos, NavModes required_groups) { - if (!context.isArmed() && estimator_status.pre_flt_fail_innov_heading) { - /* EVENT - */ - reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate, - events::ID("check_estimator_heading_not_stable"), - events::Log::Error, "Heading estimate not stable"); + estimator_status_s estimator_status; - if (reporter.mavlink_log_pub()) { - mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: heading estimate not stable"); + if (_estimator_status_sub.copy(&estimator_status)) { + + // check vertical position innovation test ratio + if (!context.isArmed() && (estimator_status.hgt_test_ratio > _param_com_arm_ekf_hgt.get())) { + /* EVENT + * @description + * + * Test ratio: {1:.3}, limit: {2:.3}. + * + * This check can be configured via COM_ARM_EKF_HGT parameter. + * + */ + reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate, + events::ID("check_estimator_hgt_est_err"), + events::Log::Error, "Height estimate error", estimator_status.hgt_test_ratio, _param_com_arm_ekf_hgt.get()); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: height estimate error"); + } } - } else if (!context.isArmed() && estimator_status.pre_flt_fail_innov_vel_horiz) { - /* EVENT - */ - reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate, - events::ID("check_estimator_hor_vel_not_stable"), - events::Log::Error, "Horizontal velocity unstable"); + // check velocity innovation test ratio + if (!context.isArmed() && (estimator_status.vel_test_ratio > _param_com_arm_ekf_vel.get())) { + /* EVENT + * @description + * + * Test ratio: {1:.3}, limit: {2:.3}. + * + * This check can be configured via COM_ARM_EKF_VEL parameter. + * + */ + reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate, + events::ID("check_estimator_vel_est_err"), + events::Log::Error, "Velocity estimate error", estimator_status.vel_test_ratio, _param_com_arm_ekf_vel.get()); - if (reporter.mavlink_log_pub()) { - mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: horizontal velocity unstable"); + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: velocity estimate error"); + } } - } else if (!context.isArmed() && estimator_status.pre_flt_fail_innov_vel_vert) { - /* EVENT - */ - reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate, - events::ID("check_estimator_vert_vel_not_stable"), - events::Log::Error, "Vertical velocity unstable"); + // check horizontal position innovation test ratio + if (!context.isArmed() && (estimator_status.pos_test_ratio > _param_com_arm_ekf_pos.get())) { + /* EVENT + * @description + * + * Test ratio: {1:.3}, limit: {2:.3}. + * + * This check can be configured via COM_ARM_EKF_POS parameter. + * + */ + reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate, + events::ID("check_estimator_pos_est_err"), + events::Log::Error, "Position estimate error", estimator_status.pos_test_ratio, _param_com_arm_ekf_pos.get()); - if (reporter.mavlink_log_pub()) { - mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: vertical velocity unstable"); + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: position estimate error"); + } } - } else if (!context.isArmed() && estimator_status.pre_flt_fail_innov_height) { - /* EVENT - */ - reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate, - events::ID("check_estimator_hgt_not_stable"), - events::Log::Error, "Height estimate not stable"); + // check magnetometer innovation test ratio + if (!context.isArmed() && (estimator_status.mag_test_ratio > _param_com_arm_ekf_yaw.get())) { + /* EVENT + * @description + * + * Test ratio: {1:.3}, limit: {2:.3}. + * + * This check can be configured via COM_ARM_EKF_YAW parameter. + * + */ + reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate, + events::ID("check_estimator_yaw_est_err"), + events::Log::Error, "Yaw estimate error", estimator_status.mag_test_ratio, _param_com_arm_ekf_yaw.get()); - if (reporter.mavlink_log_pub()) { - mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: height estimate not stable"); - } - } - - - if ((_param_com_arm_mag_str.get() >= 1) - && (!context.isArmed() && estimator_status.pre_flt_fail_mag_field_disturbed)) { - - NavModes required_groups_mag = required_groups; - - if (_param_com_arm_mag_str.get() != 1) { - required_groups_mag = NavModes::None; // optional + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Yaw estimate error"); + } } - /* EVENT - * @description - * - * 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"); + // If GPS aiding is required, declare fault condition if the required GPS quality checks are failing + if (!context.isArmed() && _param_sys_has_gps.get()) { + const bool ekf_gps_fusion = estimator_status.control_mode_flags & (1 << estimator_status_s::CS_GPS); + const bool ekf_gps_check_fail = estimator_status.gps_check_fail_flags > 0; - if (reporter.mavlink_log_pub()) { - mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Strong magnetic interference"); - } - } - - // check vertical position innovation test ratio - if (!context.isArmed() && (estimator_status.hgt_test_ratio > _param_com_arm_ekf_hgt.get())) { - /* EVENT - * @description - * - * Test ratio: {1:.3}, limit: {2:.3}. - * - * This check can be configured via COM_ARM_EKF_HGT parameter. - * - */ - reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate, - events::ID("check_estimator_hgt_est_err"), - events::Log::Error, "Height estimate error", estimator_status.hgt_test_ratio, _param_com_arm_ekf_hgt.get()); - - if (reporter.mavlink_log_pub()) { - mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: height estimate error"); - } - } - - // check velocity innovation test ratio - if (!context.isArmed() && (estimator_status.vel_test_ratio > _param_com_arm_ekf_vel.get())) { - /* EVENT - * @description - * - * Test ratio: {1:.3}, limit: {2:.3}. - * - * This check can be configured via COM_ARM_EKF_VEL parameter. - * - */ - reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate, - events::ID("check_estimator_vel_est_err"), - events::Log::Error, "Velocity estimate error", estimator_status.vel_test_ratio, _param_com_arm_ekf_vel.get()); - - if (reporter.mavlink_log_pub()) { - mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: velocity estimate error"); - } - } - - // check horizontal position innovation test ratio - if (!context.isArmed() && (estimator_status.pos_test_ratio > _param_com_arm_ekf_pos.get())) { - /* EVENT - * @description - * - * Test ratio: {1:.3}, limit: {2:.3}. - * - * This check can be configured via COM_ARM_EKF_POS parameter. - * - */ - reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate, - events::ID("check_estimator_pos_est_err"), - events::Log::Error, "Position estimate error", estimator_status.pos_test_ratio, _param_com_arm_ekf_pos.get()); - - if (reporter.mavlink_log_pub()) { - mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: position estimate error"); - } - } - - // check magnetometer innovation test ratio - if (!context.isArmed() && (estimator_status.mag_test_ratio > _param_com_arm_ekf_yaw.get())) { - /* EVENT - * @description - * - * Test ratio: {1:.3}, limit: {2:.3}. - * - * This check can be configured via COM_ARM_EKF_YAW parameter. - * - */ - reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate, - events::ID("check_estimator_yaw_est_err"), - events::Log::Error, "Yaw estimate error", estimator_status.mag_test_ratio, _param_com_arm_ekf_yaw.get()); - - if (reporter.mavlink_log_pub()) { - mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Yaw estimate error"); - } - } - - // If GPS aiding is required, declare fault condition if the required GPS quality checks are failing - if (!context.isArmed() && _param_sys_has_gps.get()) { - const bool ekf_gps_fusion = estimator_status.control_mode_flags & (1 << estimator_status_s::CS_GPS); - const bool ekf_gps_check_fail = estimator_status.gps_check_fail_flags > 0; - - if (ekf_gps_fusion) { - reporter.setIsPresent(health_component_t::gps); // should be based on the sensor data directly - } - - if (ekf_gps_check_fail) { - NavModes required_groups_gps = required_groups; - events::Log log_level = events::Log::Error; - - if (_param_com_arm_wo_gps.get()) { - required_groups_gps = NavModes::None; // optional - log_level = events::Log::Warning; + if (ekf_gps_fusion) { + reporter.setIsPresent(health_component_t::gps); // should be based on the sensor data directly } - // Only report the first failure to avoid spamming - const char *message = nullptr; + if (ekf_gps_check_fail) { + NavModes required_groups_gps = required_groups; + events::Log log_level = events::Log::Error; - 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 - * - * This check can be configured via EKF2_GPS_CHECK parameter. - * - */ - reporter.armingCheckFailure(required_groups_gps, 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 - * - * This check can be configured via EKF2_GPS_CHECK parameter. - * - */ - reporter.armingCheckFailure(required_groups_gps, 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 - * - * This check can be configured via EKF2_GPS_CHECK parameter. - * - */ - reporter.armingCheckFailure(required_groups_gps, 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 - * - * This check can be configured via EKF2_GPS_CHECK parameter. - * - */ - reporter.armingCheckFailure(required_groups_gps, 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 - * - * This check can be configured via EKF2_GPS_CHECK parameter. - * - */ - reporter.armingCheckFailure(required_groups_gps, 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 - * - * This check can be configured via EKF2_GPS_CHECK parameter. - * - */ - reporter.armingCheckFailure(required_groups_gps, 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 - * - * This check can be configured via EKF2_GPS_CHECK parameter. - * - */ - reporter.armingCheckFailure(required_groups_gps, 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 - * - * This check can be configured via EKF2_GPS_CHECK parameter. - * - */ - reporter.armingCheckFailure(required_groups_gps, 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 - * - * This check can be configured via EKF2_GPS_CHECK parameter. - * - */ - reporter.armingCheckFailure(required_groups_gps, 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 - * - * This check can be configured via EKF2_GPS_CHECK parameter. - * - */ - reporter.armingCheckFailure(required_groups_gps, health_component_t::gps, - events::ID("check_estimator_gps_vert_speed_drift_too_high"), - log_level, "GPS Vertical Speed Drift too high"); - - } else { - if (!ekf_gps_fusion) { - // Likely cause unknown - message = "Preflight%s: Estimator not using GPS"; - /* EVENT - */ - reporter.armingCheckFailure(required_groups_gps, 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_groups_gps, health_component_t::gps, - events::ID("check_estimator_gps_generic"), - log_level, "Poor GPS Quality"); + if (_param_com_arm_wo_gps.get()) { + required_groups_gps = NavModes::None; // optional + log_level = events::Log::Warning; } - } - if (message && reporter.mavlink_log_pub()) { - if (!_param_com_arm_wo_gps.get()) { - mavlink_log_critical(reporter.mavlink_log_pub(), message, " Fail"); + // 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 + * + * This check can be configured via EKF2_GPS_CHECK parameter. + * + */ + reporter.armingCheckFailure(required_groups_gps, 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 + * + * This check can be configured via EKF2_GPS_CHECK parameter. + * + */ + reporter.armingCheckFailure(required_groups_gps, 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 + * + * This check can be configured via EKF2_GPS_CHECK parameter. + * + */ + reporter.armingCheckFailure(required_groups_gps, 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 + * + * This check can be configured via EKF2_GPS_CHECK parameter. + * + */ + reporter.armingCheckFailure(required_groups_gps, 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 + * + * This check can be configured via EKF2_GPS_CHECK parameter. + * + */ + reporter.armingCheckFailure(required_groups_gps, 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 + * + * This check can be configured via EKF2_GPS_CHECK parameter. + * + */ + reporter.armingCheckFailure(required_groups_gps, 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 + * + * This check can be configured via EKF2_GPS_CHECK parameter. + * + */ + reporter.armingCheckFailure(required_groups_gps, 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 + * + * This check can be configured via EKF2_GPS_CHECK parameter. + * + */ + reporter.armingCheckFailure(required_groups_gps, 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 + * + * This check can be configured via EKF2_GPS_CHECK parameter. + * + */ + reporter.armingCheckFailure(required_groups_gps, 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 + * + * This check can be configured via EKF2_GPS_CHECK parameter. + * + */ + reporter.armingCheckFailure(required_groups_gps, health_component_t::gps, + events::ID("check_estimator_gps_vert_speed_drift_too_high"), + log_level, "GPS Vertical Speed Drift too high"); } else { - mavlink_log_critical(reporter.mavlink_log_pub(), message, ""); + if (!ekf_gps_fusion) { + // Likely cause unknown + message = "Preflight%s: Estimator not using GPS"; + /* EVENT + */ + reporter.armingCheckFailure(required_groups_gps, 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_groups_gps, health_component_t::gps, + events::ID("check_estimator_gps_generic"), + log_level, "Poor GPS Quality"); + } + } + + if (message && reporter.mavlink_log_pub()) { + if (!_param_com_arm_wo_gps.get()) { + mavlink_log_critical(reporter.mavlink_log_pub(), message, " Fail"); + + } else { + mavlink_log_critical(reporter.mavlink_log_pub(), message, ""); + } } } } } - } void EstimatorChecks::checkSensorBias(const Context &context, Report &reporter, NavModes required_groups) @@ -530,111 +464,117 @@ void EstimatorChecks::checkSensorBias(const Context &context, Report &reporter, } void EstimatorChecks::checkEstimatorStatusFlags(const Context &context, Report &reporter, - const estimator_status_s &estimator_status, const vehicle_local_position_s &lpos) + const estimator_status_flags_s &estimator_status_flags, NavModes required_groups) { - estimator_status_flags_s estimator_status_flags; + if (!context.isArmed() && estimator_status_flags.pre_flt_fail_innov_heading) { + /* EVENT + */ + reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate, + events::ID("check_estimator_heading_not_stable"), + events::Log::Error, "Heading estimate not stable"); - if (_estimator_status_flags_sub.copy(&estimator_status_flags)) { - - bool dead_reckoning = estimator_status_flags.cs_wind_dead_reckoning - || estimator_status_flags.cs_inertial_dead_reckoning; - - if (!dead_reckoning) { - // position requirements (update if not dead reckoning) - bool gps = estimator_status_flags.cs_gps; - bool optical_flow = estimator_status_flags.cs_opt_flow; - bool vision_position = estimator_status_flags.cs_ev_pos; - - _position_reliant_on_optical_flow = !gps && optical_flow && !vision_position; + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: heading estimate not stable"); } - // Check for a magnetomer 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"); + } else if (!context.isArmed() && estimator_status_flags.pre_flt_fail_innov_vel_horiz) { + /* EVENT + */ + reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate, + events::ID("check_estimator_hor_vel_not_stable"), + events::Log::Error, "Horizontal velocity unstable"); - if (reporter.mavlink_log_pub()) { - mavlink_log_critical(reporter.mavlink_log_pub(), "Compass needs calibration - Land now!\t"); - } + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: horizontal velocity unstable"); } - if (estimator_status_flags.cs_gps_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"); + } else if (!context.isArmed() && estimator_status_flags.pre_flt_fail_innov_vel_vert) { + /* EVENT + */ + reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate, + events::ID("check_estimator_vert_vel_not_stable"), + events::Log::Error, "Vertical velocity unstable"); - 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(), "Preflight Fail: vertical velocity unstable"); + } + + } else if (!context.isArmed() && estimator_status_flags.pre_flt_fail_innov_height) { + /* EVENT + */ + reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate, + events::ID("check_estimator_hgt_not_stable"), + events::Log::Error, "Height estimate not stable"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: height estimate not stable"); } } - const hrt_abstime now = hrt_absolute_time(); - /* Check estimator status for signs of bad yaw induced post takeoff navigation failure - * for a short time interval after takeoff. - * Most of the time, the drone can recover from a bad initial yaw using GPS-inertial - * heading estimation (yaw emergency estimator) or GPS heading (fixed wings only), but - * if this does not fix the issue we need to stop using a position controlled - * mode to prevent flyaway crashes. - */ + if ((_param_com_arm_mag_str.get() >= 1) + && (!context.isArmed() && estimator_status_flags.cs_mag_field_disturbed)) { - if (context.status().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + NavModes required_groups_mag = required_groups; - if (!context.isArmed()) { - _nav_test_failed = false; - _nav_test_passed = false; + if (_param_com_arm_mag_str.get() != 1) { + required_groups_mag = NavModes::None; // optional + } - } else { - if (!_nav_test_passed) { - // Both test ratios need to pass/fail together to change the nav test status - const bool innovation_pass = (estimator_status.vel_test_ratio < 1.f) && (estimator_status.pos_test_ratio < 1.f) - && (estimator_status.vel_test_ratio > FLT_EPSILON) && (estimator_status.pos_test_ratio > FLT_EPSILON); + /* EVENT + * @description + * + * 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"); - const bool innovation_fail = (estimator_status.vel_test_ratio >= 1.f) && (estimator_status.pos_test_ratio >= 1.f); + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Strong magnetic interference"); + } + } - if (innovation_pass) { - _time_last_innov_pass = now; - // if nav status is unconfirmed, confirm yaw angle as passed after 30 seconds or achieving 5 m/s of speed - const bool sufficient_time = (context.status().takeoff_time != 0) && (now > context.status().takeoff_time + 30_s); - const bool sufficient_speed = matrix::Vector2f(lpos.vx, lpos.vy).longerThan(5.f); + bool dead_reckoning = estimator_status_flags.cs_wind_dead_reckoning + || estimator_status_flags.cs_inertial_dead_reckoning; - // Even if the test already failed, allow it to pass if it did not fail during the last 10 seconds - if ((now > _time_last_innov_fail + 10_s) && (sufficient_time || sufficient_speed)) { - _nav_test_passed = true; - _nav_test_failed = false; - } + if (!dead_reckoning) { + // position requirements (update if not dead reckoning) + bool gps = estimator_status_flags.cs_gps; + bool optical_flow = estimator_status_flags.cs_opt_flow; + bool vision_position = estimator_status_flags.cs_ev_pos; - } else if (innovation_fail) { - _time_last_innov_fail = now; + _position_reliant_on_optical_flow = !gps && optical_flow && !vision_position; + } - if (now > _time_last_innov_pass + 2_s) { - // if the innovation test has failed continuously, declare the nav as failed - _nav_test_failed = true; - /* EVENT - * @description - * Land and recalibrate the sensors. - */ - reporter.healthFailure(NavModes::All, health_component_t::local_position_estimate, - events::ID("check_estimator_nav_failure"), - events::Log::Emergency, "Navigation failure"); + // Check for a magnetomer 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(), "Navigation failure! Land and recalibrate sensors\t"); - } - } - } - } + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Compass needs calibration - Land now!\t"); + } + } + + if (estimator_status_flags.cs_gps_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"); } } } @@ -693,8 +633,8 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f lpos_eph_threshold_relaxed = INFINITY; } - bool xy_valid = lpos.xy_valid && !_nav_test_failed; - bool v_xy_valid = lpos.v_xy_valid && !_nav_test_failed; + bool xy_valid = lpos.xy_valid; + bool v_xy_valid = lpos.v_xy_valid; if (!context.isArmed()) { if (pre_flt_fail_innov_heading || pre_flt_fail_innov_vel_horiz) { diff --git a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp index f591cf03da..8d748649e1 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp @@ -59,11 +59,11 @@ public: void checkAndReport(const Context &context, Report &reporter) override; private: - void checkEstimatorStatus(const Context &context, Report &reporter, const estimator_status_s &estimator_status, + void checkEstimatorStatus(const Context &context, Report &reporter, const vehicle_local_position_s &lpos, NavModes required_groups); 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); + void checkEstimatorStatusFlags(const Context &context, Report &reporter, + const estimator_status_flags_s &estimator_status_flags, NavModes required_groups); void checkGps(const Context &context, Report &reporter, const sensor_gps_s &vehicle_gps_position) const; void gpsNoLongerValid(const Context &context, Report &reporter) const; @@ -93,12 +93,6 @@ private: hrt_abstime _last_lpos_relaxed_fail_time_us{0}; ///< Last time that the relaxed local position validity recovery check failed (usec) hrt_abstime _last_lvel_fail_time_us{0}; ///< Last time that the local velocity validity recovery check failed (usec) - // variables used to check for navigation failure after takeoff - hrt_abstime _time_last_innov_pass{0}; ///< last time velocity and position innovations passed - hrt_abstime _time_last_innov_fail{0}; ///< last time velocity and position innovations failed - bool _nav_test_passed{false}; ///< true if the post takeoff navigation test has passed - bool _nav_test_failed{false}; ///< true if the post takeoff navigation test has failed - static constexpr hrt_abstime GPS_VALID_TIME{3_s}; systemlib::Hysteresis _vehicle_gps_position_valid{false}; diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index 19ce16f50d..b95d5692ad 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2015-2020 PX4 Development Team. All rights reserved. +# Copyright (c) 2015-2022 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -31,8 +31,6 @@ # ############################################################################# -add_subdirectory(Utility) - # Symforce code generation TODO:fixme # execute_process( # COMMAND ${PYTHON_EXECUTABLE} -m symforce.symbolic @@ -126,7 +124,6 @@ px4_add_module( geo hysteresis perf - EKF2Utility px4_work_queue world_magnetic_model UNITY_BUILD diff --git a/src/modules/ekf2/EKF/baro_height_control.cpp b/src/modules/ekf2/EKF/baro_height_control.cpp index 8b77b8236b..17e305b8be 100644 --- a/src/modules/ekf2/EKF/baro_height_control.cpp +++ b/src/modules/ekf2/EKF/baro_height_control.cpp @@ -80,6 +80,11 @@ void Ekf::controlBaroHeightFusion() innov_gate, aid_src); + // filtered innovation for preflight checks + if (!aid_src.innovation_rejected) { + _baro_hgt_innov_lpf.update(aid_src.innovation); + } + // Compensate for positive static pressure transients (negative vertical position innovations) // caused by rotor wash ground interaction by applying a temporary deadzone to baro innovations. if (_control_status.flags.gnd_effect && (_params.gnd_effect_deadzone > 0.f)) { diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 92a3034b15..e23f9a3cdc 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -117,6 +117,25 @@ public: _time_last_in_air = _imu_sample_delayed.time_us; } + // reset all preflight innovation filters on takeoff or landing + if (_control_status.flags.in_air != in_air) { + _baro_hgt_innov_lpf.reset(); + _gnss_hgt_innov_lpf.reset(); + _rng_hgt_innov_lpf.reset(); + _ev_hgt_innov_lpf.reset(); + + _mag_heading_innov_lpf.reset(); + _gnss_yaw_innov_lpf.reset(); + _ev_yaw_innov_lpf.reset(); + + _ev_pos_innov_lpf.reset(); + _gnss_pos_innov_lpf.reset(); + + _gnss_vel_innov_lpf.reset(); + _ev_vel_innov_lpf.reset(); + _optical_flow_vel_innov_lpf.reset(); + } + _control_status.flags.in_air = in_air; } @@ -268,6 +287,97 @@ public: float gps_vertical_position_drift_rate_m_s() const { return _gps_vertical_position_drift_rate_m_s; } float gps_filtered_horizontal_velocity_m_s() const { return _gps_filtered_horizontal_velocity_m_s; } + float filteredHorizontalVelocityInnovation() const + { + float innov = 0.f; + + if (_control_status.flags.gps) { + innov = math::max(innov, _gnss_vel_innov_lpf.getState().xy().norm()); + } + + if (_control_status.flags.ev_vel) { + innov = math::max(innov, _ev_vel_innov_lpf.getState().xy().norm()); + } + + if (_control_status.flags.opt_flow) { + innov = math::max(innov, _optical_flow_vel_innov_lpf.getState().norm()); + } + + return innov; + } + + float filteredVerticalVelocityInnovation() const + { + float innov = 0.f; + + if (_control_status.flags.gps) { + innov = math::max(innov, _gnss_vel_innov_lpf.getState()(2)); + } + + if (_control_status.flags.ev_pos) { + innov = math::max(innov, _ev_vel_innov_lpf.getState()(2)); + } + + return innov; + } + + float filteredHorizontalPositionInnovation() const + { + float innov = 0.f; + + if (_control_status.flags.gps) { + innov = math::max(innov, _gnss_pos_innov_lpf.getState().norm()); + } + + if (_control_status.flags.ev_pos) { + innov = math::max(innov, _ev_pos_innov_lpf.getState().norm()); + } + + return innov; + } + + float filteredVerticalPositionInnovation() const + { + float innov = 0.f; + + if (_control_status.flags.baro_hgt) { + innov = math::max(innov, _baro_hgt_innov_lpf.getState()); + } + + if (_control_status.flags.gps_hgt) { + innov = math::max(innov, _gnss_hgt_innov_lpf.getState()); + } + + if (_control_status.flags.ev_hgt) { + innov = math::max(innov, _ev_hgt_innov_lpf.getState()); + } + + if (_control_status.flags.rng_hgt) { + innov = math::max(innov, _rng_hgt_innov_lpf.getState()); + } + + return innov; + } + + float filteredHeadingInnovation() const + { + float innov = 0.f; + + if (_control_status.flags.mag_hdg) { + innov = math::max(innov, _mag_heading_innov_lpf.getState()); + } + + if (_control_status.flags.ev_yaw) { + innov = math::max(innov, _ev_yaw_innov_lpf.getState()); + } + + if (_control_status.flags.gps_yaw) { + innov = math::max(innov, _gnss_yaw_innov_lpf.getState()); + } + + return innov; + } + protected: EstimatorInterface() = default; @@ -399,6 +509,25 @@ protected: warning_event_status_u _warning_events{}; information_event_status_u _information_events{}; + // Preflight low pass filter time constant inverse (1/sec) + static constexpr float INNOV_LPF_TAU_INV = 0.2f; + + AlphaFilter _baro_hgt_innov_lpf{INNOV_LPF_TAU_INV}; ///< filtered height innovations (m) + AlphaFilter _gnss_hgt_innov_lpf{INNOV_LPF_TAU_INV}; ///< filtered height innovations (m) + AlphaFilter _rng_hgt_innov_lpf{INNOV_LPF_TAU_INV}; ///< filtered height innovations (m) + AlphaFilter _ev_hgt_innov_lpf{INNOV_LPF_TAU_INV}; ///< filtered height innovations (m) + + AlphaFilter _mag_heading_innov_lpf{INNOV_LPF_TAU_INV}; ///< filtered heading innovations (rad) + AlphaFilter _gnss_yaw_innov_lpf{INNOV_LPF_TAU_INV}; ///< filtered yaw innovations (rad) + AlphaFilter _ev_yaw_innov_lpf{INNOV_LPF_TAU_INV}; ///< filtered yaw innovations (rad) + + AlphaFilter _ev_pos_innov_lpf{INNOV_LPF_TAU_INV}; ///< filtered position innovations (m) + AlphaFilter _gnss_pos_innov_lpf{INNOV_LPF_TAU_INV}; ///< filtered position innovations (m) + + AlphaFilter _ev_vel_innov_lpf{INNOV_LPF_TAU_INV}; ///< filtered velocity innovations (m/s) + AlphaFilter _gnss_vel_innov_lpf{INNOV_LPF_TAU_INV}; ///< filtered velocity innovations (m/s) + AlphaFilter _optical_flow_vel_innov_lpf{INNOV_LPF_TAU_INV}; ///< filtered velocity innovations (m/s) + private: inline void setDragData(const imuSample &imu); diff --git a/src/modules/ekf2/EKF/ev_height_control.cpp b/src/modules/ekf2/EKF/ev_height_control.cpp index f03204e394..31ac245f76 100644 --- a/src/modules/ekf2/EKF/ev_height_control.cpp +++ b/src/modules/ekf2/EKF/ev_height_control.cpp @@ -90,6 +90,11 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, const bool com math::max(_params.ev_pos_innov_gate, 1.f), aid_src); + // filtered innovation for preflight checks + if (!aid_src.innovation_rejected) { + _ev_hgt_innov_lpf.update(aid_src.innovation); + } + // update the bias estimator before updating the main filter but after // using its current state to compute the vertical position innovation if (measurement_valid && quality_sufficient) { diff --git a/src/modules/ekf2/EKF/ev_pos_control.cpp b/src/modules/ekf2/EKF/ev_pos_control.cpp index bdbbe034ff..593cf150cc 100644 --- a/src/modules/ekf2/EKF/ev_pos_control.cpp +++ b/src/modules/ekf2/EKF/ev_pos_control.cpp @@ -160,6 +160,11 @@ void Ekf::controlEvPosFusion(const extVisionSample &ev_sample, const bool common math::max(_params.ev_pos_innov_gate, 1.f), // innovation gate aid_src); + // filtered innovation for preflight checks + if (!aid_src.innovation_rejected) { + _ev_pos_innov_lpf.update(Vector2f(aid_src.innovation)); + } + // update the bias estimator before updating the main filter but after // using its current state to compute the vertical position innovation if (measurement_valid && quality_sufficient) { diff --git a/src/modules/ekf2/EKF/ev_vel_control.cpp b/src/modules/ekf2/EKF/ev_vel_control.cpp index f1bddeb03a..c821a1e1ca 100644 --- a/src/modules/ekf2/EKF/ev_vel_control.cpp +++ b/src/modules/ekf2/EKF/ev_vel_control.cpp @@ -130,6 +130,11 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common math::max(_params.ev_vel_innov_gate, 1.f), // innovation gate aid_src); + // filtered innovation for preflight checks + if (!aid_src.innovation_rejected) { + _ev_vel_innov_lpf.update(Vector3f(aid_src.innovation)); + } + if (!measurement_valid) { continuing_conditions_passing = false; } diff --git a/src/modules/ekf2/EKF/ev_yaw_control.cpp b/src/modules/ekf2/EKF/ev_yaw_control.cpp index 64e85cfedc..6973c51e32 100644 --- a/src/modules/ekf2/EKF/ev_yaw_control.cpp +++ b/src/modules/ekf2/EKF/ev_yaw_control.cpp @@ -67,6 +67,11 @@ void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common - wrap_pi(getEulerYaw(ev_sample.quat) - getEulerYaw(_ev_sample_prev.quat))); } + // filtered innovation for preflight checks + if (!aid_src.innovation_rejected) { + _ev_yaw_innov_lpf.update(aid_src.innovation); + } + const bool starting_conditions_passing = common_starting_conditions_passing && continuing_conditions_passing && isTimedOut(aid_src.time_last_fuse, (uint32_t)1e6); diff --git a/src/modules/ekf2/EKF/gnss_height_control.cpp b/src/modules/ekf2/EKF/gnss_height_control.cpp index 15e3669cf7..b4c5aff6c2 100644 --- a/src/modules/ekf2/EKF/gnss_height_control.cpp +++ b/src/modules/ekf2/EKF/gnss_height_control.cpp @@ -74,6 +74,11 @@ void Ekf::controlGnssHeightFusion(const gpsSample &gps_sample) innov_gate, aid_src); + // filtered innovation for preflight checks + if (!aid_src.innovation_rejected) { + _gnss_hgt_innov_lpf.update(aid_src.innovation); + } + const bool gps_checks_passing = isTimedOut(_last_gps_fail_us, (uint64_t)5e6); const bool gps_checks_failing = isTimedOut(_last_gps_pass_us, (uint64_t)5e6); diff --git a/src/modules/ekf2/EKF/gps_control.cpp b/src/modules/ekf2/EKF/gps_control.cpp index 481319023c..983c7137dc 100644 --- a/src/modules/ekf2/EKF/gps_control.cpp +++ b/src/modules/ekf2/EKF/gps_control.cpp @@ -67,6 +67,11 @@ void Ekf::controlGpsFusion() _aid_src_gnss_vel); _aid_src_gnss_vel.fusion_enabled = (_params.gnss_ctrl & GnssCtrl::VEL); + // filtered innovation for preflight checks + if (!_aid_src_gnss_vel.innovation_rejected) { + _gnss_vel_innov_lpf.update(Vector3f(_aid_src_gnss_vel.innovation)); + } + // GNSS position const Vector2f position{gps_sample.pos}; // relax the upper observation noise limit which prevents bad GPS perturbing the position estimate @@ -89,6 +94,11 @@ void Ekf::controlGpsFusion() _aid_src_gnss_pos); _aid_src_gnss_pos.fusion_enabled = (_params.gnss_ctrl & GnssCtrl::HPOS); + // filtered innovation for preflight checks + if (!_aid_src_gnss_pos.innovation_rejected) { + _gnss_pos_innov_lpf.update(Vector2f(_aid_src_gnss_pos.innovation)); + } + // if GPS is otherwise ready to go, but yaw_align is blocked by EV give mag a chance to start if (_control_status.flags.tilt_align && _NED_origin_initialised && gps_checks_passing && !gps_checks_failing) { diff --git a/src/modules/ekf2/EKF/gps_yaw_fusion.cpp b/src/modules/ekf2/EKF/gps_yaw_fusion.cpp index 61363ce683..e13a9c1919 100644 --- a/src/modules/ekf2/EKF/gps_yaw_fusion.cpp +++ b/src/modules/ekf2/EKF/gps_yaw_fusion.cpp @@ -139,6 +139,9 @@ void Ekf::fuseGpsYaw() if (is_fused) { _time_last_heading_fuse = _imu_sample_delayed.time_us; gnss_yaw.time_last_fuse = _imu_sample_delayed.time_us; + + // filtered innovation for preflight checks + _gnss_yaw_innov_lpf.update(gnss_yaw.innovation); } } diff --git a/src/modules/ekf2/EKF/mag_control.cpp b/src/modules/ekf2/EKF/mag_control.cpp index 31018211a2..b598bf8c53 100644 --- a/src/modules/ekf2/EKF/mag_control.cpp +++ b/src/modules/ekf2/EKF/mag_control.cpp @@ -430,6 +430,11 @@ void Ekf::runMagAndMagDeclFusions(const Vector3f &mag) _aid_src_mag_heading.fusion_enabled = _control_status.flags.mag_hdg; fuseYaw(innovation, obs_var, _aid_src_mag_heading); + + // filtered innovation for preflight checks + if (!_aid_src_mag_heading.innovation_rejected) { + _mag_heading_innov_lpf.update(_aid_src_mag_heading.innovation); + } } } diff --git a/src/modules/ekf2/EKF/optflow_fusion.cpp b/src/modules/ekf2/EKF/optflow_fusion.cpp index 402bfa3647..c5d74aee2a 100644 --- a/src/modules/ekf2/EKF/optflow_fusion.cpp +++ b/src/modules/ekf2/EKF/optflow_fusion.cpp @@ -115,6 +115,9 @@ void Ekf::fuseOptFlow() return; } + // filtered optical flow velocity innovation (for preflight checks) + _optical_flow_vel_innov_lpf.update(Vector2f(_state.vel(0) - _flow_vel_ne(0), _state.vel(1) - _flow_vel_ne(1))); + bool fused[2] {false, false}; // fuse observation axes sequentially diff --git a/src/modules/ekf2/EKF/range_height_control.cpp b/src/modules/ekf2/EKF/range_height_control.cpp index 4108098a8d..569f619d62 100644 --- a/src/modules/ekf2/EKF/range_height_control.cpp +++ b/src/modules/ekf2/EKF/range_height_control.cpp @@ -63,6 +63,11 @@ void Ekf::controlRangeHeightFusion() innov_gate, aid_src); + // filtered innovation for preflight checks + if (!aid_src.innovation_rejected) { + _rng_hgt_innov_lpf.update(aid_src.innovation); + } + // update the bias estimator before updating the main filter but after // using its current state to compute the vertical position innovation if (measurement_valid && _range_sensor.isDataHealthy()) { diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 02b10ed5be..51e9d1f22b 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1067,29 +1067,6 @@ void EKF2::PublishInnovations(const hrt_abstime ×tamp) innovations.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); _estimator_innovations_pub.publish(innovations); - - // calculate noise filtered velocity innovations which are used for pre-flight checking - if (_ekf.control_status_prev_flags().in_air != _ekf.control_status_flags().in_air) { - // fully reset on takeoff or landing - _preflt_checker.reset(); - } - - if (!_ekf.control_status_flags().in_air) { - // TODO: move to run before publications - _preflt_checker.setUsingGpsAiding(_ekf.control_status_flags().gps); - _preflt_checker.setUsingFlowAiding(_ekf.control_status_flags().opt_flow); - _preflt_checker.setUsingEvPosAiding(_ekf.control_status_flags().ev_pos); - _preflt_checker.setUsingEvVelAiding(_ekf.control_status_flags().ev_vel); - - _preflt_checker.setUsingBaroHgtAiding(_ekf.control_status_flags().baro_hgt); - _preflt_checker.setUsingGpsHgtAiding(_ekf.control_status_flags().gps_hgt); - _preflt_checker.setUsingRngHgtAiding(_ekf.control_status_flags().rng_hgt); - _preflt_checker.setUsingEvHgtAiding(_ekf.control_status_flags().ev_hgt); - - _preflt_checker.setVehicleCanObserveHeadingInFlight(_ekf.control_status_flags().fixed_wing); - - _preflt_checker.update(_ekf.get_imu_sample_delayed().delta_ang_dt, innovations); - } } void EKF2::PublishInnovationTestRatios(const hrt_abstime ×tamp) @@ -1389,12 +1366,6 @@ void EKF2::PublishStatus(const hrt_abstime ×tamp) status.time_slip = _last_time_slip_us * 1e-6f; - status.pre_flt_fail_innov_heading = _preflt_checker.hasHeadingFailed(); - status.pre_flt_fail_innov_vel_horiz = _preflt_checker.hasHorizVelFailed(); - status.pre_flt_fail_innov_vel_vert = _preflt_checker.hasVertVelFailed(); - status.pre_flt_fail_innov_height = _preflt_checker.hasHeightFailed(); - status.pre_flt_fail_mag_field_disturbed = _ekf.control_status_flags().mag_field_disturbed; - status.accel_device_id = _device_id_accel; status.baro_device_id = _device_id_baro; status.gyro_device_id = _device_id_gyro; @@ -1430,6 +1401,30 @@ void EKF2::PublishStatusFlags(const hrt_abstime ×tamp) _innov_check_fail_status_changes++; } + if (!_ekf.control_status_flags().in_air) { + + bool fail_vel_horiz = (_ekf.filteredHorizontalVelocityInnovation() > kVelocityInnovationTestLimit); + bool fail_vel_vert = (_ekf.filteredVerticalVelocityInnovation() > kVelocityInnovationTestLimit); + bool fail_pos_horiz = (_ekf.filteredHorizontalPositionInnovation() > kPositionInnovationTestLimit); + bool fail_height = (_ekf.filteredVerticalPositionInnovation() > kPositionInnovationTestLimit); + bool fail_heading = (_ekf.filteredHeadingInnovation() > kHeadingInnovationTestLimit); + + if (fail_vel_horiz != _pre_flt_fail_innov_vel_horiz + || fail_vel_vert != _pre_flt_fail_innov_vel_vert + || fail_pos_horiz != _pre_flt_fail_innov_pos_horiz + || fail_height != _pre_flt_fail_innov_height + || fail_heading != _pre_flt_fail_innov_heading + ) { + _pre_flt_fail_innov_vel_horiz = fail_vel_horiz; + _pre_flt_fail_innov_vel_vert = fail_vel_vert; + _pre_flt_fail_innov_pos_horiz = fail_pos_horiz; + _pre_flt_fail_innov_height = fail_height; + _pre_flt_fail_innov_heading = fail_heading; + + update = true; + } + } + if (update) { estimator_status_flags_s status_flags{}; status_flags.timestamp_sample = _ekf.get_imu_sample_delayed().time_us; @@ -1502,6 +1497,12 @@ void EKF2::PublishStatusFlags(const hrt_abstime ×tamp) status_flags.reject_optflow_x = _ekf.innov_check_fail_status_flags().reject_optflow_X; status_flags.reject_optflow_y = _ekf.innov_check_fail_status_flags().reject_optflow_Y; + status_flags.pre_flt_fail_innov_vel_horiz = _pre_flt_fail_innov_vel_horiz; + status_flags.pre_flt_fail_innov_vel_vert = _pre_flt_fail_innov_vel_vert; + status_flags.pre_flt_fail_innov_pos_horiz = _pre_flt_fail_innov_pos_horiz; + status_flags.pre_flt_fail_innov_height = _pre_flt_fail_innov_height; + status_flags.pre_flt_fail_innov_heading = _pre_flt_fail_innov_heading; + status_flags.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); _estimator_status_flags_pub.publish(status_flags); diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index b4db8e8f40..29145d72ce 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -42,7 +42,6 @@ #define EKF2_HPP #include "EKF/ekf.h" -#include "Utility/PreFlightChecker.hpp" #include "EKF2Selector.hpp" @@ -221,6 +220,20 @@ private: uint64_t _start_time_us = 0; ///< system time at EKF start (uSec) int64_t _last_time_slip_us = 0; ///< Last time slip (uSec) + // Maximum permissible velocity innovation to pass pre-flight checks (m/sec) + static constexpr float kVelocityInnovationTestLimit = 0.5f; + // Maximum permissible position innovation to pass pre-flight checks (m) + static constexpr float kPositionInnovationTestLimit = 1.5f; + // Maximum permissible yaw innovation to pass pre-flight checks when not aiding inertial nav using NE frame observations (rad) + static constexpr float kHeadingInnovationTestLimit = 0.5f; + + // preflight failure checks + bool _pre_flt_fail_innov_heading{false}; + bool _pre_flt_fail_innov_vel_horiz{false}; + bool _pre_flt_fail_innov_vel_vert{false}; + bool _pre_flt_fail_innov_pos_horiz{false}; + bool _pre_flt_fail_innov_height{false}; + perf_counter_t _ecl_ekf_update_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": ECL update")}; perf_counter_t _ecl_ekf_update_full_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": ECL full update")}; perf_counter_t _msg_missed_imu_perf{perf_alloc(PC_COUNT, MODULE_NAME": IMU message missed")}; @@ -386,9 +399,6 @@ private: uORB::PublicationMulti _odometry_pub; uORB::PublicationMulti _wind_pub; - - PreFlightChecker _preflt_checker; - Ekf _ekf; parameters *_params; ///< pointer to ekf parameter struct (located in _ekf class instance) diff --git a/src/modules/ekf2/Utility/CMakeLists.txt b/src/modules/ekf2/Utility/CMakeLists.txt deleted file mode 100644 index 7a635acebe..0000000000 --- a/src/modules/ekf2/Utility/CMakeLists.txt +++ /dev/null @@ -1,45 +0,0 @@ -############################################################################ -# -# Copyright (c) 2019 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################# - -px4_add_library(EKF2Utility - PreFlightChecker.cpp -) - -target_include_directories(EKF2Utility - PUBLIC - ${CMAKE_CURRENT_SOURCE_DIR} -) - -target_link_libraries(EKF2Utility PRIVATE mathlib) - -px4_add_unit_gtest(SRC PreFlightCheckerTest.cpp LINKLIBS EKF2Utility) diff --git a/src/modules/ekf2/Utility/InnovationLpf.hpp b/src/modules/ekf2/Utility/InnovationLpf.hpp deleted file mode 100644 index 89964b0d71..0000000000 --- a/src/modules/ekf2/Utility/InnovationLpf.hpp +++ /dev/null @@ -1,79 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2019 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * First order "alpha" IIR digital filter with input saturation - */ - -#include - -class InnovationLpf final -{ -public: - InnovationLpf() = default; - ~InnovationLpf() = default; - - void reset(float val = 0.f) { _x = val; } - - /** - * Update the filter with a new value and returns the filtered state - * The new value is constained by the limit set in setSpikeLimit - * @param val new input - * @param alpha normalized weight of the new input - * @param spike_limit the amplitude of the saturation at the input of the filter - * @return filtered output - */ - float update(float val, float alpha, float spike_limit) - { - float val_constrained = math::constrain(val, -spike_limit, spike_limit); - float beta = 1.f - alpha; - - _x = beta * _x + alpha * val_constrained; - - return _x; - } - - /** - * Helper function to compute alpha from dt and the inverse of tau - * @param dt sampling time in seconds - * @param tau_inv inverse of the time constant of the filter - * @return alpha, the normalized weight of a new measurement - */ - static float computeAlphaFromDtAndTauInv(float dt, float tau_inv) - { - return math::constrain(dt * tau_inv, 0.f, 1.f); - } - -private: - float _x{}; ///< current state of the filter -}; diff --git a/src/modules/ekf2/Utility/PreFlightChecker.cpp b/src/modules/ekf2/Utility/PreFlightChecker.cpp deleted file mode 100644 index 4a9369764d..0000000000 --- a/src/modules/ekf2/Utility/PreFlightChecker.cpp +++ /dev/null @@ -1,167 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2019 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file PreFlightCheckHelper.cpp - * Class handling the EKF2 innovation pre flight checks - */ - -#include "PreFlightChecker.hpp" - -void PreFlightChecker::update(const float dt, const estimator_innovations_s &innov) -{ - const float alpha = InnovationLpf::computeAlphaFromDtAndTauInv(dt, _innov_lpf_tau_inv); - - _has_heading_failed = preFlightCheckHeadingFailed(innov, alpha); - _has_horiz_vel_failed = preFlightCheckHorizVelFailed(innov, alpha); - _has_vert_vel_failed = preFlightCheckVertVelFailed(innov, alpha); - _has_height_failed = preFlightCheckHeightFailed(innov, alpha); -} - -bool PreFlightChecker::preFlightCheckHeadingFailed(const estimator_innovations_s &innov, const float alpha) -{ - const float heading_test_limit = selectHeadingTestLimit(); - const float heading_innov_spike_lim = 2.0f * heading_test_limit; - - const float heading_innov_lpf = _filter_heading_innov.update(innov.heading, alpha, heading_innov_spike_lim); - - return checkInnovFailed(heading_innov_lpf, innov.heading, heading_test_limit, heading_innov_spike_lim); -} - -float PreFlightChecker::selectHeadingTestLimit() -{ - // Select the max allowed heading innovaton depending on whether we are not aiding navigation using - // observations in the NE reference frame and if the vehicle can use GPS course to realign in flight (fixedwing sideslip fusion). - const bool is_ne_aiding = _is_using_gps_aiding || _is_using_ev_pos_aiding; - - return (is_ne_aiding && !_can_observe_heading_in_flight) - ? _nav_heading_innov_test_lim // more restrictive test limit - : _heading_innov_test_lim; // less restrictive test limit -} - -bool PreFlightChecker::preFlightCheckHorizVelFailed(const estimator_innovations_s &innov, const float alpha) -{ - bool has_failed = false; - - if (_is_using_gps_aiding || _is_using_ev_vel_aiding) { - const Vector2f vel_ne_innov = Vector2f(fmaxf(fabsf(innov.gps_hvel[0]), fabsf(innov.ev_hvel[0])), - fmaxf(fabsf(innov.gps_hvel[1]), fabsf(innov.ev_hvel[1]))); - Vector2f vel_ne_innov_lpf; - vel_ne_innov_lpf(0) = _filter_vel_n_innov.update(vel_ne_innov(0), alpha, _vel_innov_spike_lim); - vel_ne_innov_lpf(1) = _filter_vel_n_innov.update(vel_ne_innov(1), alpha, _vel_innov_spike_lim); - has_failed |= checkInnov2DFailed(vel_ne_innov_lpf, vel_ne_innov, _vel_innov_test_lim, _vel_innov_spike_lim); - } - - if (_is_using_flow_aiding) { - const Vector2f flow_innov = Vector2f(innov.flow); - Vector2f flow_innov_lpf; - flow_innov_lpf(0) = _filter_flow_x_innov.update(flow_innov(0), alpha, _flow_innov_spike_lim); - flow_innov_lpf(1) = _filter_flow_y_innov.update(flow_innov(1), alpha, _flow_innov_spike_lim); - has_failed |= checkInnov2DFailed(flow_innov_lpf, flow_innov, _flow_innov_test_lim, 5.f * _flow_innov_spike_lim); - } - - return has_failed; -} - -bool PreFlightChecker::preFlightCheckVertVelFailed(const estimator_innovations_s &innov, const float alpha) -{ - const float vel_d_innov = fmaxf(fabsf(innov.gps_vvel), fabs(innov.ev_vvel)); // only temporary solution - const float vel_d_innov_lpf = _filter_vel_d_innov.update(vel_d_innov, alpha, _vel_innov_spike_lim); - return checkInnovFailed(vel_d_innov_lpf, vel_d_innov, _vel_innov_test_lim, _vel_innov_spike_lim); -} - -bool PreFlightChecker::preFlightCheckHeightFailed(const estimator_innovations_s &innov, const float alpha) -{ - bool has_failed = false; - - if (_is_using_baro_hgt_aiding) { - const float baro_hgt_innov_lpf = _filter_baro_hgt_innov.update(innov.baro_vpos, alpha, _hgt_innov_spike_lim); - has_failed |= checkInnovFailed(baro_hgt_innov_lpf, innov.baro_vpos, _hgt_innov_test_lim, _hgt_innov_spike_lim); - } - - if (_is_using_gps_hgt_aiding) { - const float gps_hgt_innov_lpf = _filter_gps_hgt_innov.update(innov.gps_vpos, alpha, _hgt_innov_spike_lim); - has_failed |= checkInnovFailed(gps_hgt_innov_lpf, innov.gps_vpos, _hgt_innov_test_lim, _hgt_innov_spike_lim); - } - - if (_is_using_rng_hgt_aiding) { - const float rng_hgt_innov_lpf = _filter_rng_hgt_innov.update(innov.rng_vpos, alpha, _hgt_innov_spike_lim); - has_failed |= checkInnovFailed(rng_hgt_innov_lpf, innov.rng_vpos, _hgt_innov_test_lim, _hgt_innov_spike_lim); - } - - if (_is_using_ev_hgt_aiding) { - const float ev_hgt_innov_lpf = _filter_ev_hgt_innov.update(innov.ev_vpos, alpha, _hgt_innov_spike_lim); - has_failed |= checkInnovFailed(ev_hgt_innov_lpf, innov.ev_vpos, _hgt_innov_test_lim, _hgt_innov_spike_lim); - } - - return has_failed; -} - -bool PreFlightChecker::checkInnovFailed(const float innov_lpf, const float innov, const float test_limit, - const float spike_limit) -{ - return fabsf(innov_lpf) > test_limit || fabsf(innov) > spike_limit; -} - -bool PreFlightChecker::checkInnov2DFailed(const Vector2f &innov_lpf, const Vector2f &innov, const float test_limit, - const float spike_limit) -{ - return innov_lpf.norm_squared() > sq(test_limit) - || innov.norm_squared() > sq(spike_limit); -} - -void PreFlightChecker::reset() -{ - _is_using_gps_aiding = false; - _is_using_flow_aiding = false; - _is_using_ev_pos_aiding = false; - _is_using_ev_vel_aiding = false; - _is_using_baro_hgt_aiding = false; - _is_using_gps_hgt_aiding = false; - _is_using_rng_hgt_aiding = false; - _is_using_ev_hgt_aiding = false; - _has_heading_failed = false; - _has_horiz_vel_failed = false; - _has_vert_vel_failed = false; - _has_height_failed = false; - _filter_vel_n_innov.reset(); - _filter_vel_e_innov.reset(); - _filter_vel_d_innov.reset(); - _filter_baro_hgt_innov.reset(); - _filter_gps_hgt_innov.reset(); - _filter_rng_hgt_innov.reset(); - _filter_ev_hgt_innov.reset(); - _filter_heading_innov.reset(); - _filter_flow_x_innov.reset(); - _filter_flow_y_innov.reset(); -} diff --git a/src/modules/ekf2/Utility/PreFlightChecker.hpp b/src/modules/ekf2/Utility/PreFlightChecker.hpp deleted file mode 100644 index 10639bb915..0000000000 --- a/src/modules/ekf2/Utility/PreFlightChecker.hpp +++ /dev/null @@ -1,195 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2019 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file PreFlightChecker.hpp - * Class handling the EKF2 innovation pre flight checks - * - * First call the update(...) function and then get the results - * using the hasXxxFailed() getters - */ - -#ifndef EKF2_PREFLIGHTCHECKER_HPP -#define EKF2_PREFLIGHTCHECKER_HPP - -#include -#include - -#include - -#include "InnovationLpf.hpp" - -using matrix::Vector2f; - -class PreFlightChecker -{ -public: - PreFlightChecker() = default; - ~PreFlightChecker() = default; - - /* - * Reset all the internal states of the class to their default value - */ - void reset(); - - /* - * Update the internal states - * @param dt the sampling time - * @param innov the ekf2_innovation_s struct containing the current innovations - */ - void update(float dt, const estimator_innovations_s &innov); - - /* - * If set to true, the checker will use a less conservative heading innovation check - */ - void setVehicleCanObserveHeadingInFlight(bool val) { _can_observe_heading_in_flight = val; } - - void setUsingGpsAiding(bool val) { _is_using_gps_aiding = val; } - void setUsingFlowAiding(bool val) { _is_using_flow_aiding = val; } - void setUsingEvPosAiding(bool val) { _is_using_ev_pos_aiding = val; } - void setUsingEvVelAiding(bool val) { _is_using_ev_vel_aiding = val; } - - void setUsingBaroHgtAiding(bool val) { _is_using_baro_hgt_aiding = val; } - void setUsingGpsHgtAiding(bool val) { _is_using_gps_hgt_aiding = val; } - void setUsingRngHgtAiding(bool val) { _is_using_rng_hgt_aiding = val; } - void setUsingEvHgtAiding(bool val) { _is_using_ev_hgt_aiding = val; } - - bool hasHeadingFailed() const { return _has_heading_failed; } - bool hasHorizVelFailed() const { return _has_horiz_vel_failed; } - bool hasVertVelFailed() const { return _has_vert_vel_failed; } - bool hasHeightFailed() const { return _has_height_failed; } - - /* - * Overall state of the pre fligh checks - * @return true if any of the check failed - */ - bool hasFailed() const { return hasHorizFailed() || hasVertFailed(); } - - /* - * Horizontal checks overall result - * @return true if one of the horizontal checks failed - */ - bool hasHorizFailed() const { return _has_heading_failed || _has_horiz_vel_failed; } - - /* - * Vertical checks overall result - * @return true if one of the vertical checks failed - */ - bool hasVertFailed() const { return _has_vert_vel_failed || _has_height_failed; } - - /* - * Check if the innovation fails the test - * To pass the test, the following conditions should be true: - * innov_lpf <= test_limit - * innov <= spike_limit - * @param innov_lpf the low-pass filtered innovation - * @param innov the current unfiltered innovation - * @param test_limit the magnitude test limit for innov_lpf - * @param spike_limit the magnitude test limit for innov - * @return true if the check failed the test, false otherwise - */ - static bool checkInnovFailed(float innov_lpf, float innov, float test_limit, float spike_limit); - - /* - * Check if the a innovation of a 2D vector fails the test - * To pass the test, the following conditions should be true: - * innov_lpf <= test_limit - * innov <= spike_limit - * @param innov_lpf the low-pass filtered innovation - * @param innov the current unfiltered innovation - * @param test_limit the magnitude test limit for innov_lpf - * @param spike_limit the magnitude test limit for innov - * @return true if the check failed the test, false otherwise - */ - static bool checkInnov2DFailed(const Vector2f &innov_lpf, const Vector2f &innov, float test_limit, float spike_limit); - - static constexpr float sq(float var) { return var * var; } - -private: - bool preFlightCheckHeadingFailed(const estimator_innovations_s &innov, float alpha); - float selectHeadingTestLimit(); - - bool preFlightCheckHorizVelFailed(const estimator_innovations_s &innov, float alpha); - bool preFlightCheckVertVelFailed(const estimator_innovations_s &innov, float alpha); - bool preFlightCheckHeightFailed(const estimator_innovations_s &innov, float alpha); - - bool _has_heading_failed{}; - bool _has_horiz_vel_failed{}; - bool _has_vert_vel_failed{}; - bool _has_height_failed{}; - - bool _can_observe_heading_in_flight{}; - bool _is_using_gps_aiding{}; - bool _is_using_flow_aiding{}; - bool _is_using_ev_pos_aiding{}; - bool _is_using_ev_vel_aiding{}; - - bool _is_using_baro_hgt_aiding{}; - bool _is_using_gps_hgt_aiding{}; - bool _is_using_rng_hgt_aiding{}; - bool _is_using_ev_hgt_aiding{}; - - // Low-pass filters for innovation pre-flight checks - InnovationLpf _filter_vel_n_innov; ///< Preflight low pass filter N axis velocity innovations (m/sec) - InnovationLpf _filter_vel_e_innov; ///< Preflight low pass filter E axis velocity innovations (m/sec) - InnovationLpf _filter_vel_d_innov; ///< Preflight low pass filter D axis velocity innovations (m/sec) - InnovationLpf _filter_heading_innov; ///< Preflight low pass filter heading innovation magntitude (rad) - InnovationLpf _filter_flow_x_innov; ///< Preflight low pass filter optical flow innovation (rad) - InnovationLpf _filter_flow_y_innov; ///< Preflight low pass filter optical flow innovation (rad) - - // Preflight low pass filter height innovation (m) - InnovationLpf _filter_baro_hgt_innov; - InnovationLpf _filter_gps_hgt_innov; - InnovationLpf _filter_rng_hgt_innov; - InnovationLpf _filter_ev_hgt_innov; - - // Preflight low pass filter time constant inverse (1/sec) - static constexpr float _innov_lpf_tau_inv = 0.2f; - // Maximum permissible velocity innovation to pass pre-flight checks (m/sec) - static constexpr float _vel_innov_test_lim = 0.5f; - // Maximum permissible height innovation to pass pre-flight checks (m) - static constexpr float _hgt_innov_test_lim = 1.5f; - // Maximum permissible yaw innovation to pass pre-flight checks when aiding inertial nav using NE frame observations (rad) - static constexpr float _nav_heading_innov_test_lim = 0.25f; - // Maximum permissible yaw innovation to pass pre-flight checks when not aiding inertial nav using NE frame observations (rad) - static constexpr float _heading_innov_test_lim = 0.52f; - // Maximum permissible flow innovation to pass pre-flight checks - static constexpr float _flow_innov_test_lim = 0.25f; - // Preflight velocity innovation spike limit (m/sec) - static constexpr float _vel_innov_spike_lim = 2.0f * _vel_innov_test_lim; - // Preflight position innovation spike limit (m) - static constexpr float _hgt_innov_spike_lim = 2.0f * _hgt_innov_test_lim; - // Preflight flow innovation spike limit (rad) - static constexpr float _flow_innov_spike_lim = 2.0f * _flow_innov_test_lim; -}; -#endif // !EKF2_PREFLIGHTCHECKER_HPP diff --git a/src/modules/ekf2/Utility/PreFlightCheckerTest.cpp b/src/modules/ekf2/Utility/PreFlightCheckerTest.cpp deleted file mode 100644 index 16e1beddf5..0000000000 --- a/src/modules/ekf2/Utility/PreFlightCheckerTest.cpp +++ /dev/null @@ -1,97 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2019 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * Test code for PreFlightChecker class - * Run this test only using make tests TESTFILTER=PreFlightChecker - */ - -#include - -#include "PreFlightChecker.hpp" - -class PreFlightCheckerTest : public ::testing::Test -{ -}; - -TEST_F(PreFlightCheckerTest, testInnovFailed) -{ - const float test_limit = 1.0; ///< is the limit for innovation_lpf - const float spike_limit = 2.f * test_limit; ///< is the limit for innovation_lpf - const float innovations[9] = {0.0, 1.5, 2.5, -1.5, -2.5, 1.5, -1.5, -2.5, -2.5}; - const float innovations_lpf[9] = {0.0, 0.9, 0.9, -0.9, -0.9, 1.1, -1.1, -1.1, 1.1}; - const bool expected_result[9] = {false, false, true, false, true, true, true, true, true}; - - for (int i = 0; i < 9; i++) { - EXPECT_EQ(PreFlightChecker::checkInnovFailed(innovations_lpf[i], innovations[i], test_limit, spike_limit), - expected_result[i]); - } - - // Smaller test limit, all the checks should fail except the first - EXPECT_FALSE(PreFlightChecker::checkInnovFailed(innovations_lpf[0], innovations[0], 0.0, 0.0)); - - for (int i = 1; i < 9; i++) { - EXPECT_TRUE(PreFlightChecker::checkInnovFailed(innovations_lpf[i], innovations[i], 0.0, 0.0)); - } - - // Larger test limit, none of the checks should fail - for (int i = 0; i < 9; i++) { - EXPECT_FALSE(PreFlightChecker::checkInnovFailed(innovations_lpf[i], innovations[i], 2.0, 4.0)); - } -} - -TEST_F(PreFlightCheckerTest, testInnov2dFailed) -{ - const float test_limit = 1.0; - const float spike_limit = 2.0; - Vector2f innovations[4] = {{0.0, 0.0}, {0.0, 0.0}, {0.0, -2.5}, {1.5, -1.5}}; - Vector2f innovations_lpf[4] = {{0.0, 0.0}, {1.1, 0.0}, {0.5, 0.5}, {1.0, -1.0}}; - const bool expected_result[4] = {false, true, true, true}; - - for (int i = 0; i < 4; i++) { - EXPECT_EQ(PreFlightChecker::checkInnov2DFailed(innovations_lpf[i], innovations[i], test_limit, spike_limit), - expected_result[i]); - } - - // Smaller test limit, all the checks should fail except the first - EXPECT_FALSE(PreFlightChecker::checkInnov2DFailed(innovations[0], innovations_lpf[0], 0.0, 0.0)); - - for (int i = 1; i < 4; i++) { - EXPECT_TRUE(PreFlightChecker::checkInnov2DFailed(innovations_lpf[i], innovations[i], 0.0, 0.0)); - } - - // Larger test limit, none of the checks should fail - for (int i = 0; i < 4; i++) { - EXPECT_FALSE(PreFlightChecker::checkInnov2DFailed(innovations_lpf[i], innovations[i], 1.42, 2.84)); - } -}