From 1948c5057aca91ad5a55ea6a3f2c4fb1f9407965 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 22 Jul 2022 12:38:28 -0400 Subject: [PATCH] ekf2: handle all time on delayed horizon (except for newest sample checks) - a growing number of samples come into the backend with the time already delayed (sensor's interrupt setting timestamp sample) - if the incoming timestamp is already delayed then the new data checks (relative to latest IMU) can be slightly wrong - handle almost all timestamps and checks on delayed time horizon, except for explicit checks of new samples - isRecent() and isTimedOut() helpers use delayed time - add new isNewestSampleRecent() used for checking the incoming timestamp of the incoming (adjusted) data --- src/modules/ekf2/EKF/airspeed_fusion.cpp | 4 +- src/modules/ekf2/EKF/baro_height_control.cpp | 2 +- src/modules/ekf2/EKF/control.cpp | 25 ++- src/modules/ekf2/EKF/covariance.cpp | 4 +- src/modules/ekf2/EKF/ekf.cpp | 12 +- src/modules/ekf2/EKF/ekf.h | 11 +- src/modules/ekf2/EKF/ekf_helper.cpp | 20 +-- src/modules/ekf2/EKF/estimator_interface.cpp | 148 +++++++++++------- src/modules/ekf2/EKF/estimator_interface.h | 24 ++- src/modules/ekf2/EKF/ev_height_control.cpp | 2 +- src/modules/ekf2/EKF/fake_pos_control.cpp | 8 +- src/modules/ekf2/EKF/gnss_height_control.cpp | 4 +- src/modules/ekf2/EKF/gps_checks.cpp | 17 +- src/modules/ekf2/EKF/gps_control.cpp | 5 +- src/modules/ekf2/EKF/gps_fusion.cpp | 8 +- src/modules/ekf2/EKF/gps_yaw_fusion.cpp | 6 +- src/modules/ekf2/EKF/height_control.cpp | 4 +- src/modules/ekf2/EKF/height_fusion.cpp | 4 +- src/modules/ekf2/EKF/mag_fusion.cpp | 6 +- src/modules/ekf2/EKF/optflow_fusion.cpp | 2 +- src/modules/ekf2/EKF/range_height_control.cpp | 2 +- src/modules/ekf2/EKF/sideslip_fusion.cpp | 2 +- src/modules/ekf2/EKF/terrain_estimator.cpp | 12 +- src/modules/ekf2/EKF/vel_pos_fusion.cpp | 24 +-- src/modules/ekf2/EKF/zero_velocity_update.cpp | 2 +- 25 files changed, 193 insertions(+), 165 deletions(-) diff --git a/src/modules/ekf2/EKF/airspeed_fusion.cpp b/src/modules/ekf2/EKF/airspeed_fusion.cpp index 18749224e8..414ce4cf11 100644 --- a/src/modules/ekf2/EKF/airspeed_fusion.cpp +++ b/src/modules/ekf2/EKF/airspeed_fusion.cpp @@ -180,7 +180,7 @@ void Ekf::fuseAirspeed(estimator_aid_source_1d_s &airspeed) _fault_status.flags.bad_airspeed = !is_fused; if (is_fused) { - airspeed.time_last_fuse = _time_last_imu; + airspeed.time_last_fuse = _imu_sample_delayed.time_us; } } @@ -212,7 +212,7 @@ void Ekf::resetWindUsingAirspeed() resetWindCovarianceUsingAirspeed(); - _aid_src_airspeed.time_last_fuse = _time_last_imu; + _aid_src_airspeed.time_last_fuse = _imu_sample_delayed.time_us; } void Ekf::resetWindToZero() diff --git a/src/modules/ekf2/EKF/baro_height_control.cpp b/src/modules/ekf2/EKF/baro_height_control.cpp index 2a909c9ff7..91870fb0a9 100644 --- a/src/modules/ekf2/EKF/baro_height_control.cpp +++ b/src/modules/ekf2/EKF/baro_height_control.cpp @@ -48,7 +48,7 @@ void Ekf::controlBaroHeightFusion() _baro_b_est.predict(_dt_ekf_avg); // check for intermittent data - const bool baro_hgt_intermittent = !isRecent(_time_last_baro, 2 * BARO_MAX_INTERVAL); + const bool baro_hgt_intermittent = !isNewestSampleRecent(_time_last_baro_buffer_push, 2 * BARO_MAX_INTERVAL); if (_baro_data_ready) { _baro_lpf.update(_baro_sample_delayed.hgt); diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index a667afa43a..72ee7bb568 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -98,7 +98,7 @@ void Ekf::controlFusionModes() if (_gps_buffer) { - _gps_intermittent = !isRecent(_time_last_gps, 2 * GPS_MAX_INTERVAL); + _gps_intermittent = !isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GPS_MAX_INTERVAL); // check for arrival of new sensor data at the fusion time horizon _time_prev_gps_us = _gps_sample_delayed.time_us; @@ -142,7 +142,7 @@ void Ekf::controlFusionModes() if (_control_status.flags.in_air && !_control_status.flags.fixed_wing && (sq(_state.vel(0)) + sq(_state.vel(1)) < fmaxf(P(4, 4) + P(5, 5), 0.1f))) { _rng_consistency_check.setGate(_params.range_kin_consistency_gate); - _rng_consistency_check.update(_range_sensor.getDistBottom(), getRngHeightVariance(), _state.vel(2), P(6, 6), _time_last_imu); + _rng_consistency_check.update(_range_sensor.getDistBottom(), getRngHeightVariance(), _state.vel(2), P(6, 6), _imu_sample_delayed.time_us); } } @@ -223,7 +223,7 @@ void Ekf::controlExternalVisionFusion() if (_control_status.flags.tilt_align && _control_status.flags.yaw_align) { // check for a external vision measurement that has fallen behind the fusion time horizon - if (isRecent(_time_last_ext_vision, 2 * EV_MAX_INTERVAL)) { + if (isNewestSampleRecent(_time_last_ext_vision_buffer_push, 2 * EV_MAX_INTERVAL)) { // turn on use of external vision measurements for position if (_params.fusion_mode & SensorFusionMask::USE_EXT_VIS_POS && !_control_status.flags.ev_pos) { startEvPosFusion(); @@ -241,7 +241,7 @@ void Ekf::controlExternalVisionFusion() && _control_status.flags.tilt_align) { // don't start using EV data unless data is arriving frequently - if (isRecent(_time_last_ext_vision, 2 * EV_MAX_INTERVAL)) { + if (isNewestSampleRecent(_time_last_ext_vision_buffer_push, 2 * EV_MAX_INTERVAL)) { if (resetYawToEv()) { _control_status.flags.yaw_align = true; startEvYawFusion(); @@ -392,7 +392,7 @@ void Ekf::controlExternalVisionFusion() _yaw_pred_prev = getEulerYaw(_state.quat_nominal); } else if ((_control_status.flags.ev_pos || _control_status.flags.ev_vel || _control_status.flags.ev_yaw) - && isTimedOut(_time_last_ext_vision, (uint64_t)_params.reset_timeout_max)) { + && !isRecent(_ev_sample_delayed.time_us, (uint64_t)_params.reset_timeout_max)) { // Turn off EV fusion mode if no data has been received stopEvFusion(); @@ -502,7 +502,7 @@ void Ekf::controlOpticalFlowFusion() // set the flag and reset the fusion timeout ECL_INFO("starting optical flow fusion"); _control_status.flags.opt_flow = true; - _time_last_of_fuse = _time_last_imu; + _time_last_of_fuse = _imu_sample_delayed.time_us; // if we are not using GPS or external vision aiding, then the velocity and position states and covariances need to be set const bool flow_aid_only = !isOtherSourceOfHorizontalAidingThan(_control_status.flags.opt_flow); @@ -536,8 +536,7 @@ void Ekf::controlOpticalFlowFusion() } } - } else if (_control_status.flags.opt_flow - && (_imu_sample_delayed.time_us > _flow_sample_delayed.time_us + (uint64_t)10e6)) { + } else if (_control_status.flags.opt_flow && !isRecent(_flow_sample_delayed.time_us, (uint64_t)10e6)) { stopFlowFusion(); } @@ -582,7 +581,7 @@ void Ekf::controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing) const bool continuing_conditions_passing = !gps_checks_failing; - const bool is_gps_yaw_data_intermittent = !isRecent(_time_last_gps_yaw_data, 2 * GPS_MAX_INTERVAL); + const bool is_gps_yaw_data_intermittent = !isNewestSampleRecent(_time_last_gps_yaw_buffer_push, 2 * GPS_MAX_INTERVAL); const bool starting_conditions_passing = continuing_conditions_passing && _control_status.flags.tilt_align @@ -590,8 +589,6 @@ void Ekf::controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing) && !is_gps_yaw_data_intermittent && !_gps_intermittent; - _time_last_gps_yaw_data = _time_last_imu; - if (_control_status.flags.gps_yaw) { if (continuing_conditions_passing) { @@ -640,7 +637,7 @@ void Ekf::controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing) } } - } else if (_control_status.flags.gps_yaw && isTimedOut(_time_last_gps_yaw_data, _params.reset_timeout_max)) { + } else if (_control_status.flags.gps_yaw && !isNewestSampleRecent(_time_last_gps_yaw_buffer_push, _params.reset_timeout_max)) { // No yaw data in the message anymore. Stop until it comes back. stopGpsYawFusion(); } @@ -702,7 +699,7 @@ void Ekf::controlAirDataFusion() startAirspeedFusion(); } - } else if (_control_status.flags.fuse_aspd && (_imu_sample_delayed.time_us - _airspeed_sample_delayed.time_us > (uint64_t) 1e6)) { + } else if (_control_status.flags.fuse_aspd && !isRecent(_airspeed_sample_delayed.time_us, (uint64_t)1e6)) { ECL_WARN("Airspeed data stopped"); stopAirspeedFusion(); } @@ -725,7 +722,7 @@ void Ekf::controlBetaFusion() // activate the wind states _control_status.flags.wind = true; // reset the timeout timers to prevent repeated resets - _time_last_beta_fuse = _time_last_imu; + _time_last_beta_fuse = _imu_sample_delayed.time_us; resetWind(); } diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index 4af55707a5..21a7fc2953 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -1006,7 +1006,7 @@ void Ekf::fixCovarianceErrors(bool force_symmetry) // record the pass/fail if (!bad_acc_bias) { _fault_status.flags.bad_acc_bias = false; - _time_acc_bias_check = _time_last_imu; + _time_acc_bias_check = _imu_sample_delayed.time_us; } else { _fault_status.flags.bad_acc_bias = true; @@ -1018,7 +1018,7 @@ void Ekf::fixCovarianceErrors(bool force_symmetry) P.uncorrelateCovariance<3>(13); - _time_acc_bias_check = _time_last_imu; + _time_acc_bias_check = _imu_sample_delayed.time_us; _fault_status.flags.bad_acc_bias = false; _warning_events.flags.invalid_accel_bias_cov_reset = true; ECL_WARN("invalid accel bias - covariance reset"); diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index 15faaf1ad1..00740cf1d5 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -225,12 +225,12 @@ bool Ekf::initialiseFilter() initHagl(); // reset the essential fusion timeout counters - _time_last_hgt_fuse = _time_last_imu; - _time_last_hor_pos_fuse = _time_last_imu; - _time_last_hor_vel_fuse = _time_last_imu; - _time_last_hagl_fuse = _time_last_imu; - _time_last_flow_terrain_fuse = _time_last_imu; - _time_last_of_fuse = _time_last_imu; + _time_last_hgt_fuse = _imu_sample_delayed.time_us; + _time_last_hor_pos_fuse = _imu_sample_delayed.time_us; + _time_last_hor_vel_fuse = _imu_sample_delayed.time_us; + _time_last_hagl_fuse = _imu_sample_delayed.time_us; + _time_last_flow_terrain_fuse = _imu_sample_delayed.time_us; + _time_last_of_fuse = _imu_sample_delayed.time_us; // reset the output predictor state history to match the EKF initial values alignOutputFilter(); diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index b61efae04c..05bf64a153 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -477,7 +477,6 @@ private: uint64_t _time_last_flow_terrain_fuse{0}; ///< time the last fusion of optical flow measurements for terrain estimation were performed (uSec) uint64_t _time_last_beta_fuse{0}; ///< time the last fusion of synthetic sideslip measurements were performed (uSec) uint64_t _time_last_zero_velocity_fuse{0}; ///< last time of zero velocity update (uSec) - uint64_t _time_last_gps_yaw_data{0}; ///< time the last GPS yaw measurement was available (uSec) 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 @@ -581,7 +580,6 @@ private: bool _gps_checks_passed{false}; ///> true when all active GPS checks have passed // Variables used to publish the WGS-84 location of the EKF local NED origin - uint64_t _last_gps_origin_time_us{0}; ///< time the origin was last set (uSec) float _gps_alt_ref{NAN}; ///< WGS-84 height (m) // Variables used by the initial filter alignment @@ -1026,12 +1024,17 @@ private: bool isTimedOut(uint64_t last_sensor_timestamp, uint64_t timeout_period) const { - return last_sensor_timestamp + timeout_period < _time_last_imu; + return last_sensor_timestamp + timeout_period < _imu_sample_delayed.time_us; } bool isRecent(uint64_t sensor_timestamp, uint64_t acceptance_interval) const { - return sensor_timestamp + acceptance_interval > _time_last_imu; + return sensor_timestamp + acceptance_interval > _imu_sample_delayed.time_us; + } + + bool isNewestSampleRecent(uint64_t sensor_timestamp, uint64_t acceptance_interval) const + { + return sensor_timestamp + acceptance_interval > _newest_high_rate_imu_sample.time_us; } void startAirspeedFusion(); diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index ad3ce11d56..588e25f9f3 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -121,7 +121,7 @@ void Ekf::resetHorizontalVelocityTo(const Vector2f &new_horz_vel) _state_reset_status.velNE_counter++; // Reset the timout timer - _time_last_hor_vel_fuse = _time_last_imu; + _time_last_hor_vel_fuse = _imu_sample_delayed.time_us; } void Ekf::resetVerticalVelocityTo(float new_vert_vel) @@ -141,7 +141,7 @@ void Ekf::resetVerticalVelocityTo(float new_vert_vel) _state_reset_status.velD_counter++; // Reset the timout timer - _time_last_ver_vel_fuse = _time_last_imu; + _time_last_ver_vel_fuse = _imu_sample_delayed.time_us; } void Ekf::resetHorizontalPositionToGps(const gpsSample &gps_sample_delayed) @@ -210,7 +210,7 @@ void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos) _state_reset_status.posNE_counter++; // Reset the timout timer - _time_last_hor_pos_fuse = _time_last_imu; + _time_last_hor_pos_fuse = _imu_sample_delayed.time_us; } bool Ekf::isHeightResetRequired() const @@ -248,7 +248,7 @@ void Ekf::resetVerticalPositionTo(const float new_vert_pos) _output_vert_new.vert_vel_integ = _state.pos(2); // Reset the timout timer - _time_last_hgt_fuse = _time_last_imu; + _time_last_hgt_fuse = _imu_sample_delayed.time_us; } void Ekf::resetVerticalVelocityToGps(const gpsSample &gps_sample_delayed) @@ -414,7 +414,7 @@ bool Ekf::resetMagHeading() const Vector3f mag_init = _mag_lpf.getState(); - const bool mag_available = (_mag_counter != 0) && isRecent(_time_last_mag, 500000) + const bool mag_available = (_mag_counter != 0) && isNewestSampleRecent(_time_last_mag_buffer_push, 500'000) && !magFieldStrengthDisturbed(mag_init); // low pass filtered mag required @@ -629,7 +629,7 @@ matrix::Vector Ekf::getStateAtFusionHorizonAsVector() const bool Ekf::getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &longitude, float &origin_alt) const { - origin_time = _last_gps_origin_time_us; + origin_time = _pos_ref.getProjectionReferenceTimestamp(); latitude = _pos_ref.getProjectionReferenceLat(); longitude = _pos_ref.getProjectionReferenceLon(); origin_alt = getEkfGlobalOriginAltitude(); @@ -656,7 +656,7 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons const float gps_alt_ref_prev = getEkfGlobalOriginAltitude(); // reinitialize map projection to latitude, longitude, altitude, and reset position - _pos_ref.initReference(latitude, longitude, _time_last_imu); + _pos_ref.initReference(latitude, longitude, _imu_sample_delayed.time_us); _gps_alt_ref = altitude; // minimum change in position or height that triggers a reset @@ -1014,8 +1014,8 @@ void Ekf::update_deadreckoning_status() _control_status.flags.inertial_dead_reckoning = !velPosAiding && !optFlowAiding && !airDataAiding; if (!_control_status.flags.inertial_dead_reckoning) { - if (_time_last_imu > _params.no_aid_timeout_max) { - _time_last_aiding = _time_last_imu - _params.no_aid_timeout_max; + if (_imu_sample_delayed.time_us > _params.no_aid_timeout_max) { + _time_last_aiding = _imu_sample_delayed.time_us - _params.no_aid_timeout_max; } } @@ -1638,7 +1638,7 @@ bool Ekf::resetYawToEKFGSF() _inhibit_ev_yaw_use = true; } - _ekfgsf_yaw_reset_time = _time_last_imu; + _ekfgsf_yaw_reset_time = _imu_sample_delayed.time_us; _ekfgsf_yaw_reset_count++; return true; diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index a2caec8914..f6da782727 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -65,11 +65,9 @@ void EstimatorInterface::setIMUData(const imuSample &imu_sample) _initialised = init(imu_sample.time_us); } - const float dt = math::constrain((imu_sample.time_us - _time_last_imu) / 1e6f, 1.0e-4f, 0.02f); + const float dt = math::constrain((imu_sample.time_us - _newest_high_rate_imu_sample.time_us) / 1e6f, 0.0001f, 0.02f); - _time_last_imu = imu_sample.time_us; - - if (_time_last_imu > 0) { + if (_newest_high_rate_imu_sample.time_us > 0) { _dt_imu_avg = 0.8f * _dt_imu_avg + 0.2f * dt; } @@ -111,21 +109,21 @@ void EstimatorInterface::setMagData(const magSample &mag_sample) } } + const uint64_t time_us = mag_sample.time_us + - static_cast(_params.mag_delay_ms * 1000) + - static_cast(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2 + // limit data rate to prevent data being lost - if ((mag_sample.time_us - _time_last_mag) > _min_obs_interval_us) { - _time_last_mag = mag_sample.time_us; + if (time_us >= _mag_buffer->get_newest().time_us + _min_obs_interval_us) { - magSample mag_sample_new; - - mag_sample_new.time_us = mag_sample.time_us; - mag_sample_new.time_us -= static_cast(_params.mag_delay_ms * 1000); - mag_sample_new.time_us -= static_cast(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2 - - mag_sample_new.mag = mag_sample.mag; + magSample mag_sample_new{mag_sample}; + mag_sample_new.time_us = time_us; _mag_buffer->push(mag_sample_new); + _time_last_mag_buffer_push = _newest_high_rate_imu_sample.time_us; + } else { - ECL_ERR("mag data too fast %" PRIu64, mag_sample.time_us - _time_last_mag); + ECL_WARN("mag data too fast %" PRIu64 " < %" PRIu64 " + %d", time_us, _mag_buffer->get_newest().time_us, _min_obs_interval_us); } } @@ -147,13 +145,15 @@ void EstimatorInterface::setGpsData(const gpsMessage &gps) } } - if ((gps.time_usec - _time_last_gps) > _min_obs_interval_us) { - _time_last_gps = gps.time_usec; + const uint64_t time_us = gps.time_usec + - static_cast(_params.gps_delay_ms * 1000) + - static_cast(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2 + + if (time_us >= _gps_buffer->get_newest().time_us + _min_obs_interval_us) { gpsSample gps_sample_new; - gps_sample_new.time_us = gps.time_usec - static_cast(_params.gps_delay_ms * 1000); - gps_sample_new.time_us -= static_cast(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2 + gps_sample_new.time_us = time_us; gps_sample_new.vel = gps.vel_ned; @@ -183,8 +183,14 @@ void EstimatorInterface::setGpsData(const gpsMessage &gps) } _gps_buffer->push(gps_sample_new); + _time_last_gps_buffer_push = _newest_high_rate_imu_sample.time_us; + + if (PX4_ISFINITE(gps.yaw)) { + _time_last_gps_yaw_buffer_push = _newest_high_rate_imu_sample.time_us; + } + } else { - ECL_ERR("GPS data too fast %" PRIu64, gps.time_usec - _time_last_gps); + ECL_WARN("GPS data too fast %" PRIu64 " < %" PRIu64 " + %d", time_us, _gps_buffer->get_newest().time_us, _min_obs_interval_us); } } @@ -206,20 +212,22 @@ void EstimatorInterface::setBaroData(const baroSample &baro_sample) } } + const uint64_t time_us = baro_sample.time_us + - static_cast(_params.baro_delay_ms * 1000) + - static_cast(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2 + // limit data rate to prevent data being lost - if ((baro_sample.time_us - _time_last_baro) > _min_obs_interval_us) { - _time_last_baro = baro_sample.time_us; + if (time_us >= _baro_buffer->get_newest().time_us + _min_obs_interval_us) { baroSample baro_sample_new; + baro_sample_new.time_us = time_us; baro_sample_new.hgt = compensateBaroForDynamicPressure(baro_sample.hgt); - baro_sample_new.time_us = baro_sample.time_us; - baro_sample_new.time_us -= static_cast(_params.baro_delay_ms * 1000); - baro_sample_new.time_us -= static_cast(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2 - _baro_buffer->push(baro_sample_new); + _time_last_baro_buffer_push = _newest_high_rate_imu_sample.time_us; + } else { - ECL_ERR("baro data too fast %" PRIu64, baro_sample.time_us - _time_last_baro); + ECL_WARN("baro data too fast %" PRIu64 " < %" PRIu64 " + %d", time_us, _baro_buffer->get_newest().time_us, _min_obs_interval_us); } } @@ -241,16 +249,20 @@ void EstimatorInterface::setAirspeedData(const airspeedSample &airspeed_sample) } } + const uint64_t time_us = airspeed_sample.time_us + - static_cast(_params.airspeed_delay_ms * 1000) + - static_cast(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2 + // limit data rate to prevent data being lost - if ((airspeed_sample.time_us - _time_last_airspeed) > _min_obs_interval_us) { - _time_last_airspeed = airspeed_sample.time_us; + if (time_us >= _airspeed_buffer->get_newest().time_us + _min_obs_interval_us) { - airspeedSample airspeed_sample_new = airspeed_sample; - - airspeed_sample_new.time_us -= static_cast(_params.airspeed_delay_ms * 1000); - airspeed_sample_new.time_us -= static_cast(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2 + airspeedSample airspeed_sample_new{airspeed_sample}; + airspeed_sample_new.time_us = time_us; _airspeed_buffer->push(airspeed_sample_new); + + } else { + ECL_WARN("airspeed data too fast %" PRIu64 " < %" PRIu64 " + %d", time_us, _airspeed_buffer->get_newest().time_us, _min_obs_interval_us); } } @@ -272,15 +284,21 @@ void EstimatorInterface::setRangeData(const rangeSample &range_sample) } } - // limit data rate to prevent data being lost - if ((range_sample.time_us - _time_last_range) > _min_obs_interval_us) { - _time_last_range = range_sample.time_us; + const uint64_t time_us = range_sample.time_us + - static_cast(_params.range_delay_ms * 1000) + - static_cast(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2 - rangeSample range_sample_new = range_sample; - range_sample_new.time_us -= static_cast(_params.range_delay_ms * 1000); - range_sample_new.time_us -= static_cast(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2 + // limit data rate to prevent data being lost + if (time_us >= _range_buffer->get_newest().time_us + _min_obs_interval_us) { + + rangeSample range_sample_new{range_sample}; + range_sample_new.time_us = time_us; _range_buffer->push(range_sample_new); + _time_last_range_buffer_push = _newest_high_rate_imu_sample.time_us; + + } else { + ECL_WARN("range data too fast %" PRIu64 " < %" PRIu64 " + %d", time_us, _range_buffer->get_newest().time_us, _min_obs_interval_us); } } @@ -302,16 +320,20 @@ void EstimatorInterface::setOpticalFlowData(const flowSample &flow) } } + const uint64_t time_us = flow.time_us + - static_cast(_params.flow_delay_ms * 1000) + - static_cast(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2 + // limit data rate to prevent data being lost - if ((flow.time_us - _time_last_optflow) > _min_obs_interval_us) { - _time_last_optflow = flow.time_us; + if (time_us >= _flow_buffer->get_newest().time_us + _min_obs_interval_us) { - flowSample optflow_sample_new = flow; - - optflow_sample_new.time_us -= static_cast(_params.flow_delay_ms * 1000); - optflow_sample_new.time_us -= static_cast(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2 + flowSample optflow_sample_new{flow}; + optflow_sample_new.time_us = time_us; _flow_buffer->push(optflow_sample_new); + + } else { + ECL_WARN("optical flow data too fast %" PRIu64 " < %" PRIu64 " + %d", time_us, _flow_buffer->get_newest().time_us, _min_obs_interval_us); } } @@ -334,16 +356,22 @@ void EstimatorInterface::setExtVisionData(const extVisionSample &evdata) } } - // limit data rate to prevent data being lost - if ((evdata.time_us - _time_last_ext_vision) > _min_obs_interval_us) { - _time_last_ext_vision = evdata.time_us; + // calculate the system time-stamp for the mid point of the integration period + const uint64_t time_us = evdata.time_us + - static_cast(_params.ev_delay_ms * 1000) + - static_cast(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2 - extVisionSample ev_sample_new = evdata; - // calculate the system time-stamp for the mid point of the integration period - ev_sample_new.time_us -= static_cast(_params.ev_delay_ms * 1000); - ev_sample_new.time_us -= static_cast(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2 + // limit data rate to prevent data being lost + if (time_us >= _ext_vision_buffer->get_newest().time_us + _min_obs_interval_us) { + + extVisionSample ev_sample_new{evdata}; + ev_sample_new.time_us = time_us; _ext_vision_buffer->push(ev_sample_new); + _time_last_ext_vision_buffer_push = _newest_high_rate_imu_sample.time_us; + + } else { + ECL_WARN("EV data too fast %" PRIu64 " < %" PRIu64 " + %d", time_us, _ext_vision_buffer->get_newest().time_us, _min_obs_interval_us); } } @@ -365,16 +393,20 @@ void EstimatorInterface::setAuxVelData(const auxVelSample &auxvel_sample) } } + const uint64_t time_us = auxvel_sample.time_us + - static_cast(_params.auxvel_delay_ms * 1000) + - static_cast(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2 + // limit data rate to prevent data being lost - if ((auxvel_sample.time_us - _time_last_auxvel) > _min_obs_interval_us) { - _time_last_auxvel = auxvel_sample.time_us; + if (time_us >= _auxvel_buffer->get_newest().time_us + _min_obs_interval_us) { - auxVelSample auxvel_sample_new = auxvel_sample; - - auxvel_sample_new.time_us -= static_cast(_params.auxvel_delay_ms * 1000); - auxvel_sample_new.time_us -= static_cast(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2 + auxVelSample auxvel_sample_new{auxvel_sample}; + auxvel_sample_new.time_us = time_us; _auxvel_buffer->push(auxvel_sample_new); + + } else { + ECL_WARN("aux velocity data too fast %" PRIu64 " < %" PRIu64 " + %d", time_us, _auxvel_buffer->get_newest().time_us, _min_obs_interval_us); } } @@ -396,7 +428,7 @@ void EstimatorInterface::setDragData(const imuSample &imu) } } - _drag_sample_count ++; + _drag_sample_count++; // note acceleration is accumulated as a delta velocity _drag_down_sampled.accelXY(0) += imu.delta_vel(0); _drag_down_sampled.accelXY(1) += imu.delta_vel(1); diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 0bb9eb2fcf..567ef7e522 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -108,10 +108,10 @@ public: void set_in_air_status(bool in_air) { if (!in_air) { - _time_last_on_ground_us = _time_last_imu; + _time_last_on_ground_us = _imu_sample_delayed.time_us; } else { - _time_last_in_air = _time_last_imu; + _time_last_in_air = _imu_sample_delayed.time_us; } _control_status.flags.in_air = in_air; @@ -140,7 +140,7 @@ public: void set_gnd_effect() { _control_status.flags.gnd_effect = true; - _time_last_gnd_effect_on = _time_last_imu; + _time_last_gnd_effect_on = _imu_sample_delayed.time_us; } // set air density used by the multi-rotor specific drag force fusion @@ -373,17 +373,13 @@ protected: RingBuffer *_drag_buffer{nullptr}; RingBuffer *_auxvel_buffer{nullptr}; - // timestamps of latest in buffer saved measurement in microseconds - uint64_t _time_last_imu{0}; - uint64_t _time_last_gps{0}; - uint64_t _time_last_mag{0}; ///< measurement time of last magnetomter sample (uSec) - uint64_t _time_last_baro{0}; - uint64_t _time_last_range{0}; - uint64_t _time_last_airspeed{0}; - uint64_t _time_last_ext_vision{0}; - uint64_t _time_last_optflow{0}; - uint64_t _time_last_auxvel{0}; - //last time the baro ground effect compensation was turned on externally (uSec) + uint64_t _time_last_gps_buffer_push{0}; + uint64_t _time_last_gps_yaw_buffer_push{0}; + uint64_t _time_last_mag_buffer_push{0}; + uint64_t _time_last_baro_buffer_push{0}; + uint64_t _time_last_range_buffer_push{0}; + uint64_t _time_last_ext_vision_buffer_push{0}; + uint64_t _time_last_gnd_effect_on{0}; fault_status_u _fault_status{}; diff --git a/src/modules/ekf2/EKF/ev_height_control.cpp b/src/modules/ekf2/EKF/ev_height_control.cpp index 71d8378cb9..73f6421cff 100644 --- a/src/modules/ekf2/EKF/ev_height_control.cpp +++ b/src/modules/ekf2/EKF/ev_height_control.cpp @@ -47,7 +47,7 @@ void Ekf::controlEvHeightFusion() _ev_hgt_b_est.predict(_dt_ekf_avg); - const bool ev_intermittent = !isRecent(_time_last_ext_vision, 2 * EV_MAX_INTERVAL); + const bool ev_intermittent = !isNewestSampleRecent(_time_last_ext_vision_buffer_push, 2 * EV_MAX_INTERVAL); if (_ev_data_ready) { const bool continuing_conditions_passing = !ev_intermittent; diff --git a/src/modules/ekf2/EKF/fake_pos_control.cpp b/src/modules/ekf2/EKF/fake_pos_control.cpp index 81add54243..a59c006fdb 100644 --- a/src/modules/ekf2/EKF/fake_pos_control.cpp +++ b/src/modules/ekf2/EKF/fake_pos_control.cpp @@ -99,8 +99,8 @@ void Ekf::resetFakePosFusion() resetHorizontalPositionToLastKnown(); resetHorizontalVelocityToZero(); - _aid_src_fake_pos.time_last_fuse[0] = _time_last_imu; - _aid_src_fake_pos.time_last_fuse[1] = _time_last_imu; + _aid_src_fake_pos.time_last_fuse[0] = _imu_sample_delayed.time_us; + _aid_src_fake_pos.time_last_fuse[1] = _imu_sample_delayed.time_us; } void Ekf::stopFakePosFusion() @@ -151,10 +151,10 @@ void Ekf::fuseFakePosition() if (fake_pos.fusion_enabled[i] && !fake_pos.innovation_rejected[i]) { if (fuseVelPosHeight(fake_pos.innovation[i], fake_pos.innovation_variance[i], 3 + i)) { fake_pos.fused[i] = true; - fake_pos.time_last_fuse[i] = _time_last_imu; + fake_pos.time_last_fuse[i] = _imu_sample_delayed.time_us; } } } - fake_pos.timestamp_sample = _time_last_imu; + fake_pos.timestamp_sample = _imu_sample_delayed.time_us; } diff --git a/src/modules/ekf2/EKF/gnss_height_control.cpp b/src/modules/ekf2/EKF/gnss_height_control.cpp index 5f30e60669..7b24e0bf27 100644 --- a/src/modules/ekf2/EKF/gnss_height_control.cpp +++ b/src/modules/ekf2/EKF/gnss_height_control.cpp @@ -94,7 +94,7 @@ void Ekf::startGpsHgtFusion() _gps_hgt_b_est.setBias(_state.pos(2) + (_gps_sample_delayed.hgt - getEkfGlobalOriginAltitude())); // Reset the timeout value here because the fusion isn't done at the same place and would immediately trigger a timeout - _aid_src_gnss_pos.time_last_fuse[2] = _time_last_imu; + _aid_src_gnss_pos.time_last_fuse[2] = _imu_sample_delayed.time_us; } _control_status.flags.gps_hgt = true; @@ -117,7 +117,7 @@ void Ekf::resetHeightToGps() _rng_hgt_b_est.setBias(_rng_hgt_b_est.getBias() + _state_reset_status.posD_change); _ev_hgt_b_est.setBias(_ev_hgt_b_est.getBias() - _state_reset_status.posD_change); - _aid_src_gnss_pos.time_last_fuse[2] = _time_last_imu; + _aid_src_gnss_pos.time_last_fuse[2] = _imu_sample_delayed.time_us; } void Ekf::stopGpsHgtFusion() diff --git a/src/modules/ekf2/EKF/gps_checks.cpp b/src/modules/ekf2/EKF/gps_checks.cpp index 8b69f2f71b..97385fbd25 100644 --- a/src/modules/ekf2/EKF/gps_checks.cpp +++ b/src/modules/ekf2/EKF/gps_checks.cpp @@ -66,14 +66,14 @@ bool Ekf::collect_gps(const gpsMessage &gps) const double lon = gps.lon * 1.0e-7; if (!_pos_ref.isInitialized()) { - _pos_ref.initReference(lat, lon, _time_last_imu); + _pos_ref.initReference(lat, lon, gps.time_usec); // if we are already doing aiding, correct for the change in position since the EKF started navigating if (isHorizontalAidingActive()) { double est_lat; double est_lon; _pos_ref.reproject(-_state.pos(0), -_state.pos(1), est_lat, est_lon); - _pos_ref.initReference(est_lat, est_lon, _time_last_imu); + _pos_ref.initReference(est_lat, est_lon, gps.time_usec); } } @@ -85,7 +85,6 @@ bool Ekf::collect_gps(const gpsMessage &gps) _NED_origin_initialised = true; _earth_rate_NED = calcEarthRateNED((float)math::radians(_pos_ref.getProjectionReferenceLat())); - _last_gps_origin_time_us = _time_last_imu; const bool declination_was_valid = PX4_ISFINITE(_mag_declination_gps); @@ -168,7 +167,7 @@ bool Ekf::gps_is_good(const gpsMessage &gps) // Calculate time lapsed since last update, limit to prevent numerical errors and calculate a lowpass filter coefficient constexpr float filt_time_const = 10.0f; - const float dt = math::constrain(float(int64_t(_time_last_imu) - int64_t(_gps_pos_prev.getProjectionReferenceTimestamp())) * 1e-6f, 0.001f, filt_time_const); + const float dt = math::constrain(float(int64_t(gps.time_usec) - int64_t(_gps_pos_prev.getProjectionReferenceTimestamp())) * 1e-6f, 0.001f, filt_time_const); const float filter_coef = dt / filt_time_const; // The following checks are only valid when the vehicle is at rest @@ -186,7 +185,7 @@ bool Ekf::gps_is_good(const gpsMessage &gps) } else { // no previous position has been set - _gps_pos_prev.initReference(lat, lon, _time_last_imu); + _gps_pos_prev.initReference(lat, lon, gps.time_usec); _gps_alt_prev = 1e-3f * (float)gps.alt; } @@ -229,7 +228,7 @@ bool Ekf::gps_is_good(const gpsMessage &gps) } // save GPS fix for next time - _gps_pos_prev.initReference(lat, lon, _time_last_imu); + _gps_pos_prev.initReference(lat, lon, gps.time_usec); _gps_alt_prev = 1e-3f * (float)gps.alt; // Check the filtered difference between GPS and EKF vertical velocity @@ -240,7 +239,7 @@ bool Ekf::gps_is_good(const gpsMessage &gps) // assume failed first time through if (_last_gps_fail_us == 0) { - _last_gps_fail_us = _time_last_imu; + _last_gps_fail_us = _imu_sample_delayed.time_us; } // if any user selected checks have failed, record the fail time @@ -256,10 +255,10 @@ bool Ekf::gps_is_good(const gpsMessage &gps) (_gps_check_fail_status.flags.hspeed && (_params.gps_check_mask & MASK_GPS_HSPD)) || (_gps_check_fail_status.flags.vspeed && (_params.gps_check_mask & MASK_GPS_VSPD)) ) { - _last_gps_fail_us = _time_last_imu; + _last_gps_fail_us = _imu_sample_delayed.time_us; } else { - _last_gps_pass_us = _time_last_imu; + _last_gps_pass_us = _imu_sample_delayed.time_us; } // continuous period without fail of x seconds required to return a healthy status diff --git a/src/modules/ekf2/EKF/gps_control.cpp b/src/modules/ekf2/EKF/gps_control.cpp index 8880737bf0..d5e2ac1567 100644 --- a/src/modules/ekf2/EKF/gps_control.cpp +++ b/src/modules/ekf2/EKF/gps_control.cpp @@ -158,13 +158,14 @@ void Ekf::controlGpsFusion() } } - } else if (_control_status.flags.gps && (_imu_sample_delayed.time_us - _gps_sample_delayed.time_us > (uint64_t)10e6)) { + } else if (_control_status.flags.gps && !isNewestSampleRecent(_time_last_gps_buffer_push, (uint64_t)10e6)) { stopGpsFusion(); _warning_events.flags.gps_data_stopped = true; ECL_WARN("GPS data stopped"); - } else if (_control_status.flags.gps && (_imu_sample_delayed.time_us - _gps_sample_delayed.time_us > (uint64_t)1e6) + } else if (_control_status.flags.gps && !isNewestSampleRecent(_time_last_gps_buffer_push, (uint64_t)1e6) && isOtherSourceOfHorizontalAidingThan(_control_status.flags.gps)) { + // Handle the case where we are fusing another position source along GPS, // stop waiting for GPS after 1 s of lost signal stopGpsFusion(); diff --git a/src/modules/ekf2/EKF/gps_fusion.cpp b/src/modules/ekf2/EKF/gps_fusion.cpp index 1497713bb9..82ba9e1e65 100644 --- a/src/modules/ekf2/EKF/gps_fusion.cpp +++ b/src/modules/ekf2/EKF/gps_fusion.cpp @@ -173,7 +173,7 @@ void Ekf::fuseGpsVel() for (int i = 0; i < 2; i++) { if (fuseVelPosHeight(gps_vel.innovation[i], gps_vel.innovation_variance[i], i)) { gps_vel.fused[i] = true; - gps_vel.time_last_fuse[i] = _time_last_imu; + gps_vel.time_last_fuse[i] = _imu_sample_delayed.time_us; } } } @@ -184,7 +184,7 @@ void Ekf::fuseGpsVel() if (gps_vel.fusion_enabled[2] && !gps_vel.innovation_rejected[2]) { if (fuseVelPosHeight(gps_vel.innovation[2], gps_vel.innovation_variance[2], 2)) { gps_vel.fused[2] = true; - gps_vel.time_last_fuse[2] = _time_last_imu; + gps_vel.time_last_fuse[2] = _imu_sample_delayed.time_us; } } } @@ -201,7 +201,7 @@ void Ekf::fuseGpsPos() for (int i = 0; i < 2; i++) { if (fuseVelPosHeight(gps_pos.innovation[i], gps_pos.innovation_variance[i], 3 + i)) { gps_pos.fused[i] = true; - gps_pos.time_last_fuse[i] = _time_last_imu; + gps_pos.time_last_fuse[i] = _imu_sample_delayed.time_us; } } } @@ -212,7 +212,7 @@ void Ekf::fuseGpsPos() if (gps_pos.fusion_enabled[2] && !gps_pos.innovation_rejected[2]) { if (fuseVelPosHeight(gps_pos.innovation[2], gps_pos.innovation_variance[2], 5)) { gps_pos.fused[2] = true; - gps_pos.time_last_fuse[2] = _time_last_imu; + gps_pos.time_last_fuse[2] = _imu_sample_delayed.time_us; } } } diff --git a/src/modules/ekf2/EKF/gps_yaw_fusion.cpp b/src/modules/ekf2/EKF/gps_yaw_fusion.cpp index f363bca17a..d0ff437560 100644 --- a/src/modules/ekf2/EKF/gps_yaw_fusion.cpp +++ b/src/modules/ekf2/EKF/gps_yaw_fusion.cpp @@ -196,8 +196,8 @@ void Ekf::fuseGpsYaw() _aid_src_gnss_yaw.fused = is_fused; if (is_fused) { - _time_last_heading_fuse = _time_last_imu; - _aid_src_gnss_yaw.time_last_fuse = _time_last_imu; + _time_last_heading_fuse = _imu_sample_delayed.time_us; + _aid_src_gnss_yaw.time_last_fuse = _imu_sample_delayed.time_us; } } @@ -218,7 +218,7 @@ bool Ekf::resetYawToGps() const float yaw_variance = sq(fmaxf(_params.gps_heading_noise, 1.e-2f)); resetQuatStateYaw(measured_yaw, yaw_variance, true); - _aid_src_gnss_yaw.time_last_fuse = _time_last_imu; + _aid_src_gnss_yaw.time_last_fuse = _imu_sample_delayed.time_us; _gnss_yaw_signed_test_ratio_lpf.reset(0.f); return true; diff --git a/src/modules/ekf2/EKF/height_control.cpp b/src/modules/ekf2/EKF/height_control.cpp index bba9d5e84d..5dc2919b7f 100644 --- a/src/modules/ekf2/EKF/height_control.cpp +++ b/src/modules/ekf2/EKF/height_control.cpp @@ -142,10 +142,10 @@ void Ekf::checkVerticalAccelerationHealth() || (inertial_nav_falling_likelihood == Likelihood::HIGH); if (bad_vert_accel) { - _time_bad_vert_accel = _time_last_imu; + _time_bad_vert_accel = _imu_sample_delayed.time_us; } else { - _time_good_vert_accel = _time_last_imu; + _time_good_vert_accel = _imu_sample_delayed.time_us; } // declare a bad vertical acceleration measurement and make the declaration persist diff --git a/src/modules/ekf2/EKF/height_fusion.cpp b/src/modules/ekf2/EKF/height_fusion.cpp index d83232d73d..e3be98581b 100644 --- a/src/modules/ekf2/EKF/height_fusion.cpp +++ b/src/modules/ekf2/EKF/height_fusion.cpp @@ -102,7 +102,7 @@ void Ekf::fuseBaroHgt(estimator_aid_source_1d_s &baro_hgt) && fuseVelPosHeight(baro_hgt.innovation, baro_hgt.innovation_variance, 5)) { baro_hgt.fused = true; - baro_hgt.time_last_fuse = _time_last_imu; + baro_hgt.time_last_fuse = _imu_sample_delayed.time_us; } } @@ -155,7 +155,7 @@ void Ekf::fuseRngHgt(estimator_aid_source_1d_s &rng_hgt) && fuseVelPosHeight(rng_hgt.innovation, rng_hgt.innovation_variance, 5)) { rng_hgt.fused = true; - rng_hgt.time_last_fuse = _time_last_imu; + rng_hgt.time_last_fuse = _imu_sample_delayed.time_us; } } diff --git a/src/modules/ekf2/EKF/mag_fusion.cpp b/src/modules/ekf2/EKF/mag_fusion.cpp index 1b1ffe23e2..7da4b11893 100644 --- a/src/modules/ekf2/EKF/mag_fusion.cpp +++ b/src/modules/ekf2/EKF/mag_fusion.cpp @@ -401,7 +401,7 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source_3d_s &aid_src_mag, b if (is_fused) { aid_src_mag.fused[index] = true; - aid_src_mag.time_last_fuse[index] = _time_last_imu; + aid_src_mag.time_last_fuse[index] = _imu_sample_delayed.time_us; } else { aid_src_mag.fused[index] = false; @@ -672,8 +672,8 @@ bool Ekf::fuseYaw(const float innovation, const float variance, estimator_aid_so // apply the state corrections fuse(Kfusion, aid_src_status.innovation); - _time_last_heading_fuse = _time_last_imu; - aid_src_status.time_last_fuse = _time_last_imu; + _time_last_heading_fuse = _imu_sample_delayed.time_us; + aid_src_status.time_last_fuse = _imu_sample_delayed.time_us; aid_src_status.fused = true; return true; diff --git a/src/modules/ekf2/EKF/optflow_fusion.cpp b/src/modules/ekf2/EKF/optflow_fusion.cpp index 69470e0778..683fb35cd6 100644 --- a/src/modules/ekf2/EKF/optflow_fusion.cpp +++ b/src/modules/ekf2/EKF/optflow_fusion.cpp @@ -328,7 +328,7 @@ void Ekf::fuseOptFlow() } if (is_fused) { - _time_last_of_fuse = _time_last_imu; + _time_last_of_fuse = _imu_sample_delayed.time_us; } } } diff --git a/src/modules/ekf2/EKF/range_height_control.cpp b/src/modules/ekf2/EKF/range_height_control.cpp index d654caf4ca..cec9ab6dbc 100644 --- a/src/modules/ekf2/EKF/range_height_control.cpp +++ b/src/modules/ekf2/EKF/range_height_control.cpp @@ -47,7 +47,7 @@ void Ekf::controlRangeHeightFusion() _rng_hgt_b_est.predict(_dt_ekf_avg); - const bool rng_intermittent = !isRecent(_time_last_range, 2 * RNG_MAX_INTERVAL); + const bool rng_intermittent = !isNewestSampleRecent(_time_last_range_buffer_push, 2 * RNG_MAX_INTERVAL); // If we are supposed to be using range finder data as the primary height sensor, have bad range measurements // and are on the ground, then synthesise a measurement at the expected on ground value diff --git a/src/modules/ekf2/EKF/sideslip_fusion.cpp b/src/modules/ekf2/EKF/sideslip_fusion.cpp index 25a610a4ce..b8cf037095 100644 --- a/src/modules/ekf2/EKF/sideslip_fusion.cpp +++ b/src/modules/ekf2/EKF/sideslip_fusion.cpp @@ -211,7 +211,7 @@ void Ekf::fuseSideslip() _fault_status.flags.bad_sideslip = !is_fused; if (is_fused) { - _time_last_beta_fuse = _time_last_imu; + _time_last_beta_fuse = _imu_sample_delayed.time_us; } } } diff --git a/src/modules/ekf2/EKF/terrain_estimator.cpp b/src/modules/ekf2/EKF/terrain_estimator.cpp index 14255f6d71..218b993abb 100644 --- a/src/modules/ekf2/EKF/terrain_estimator.cpp +++ b/src/modules/ekf2/EKF/terrain_estimator.cpp @@ -99,7 +99,7 @@ void Ekf::controlHaglRngFusion() //const bool continuing_conditions_passing = _control_status.flags.in_air && !_control_status.flags.rng_hgt; // TODO: should not be fused when using range height const bool starting_conditions_passing = continuing_conditions_passing && _range_sensor.isRegularlySendingData() && (_rng_consistency_check.getTestRatio() < 1.f); - _time_last_healthy_rng_data = _time_last_imu; + _time_last_healthy_rng_data = _imu_sample_delayed.time_us; if (_hagl_sensor_status.flags.range_finder) { if (continuing_conditions_passing) { @@ -188,7 +188,7 @@ void Ekf::resetHaglRng() _terrain_vpos = _state.pos(2) + _range_sensor.getDistBottom(); _terrain_var = getRngVar(); _terrain_vpos_reset_counter++; - _time_last_hagl_fuse = _time_last_imu; + _time_last_hagl_fuse = _imu_sample_delayed.time_us; } void Ekf::stopHaglRngFusion() @@ -229,7 +229,7 @@ void Ekf::fuseHaglRng() // correct the variance _terrain_var = fmaxf(_terrain_var * (1.0f - gain), 0.0f); // record last successful fusion event - _time_last_hagl_fuse = _time_last_imu; + _time_last_hagl_fuse = _imu_sample_delayed.time_us; _innov_check_fail_status.flags.reject_hagl = false; } else { @@ -376,7 +376,7 @@ void Ekf::fuseFlowForTerrain() _terrain_vpos += Kx * _flow_innov(0); // guard against negative variance _terrain_var = fmaxf(_terrain_var - KxHxP, 0.0f); - _time_last_flow_terrain_fuse = _time_last_imu; + _time_last_flow_terrain_fuse = _imu_sample_delayed.time_us; } // Calculate observation matrix for flow around the vehicle y axis @@ -404,7 +404,7 @@ void Ekf::fuseFlowForTerrain() _terrain_vpos += Ky * _flow_innov(1); // guard against negative variance _terrain_var = fmaxf(_terrain_var - KyHyP, 0.0f); - _time_last_flow_terrain_fuse = _time_last_imu; + _time_last_flow_terrain_fuse = _imu_sample_delayed.time_us; } } @@ -423,7 +423,7 @@ void Ekf::resetHaglFake() _terrain_vpos = _state.pos(2) + _params.rng_gnd_clearance; // use the ground clearance value as our uncertainty _terrain_var = sq(_params.rng_gnd_clearance); - _time_last_hagl_fuse = _time_last_imu; + _time_last_hagl_fuse = _imu_sample_delayed.time_us; } void Ekf::updateTerrainValidity() diff --git a/src/modules/ekf2/EKF/vel_pos_fusion.cpp b/src/modules/ekf2/EKF/vel_pos_fusion.cpp index 8a29487eb4..1ffe0e0ce1 100644 --- a/src/modules/ekf2/EKF/vel_pos_fusion.cpp +++ b/src/modules/ekf2/EKF/vel_pos_fusion.cpp @@ -74,7 +74,7 @@ bool Ekf::fuseVerticalVelocity(const Vector3f &innov, const float innov_gate, co innov_var(2) = P(6, 6) + obs_var(2); test_ratio(1) = sq(innov(2)) / (sq(innov_gate) * innov_var(2)); _vert_vel_innov_ratio = innov(2) / sqrtf(innov_var(2)); - _vert_vel_fuse_time_us = _time_last_imu; + _vert_vel_fuse_time_us = _imu_sample_delayed.time_us; bool innov_check_pass = (test_ratio(1) <= 1.0f); // if there is bad vertical acceleration data, then don't reject measurement, @@ -132,7 +132,7 @@ bool Ekf::fuseVerticalPosition(const float innov, const float innov_gate, const innov_var = P(9, 9) + obs_var; test_ratio = sq(innov) / (sq(innov_gate) * innov_var); _vert_pos_innov_ratio = innov / sqrtf(innov_var); - _vert_pos_fuse_attempt_time_us = _time_last_imu; + _vert_pos_fuse_attempt_time_us = _imu_sample_delayed.time_us; bool innov_check_pass = test_ratio <= 1.0f; // if there is bad vertical acceleration data, then don't reject measurement, @@ -218,7 +218,7 @@ void Ekf::fuseVelocity(estimator_aid_source_3d_s& vel_aid_src) for (int i = 0; i < 2; i++) { if (fuseVelPosHeight(vel_aid_src.innovation[i], vel_aid_src.innovation_variance[i], i)) { vel_aid_src.fused[i] = true; - vel_aid_src.time_last_fuse[i] = _time_last_imu; + vel_aid_src.time_last_fuse[i] = _imu_sample_delayed.time_us; } } } @@ -227,7 +227,7 @@ void Ekf::fuseVelocity(estimator_aid_source_3d_s& vel_aid_src) if (vel_aid_src.fusion_enabled[2] && !vel_aid_src.innovation_rejected[2]) { if (fuseVelPosHeight(vel_aid_src.innovation[2], vel_aid_src.innovation_variance[2], 2)) { vel_aid_src.fused[2] = true; - vel_aid_src.time_last_fuse[2] = _time_last_imu; + vel_aid_src.time_last_fuse[2] = _imu_sample_delayed.time_us; } } } @@ -241,7 +241,7 @@ void Ekf::fusePosition(estimator_aid_source_3d_s& pos_aid_src) for (int i = 0; i < 2; i++) { if (fuseVelPosHeight(pos_aid_src.innovation[i], pos_aid_src.innovation_variance[i], 3 + i)) { pos_aid_src.fused[i] = true; - pos_aid_src.time_last_fuse[i] = _time_last_imu; + pos_aid_src.time_last_fuse[i] = _imu_sample_delayed.time_us; } } } @@ -250,7 +250,7 @@ void Ekf::fusePosition(estimator_aid_source_3d_s& pos_aid_src) if (pos_aid_src.fusion_enabled[2] && !pos_aid_src.innovation_rejected[2]) { if (fuseVelPosHeight(pos_aid_src.innovation[2], pos_aid_src.innovation_variance[2], 5)) { pos_aid_src.fused[2] = true; - pos_aid_src.time_last_fuse[2] = _time_last_imu; + pos_aid_src.time_last_fuse[2] = _imu_sample_delayed.time_us; } } } @@ -310,7 +310,7 @@ void Ekf::setVelPosStatus(const int index, const bool healthy) case 0: if (healthy) { _fault_status.flags.bad_vel_N = false; - _time_last_hor_vel_fuse = _time_last_imu; + _time_last_hor_vel_fuse = _imu_sample_delayed.time_us; } else { _fault_status.flags.bad_vel_N = true; @@ -321,7 +321,7 @@ void Ekf::setVelPosStatus(const int index, const bool healthy) case 1: if (healthy) { _fault_status.flags.bad_vel_E = false; - _time_last_hor_vel_fuse = _time_last_imu; + _time_last_hor_vel_fuse = _imu_sample_delayed.time_us; } else { _fault_status.flags.bad_vel_E = true; @@ -332,7 +332,7 @@ void Ekf::setVelPosStatus(const int index, const bool healthy) case 2: if (healthy) { _fault_status.flags.bad_vel_D = false; - _time_last_ver_vel_fuse = _time_last_imu; + _time_last_ver_vel_fuse = _imu_sample_delayed.time_us; } else { _fault_status.flags.bad_vel_D = true; @@ -343,7 +343,7 @@ void Ekf::setVelPosStatus(const int index, const bool healthy) case 3: if (healthy) { _fault_status.flags.bad_pos_N = false; - _time_last_hor_pos_fuse = _time_last_imu; + _time_last_hor_pos_fuse = _imu_sample_delayed.time_us; } else { _fault_status.flags.bad_pos_N = true; @@ -354,7 +354,7 @@ void Ekf::setVelPosStatus(const int index, const bool healthy) case 4: if (healthy) { _fault_status.flags.bad_pos_E = false; - _time_last_hor_pos_fuse = _time_last_imu; + _time_last_hor_pos_fuse = _imu_sample_delayed.time_us; } else { _fault_status.flags.bad_pos_E = true; @@ -365,7 +365,7 @@ void Ekf::setVelPosStatus(const int index, const bool healthy) case 5: if (healthy) { _fault_status.flags.bad_pos_D = false; - _time_last_hgt_fuse = _time_last_imu; + _time_last_hgt_fuse = _imu_sample_delayed.time_us; } else { _fault_status.flags.bad_pos_D = true; diff --git a/src/modules/ekf2/EKF/zero_velocity_update.cpp b/src/modules/ekf2/EKF/zero_velocity_update.cpp index 3a750292e8..a1a7530f09 100644 --- a/src/modules/ekf2/EKF/zero_velocity_update.cpp +++ b/src/modules/ekf2/EKF/zero_velocity_update.cpp @@ -61,7 +61,7 @@ void Ekf::controlZeroVelocityUpdate() fuseVelPosHeight(innovation(1), innov_var(1), 1); fuseVelPosHeight(innovation(2), innov_var(2), 2); - _time_last_zero_velocity_fuse = _time_last_imu; + _time_last_zero_velocity_fuse = _imu_sample_delayed.time_us; } } }