dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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:
Daniel Agar 2022-08-06 11:02:30 -04:00
parent 1c49a4349f
commit 34dee09b74
1 changed files with 8 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
src/modules/ekf2/EKF2.cpp
View File

@ -1475,7 +1475,7 @@ void EKF2::UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps)
_airspeed_validated_timestamp_last = airspeed_validated.timestamp; _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 // use ORB_ID(airspeed) if ORB_ID(airspeed_validated) is unavailable
const unsigned last_generation = _airspeed_sub.get_last_generation(); const unsigned last_generation = _airspeed_sub.get_last_generation();
airspeed_s airspeed; 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 // 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; 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) { 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()) { if (_distance_sensor_subs.advertised()) {
for (unsigned i = 0; i < _distance_sensor_subs.size(); i++) { for (unsigned i = 0; i < _distance_sensor_subs.size(); i++) {
if (_distance_sensor_subs[i].update(&distance_sensor)) { if (_distance_sensor_subs[i].update(&distance_sensor)) {
// only use the first instace which has the correct orientation // 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)) { && (distance_sensor.orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING)) {
int ndist = orb_group_count(ORB_ID(distance_sensor)); 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); (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; _distance_sensor_selected = -1;
} }
} }