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 2b32ba473ac3..9b169ae36005 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 @@ -335,7 +335,6 @@ *(.text._ZThn16_N7sensors22VehicleAngularVelocity3RunEv) *(.text._ZN29MavlinkStreamObstacleDistance4sendEv) *(.text._ZN24MavlinkStreamOrbitStatus4sendEv) -*(.text._ZN16PreFlightChecker26preFlightCheckHeightFailedERK23estimator_innovations_sf) *(.text._ZN9Navigator3runEv) *(.text._ZN24MavlinkParametersManager11send_paramsEv) *(.text._ZN17MavlinkLogHandler4sendEv) @@ -374,7 +373,6 @@ *(.text.imxrt_ioctl) *(.text._ZN3Ekf25checkMagBiasObservabilityEv) *(.text._ZN36MavlinkStreamGimbalDeviceSetAttitude4sendEv) -*(.text._ZN16PreFlightChecker6updateEfRK23estimator_innovations_s) *(.text._ZN4math13expo_deadzoneIfEEKT_RS2_S3_S3_.isra.0) *(.text._ZN19StickAccelerationXYC1EP12ModuleParams) *(.text.imxrt_epsubmit) @@ -458,7 +456,6 @@ *(.text._ZN14FlightTaskAuto17_evaluateTripletsEv) *(.text._ZN11calibration9Gyroscope23SensorCorrectionsUpdateEb) *(.text._ZN25MavlinkStreamMagCalReport4sendEv) -*(.text._ZN16PreFlightChecker27preFlightCheckHeadingFailedERK23estimator_innovations_sf) *(.text.imxrt_config_gpio) *(.text.nxsig_timeout) *(.text._ZN11RateControl19setSaturationStatusERKN6matrix7Vector3IbEES4_) @@ -593,7 +590,6 @@ *(.text.uart_xmitchars_done) *(.text._ZN4EKF225PublishYawEstimatorStatusERKy) *(.text.sin) -*(.text._ZN16PreFlightChecker27preFlightCheckVertVelFailedERK23estimator_innovations_sf) *(.text._ZN6Safety19safetyButtonHandlerEv) *(.text._ZN3Ekf19controlAuxVelFusionEv) *(.text._ZNK6matrix6MatrixIfLj2ELj1EEplERKS1_) @@ -667,10 +663,8 @@ *(.text._ZN26MavlinkStreamCameraTrigger8get_sizeEv) *(.text.iob_navail) *(.text._ZN12FailsafeBase25removeNonActivatedActionsEv) -*(.text._ZN16PreFlightChecker28preFlightCheckHorizVelFailedERK23estimator_innovations_sf) *(.text._ZN15TakeoffHandling10updateRampEff) *(.text._Z7led_offi) -*(.text._ZN16PreFlightChecker22selectHeadingTestLimitEv) *(.text.led_off) *(.text.udp_wrbuffer_test) *(.text._ZNK3Ekf34updateVerticalPositionAidSrcStatusERKyfffR24estimator_aid_source1d_s) diff --git a/msg/EstimatorStatus.msg b/msg/EstimatorStatus.msg index a603a0f5ad86..0b479b1545b4 100644 --- a/msg/EstimatorStatus.msg +++ b/msg/EstimatorStatus.msg @@ -102,9 +102,10 @@ 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_height +bool pre_flt_fail_innov_pos_horiz 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 diff --git a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp index d1bc06a74218..58f8e582c982 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp @@ -63,6 +63,7 @@ void EstimatorChecks::checkAndReport(const Context &context, Report &reporter) 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; const NavModes required_groups = (NavModes)reporter.failsafeFlags().mode_req_attitude; @@ -90,6 +91,7 @@ void EstimatorChecks::checkAndReport(const Context &context, Report &reporter) 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; checkEstimatorStatus(context, reporter, estimator_status, required_groups); checkEstimatorStatusFlags(context, reporter, estimator_status, lpos); @@ -123,7 +125,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, lpos, vehicle_gps_position, + setModeRequirementFlags(context, pre_flt_fail_innov_heading, pre_flt_fail_innov_vel_horiz, pre_flt_fail_innov_pos_horiz, + lpos, vehicle_gps_position, reporter.failsafeFlags(), reporter); @@ -166,6 +169,17 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: vertical velocity unstable"); } + } else if (!context.isArmed() && estimator_status.pre_flt_fail_innov_pos_horiz) { + /* EVENT + */ + reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate, + events::ID("check_estimator_hor_pos_not_stable"), + events::Log::Error, "Horizontal position unstable"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: horizontal position unstable"); + } + } else if (!context.isArmed() && estimator_status.pre_flt_fail_innov_height) { /* EVENT */ @@ -208,82 +222,6 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor } } - // 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 (_param_sys_has_gps.get()) { const bool ekf_gps_fusion = estimator_status.control_mode_flags & (1 << estimator_status_s::CS_GPS); @@ -767,7 +705,7 @@ void EstimatorChecks::lowPositionAccuracy(const Context &context, Report &report } void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_flt_fail_innov_heading, - bool pre_flt_fail_innov_vel_horiz, + bool pre_flt_fail_innov_vel_horiz, bool pre_flt_fail_innov_pos_horiz, const vehicle_local_position_s &lpos, const sensor_gps_s &vehicle_gps_position, failsafe_flags_s &failsafe_flags, Report &reporter) { @@ -797,7 +735,7 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f bool v_xy_valid = lpos.v_xy_valid && !_nav_test_failed; if (!context.isArmed()) { - if (pre_flt_fail_innov_heading || pre_flt_fail_innov_vel_horiz) { + if (pre_flt_fail_innov_heading || pre_flt_fail_innov_pos_horiz) { xy_valid = false; } diff --git a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp index c900e95b8a0d..6578776ee99d 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp @@ -66,7 +66,9 @@ class EstimatorChecks : public HealthAndArmingCheckBase const vehicle_local_position_s &lpos); void checkGps(const Context &context, Report &reporter, const sensor_gps_s &vehicle_gps_position) const; void lowPositionAccuracy(const Context &context, Report &reporter, const vehicle_local_position_s &lpos) const; - void setModeRequirementFlags(const Context &context, bool pre_flt_fail_innov_heading, bool pre_flt_fail_innov_vel_horiz, + + 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 sensor_gps_s &vehicle_gps_position, failsafe_flags_s &failsafe_flags, Report &reporter); @@ -108,10 +110,6 @@ class EstimatorChecks : public HealthAndArmingCheckBase DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase, (ParamInt) _param_sens_imu_mode, (ParamInt) _param_com_arm_mag_str, - (ParamFloat) _param_com_arm_ekf_hgt, - (ParamFloat) _param_com_arm_ekf_vel, - (ParamFloat) _param_com_arm_ekf_pos, - (ParamFloat) _param_com_arm_ekf_yaw, (ParamBool) _param_com_arm_wo_gps, (ParamBool) _param_sys_has_gps, (ParamFloat) _param_com_pos_fs_eph, diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index e9d5f4de3acc..a462c8ae898c 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -358,50 +358,6 @@ PARAM_DEFINE_INT32(COM_OBL_RC_ACT, 0); */ PARAM_DEFINE_FLOAT(COM_OBC_LOSS_T, 5.0f); -/** - * Maximum EKF position innovation test ratio that will allow arming - * - * @group Commander - * @min 0.1 - * @max 1.0 - * @decimal 2 - * @increment 0.05 - */ -PARAM_DEFINE_FLOAT(COM_ARM_EKF_POS, 0.5f); - -/** - * Maximum EKF velocity innovation test ratio that will allow arming - * - * @group Commander - * @min 0.1 - * @max 1.0 - * @decimal 2 - * @increment 0.05 - */ -PARAM_DEFINE_FLOAT(COM_ARM_EKF_VEL, 0.5f); - -/** - * Maximum EKF height innovation test ratio that will allow arming - * - * @group Commander - * @min 0.1 - * @max 1.0 - * @decimal 2 - * @increment 0.05 - */ -PARAM_DEFINE_FLOAT(COM_ARM_EKF_HGT, 1.0f); - -/** - * Maximum EKF yaw innovation test ratio that will allow arming - * - * @group Commander - * @min 0.1 - * @max 1.0 - * @decimal 2 - * @increment 0.05 - */ -PARAM_DEFINE_FLOAT(COM_ARM_EKF_YAW, 0.5f); - /** * Maximum accelerometer inconsistency between IMU units that will allow arming * diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index c4d0d1162584..a71c1b06e611 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -31,8 +31,6 @@ # ############################################################################# -add_subdirectory(Utility) - option(EKF2_SYMFORCE_GEN "ekf2 generate symforce output" OFF) # Symforce code generation TODO:fixme @@ -254,7 +252,6 @@ px4_add_module( geo hysteresis perf - EKF2Utility px4_work_queue world_magnetic_model diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 7070c71bf363..9bdb436bc206 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -396,7 +396,8 @@ class Ekf final : public EstimatorInterface float getHeadingInnovationTestRatio() const; - float getVelocityInnovationTestRatio() const; + float getHorizontalVelocityInnovationTestRatio() const; + float getVerticalVelocityInnovationTestRatio() const; float getHorizontalPositionInnovationTestRatio() const; float getVerticalPositionInnovationTestRatio() const; diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 02675d1f746f..3c4503e25700 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -319,7 +319,7 @@ void Ekf::resetAccelBias() float Ekf::getHeadingInnovationTestRatio() const { // return the largest heading innovation test ratio - float test_ratio = 0.f; + float test_ratio = -1.f; #if defined(CONFIG_EKF2_MAGNETOMETER) @@ -347,10 +347,14 @@ float Ekf::getHeadingInnovationTestRatio() const #endif // CONFIG_EKF2_EXTERNAL_VISION - return sqrtf(test_ratio); + if (PX4_ISFINITE(test_ratio) && (test_ratio >= 0.f)) { + return sqrtf(test_ratio); + } + + return NAN; } -float Ekf::getVelocityInnovationTestRatio() const +float Ekf::getHorizontalVelocityInnovationTestRatio() const { // return the largest velocity innovation test ratio float test_ratio = -1.f; @@ -358,7 +362,7 @@ float Ekf::getVelocityInnovationTestRatio() const #if defined(CONFIG_EKF2_GNSS) if (_control_status.flags.gps) { - for (int i = 0; i < 3; i++) { + for (int i = 0; i < 2; i++) { // only xy test_ratio = math::max(test_ratio, fabsf(_aid_src_gnss_vel.test_ratio_filtered[i])); } } @@ -368,7 +372,7 @@ float Ekf::getVelocityInnovationTestRatio() const #if defined(CONFIG_EKF2_EXTERNAL_VISION) if (_control_status.flags.ev_vel) { - for (int i = 0; i < 3; i++) { + for (int i = 0; i < 2; i++) { // only xy test_ratio = math::max(test_ratio, fabsf(_aid_src_ev_vel.test_ratio_filtered[i])); } } @@ -392,6 +396,34 @@ float Ekf::getVelocityInnovationTestRatio() const return NAN; } +float Ekf::getVerticalVelocityInnovationTestRatio() const +{ + // return the largest velocity innovation test ratio + float test_ratio = -1.f; + +#if defined(CONFIG_EKF2_GNSS) + + if (_control_status.flags.gps) { + test_ratio = math::max(test_ratio, fabsf(_aid_src_gnss_vel.test_ratio_filtered[2])); + } + +#endif // CONFIG_EKF2_GNSS + +#if defined(CONFIG_EKF2_EXTERNAL_VISION) + + if (_control_status.flags.ev_vel) { + test_ratio = math::max(test_ratio, fabsf(_aid_src_ev_vel.test_ratio_filtered[2])); + } + +#endif // CONFIG_EKF2_EXTERNAL_VISION + + if (PX4_ISFINITE(test_ratio) && (test_ratio >= 0.f)) { + return sqrtf(test_ratio); + } + + return NAN; +} + float Ekf::getHorizontalPositionInnovationTestRatio() const { // return the largest position innovation test ratio @@ -511,21 +543,35 @@ float Ekf::getSyntheticSideslipInnovationTestRatio() const float Ekf::getHeightAboveGroundInnovationTestRatio() const { - float test_ratio = -1.f; + // return the combined HAGL innovation test ratio + float hagl_sum = 0.f; + int n_hagl_sources = 0; #if defined(CONFIG_EKF2_TERRAIN) + +# if defined(CONFIG_EKF2_OPTICAL_FLOW) + + if (_control_status.flags.opt_flow_terrain) { + hagl_sum += sqrtf(math::max(fabsf(_aid_src_optical_flow.test_ratio_filtered[0]), + _aid_src_optical_flow.test_ratio_filtered[1])); + n_hagl_sources++; + } + +# endif // CONFIG_EKF2_OPTICAL_FLOW + # if defined(CONFIG_EKF2_RANGE_FINDER) if (_control_status.flags.rng_terrain) { - // return the terrain height innovation test ratio - test_ratio = math::max(test_ratio, fabsf(_aid_src_rng_hgt.test_ratio_filtered)); + hagl_sum += sqrtf(fabsf(_aid_src_rng_hgt.test_ratio_filtered)); + n_hagl_sources++; } # endif // CONFIG_EKF2_RANGE_FINDER + #endif // CONFIG_EKF2_TERRAIN - if (PX4_ISFINITE(test_ratio) && (test_ratio >= 0.f)) { - return sqrtf(test_ratio); + if (n_hagl_sources > 0) { + return math::max(hagl_sum / static_cast(n_hagl_sources), FLT_MIN); } return NAN; diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 7f4c8d6516c3..4c5bc45eae78 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1333,43 +1333,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); -#if defined(CONFIG_EKF2_OPTICAL_FLOW) - _preflt_checker.setUsingFlowAiding(_ekf.control_status_flags().opt_flow); -#endif // CONFIG_EKF2_OPTICAL_FLOW - -#if defined(CONFIG_EKF2_TERRAIN) && defined(CONFIG_EKF2_OPTICAL_FLOW) - // set dist bottom to scale flow innovation - const float dist_bottom = _ekf.getHagl(); - _preflt_checker.setDistBottom(dist_bottom); -#endif // CONFIG_EKF2_TERRAIN && CONFIG_EKF2_OPTICAL_FLOW - -#if defined(CONFIG_EKF2_EXTERNAL_VISION) - _preflt_checker.setUsingEvPosAiding(_ekf.control_status_flags().ev_pos); - _preflt_checker.setUsingEvVelAiding(_ekf.control_status_flags().ev_vel); - _preflt_checker.setUsingEvHgtAiding(_ekf.control_status_flags().ev_hgt); -#endif // CONFIG_EKF2_EXTERNAL_VISION -#if defined(CONFIG_EKF2_BAROMETER) - _preflt_checker.setUsingBaroHgtAiding(_ekf.control_status_flags().baro_hgt); -#endif // CONFIG_EKF2_BAROMETER - _preflt_checker.setUsingGpsHgtAiding(_ekf.control_status_flags().gps_hgt); -#if defined(CONFIG_EKF2_RANGE_FINDER) - _preflt_checker.setUsingRngHgtAiding(_ekf.control_status_flags().rng_hgt); -#endif // CONFIG_EKF2_RANGE_FINDER - - _preflt_checker.setVehicleCanObserveHeadingInFlight(_ekf.control_status_flags().fixed_wing); - - _preflt_checker.update(_ekf.get_dt_ekf_avg(), innovations); - } } void EKF2::PublishInnovationTestRatios(const hrt_abstime ×tamp) @@ -1816,8 +1779,24 @@ void EKF2::PublishStatus(const hrt_abstime ×tamp) status.control_mode_flags = _ekf.control_status().value; status.filter_fault_flags = _ekf.fault_status().value; + // vel_test_ratio + float vel_xy_test_ratio = _ekf.getHorizontalVelocityInnovationTestRatio(); + float vel_z_test_ratio = _ekf.getVerticalVelocityInnovationTestRatio(); + + if (PX4_ISFINITE(vel_xy_test_ratio) && PX4_ISFINITE(vel_z_test_ratio)) { + status.vel_test_ratio = math::max(vel_xy_test_ratio, vel_z_test_ratio); + + } else if (PX4_ISFINITE(vel_xy_test_ratio)) { + status.vel_test_ratio = vel_xy_test_ratio; + + } else if (PX4_ISFINITE(vel_z_test_ratio)) { + status.vel_test_ratio = vel_z_test_ratio; + + } else { + status.vel_test_ratio = NAN; + } + status.mag_test_ratio = _ekf.getHeadingInnovationTestRatio(); - status.vel_test_ratio = _ekf.getVelocityInnovationTestRatio(); status.pos_test_ratio = _ekf.getHorizontalPositionInnovationTestRatio(); status.hgt_test_ratio = _ekf.getVerticalPositionInnovationTestRatio(); status.tas_test_ratio = _ekf.getAirspeedInnovationTestRatio(); @@ -1836,10 +1815,13 @@ 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(); + static constexpr float kMinTestRatioPreflight = 0.5f; + status.pre_flt_fail_innov_heading = (kMinTestRatioPreflight < status.mag_test_ratio); + status.pre_flt_fail_innov_height = (kMinTestRatioPreflight < status.hgt_test_ratio); + status.pre_flt_fail_innov_pos_horiz = (kMinTestRatioPreflight < status.pos_test_ratio); + status.pre_flt_fail_innov_vel_horiz = (kMinTestRatioPreflight < vel_xy_test_ratio); + status.pre_flt_fail_innov_vel_vert = (kMinTestRatioPreflight < vel_z_test_ratio); + status.pre_flt_fail_mag_field_disturbed = _ekf.control_status_flags().mag_field_disturbed; status.accel_device_id = _device_id_accel; diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 8b832d60ccd8..326b0833a4e4 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" @@ -478,8 +477,6 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem uORB::PublicationMulti _estimator_aid_src_gravity_pub{ORB_ID(estimator_aid_src_gravity)}; #endif // CONFIG_EKF2_GRAVITY_FUSION - 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 7a635acebe7c..000000000000 --- 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 89964b0d7161..000000000000 --- 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 1ac4b2dd2629..000000000000 --- a/src/modules/ekf2/Utility/PreFlightChecker.cpp +++ /dev/null @@ -1,180 +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_e_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 defined(CONFIG_EKF2_OPTICAL_FLOW) - - if (_is_using_flow_aiding) { - const Vector2f flow_innov = Vector2f(innov.flow) * _flow_dist_bottom; - 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); - } - -#endif // CONFIG_EKF2_OPTICAL_FLOW - 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 defined(CONFIG_EKF2_RANGE_FINDER) - - 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); - } - -#endif // CONFIG_EKF2_RANGE_FINDER - - 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_ev_pos_aiding = false; - _is_using_ev_vel_aiding = false; - _is_using_baro_hgt_aiding = false; - _is_using_gps_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_ev_hgt_innov.reset(); - _filter_heading_innov.reset(); - -#if defined(CONFIG_EKF2_RANGE_FINDER) - _is_using_rng_hgt_aiding = false; - _filter_rng_hgt_innov.reset(); -#endif // CONFIG_EKF2_RANGE_FINDER - -#if defined(CONFIG_EKF2_OPTICAL_FLOW) - _is_using_flow_aiding = false; - _filter_flow_x_innov.reset(); - _filter_flow_y_innov.reset(); -#endif // CONFIG_EKF2_OPTICAL_FLOW -} diff --git a/src/modules/ekf2/Utility/PreFlightChecker.hpp b/src/modules/ekf2/Utility/PreFlightChecker.hpp deleted file mode 100644 index a4fe6ab482a1..000000000000 --- a/src/modules/ekf2/Utility/PreFlightChecker.hpp +++ /dev/null @@ -1,211 +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; } -#if defined(CONFIG_EKF2_OPTICAL_FLOW) - void setUsingFlowAiding(bool val) { _is_using_flow_aiding = val; } - void setDistBottom(float dist_bottom) { _flow_dist_bottom = dist_bottom; } -#endif // CONFIG_EKF2_OPTICAL_FLOW - 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; } -#if defined(CONFIG_EKF2_RANGE_FINDER) - void setUsingRngHgtAiding(bool val) { _is_using_rng_hgt_aiding = val; } -#endif // CONFIG_EKF2_RANGE_FINDER - 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_ev_pos_aiding{}; - bool _is_using_ev_vel_aiding{}; - - bool _is_using_baro_hgt_aiding{}; - bool _is_using_gps_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) - - // Preflight low pass filter height innovation (m) - InnovationLpf _filter_baro_hgt_innov; - InnovationLpf _filter_gps_hgt_innov; - InnovationLpf _filter_ev_hgt_innov; - -#if defined(CONFIG_EKF2_RANGE_FINDER) - bool _is_using_rng_hgt_aiding {}; - InnovationLpf _filter_rng_hgt_innov; -#endif // CONFIG_EKF2_RANGE_FINDER - -#if defined(CONFIG_EKF2_OPTICAL_FLOW) - bool _is_using_flow_aiding {}; - float _flow_dist_bottom {}; - 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) - - // Maximum permissible flow innovation to pass pre-flight checks - static constexpr float _flow_innov_test_lim = 0.5f; - - // Preflight flow innovation spike limit (rad) - static constexpr float _flow_innov_spike_lim = 2.0f * _flow_innov_test_lim; -#endif // CONFIG_EKF2_OPTICAL_FLOW - - // 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; - - // 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; - -}; -#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 16e1beddf5e8..000000000000 --- 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)); - } -}