fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF2: Allocate distance sensor subscriptions as member variables

Browse Source 
Just create the subscriptions and keep them, instead of
re-creating them continuously

Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>
This commit is contained in:
Jukka Laitinen 2021-02-09 16:11:17 +02:00
parent 0fdf682140
commit ec9aa8b303
2 changed files with 5 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -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_s> 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));

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

@ -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_s> _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