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 9b169ae360..0a4e0faa6a 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 @@ -377,7 +377,6 @@ *(.text._ZN19StickAccelerationXYC1EP12ModuleParams) *(.text.imxrt_epsubmit) *(.text._ZN15PositionControl6updateEf) -*(.text._ZN3Ekf29checkVerticalAccelerationBiasERKN9estimator9imuSampleE) *(.text._ZN23MavlinkStreamScaledIMU24sendEv) *(.text._ZN5PX4IO10io_reg_getEhhPtj) *(.text.imxrt_dma_send) diff --git a/msg/EstimatorStatusFlags.msg b/msg/EstimatorStatusFlags.msg index 79b900f6a8..6b17842c26 100644 --- a/msg/EstimatorStatusFlags.msg +++ b/msg/EstimatorStatusFlags.msg @@ -59,7 +59,6 @@ bool fs_bad_airspeed # 5 - true if fusion of the airspeed has encounte bool fs_bad_sideslip # 6 - true if fusion of the synthetic sideslip constraint has encountered a numerical error bool fs_bad_optflow_x # 7 - true if fusion of the optical flow X axis has encountered a numerical error bool fs_bad_optflow_y # 8 - true if fusion of the optical flow Y axis has encountered a numerical error -bool fs_bad_acc_bias # 9 - true if bad delta velocity bias estimates have been detected bool fs_bad_acc_vertical # 10 - true if bad vertical accelerometer data has been detected bool fs_bad_acc_clipping # 11 - true if delta velocity data contains clipping (asymmetric railing) diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 13c2767f75..6eda8b9cc7 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -513,7 +513,7 @@ bool bad_sideslip : 1; ///< 6 - true if fusion of the synthetic sideslip constraint has encountered a numerical error bool bad_optflow_X : 1; ///< 7 - true if fusion of the optical flow X axis has encountered a numerical error bool bad_optflow_Y : 1; ///< 8 - true if fusion of the optical flow Y axis has encountered a numerical error - bool bad_acc_bias : 1; ///< 9 - true if bad delta velocity bias estimates have been detected + bool __UNUSED : 1; ///< 9 - bool bad_acc_vertical : 1; ///< 10 - true if bad vertical accelerometer data has been detected bool bad_acc_clipping : 1; ///< 11 - true if delta velocity data contains clipping (asymmetric railing) } flags; diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index 24734dcfb3..c8448daa9b 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -114,8 +114,6 @@ void Ekf::reset() _last_known_pos.setZero(); - _time_acc_bias_check = 0; - #if defined(CONFIG_EKF2_BAROMETER) _baro_counter = 0; #endif // CONFIG_EKF2_BAROMETER diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 5be5cee3bb..a7d59b47ef 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -612,8 +612,6 @@ private: 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) - Vector3f _earth_rate_NED{}; ///< earth rotation vector (NED) in rad/s Dcmf _R_to_earth{}; ///< transformation matrix from body frame to earth frame from last EKF prediction @@ -1094,7 +1092,6 @@ private: void stopAuxVelFusion(); #endif // CONFIG_EKF2_AUXVEL - void checkVerticalAccelerationBias(const imuSample &imu_delayed); void checkVerticalAccelerationHealth(const imuSample &imu_delayed); Likelihood estimateInertialNavFallingLikelihood() const; diff --git a/src/modules/ekf2/EKF/height_control.cpp b/src/modules/ekf2/EKF/height_control.cpp index 4db232ca05..147e198921 100644 --- a/src/modules/ekf2/EKF/height_control.cpp +++ b/src/modules/ekf2/EKF/height_control.cpp @@ -39,7 +39,6 @@ void Ekf::controlHeightFusion(const imuSample &imu_delayed) { - checkVerticalAccelerationBias(imu_delayed); checkVerticalAccelerationHealth(imu_delayed); #if defined(CONFIG_EKF2_BAROMETER) @@ -120,106 +119,6 @@ void Ekf::checkHeightSensorRefFallback() } } -void Ekf::checkVerticalAccelerationBias(const imuSample &imu_delayed) -{ - // Run additional checks to see if the delta velocity bias has hit limits in a direction that is clearly wrong - // calculate accel bias term aligned with the gravity vector - const float dVel_bias_lim = 0.9f * _params.acc_bias_lim * _dt_ekf_avg; - const Vector3f delta_vel_bias = _state.accel_bias * _dt_ekf_avg; - const float down_dvel_bias = delta_vel_bias.dot(Vector3f(_R_to_earth.row(2))); - - // check that the vertical component of accel bias is consistent with both the vertical position and velocity innovation - bool bad_acc_bias = false; - - if (fabsf(down_dvel_bias) > dVel_bias_lim) { - - bool bad_vz = false; - -#if defined(CONFIG_EKF2_EXTERNAL_VISION) - - if (_control_status.flags.ev_hgt) { - if (down_dvel_bias * _aid_src_ev_vel.innovation[2] < 0.f) { - bad_vz = true; - } - } - -#endif // CONFIG_EKF2_EXTERNAL_VISION - -#if defined(CONFIG_EKF2_GNSS) - - if (_control_status.flags.gps) { - if (down_dvel_bias * _aid_src_gnss_vel.innovation[2] < 0.f) { - bad_vz = true; - } - } - -#endif // CONFIG_EKF2_GNSS - - if (bad_vz) { -#if defined(CONFIG_EKF2_BAROMETER) - - if (_control_status.flags.baro_hgt) { - if (down_dvel_bias * _aid_src_baro_hgt.innovation < 0.f) { - bad_acc_bias = true; - } - } - -#endif // CONFIG_EKF2_BAROMETER - -#if defined(CONFIG_EKF2_EXTERNAL_VISION) - - if (_control_status.flags.ev_hgt) { - if (down_dvel_bias * _aid_src_ev_hgt.innovation < 0.f) { - bad_acc_bias = true; - } - } - -#endif // CONFIG_EKF2_EXTERNAL_VISION - -#if defined(CONFIG_EKF2_GNSS) - - if (_control_status.flags.gps_hgt) { - if (down_dvel_bias * _aid_src_gnss_hgt.innovation < 0.f) { - bad_acc_bias = true; - } - } - -#endif // CONFIG_EKF2_GNSS - -#if defined(CONFIG_EKF2_RANGE_FINDER) - - if (_control_status.flags.rng_hgt) { - if (down_dvel_bias * _aid_src_rng_hgt.innovation < 0.f) { - bad_acc_bias = true; - } - } - -#endif // CONFIG_EKF2_RANGE_FINDER - } - } - - // record the pass/fail - if (!bad_acc_bias) { - _fault_status.flags.bad_acc_bias = false; - _time_acc_bias_check = _time_delayed_us; - - } else { - _fault_status.flags.bad_acc_bias = true; - } - - // if we have failed for 7 seconds continuously, reset the accel bias covariances to fix bad conditioning of - // the covariance matrix but preserve the variances (diagonals) to allow bias learning to continue - if (_fault_status.flags.bad_acc_bias && isTimedOut(_time_acc_bias_check, (uint64_t)7e6)) { - - resetAccelBiasCov(); - - _time_acc_bias_check = imu_delayed.time_us; - - _fault_status.flags.bad_acc_bias = false; - ECL_WARN("invalid accel bias - covariance reset"); - } -} - void Ekf::checkVerticalAccelerationHealth(const imuSample &imu_delayed) { // Check for IMU accelerometer vibration induced clipping as evidenced by the vertical diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 7769543d2b..97ff93bf2a 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1937,7 +1937,6 @@ void EKF2::PublishStatusFlags(const hrt_abstime ×tamp) status_flags.fs_bad_sideslip = _ekf.fault_status_flags().bad_sideslip; status_flags.fs_bad_optflow_x = _ekf.fault_status_flags().bad_optflow_X; status_flags.fs_bad_optflow_y = _ekf.fault_status_flags().bad_optflow_Y; - status_flags.fs_bad_acc_bias = _ekf.fault_status_flags().bad_acc_bias; status_flags.fs_bad_acc_vertical = _ekf.fault_status_flags().bad_acc_vertical; status_flags.fs_bad_acc_clipping = _ekf.fault_status_flags().bad_acc_clipping; @@ -2666,7 +2665,6 @@ void EKF2::UpdateAccelCalibration(const hrt_abstime ×tamp) const bool bias_valid = (_param_ekf2_imu_ctrl.get() & static_cast(ImuCtrl::AccelBias)) && _ekf.control_status_flags().tilt_align && (_ekf.fault_status().value == 0) - && !_ekf.fault_status_flags().bad_acc_bias && !_ekf.fault_status_flags().bad_acc_clipping && !_ekf.fault_status_flags().bad_acc_vertical;