fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: handle all time on delayed horizon (except for newest sample checks)

Browse Source 
- 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
This commit is contained in:
Daniel Agar 2022-07-22 12:38:28 -04:00
parent d6a4e158cf
commit 1948c5057a
25 changed files with 193 additions and 165 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/modules/ekf2/EKF/airspeed_fusion.cpp
View File

@ -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()

2
src/modules/ekf2/EKF/baro_height_control.cpp
View File

@ -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);

25
src/modules/ekf2/EKF/control.cpp
View File

@ -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();
}

4
src/modules/ekf2/EKF/covariance.cpp
View File

@ -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");

12
src/modules/ekf2/EKF/ekf.cpp
View File

@ -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();

11
src/modules/ekf2/EKF/ekf.h
View File

@ -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();

20
src/modules/ekf2/EKF/ekf_helper.cpp
View File

@ -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<float, 24> 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;

148
src/modules/ekf2/EKF/estimator_interface.cpp
View File

@ -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<uint64_t>(_params.mag_delay_ms * 1000)
- static_cast<uint64_t>(_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<uint64_t>(_params.mag_delay_ms * 1000);
mag_sample_new.time_us -= static_cast<uint64_t>(_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<uint64_t>(_params.gps_delay_ms * 1000)
- static_cast<uint64_t>(_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<uint64_t>(_params.gps_delay_ms * 1000);
gps_sample_new.time_us -= static_cast<uint64_t>(_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<uint64_t>(_params.baro_delay_ms * 1000)
- static_cast<uint64_t>(_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<uint64_t>(_params.baro_delay_ms * 1000);
baro_sample_new.time_us -= static_cast<uint64_t>(_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<uint64_t>(_params.airspeed_delay_ms * 1000)
- static_cast<uint64_t>(_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<uint64_t>(_params.airspeed_delay_ms * 1000);
airspeed_sample_new.time_us -= static_cast<uint64_t>(_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<uint64_t>(_params.range_delay_ms * 1000)
- static_cast<uint64_t>(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2
rangeSample range_sample_new = range_sample;
range_sample_new.time_us -= static_cast<uint64_t>(_params.range_delay_ms * 1000);
range_sample_new.time_us -= static_cast<uint64_t>(_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<uint64_t>(_params.flow_delay_ms * 1000)
- static_cast<uint64_t>(_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<uint64_t>(_params.flow_delay_ms * 1000);
optflow_sample_new.time_us -= static_cast<uint64_t>(_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<uint64_t>(_params.ev_delay_ms * 1000)
- static_cast<uint64_t>(_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<uint64_t>(_params.ev_delay_ms * 1000);
ev_sample_new.time_us -= static_cast<uint64_t>(_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<uint64_t>(_params.auxvel_delay_ms * 1000)
- static_cast<uint64_t>(_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<uint64_t>(_params.auxvel_delay_ms * 1000);
auxvel_sample_new.time_us -= static_cast<uint64_t>(_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);

24
src/modules/ekf2/EKF/estimator_interface.h
View File

@ -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<dragSample> *_drag_buffer{nullptr};
RingBuffer<auxVelSample> *_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{};

2
src/modules/ekf2/EKF/ev_height_control.cpp
View File

@ -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;

8
src/modules/ekf2/EKF/fake_pos_control.cpp
View File

@ -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;
}

4
src/modules/ekf2/EKF/gnss_height_control.cpp
View File

@ -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()

17
src/modules/ekf2/EKF/gps_checks.cpp
View File

@ -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

5
src/modules/ekf2/EKF/gps_control.cpp
View File

@ -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();

8
src/modules/ekf2/EKF/gps_fusion.cpp
View File

@ -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;
}
}
}

6
src/modules/ekf2/EKF/gps_yaw_fusion.cpp
View File

@ -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;

4
src/modules/ekf2/EKF/height_control.cpp
View File

@ -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

4
src/modules/ekf2/EKF/height_fusion.cpp
View File

@ -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;
}
}

6
src/modules/ekf2/EKF/mag_fusion.cpp
View File

@ -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;

2
src/modules/ekf2/EKF/optflow_fusion.cpp
View File

@ -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;
}
}
}

2
src/modules/ekf2/EKF/range_height_control.cpp
View File

@ -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

2
src/modules/ekf2/EKF/sideslip_fusion.cpp
View File

@ -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;
}
}
}

12
src/modules/ekf2/EKF/terrain_estimator.cpp
View File

@ -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()

24
src/modules/ekf2/EKF/vel_pos_fusion.cpp
View File

@ -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;

2
src/modules/ekf2/EKF/zero_velocity_update.cpp
View File

@ -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;
}
}
}