Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-18 06:38:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_Avoid: move check for zero-desired-velocity into adjust_velocity_polygon

Browse Source 
This is the method which divides by the length of this vector; the
check belongs in here.
This commit is contained in:
Peter Barker 2019-05-29 16:42:18 +10:00 committed by Randy Mackay
parent 3177d24429
commit cd6e5b48a1
1 changed files with 5 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
libraries/AC_Avoidance/AC_Avoid.cpp
View File

@ -356,11 +356,6 @@ void AC_Avoid::adjust_velocity_polygon_fence(float kP, float accel_cmss, Vector2
return;
}
// exit immediately if no desired velocity
if (desired_vel_cms.is_zero()) {
return;
}
// get polygon boundary
uint16_t num_points;
const Vector2f* boundary = _fence.get_boundary_points(num_points);
@ -381,11 +376,6 @@ void AC_Avoid::adjust_velocity_beacon_fence(float kP, float accel_cmss, Vector2f
return;
}
// exit immediately if no desired velocity
if (desired_vel_cms.is_zero()) {
return;
}
// get boundary from beacons
uint16_t num_points;
const Vector2f* boundary = _beacon->get_boundary_points(num_points);
@ -418,11 +408,6 @@ void AC_Avoid::adjust_velocity_proximity(float kP, float accel_cmss, Vector2f &d
return;
}
// exit immediately if no desired velocity
if (desired_vel_cms.is_zero()) {
return;
}
// get boundary from proximity sensor
uint16_t num_points;
const Vector2f *boundary = _proximity.get_boundary_points(num_points);
@ -439,6 +424,11 @@ void AC_Avoid::adjust_velocity_polygon(float kP, float accel_cmss, Vector2f &des
return;
}
// exit immediately if no desired velocity
if (desired_vel_cms.is_zero()) {
return;
}
const AP_AHRS &_ahrs = AP::ahrs();
// do not adjust velocity if vehicle is outside the polygon fence