fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 20:03:34 +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-07-15 10:58:00 +02:00
committed by Beat Küng
parent a9b1946bea
commit e6e4d846fb
3 changed files with 17 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/CollisionPrevention/CollisionPrevention.cpp
+13
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,