diff --git a/src/lib/CollisionPrevention/CollisionPrevention.cpp b/src/lib/CollisionPrevention/CollisionPrevention.cpp
index 21ee50c0cc..6fe40ef712 100644
--- a/src/lib/CollisionPrevention/CollisionPrevention.cpp
+++ b/src/lib/CollisionPrevention/CollisionPrevention.cpp
@@ -267,7 +267,9 @@ 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
-				if (getElapsedTime(&_data_timestamps[i]) > RANGE_STREAM_TIMEOUT_US) {
+				auto data_age = getElapsedTime(&_data_timestamps[i]);
+
+				if (data_age > RANGE_STREAM_TIMEOUT_US) {
 					_obstacle_map_body_frame.distances[i] = UINT16_MAX;
 				}
 
@@ -287,7 +289,7 @@ void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint,
 					    && setpoint_dir.dot(bin_direction) > cosf(col_prev_ang_rad)) {
 						//calculate max allowed velocity with a P-controller (same gain as in the position controller)
 						float curr_vel_parallel = math::max(0.f, curr_vel.dot(bin_direction));
-						float delay_distance = curr_vel_parallel * col_prev_dly;
+						float delay_distance = curr_vel_parallel * (col_prev_dly + data_age * 1e-6f);
 						float stop_distance =  math::max(0.f, distance - min_dist_to_keep - delay_distance);
 						float vel_max_posctrl = xy_p * stop_distance;
 						float vel_max_smooth = trajmath::computeMaxSpeedFromBrakingDistance(max_jerk, max_accel, stop_distance);