diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 24731fb148..a2d72c3ec5 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1475,7 +1475,7 @@ void EKF2::UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps) _airspeed_validated_timestamp_last = airspeed_validated.timestamp; } - } else if ((hrt_elapsed_time(&_airspeed_validated_timestamp_last) > 3_s) && _airspeed_sub.updated()) { + } else if (((ekf2_timestamps.timestamp - _airspeed_validated_timestamp_last) > 3_s) && _airspeed_sub.updated()) { // use ORB_ID(airspeed) if ORB_ID(airspeed_validated) is unavailable const unsigned last_generation = _airspeed_sub.get_last_generation(); airspeed_s airspeed; @@ -1776,7 +1776,7 @@ bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps) } // use optical_flow distance as range sample if distance_sensor unavailable - if (PX4_ISFINITE(optical_flow.distance_m) && (hrt_elapsed_time(&_last_range_sensor_update) > 1_s)) { + if (PX4_ISFINITE(optical_flow.distance_m) && ((ekf2_timestamps.timestamp - _last_range_sensor_update) > 1_s)) { int8_t quality = static_cast(optical_flow.quality) / static_cast(UINT8_MAX) * 100.f; @@ -1892,12 +1892,15 @@ void EKF2::UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps) if (_distance_sensor_selected < 0) { + // only consider distance sensors that have updated within the last 0.1s + const hrt_abstime timestamp_stale = math::max(ekf2_timestamps.timestamp, 100_ms) - 100_ms; + if (_distance_sensor_subs.advertised()) { for (unsigned i = 0; i < _distance_sensor_subs.size(); i++) { if (_distance_sensor_subs[i].update(&distance_sensor)) { // only use the first instace which has the correct orientation - if ((hrt_elapsed_time(&distance_sensor.timestamp) < 100_ms) + if ((distance_sensor.timestamp != 0) && (distance_sensor.timestamp > timestamp_stale) && (distance_sensor.orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING)) { int ndist = orb_group_count(ORB_ID(distance_sensor)); @@ -1947,7 +1950,8 @@ void EKF2::UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps) (int64_t)ekf2_timestamps.timestamp / 100); } - if (hrt_elapsed_time(&_last_range_sensor_update) > 1_s) { + if (_last_range_sensor_update < ekf2_timestamps.timestamp - 1_s) { + // force reselection after timeout _distance_sensor_selected = -1; } }