fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-17 10:40:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF2: Use _distance_sensor_subs vector directly

Browse Source 
Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>
This commit is contained in:
Jukka Laitinen
2021-11-15 17:48:02 +02:00
committed by Beat Küng
parent b66270f8a8
commit 5dbbddb13f
2 changed files with 20 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF2.cpp
+18 -15
View File
@@ -1707,44 +1707,47 @@ void EKF2::UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps)
void EKF2::UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps)
{
if (!_distance_sensor_selected) {
distance_sensor_s distance_sensor;
if (_distance_sensor_selected < 0) {
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].update(&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));
if (ndist > 1) {
PX4_INFO("%d - selected distance_sensor:%d (%d advertised)", _instance, i, ndist);
}
int ndist = orb_group_count(ORB_ID(distance_sensor));
_distance_sensor_selected = true;
break;
if (ndist > 1) {
PX4_INFO("%d - selected distance_sensor:%d (%d advertised)", _instance, i, ndist);
}
_distance_sensor_selected = i;
_last_range_sensor_update = distance_sensor.timestamp;
_distance_sensor_last_generation = _distance_sensor_subs[_distance_sensor_selected].get_last_generation() - 1;
break;
}
}
}
}
}
// EKF range sample
const unsigned last_generation = _distance_sensor_sub.get_last_generation();
distance_sensor_s distance_sensor;
if (_distance_sensor_selected >= 0 && _distance_sensor_subs[_distance_sensor_selected].update(&distance_sensor)) {
// EKF range sample
if (_distance_sensor_sub.update(&distance_sensor)) {
if (_msg_missed_distance_sensor_perf == nullptr) {
_msg_missed_distance_sensor_perf = perf_alloc(PC_COUNT, MODULE_NAME": distance_sensor messages missed");
} else if (_distance_sensor_sub.get_last_generation() != last_generation + 1) {
} else if (_distance_sensor_subs[_distance_sensor_selected].get_last_generation() != _distance_sensor_last_generation +
1) {
perf_count(_msg_missed_distance_sensor_perf);
}
_distance_sensor_last_generation = _distance_sensor_subs[_distance_sensor_selected].get_last_generation();
if (distance_sensor.orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING) {
rangeSample range_sample {
.time_us = distance_sensor.timestamp,
@@ -1765,7 +1768,7 @@ void EKF2::UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps)
}
if (hrt_elapsed_time(&_last_range_sensor_update) > 1_s) {
_distance_sensor_selected = false;
_distance_sensor_selected = -1;
}
}
src/modules/ekf2/EKF2.hpp
+2 -2
View File
@@ -249,7 +249,6 @@ private:
uORB::Subscription _airdata_sub{ORB_ID(vehicle_air_data)};
uORB::Subscription _airspeed_sub{ORB_ID(airspeed)};
uORB::Subscription _distance_sensor_sub{ORB_ID(distance_sensor)};
uORB::Subscription _ev_odom_sub{ORB_ID(vehicle_visual_odometry)};
uORB::Subscription _landing_target_pose_sub{ORB_ID(landing_target_pose)};
uORB::Subscription _magnetometer_sub{ORB_ID(vehicle_magnetometer)};
@@ -265,10 +264,11 @@ private:
uORB::SubscriptionCallbackWorkItem _vehicle_imu_sub{this, ORB_ID(vehicle_imu)};
uORB::SubscriptionMultiArray<distance_sensor_s> _distance_sensor_subs{ORB_ID::distance_sensor};
int _distance_sensor_selected{-1}; // because we can have several distance sensor instances with different orientations
unsigned _distance_sensor_last_generation{0};
bool _callback_registered{false};
bool _distance_sensor_selected{false}; // because we can have several distance sensor instances with different orientations
bool _armed{false};
bool _standby{false}; // standby arming state