From 30eec33e09a70b763e0df1d0e97383b8253bdf7e Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 19 Nov 2024 19:59:09 +0100 Subject: [PATCH] CollisionPrevention: slightly simplify _transformSetpoint() --- src/lib/collision_prevention/CollisionPrevention.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index 801e29e34d..534426a304 100644 --- a/src/lib/collision_prevention/CollisionPrevention.cpp +++ b/src/lib/collision_prevention/CollisionPrevention.cpp @@ -371,12 +371,12 @@ CollisionPrevention::_checkSetpointDirectionFeasability() void CollisionPrevention::_transformSetpoint(const Vector2f &setpoint) { - _setpoint_dir = setpoint / setpoint.norm();; - const float sp_angle_body_frame = atan2f(_setpoint_dir(1), _setpoint_dir(0)) - _vehicle_yaw; + const float sp_angle_body_frame = atan2f(setpoint(1), setpoint(0)) - _vehicle_yaw; const float sp_angle_with_offset_deg = _wrap_360(math::degrees(sp_angle_body_frame) - _obstacle_map_body_frame.angle_offset); _setpoint_index = floor(sp_angle_with_offset_deg / BIN_SIZE); // change setpoint direction slightly (max by _param_cp_guide_ang degrees) to help guide through narrow gaps + _setpoint_dir = setpoint.unit_or_zero(); _adaptSetpointDirection(_setpoint_dir, _setpoint_index, _vehicle_yaw); }