diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index d902ebf9e0..75317521cf 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1706,18 +1706,15 @@ void EKF2::UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps) void EKF2::UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps) { if (!_distance_sensor_selected) { - // get subscription index of first downward-facing range sensor - uORB::SubscriptionMultiArray distance_sensor_subs{ORB_ID::distance_sensor}; - if (distance_sensor_subs.advertised()) { - for (unsigned i = 0; i < distance_sensor_subs.size(); i++) { + if (_distance_sensor_subs.advertised()) { + for (unsigned i = 0; i < _distance_sensor_subs.size(); i++) { distance_sensor_s distance_sensor; - if (distance_sensor_subs[i].copy(&distance_sensor)) { + if (_distance_sensor_subs[i].copy(&distance_sensor)) { // only use the first instace which has the correct orientation if ((hrt_elapsed_time(&distance_sensor.timestamp) < 100_ms) && (distance_sensor.orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING)) { - if (_distance_sensor_sub.ChangeInstance(i)) { int ndist = orb_group_count(ORB_ID(distance_sensor)); diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 63ba984726..14f144127f 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -264,6 +264,8 @@ private: uORB::SubscriptionCallbackWorkItem _sensor_combined_sub{this, ORB_ID(sensor_combined)}; uORB::SubscriptionCallbackWorkItem _vehicle_imu_sub{this, ORB_ID(vehicle_imu)}; + uORB::SubscriptionMultiArray _distance_sensor_subs{ORB_ID::distance_sensor}; + bool _callback_registered{false}; bool _distance_sensor_selected{false}; // because we can have several distance sensor instances with different orientations