Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_Avoid: use vector.xy().length() instead of norm(x,y)

This commit is contained in:
Josh Henderson 2021-09-11 06:28:25 -04:00 committed by Andrew Tridgell
parent a3e475822b
commit c0b18e4d52
1 changed files with 1 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AC_Avoidance/AC_Avoid.cpp
View File

@ -1174,7 +1174,7 @@ void AC_Avoid::adjust_velocity_proximity(float kP, float accel_cmss, Vector3f &d
if (breach_dist > deadzone) { if (breach_dist > deadzone) {
// this vector will help us decide how much we have to back away horizontally and vertically // this vector will help us decide how much we have to back away horizontally and vertically
const Vector3f margin_vector = vector_to_obstacle.normalized() * breach_dist; const Vector3f margin_vector = vector_to_obstacle.normalized() * breach_dist;
const float xy_back_dist = norm(margin_vector.x, margin_vector.y); const float xy_back_dist = margin_vector.xy().length();
const float z_back_dist = margin_vector.z; const float z_back_dist = margin_vector.z;
calc_backup_velocity_3D(kP, accel_cmss, quad_1_back_vel, quad_2_back_vel, quad_3_back_vel, quad_4_back_vel, xy_back_dist, vector_to_obstacle, kP_z, accel_cmss_z, z_back_dist, min_back_vel_z, max_back_vel_z, dt); calc_backup_velocity_3D(kP, accel_cmss, quad_1_back_vel, quad_2_back_vel, quad_3_back_vel, quad_4_back_vel, xy_back_dist, vector_to_obstacle, kP_z, accel_cmss_z, z_back_dist, min_back_vel_z, max_back_vel_z, dt);
} }