From 39451b811a12cd6cb89f326d8f57905a3f4c6598 Mon Sep 17 00:00:00 2001 From: Julian Kent Date: Wed, 11 Sep 2019 16:01:32 +0200 Subject: [PATCH] Do syscall for time once, outside of loop --- src/lib/CollisionPrevention/CollisionPrevention.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/lib/CollisionPrevention/CollisionPrevention.cpp b/src/lib/CollisionPrevention/CollisionPrevention.cpp index 6fe40ef712..a3d2aa8918 100644 --- a/src/lib/CollisionPrevention/CollisionPrevention.cpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.cpp @@ -253,7 +253,9 @@ void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint, float setpoint_length = setpoint.norm(); - if (getElapsedTime(&_obstacle_map_body_frame.timestamp) < RANGE_STREAM_TIMEOUT_US) { + hrt_abstime constrain_time = getTime(); + + if ((constrain_time - _obstacle_map_body_frame.timestamp) < RANGE_STREAM_TIMEOUT_US) { if (setpoint_length > 0.001f) { Vector2f setpoint_dir = setpoint / setpoint_length; @@ -267,7 +269,7 @@ void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint, for (int i = 0; i < INTERNAL_MAP_USED_BINS; i++) { //disregard unused bins at the end of the message //delete stale values - auto data_age = getElapsedTime(&_data_timestamps[i]); + hrt_abstime data_age = constrain_time - _data_timestamps[i]; if (data_age > RANGE_STREAM_TIMEOUT_US) { _obstacle_map_body_frame.distances[i] = UINT16_MAX;