diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index 4cc769f253..0be60f64d3 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -250,6 +250,27 @@ void Ekf::predictState() _state.quat_nominal = (_state.quat_nominal * dq).normalized(); _R_to_earth = Dcmf(_state.quat_nominal); + // if there's accel clipping (asymmetric railing) then for convenience register in earth frame + if (_imu_sample_delayed.delta_vel_clipping[0] || _imu_sample_delayed.delta_vel_clipping[1] || _imu_sample_delayed.delta_vel_clipping[2]) { + + // delta velocity clipping in body fixed frame + const Vector3f clipping_bf { + _imu_sample_delayed.delta_vel_clipping[0] ? 1.f : 0.f, + _imu_sample_delayed.delta_vel_clipping[1] ? 1.f : 0.f, + _imu_sample_delayed.delta_vel_clipping[2] ? 1.f : 0.f + }; + + // delta velocity clipping in earth frame + const Vector3f clipping_ef = _R_to_earth * clipping_bf; + + _imu_accel_clipping_NE = fabsf(clipping_ef(0)) > 0.5f || fabsf(clipping_ef(1)) > 0.5f; // north or east + _imu_accel_clipping_D = fabsf(clipping_ef(2)) > 0.5f; // down + + } else { + _imu_accel_clipping_NE = false; // north or east + _imu_accel_clipping_D = false; // down + } + // Calculate an earth frame delta velocity const Vector3f delta_vel_bias_scaled = getAccelBias() * _imu_sample_delayed.delta_vel_dt; const Vector3f corrected_delta_vel = _imu_sample_delayed.delta_vel - delta_vel_bias_scaled; diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 788d00f5f9..d936759ae8 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -593,6 +593,7 @@ private: float _gps_error_norm{1.0f}; ///< normalised gps error uint32_t _min_gps_health_time_us{10000000}; ///< GPS is marked as healthy only after this amount of time bool _gps_checks_passed{false}; ///> true when all active GPS checks have passed + uint64_t _last_gnss_reset_accel_clipping_us{0}; ///< last time GPS control performed an emergency reset due to accel clipping // Variables used to publish the WGS-84 location of the EKF local NED origin float _gps_alt_ref{NAN}; ///< WGS-84 height (m) diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 23bee11ca0..9f693277b6 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -297,6 +297,8 @@ protected: float _dt_ekf_avg{0.010f}; ///< average update rate of the ekf in s imuSample _imu_sample_delayed{}; // captures the imu sample on the delayed time horizon + bool _imu_accel_clipping_NE{false}; // IMU sample delayed accel clipping in earth NE axis + bool _imu_accel_clipping_D{false}; // IMU sample delayed accel clipping in earth D axis // measurement samples capturing measurements on the delayed time horizon gpsSample _gps_sample_delayed{}; diff --git a/src/modules/ekf2/EKF/gnss_height_control.cpp b/src/modules/ekf2/EKF/gnss_height_control.cpp index ce13617e42..8f7558450a 100644 --- a/src/modules/ekf2/EKF/gnss_height_control.cpp +++ b/src/modules/ekf2/EKF/gnss_height_control.cpp @@ -114,15 +114,6 @@ void Ekf::controlGnssHeightFusion(const gpsSample &gps_sample) resetVerticalPositionTo(-(measurement - bias_est.getBias()), measurement_var); bias_est.setBias(_state.pos(2) + measurement); - // reset vertical velocity - if (PX4_ISFINITE(gps_sample.vel(2)) && (_params.gnss_ctrl & GnssCtrl::VEL)) { - // use 1.5 as a typical ratio of vacc/hacc - resetVerticalVelocityTo(gps_sample.vel(2), sq(1.5f * gps_sample.sacc)); - - } else { - resetVerticalVelocityToZero(); - } - aid_src.time_last_fuse = _imu_sample_delayed.time_us; } else if (is_fusion_failing) { diff --git a/src/modules/ekf2/EKF/gps_control.cpp b/src/modules/ekf2/EKF/gps_control.cpp index 982b61cb05..54d852422c 100644 --- a/src/modules/ekf2/EKF/gps_control.cpp +++ b/src/modules/ekf2/EKF/gps_control.cpp @@ -136,6 +136,39 @@ void Ekf::controlGpsFusion() _information_events.flags.reset_pos_to_gps = true; resetVelocityTo(gps_sample.vel, vel_obs_var); resetHorizontalPositionTo(gps_sample.pos, pos_obs_var); + + } else if ((_imu_accel_clipping_NE || _imu_accel_clipping_D) && gps_checks_passing + && isTimedOut(_last_gnss_reset_accel_clipping_us, (uint64_t)3e6)) { + // check for accel clipping and perform an emergency reset if GNSS is good (our last resort) + if (_imu_accel_clipping_NE) { + // accel clipping on horizontal axis + + // reset velocity (if current sample speed accuracy is good) + if (gps_sample.sacc < _params.req_sacc) { + _information_events.flags.reset_vel_to_gps = true; + resetVelocityTo(gps_sample.vel, vel_obs_var); + _aid_src_gnss_vel.time_last_fuse = _imu_sample_delayed.time_us; + + _last_gnss_reset_accel_clipping_us = _imu_sample_delayed.time_us; + } + + // reset position (if current sample horizontal accuracy is good) + if (gps_sample.hacc < _params.req_hacc) { + _information_events.flags.reset_pos_to_gps = true; + resetHorizontalPositionTo(gps_sample.pos, pos_obs_var); + _aid_src_gnss_pos.time_last_fuse = _imu_sample_delayed.time_us; + + _last_gnss_reset_accel_clipping_us = _imu_sample_delayed.time_us; + } + + } else if (_imu_accel_clipping_D || isHeightResetRequired()) { + // accel clipping on vertical axis or height failure, reset vertical velocity + if (gps_sample.sacc < _params.req_sacc) { + resetVerticalVelocityTo(gps_sample.vel(2), vel_obs_var(2)); + + _last_gnss_reset_accel_clipping_us = _imu_sample_delayed.time_us; + } + } } } else {