forked from Archive/PX4-Autopilot
ekf2: replay fixes, don't use HRT for timeout checks
- this interferes with current ekf2 replay where the latest IMU sample is effectively the current timestamp
This commit is contained in:
parent
1c49a4349f
commit
34dee09b74
|
@ -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<float>(optical_flow.quality) / static_cast<float>(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;
|
||||
}
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue