|
|
|
@ -77,7 +77,8 @@ void CollisionPrevention::calculate_constrained_setpoint(Vector2f &setpoint, con
@@ -77,7 +77,8 @@ void CollisionPrevention::calculate_constrained_setpoint(Vector2f &setpoint, con
|
|
|
|
|
//Limit the deviation of the adapted setpoint from the originally given joystick input (slightly less than 90 degrees)
|
|
|
|
|
float max_slide_angle_rad = 1.2f; |
|
|
|
|
|
|
|
|
|
if (hrt_elapsed_time(&obstacle_distance.timestamp) < RANGE_STREAM_TIMEOUT_US && setpoint_length > 0.001f) { |
|
|
|
|
if (hrt_elapsed_time(&obstacle_distance.timestamp) < RANGE_STREAM_TIMEOUT_US) { |
|
|
|
|
if (setpoint_length > 0.001f) { |
|
|
|
|
|
|
|
|
|
int distances_array_size = sizeof(obstacle_distance.distances) / sizeof(obstacle_distance.distances[0]); |
|
|
|
|
|
|
|
|
@ -132,6 +133,7 @@ void CollisionPrevention::calculate_constrained_setpoint(Vector2f &setpoint, con
@@ -132,6 +133,7 @@ void CollisionPrevention::calculate_constrained_setpoint(Vector2f &setpoint, con
|
|
|
|
|
} else { |
|
|
|
|
setpoint.zero(); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
} else if (_last_message + MESSAGE_THROTTLE_US < hrt_absolute_time()) { |
|
|
|
|
mavlink_log_critical(&_mavlink_log_pub, "No range data received"); |
|
|
|
|