fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-28 06:50:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

CollisionPrevention: slightly simplify _transformSetpoint()

Browse Source
This commit is contained in:
Matthias Grob
2024-11-19 19:59:09 +01:00
committed by Claudio Chies
parent 399a8ad5b7
commit 30eec33e09
1 changed files with 2 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/collision_prevention/CollisionPrevention.cpp
+2 -2
View File
@@ -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);
}