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

CollisionPrevention move orb subscriptions to uORB::Subscription

Browse Source
This commit is contained in:
Daniel Agar
2019-06-07 09:22:35 -04:00
parent 5669df4ca4
commit 90bf26b239
3 changed files with 5 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/CollisionPrevention/CollisionPrevention.cpp
+3 -12
View File
@@ -56,20 +56,10 @@ CollisionPrevention::~CollisionPrevention()
}
}
bool CollisionPrevention::initializeSubscriptions(SubscriptionArray &subscription_array)
{
if (!subscription_array.get(ORB_ID(obstacle_distance), _sub_obstacle_distance)) {
return false;
}
return true;
}
void CollisionPrevention::publishConstrainedSetpoint(const Vector2f &original_setpoint,
const Vector2f &adapted_setpoint)
{
collision_constraints_s constraints; /**< collision constraints message */
collision_constraints_s constraints{}; /**< collision constraints message */
//fill in values
constraints.timestamp = hrt_absolute_time();
@@ -91,7 +81,8 @@ void CollisionPrevention::publishConstrainedSetpoint(const Vector2f &original_se
void CollisionPrevention::calculateConstrainedSetpoint(Vector2f &setpoint,
const Vector2f &curr_pos, const Vector2f &curr_vel)
{
const obstacle_distance_s &obstacle_distance = _sub_obstacle_distance->get();
_sub_obstacle_distance.update();
const obstacle_distance_s &obstacle_distance = _sub_obstacle_distance.get();
//The maximum velocity formula contains a square root, therefore the whole calculation is done with squared norms.
//that way the root does not have to be calculated for every range bin but once at the end.