Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-31 18:47:21 +08:00
Code Issues Actions 20 Packages Projects Releases Wiki Activity

uORB: introduce SubscriptionMultiArray for working with multi-instances

Browse Source
This commit is contained in:
Daniel Agar
2020-09-02 12:46:47 -04:00
committed by GitHub
parent 71f381ada9
commit 6ff361479c
20 changed files with 234 additions and 169 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/collision_prevention/CollisionPrevention.cpp
+3 -6
View File
@@ -200,13 +200,10 @@ CollisionPrevention::_updateObstacleMap()
_sub_vehicle_attitude.update();
// add distance sensor data
for (unsigned i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
// if a new distance sensor message has arrived
if (_sub_distance_sensor[i].updated()) {
distance_sensor_s distance_sensor {};
_sub_distance_sensor[i].copy(&distance_sensor);
for (auto &dist_sens_sub : _distance_sensor_subs) {
distance_sensor_s distance_sensor;
if (dist_sens_sub.update(&distance_sensor)) {
// consider only instances with valid data and orientations useful for collision prevention
if ((getElapsedTime(&distance_sensor.timestamp) < RANGE_STREAM_TIMEOUT_US) &&
(distance_sensor.orientation != distance_sensor_s::ROTATION_DOWNWARD_FACING) &&
src/lib/collision_prevention/CollisionPrevention.hpp
+2 -1
View File
@@ -51,6 +51,7 @@
#include <systemlib/mavlink_log.h>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionMultiArray.hpp>
#include <uORB/topics/collision_constraints.h>
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/mavlink_log.h>
@@ -131,8 +132,8 @@ private:
uORB::PublicationQueued<vehicle_command_s> _vehicle_command_pub{ORB_ID(vehicle_command)}; /**< vehicle command do publication */
uORB::SubscriptionData<obstacle_distance_s> _sub_obstacle_distance{ORB_ID(obstacle_distance)}; /**< obstacle distances received form a range sensor */
uORB::Subscription _sub_distance_sensor[ORB_MULTI_MAX_INSTANCES] {{ORB_ID(distance_sensor), 0}, {ORB_ID(distance_sensor), 1}, {ORB_ID(distance_sensor), 2}, {ORB_ID(distance_sensor), 3}}; /**< distance data received from onboard rangefinders */
uORB::SubscriptionData<vehicle_attitude_s> _sub_vehicle_attitude{ORB_ID(vehicle_attitude)};
uORB::SubscriptionMultiArray<distance_sensor_s> _distance_sensor_subs{ORB_ID::distance_sensor};
static constexpr uint64_t RANGE_STREAM_TIMEOUT_US{500_ms};
static constexpr uint64_t TIMEOUT_HOLD_US{5_s};