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

ekf2: try reselecting distance_sensor on timeout

This commit is contained in:
Daniel Agar 2021-02-08 16:35:40 -05:00
parent a002a07ed5
commit c3172a91d9
2 changed files with 15 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -1477,9 +1477,11 @@ void EKF2::UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps)
if (distance_sensor_subs[i].copy(&distance_sensor)) {
// only use the first instace which has the correct orientation
if ((distance_sensor.timestamp != 0) && (distance_sensor.orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING)) {
if ((hrt_elapsed_time(&distance_sensor.timestamp) < 100_ms)
&& (distance_sensor.orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING)) {
if (_distance_sensor_sub.ChangeInstance(i)) {
PX4_INFO("%d - found range finder with instance %d", _instance, i);
PX4_INFO("%d - selected distance_sensor:%d", _instance, i);
_distance_sensor_selected = true;
}
}
@ -1498,6 +1500,9 @@ void EKF2::UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps)
_distance_sensor_sub.get_last_generation());
}
ekf2_timestamps.distance_sensor_timestamp_rel = (int16_t)((int64_t)distance_sensor.timestamp / 100 -
(int64_t)ekf2_timestamps.timestamp / 100);
if (distance_sensor.orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING) {
rangeSample range_sample {
.time_us = distance_sensor.timestamp,
@ -1508,10 +1513,14 @@ void EKF2::UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps)
// Save sensor limits reported by the rangefinder
_ekf.set_rangefinder_limits(distance_sensor.min_distance, distance_sensor.max_distance);
}
ekf2_timestamps.distance_sensor_timestamp_rel = (int16_t)((int64_t)distance_sensor.timestamp / 100 -
(int64_t)ekf2_timestamps.timestamp / 100);
_last_range_sensor_update = distance_sensor.timestamp;
return;
}
}
if (hrt_elapsed_time(&_last_range_sensor_update) > 1_s) {
_distance_sensor_selected = false;
}
}

1
src/modules/ekf2/EKF2.hpp
View File

@ -232,6 +232,7 @@ private:
bool _standby{false}; // standby arming state
hrt_abstime _last_status_flag_update{0};
hrt_abstime _last_range_sensor_update{0};
uint32_t _filter_control_status{0};
uint32_t _filter_fault_status{0};