diff --git a/msg/estimator_status.msg b/msg/estimator_status.msg index 55c996621e..cadf06c495 100644 --- a/msg/estimator_status.msg +++ b/msg/estimator_status.msg @@ -116,6 +116,7 @@ float32 time_slip # cumulative amount of time in seconds that the EKF inertial c 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 bool pre_flt_fail_mag_field_disturbed diff --git a/msg/estimator_status_flags.msg b/msg/estimator_status_flags.msg index 2905011582..a613f9e3e6 100644 --- a/msg/estimator_status_flags.msg +++ b/msg/estimator_status_flags.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/hysteresis/hysteresis.cpp b/src/lib/hysteresis/hysteresis.cpp index 07f14e4b2e..621fe6b009 100644 --- a/src/lib/hysteresis/hysteresis.cpp +++ b/src/lib/hysteresis/hysteresis.cpp @@ -42,8 +42,7 @@ namespace systemlib { -void -Hysteresis::set_hysteresis_time_from(const bool from_state, const hrt_abstime new_hysteresis_time_us) +void Hysteresis::set_hysteresis_time_from(const bool from_state, const hrt_abstime new_hysteresis_time_us) { if (from_state) { _time_from_true_us = new_hysteresis_time_us; @@ -53,8 +52,7 @@ Hysteresis::set_hysteresis_time_from(const bool from_state, const hrt_abstime ne } } -void -Hysteresis::set_state_and_update(const bool new_state, const hrt_abstime &now_us) +bool Hysteresis::set_state_and_update(const bool new_state, const hrt_abstime &now_us) { if (new_state != _state) { if (new_state != _requested_state) { @@ -66,12 +64,13 @@ Hysteresis::set_state_and_update(const bool new_state, const hrt_abstime &now_us _requested_state = _state; } - update(now_us); + return update(now_us); } -void -Hysteresis::update(const hrt_abstime &now_us) +bool Hysteresis::update(const hrt_abstime &now_us) { + const bool state_prev = _state; + if (_requested_state != _state) { const hrt_abstime elapsed = now_us - _last_time_to_change_state; @@ -89,6 +88,8 @@ Hysteresis::update(const hrt_abstime &now_us) } } } + + return _state != state_prev; } } // namespace systemlib diff --git a/src/lib/hysteresis/hysteresis.h b/src/lib/hysteresis/hysteresis.h index 9c16ce4933..20617de784 100644 --- a/src/lib/hysteresis/hysteresis.h +++ b/src/lib/hysteresis/hysteresis.h @@ -61,9 +61,11 @@ public: void set_hysteresis_time_from(const bool from_state, const hrt_abstime new_hysteresis_time_us); - void set_state_and_update(const bool new_state, const hrt_abstime &now_us); + // returns true if state changed + bool set_state_and_update(const bool new_state, const hrt_abstime &now_us); - void update(const hrt_abstime &now_us); + // returns true if state changed + bool update(const hrt_abstime &now_us); private: diff --git a/src/lib/mathlib/math/filter/AlphaFilter.hpp b/src/lib/mathlib/math/filter/AlphaFilter.hpp index 0635ae438c..48d2357246 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; diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index 06a78d6809..28f7fed060 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -31,8 +31,6 @@ # ############################################################################# -add_subdirectory(Utility) - px4_add_module( MODULE modules__ekf2 MAIN ekf2 @@ -90,7 +88,6 @@ px4_add_module( geo hysteresis perf - EKF2Utility px4_work_queue world_magnetic_model UNITY_BUILD diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index ff8a39c87c..b65780f197 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -114,6 +114,10 @@ public: const Vector3f getFlowGyro() const { return _flow_sample_delayed.gyro_xyz * (1.f / _flow_sample_delayed.dt); } const Vector3f &getFlowGyroIntegral() const { return _flow_sample_delayed.gyro_xyz; } + const auto& filteredVelocityInnovation(int index) const { return _vel_innov_lpf[index].getState(); } + const auto& filteredPositionInnovation(int index) const { return _pos_innov_lpf[index].getState(); } + const auto& filteredHeadingInnovation() const { return _heading_innov_lpf.getState(); } + void getHeadingInnov(float &heading_innov) const { if (_control_status.flags.mag_hdg) { heading_innov = _aid_src_mag_heading.innovation; @@ -275,6 +279,32 @@ public: return (!_deadreckon_time_exceeded && !_control_status.flags.fake_pos); } + bool horizontalPositionValid(const float innov_limit = 5.f) const + { + return (!_deadreckon_time_exceeded && !_control_status.flags.fake_pos) + && isRecent(_time_last_hor_pos_fuse, _params.no_aid_timeout_max); + } + + bool verticalPositionValid(const float innov_limit = 5.f) const + { + return isRecent(_time_last_hgt_fuse, _params.no_aid_timeout_max); + } + + bool horizontalVelocityValid(const float innov_limit = 1.f) const + { + return isRecent(_time_last_hor_vel_fuse, _params.no_aid_timeout_max); + } + + bool verticalVelocityValid(const float innov_limit = 1.f) const + { + return isRecent(_time_last_ver_vel_fuse, _params.no_aid_timeout_max); + } + + bool headingValid(const float innov_limit = math::radians(30.f)) const + { + return isRecent(_time_last_heading_fuse, _params.no_aid_timeout_max); + } + bool isTerrainEstimateValid() const { return _hagl_valid; }; bool isYawFinalAlignComplete() const @@ -479,6 +509,13 @@ private: uint64_t _time_last_healthy_rng_data{0}; uint8_t _nb_gps_yaw_reset_available{0}; ///< remaining number of resets allowed before switching to another aiding source + // Preflight low pass filter time constant inverse (1/sec) + static constexpr float INNOV_LPF_TAU_INV = 0.2f; + + AlphaFilter _vel_innov_lpf[3]{{INNOV_LPF_TAU_INV}, {INNOV_LPF_TAU_INV}, {INNOV_LPF_TAU_INV}}; ///< filtered velocity innovations (m/s) + AlphaFilter _pos_innov_lpf[3]{{INNOV_LPF_TAU_INV}, {INNOV_LPF_TAU_INV}, {INNOV_LPF_TAU_INV}}; ///< filtered position innovations (m) + AlphaFilter _heading_innov_lpf{{INNOV_LPF_TAU_INV}}; ///< filtered heading innovations (rad) + Vector3f _last_known_pos{}; ///< last known local position vector (m) uint64_t _time_acc_bias_check{0}; ///< last time the accel bias check passed (uSec) @@ -1045,6 +1082,9 @@ private: return sensor_timestamp + acceptance_interval > _newest_high_rate_imu_sample.time_us; } + bool horizontalVelocityFusedRecently() const { return isTimedOut(_time_last_hor_vel_fuse, _params.valid_timeout_max); } + + void startAirspeedFusion(); void stopAirspeedFusion(); @@ -1074,7 +1114,7 @@ private: void stopFakePosFusion(); void fuseFakePosition(); - void setVelPosStatus(const int index, const bool healthy); + void setVelPosStatus(const int index, const float innov, const bool healthy); // reset the quaternion states and covariances to the new yaw value, preserving the roll and pitch // yaw : Euler yaw angle (rad) diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index acf70a010c..7f411cb1e8 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -224,7 +224,6 @@ bool Ekf::isHeightResetRequired() const return (continuous_bad_accel_hgt || hgt_fusion_timeout); } - void Ekf::resetVerticalPositionTo(const float new_vert_pos) { const float old_vert_pos = _state.pos(2); diff --git a/src/modules/ekf2/EKF/gps_yaw_fusion.cpp b/src/modules/ekf2/EKF/gps_yaw_fusion.cpp index c5017d5fed..7f8b80cc43 100644 --- a/src/modules/ekf2/EKF/gps_yaw_fusion.cpp +++ b/src/modules/ekf2/EKF/gps_yaw_fusion.cpp @@ -198,6 +198,7 @@ void Ekf::fuseGpsYaw(const gpsSample& gps_sample) if (is_fused) { _time_last_heading_fuse = _imu_sample_delayed.time_us; _aid_src_gnss_yaw.time_last_fuse = _imu_sample_delayed.time_us; + _heading_innov_lpf.update(heading_innov); } } diff --git a/src/modules/ekf2/EKF/mag_fusion.cpp b/src/modules/ekf2/EKF/mag_fusion.cpp index 7da4b11893..777b62f024 100644 --- a/src/modules/ekf2/EKF/mag_fusion.cpp +++ b/src/modules/ekf2/EKF/mag_fusion.cpp @@ -676,6 +676,8 @@ bool Ekf::fuseYaw(const float innovation, const float variance, estimator_aid_so aid_src_status.time_last_fuse = _imu_sample_delayed.time_us; aid_src_status.fused = true; + _heading_innov_lpf.update(aid_src_status.innovation); + return true; } diff --git a/src/modules/ekf2/EKF/optflow_fusion.cpp b/src/modules/ekf2/EKF/optflow_fusion.cpp index 683fb35cd6..8d7373deee 100644 --- a/src/modules/ekf2/EKF/optflow_fusion.cpp +++ b/src/modules/ekf2/EKF/optflow_fusion.cpp @@ -329,6 +329,9 @@ void Ekf::fuseOptFlow() if (is_fused) { _time_last_of_fuse = _imu_sample_delayed.time_us; + + const float vel_innov = _flow_innov(obs_index) * range; + _vel_innov_lpf[obs_index].update(vel_innov); } } } diff --git a/src/modules/ekf2/EKF/vel_pos_fusion.cpp b/src/modules/ekf2/EKF/vel_pos_fusion.cpp index 1ffe0e0ce1..fc427b39d9 100644 --- a/src/modules/ekf2/EKF/vel_pos_fusion.cpp +++ b/src/modules/ekf2/EKF/vel_pos_fusion.cpp @@ -287,7 +287,7 @@ bool Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int o } } - setVelPosStatus(obs_index, healthy); + setVelPosStatus(obs_index, innov, healthy); if (healthy) { // apply the covariance corrections @@ -304,13 +304,14 @@ bool Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int o return false; } -void Ekf::setVelPosStatus(const int index, const bool healthy) +void Ekf::setVelPosStatus(const int index, const float innov, const bool healthy) { switch (index) { case 0: if (healthy) { _fault_status.flags.bad_vel_N = false; _time_last_hor_vel_fuse = _imu_sample_delayed.time_us; + _vel_innov_lpf[0].update(innov); } else { _fault_status.flags.bad_vel_N = true; @@ -322,6 +323,7 @@ void Ekf::setVelPosStatus(const int index, const bool healthy) if (healthy) { _fault_status.flags.bad_vel_E = false; _time_last_hor_vel_fuse = _imu_sample_delayed.time_us; + _vel_innov_lpf[1].update(innov); } else { _fault_status.flags.bad_vel_E = true; @@ -333,6 +335,7 @@ void Ekf::setVelPosStatus(const int index, const bool healthy) if (healthy) { _fault_status.flags.bad_vel_D = false; _time_last_ver_vel_fuse = _imu_sample_delayed.time_us; + _vel_innov_lpf[2].update(innov); } else { _fault_status.flags.bad_vel_D = true; @@ -344,6 +347,7 @@ void Ekf::setVelPosStatus(const int index, const bool healthy) if (healthy) { _fault_status.flags.bad_pos_N = false; _time_last_hor_pos_fuse = _imu_sample_delayed.time_us; + _pos_innov_lpf[0].update(innov); } else { _fault_status.flags.bad_pos_N = true; @@ -355,6 +359,7 @@ void Ekf::setVelPosStatus(const int index, const bool healthy) if (healthy) { _fault_status.flags.bad_pos_E = false; _time_last_hor_pos_fuse = _imu_sample_delayed.time_us; + _pos_innov_lpf[1].update(innov); } else { _fault_status.flags.bad_pos_E = true; @@ -366,6 +371,7 @@ void Ekf::setVelPosStatus(const int index, const bool healthy) if (healthy) { _fault_status.flags.bad_pos_D = false; _time_last_hgt_fuse = _imu_sample_delayed.time_us; + _pos_innov_lpf[2].update(innov); } else { _fault_status.flags.bad_pos_D = true; diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index f286c2f990..16de4ba7b3 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -180,6 +180,18 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _estimator_states_pub.advertise(); _estimator_status_flags_pub.advertise(); _estimator_status_pub.advertise(); + + _pre_flt_fail_innov_heading.set_hysteresis_time_from(false, 100_ms); + _pre_flt_fail_innov_vel_horiz.set_hysteresis_time_from(false, 100_ms); + _pre_flt_fail_innov_vel_vert.set_hysteresis_time_from(false, 100_ms); + _pre_flt_fail_innov_pos_horiz.set_hysteresis_time_from(false, 100_ms); + _pre_flt_fail_innov_height.set_hysteresis_time_from(false, 100_ms); + + _pre_flt_fail_innov_heading.set_hysteresis_time_from(true, 100_ms); + _pre_flt_fail_innov_vel_horiz.set_hysteresis_time_from(true, 100_ms); + _pre_flt_fail_innov_vel_vert.set_hysteresis_time_from(true, 100_ms); + _pre_flt_fail_innov_pos_horiz.set_hysteresis_time_from(true, 100_ms); + _pre_flt_fail_innov_height.set_hysteresis_time_from(true, 100_ms); } EKF2::~EKF2() @@ -537,9 +549,6 @@ void EKF2::Run() // let the EKF know if the vehicle motion is that of a fixed wing (forward flight only relative to wind) _ekf.set_is_fixed_wing(is_fixed_wing); - - _preflt_checker.setVehicleCanObserveHeadingInFlight(vehicle_status.vehicle_type != - vehicle_status_s::VEHICLE_TYPE_ROTARY_WING); } } @@ -566,8 +575,6 @@ void EKF2::Run() _gyro_cal = {}; _mag_cal = {}; - _preflt_checker.reset(); - } else if (was_in_air && !in_air) { // landed _ekf.set_in_air_status(false); @@ -603,6 +610,34 @@ void EKF2::Run() if (_ekf.update()) { perf_set_elapsed(_ecl_ekf_update_full_perf, hrt_elapsed_time(&ekf_update_start)); + bool pre_flt_fail_updated = false; + + if (!_ekf.control_status_flags().in_air) { + bool pre_flt_fail_innov_heading = _ekf.filteredHeadingInnovation() > kHeadingInnovationTestLimit; + + bool pre_flt_fail_innov_vel_horiz = (_ekf.filteredVelocityInnovation(0) > kVelocityInnovationTestLimit) + || (_ekf.filteredVelocityInnovation(1) > kVelocityInnovationTestLimit); + bool pre_flt_fail_innov_vel_vert = (_ekf.filteredVelocityInnovation(2) > kVelocityInnovationTestLimit); + + bool pre_flt_fail_innov_pos_horiz = (_ekf.filteredPositionInnovation(0) > kPositionInnovationTestLimit) + || (_ekf.filteredPositionInnovation(1) > kPositionInnovationTestLimit); + bool pre_flt_fail_innov_height = (_ekf.filteredPositionInnovation(2) > kPositionInnovationTestLimit); + + pre_flt_fail_updated |= _pre_flt_fail_innov_heading.set_state_and_update(pre_flt_fail_innov_heading, now); + pre_flt_fail_updated |= _pre_flt_fail_innov_vel_horiz.set_state_and_update(pre_flt_fail_innov_vel_horiz, now); + pre_flt_fail_updated |= _pre_flt_fail_innov_vel_vert.set_state_and_update(pre_flt_fail_innov_vel_vert, now); + pre_flt_fail_updated |= _pre_flt_fail_innov_pos_horiz.set_state_and_update(pre_flt_fail_innov_pos_horiz, now); + pre_flt_fail_updated |= _pre_flt_fail_innov_height.set_state_and_update(pre_flt_fail_innov_height, now); + + } else if (_ekf.control_status_prev_flags().in_air != _ekf.control_status_flags().in_air) { + // reset + _pre_flt_fail_innov_heading.set_state_and_update(false, now); + _pre_flt_fail_innov_vel_horiz.set_state_and_update(false, now); + _pre_flt_fail_innov_vel_vert.set_state_and_update(false, now); + _pre_flt_fail_innov_pos_horiz.set_state_and_update(false, now); + _pre_flt_fail_innov_height.set_state_and_update(false, now); + } + PublishLocalPosition(now); PublishOdometry(now); PublishGlobalPosition(now); @@ -621,7 +656,7 @@ void EKF2::Run() PublishOpticalFlowVel(now); PublishStates(now); PublishStatus(now); - PublishStatusFlags(now); + PublishStatusFlags(now, pre_flt_fail_updated); PublishYawEstimatorStatus(now); UpdateAccelCalibration(now); @@ -1000,22 +1035,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_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.update(_ekf.get_imu_sample_delayed().delta_ang_dt, innovations); - } } void EKF2::PublishInnovationTestRatios(const hrt_abstime ×tamp) @@ -1096,11 +1115,18 @@ void EKF2::PublishLocalPosition(const hrt_abstime ×tamp) lpos.ay = vel_deriv(1); lpos.az = vel_deriv(2); - // TODO: better status reporting - lpos.xy_valid = _ekf.local_position_is_valid(); - lpos.z_valid = !_preflt_checker.hasVertFailed(); - lpos.v_xy_valid = _ekf.local_position_is_valid(); - lpos.v_z_valid = !_preflt_checker.hasVertFailed(); + if (!_ekf.control_status_flags().in_air) { + lpos.xy_valid = _ekf.horizontalPositionValid() && _pre_flt_fail_innov_pos_horiz.get_state(); + lpos.z_valid = _ekf.verticalPositionValid() && _pre_flt_fail_innov_height.get_state(); + lpos.v_xy_valid = _ekf.horizontalVelocityValid() && _pre_flt_fail_innov_vel_horiz.get_state(); + lpos.v_z_valid = _ekf.verticalVelocityValid() && _pre_flt_fail_innov_vel_vert.get_state(); + + } else { + lpos.xy_valid = _ekf.horizontalPositionValid(); + lpos.z_valid = _ekf.verticalPositionValid(); + lpos.v_xy_valid = _ekf.horizontalVelocityValid(); + lpos.v_z_valid = _ekf.verticalVelocityValid(); + } // Position of local NED origin in GPS / WGS84 frame if (_ekf.global_origin_valid()) { @@ -1351,10 +1377,11 @@ 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_innov_heading = _pre_flt_fail_innov_heading.get_state(); + status.pre_flt_fail_innov_vel_horiz = _pre_flt_fail_innov_vel_horiz.get_state(); + status.pre_flt_fail_innov_vel_vert = _pre_flt_fail_innov_vel_vert.get_state(); + status.pre_flt_fail_innov_pos_horiz = _pre_flt_fail_innov_pos_horiz.get_state(); + status.pre_flt_fail_innov_height = _pre_flt_fail_innov_height.get_state(); status.pre_flt_fail_mag_field_disturbed = _ekf.control_status_flags().mag_field_disturbed; status.accel_device_id = _device_id_accel; @@ -1366,7 +1393,7 @@ void EKF2::PublishStatus(const hrt_abstime ×tamp) _estimator_status_pub.publish(status); } -void EKF2::PublishStatusFlags(const hrt_abstime ×tamp) +void EKF2::PublishStatusFlags(const hrt_abstime ×tamp, bool force) { // publish at ~ 1 Hz (or immediately if filter control status or fault status changes) bool update = (timestamp >= _last_status_flags_publish + 1_s); @@ -1404,7 +1431,7 @@ void EKF2::PublishStatusFlags(const hrt_abstime ×tamp) status_flags.cs_mag_hdg = _ekf.control_status_flags().mag_hdg; status_flags.cs_mag_3d = _ekf.control_status_flags().mag_3D; status_flags.cs_mag_dec = _ekf.control_status_flags().mag_dec; - status_flags.cs_in_air = _ekf.control_status_flags().in_air; + status_flags.cs_in_air = _ekf.control_status_flags().in_air; status_flags.cs_wind = _ekf.control_status_flags().wind; status_flags.cs_baro_hgt = _ekf.control_status_flags().baro_hgt; status_flags.cs_rng_hgt = _ekf.control_status_flags().rng_hgt; @@ -1464,6 +1491,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_heading = _pre_flt_fail_innov_heading.get_state(); + status_flags.pre_flt_fail_innov_vel_horiz = _pre_flt_fail_innov_vel_horiz.get_state(); + status_flags.pre_flt_fail_innov_vel_vert = _pre_flt_fail_innov_vel_vert.get_state(); + status_flags.pre_flt_fail_innov_pos_horiz = _pre_flt_fail_innov_pos_horiz.get_state(); + status_flags.pre_flt_fail_innov_height = _pre_flt_fail_innov_height.get_state(); + status_flags.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); _estimator_status_flags_pub.publish(status_flags); @@ -2085,7 +2118,6 @@ void EKF2::UpdateAccelCalibration(const hrt_abstime ×tamp) // the EKF is operating in the correct mode and there are no filter faults if ((_ekf.fault_status().value == 0) && !_ekf.accel_bias_inhibited() - && !_preflt_checker.hasHorizFailed() && !_preflt_checker.hasVertFailed() && (_ekf.control_status_flags().baro_hgt || _ekf.control_status_flags().rng_hgt || _ekf.control_status_flags().gps_hgt || _ekf.control_status_flags().ev_hgt) && !_ekf.warning_event_flags().height_sensor_timeout && !_ekf.warning_event_flags().invalid_accel_bias_cov_reset diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 79f37f1a68..ac3ca34fbb 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" @@ -158,7 +157,7 @@ private: void PublishSensorBias(const hrt_abstime ×tamp); void PublishStates(const hrt_abstime ×tamp); void PublishStatus(const hrt_abstime ×tamp); - void PublishStatusFlags(const hrt_abstime ×tamp); + void PublishStatusFlags(const hrt_abstime ×tamp, bool force = false); void PublishWindEstimate(const hrt_abstime ×tamp); void PublishYawEstimatorStatus(const hrt_abstime ×tamp); @@ -209,6 +208,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 + systemlib::Hysteresis _pre_flt_fail_innov_heading{false}; + systemlib::Hysteresis _pre_flt_fail_innov_vel_horiz{false}; + systemlib::Hysteresis _pre_flt_fail_innov_vel_vert{false}; + systemlib::Hysteresis _pre_flt_fail_innov_pos_horiz{false}; + systemlib::Hysteresis _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")}; @@ -368,9 +381,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)); - } -} diff --git a/src/modules/simulation/simulator_ignition_bridge/SimulatorIgnitionBridge.cpp b/src/modules/simulation/simulator_ignition_bridge/SimulatorIgnitionBridge.cpp index 60aa1de546..dcdffb0d36 100644 --- a/src/modules/simulation/simulator_ignition_bridge/SimulatorIgnitionBridge.cpp +++ b/src/modules/simulation/simulator_ignition_bridge/SimulatorIgnitionBridge.cpp @@ -76,14 +76,14 @@ int SimulatorIgnitionBridge::init() if (!_model_pose.empty()) { PX4_INFO("Requested Model Position: %s", _model_pose.c_str()); - std::vector model_pose_v; + std::vector model_pose_v; std::stringstream ss(_model_pose); while (ss.good()) { std::string substr; std::getline(ss, substr, ','); - model_pose_v.push_back(std::stof(substr)); + model_pose_v.push_back(std::stod(substr)); } while (model_pose_v.size() < 6) {