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

add uORB message obstacle_distance_fused with data from offboard

Browse Source 
obstacle_distance and distance sensor
This commit is contained in:
Martina Rivizzigno 2019-06-17 11:52:27 +02:00 committed by Beat Küng
parent a9b1946bea
commit e6e4d846fb
3 changed files with 17 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
msg/obstacle_distance.msg
View File

@ -15,3 +15,5 @@ uint16 min_distance # Minimum distance the sensor can measure in centimeters.
uint16 max_distance # Maximum distance the sensor can measure in centimeters.
float32 angle_offset # Relative angle offset of the 0-index element in the distances array. Value of 0 corresponds to forward. Positive values are offsets to the right.
# TOPICS obstacle_distance obstacle_distance_fused

13
src/lib/CollisionPrevention/CollisionPrevention.cpp
View File

@ -77,6 +77,17 @@ void CollisionPrevention::publishConstrainedSetpoint(const Vector2f &original_se
}
}
void CollisionPrevention::publishObstacleDistance(obstacle_distance_s &obstacle)
{
// publish fused obtacle distance message with data from offboard obstacle_distance and distance sensor
if (_obstacle_distance_pub != nullptr) {
orb_publish(ORB_ID(obstacle_distance_fused), _obstacle_distance_pub, &obstacle);
} else {
_obstacle_distance_pub = orb_advertise(ORB_ID(obstacle_distance_fused), &obstacle);
}
}
void CollisionPrevention::updateOffboardObstacleDistance(obstacle_distance_s &obstacle)
{
_sub_obstacle_distance.update();
@ -171,6 +182,8 @@ void CollisionPrevention::updateDistanceSensor(obstacle_distance_s &obstacle)
}
}
}
publishObstacleDistance(obstacle);
}
void CollisionPrevention::calculateConstrainedSetpoint(Vector2f &setpoint,

2
src/lib/CollisionPrevention/CollisionPrevention.hpp
View File

@ -72,6 +72,7 @@ private:
orb_advert_t _constraints_pub{nullptr}; /**< constraints publication */
orb_advert_t _mavlink_log_pub{nullptr}; /**< Mavlink log uORB handle */
orb_advert_t _obstacle_distance_pub{nullptr}; /**< obstacle_distance 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 */
@ -120,6 +121,7 @@ private:
const matrix::Vector2f &curr_vel);
void publishConstrainedSetpoint(const matrix::Vector2f &original_setpoint, const matrix::Vector2f &adapted_setpoint);
void publishObstacleDistance(obstacle_distance_s &obstacle);
void updateOffboardObstacleDistance(obstacle_distance_s &obstacle);
void updateDistanceSensor(obstacle_distance_s &obstacle);