aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AC_Avoid: Remove unnecessary sanity checks

Browse Source
This commit is contained in:
Rishabh 2020-03-07 23:48:32 +05:30 committed by Randy Mackay
parent 0efae38af0
commit 4ec8602de2
3 changed files with 7 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AC_Avoidance/AC_Avoid.cpp
View File

@ -406,10 +406,7 @@ void AC_Avoid::adjust_velocity_inclusion_and_exclusion_polygons(float kP, float
for (uint8_t i = 0; i < num_inclusion_polygons; i++) {
uint16_t num_points;
const Vector2f* boundary = fence->polyfence().get_inclusion_polygon(i, num_points);
if (num_points < 3) {
// ignore exclusion polygons with less than 3 points
continue;
}
// adjust velocity
adjust_velocity_polygon(kP, accel_cmss, desired_vel_cms, boundary, num_points, true, fence->get_margin(), dt, true);
}
@ -419,10 +416,7 @@ void AC_Avoid::adjust_velocity_inclusion_and_exclusion_polygons(float kP, float
for (uint8_t i = 0; i < num_exclusion_polygons; i++) {
uint16_t num_points;
const Vector2f* boundary = fence->polyfence().get_exclusion_polygon(i, num_points);
if (num_points < 3) {
// ignore exclusion polygons with less than 3 points
continue;
}
// adjust velocity
adjust_velocity_polygon(kP, accel_cmss, desired_vel_cms, boundary, num_points, true, fence->get_margin(), dt, false);
}

12
libraries/AC_Avoidance/AP_OABendyRuler.cpp
View File

@ -236,11 +236,7 @@ bool AP_OABendyRuler::calc_margin_from_inclusion_and_exclusion_polygons(const Lo
for (uint8_t i = 0; i < num_inclusion_polygons; i++) {
uint16_t num_points;
const Vector2f* boundary = fence->polyfence().get_inclusion_polygon(i, num_points);
if (num_points < 3) {
// ignore exclusion polygons with less than 3 points
continue;
}
// if outside the fence margin is the closest distance but with negative sign
const float sign = Polygon_outside(start_NE, boundary, num_points) ? -1.0f : 1.0f;
@ -256,11 +252,7 @@ bool AP_OABendyRuler::calc_margin_from_inclusion_and_exclusion_polygons(const Lo
for (uint8_t i = 0; i < num_exclusion_polygons; i++) {
uint16_t num_points;
const Vector2f* boundary = fence->polyfence().get_exclusion_polygon(i, num_points);
if (num_points < 3) {
// ignore exclusion polygons with less than 3 points
continue;
}
// if start is inside the polygon the margin's sign is reversed
const float sign = Polygon_outside(start_NE, boundary, num_points) ? 1.0f : -1.0f;

14
libraries/AC_Avoidance/AP_OADijkstra.cpp
View File

@ -265,10 +265,6 @@ bool AP_OADijkstra::create_inclusion_polygon_with_margin(float margin_cm, AP_OAD
for (uint8_t i = 0; i < num_inclusion_polygons; i++) {
uint16_t num_points;
const Vector2f* boundary = fence->polyfence().get_inclusion_polygon(i, num_points);
if (num_points < 3) {
// ignore exclusion polygons with less than 3 points
continue;
}
// expand array if required
if (!_inclusion_polygon_pts.expand_to_hold(_inclusion_polygon_numpoints + num_points)) {
@ -356,11 +352,7 @@ bool AP_OADijkstra::create_exclusion_polygon_with_margin(float margin_cm, AP_OAD
for (uint8_t i = 0; i < num_exclusion_polygons; i++) {
uint16_t num_points;
const Vector2f* boundary = fence->polyfence().get_exclusion_polygon(i, num_points);
if (num_points < 3) {
// ignore exclusion polygons with less than 3 points
continue;
}
// expand array if required
if (!_exclusion_polygon_pts.expand_to_hold(_exclusion_polygon_numpoints + num_points)) {
err_id = AP_OADijkstra_Error::DIJKSTRA_ERROR_OUT_OF_MEMORY;
@ -533,7 +525,7 @@ bool AP_OADijkstra::intersects_fence(const Vector2f &seg_start, const Vector2f &
uint16_t num_points = 0;
for (uint8_t i = 0; i < fence->polyfence().get_inclusion_polygon_count(); i++) {
const Vector2f* boundary = fence->polyfence().get_inclusion_polygon(i, num_points);
if ((boundary != nullptr) && (num_points >= 3)) {
if (boundary != nullptr) {
Vector2f intersection;
if (Polygon_intersects(boundary, num_points, seg_start, seg_end, intersection)) {
return true;
@ -544,7 +536,7 @@ bool AP_OADijkstra::intersects_fence(const Vector2f &seg_start, const Vector2f &
// determine if segment crosses any of the exclusion polygons
for (uint8_t i = 0; i < fence->polyfence().get_exclusion_polygon_count(); i++) {
const Vector2f* boundary = fence->polyfence().get_exclusion_polygon(i, num_points);
if ((boundary != nullptr) && (num_points >= 3)) {
if (boundary != nullptr) {
Vector2f intersection;
if (Polygon_intersects(boundary, num_points, seg_start, seg_end, intersection)) {
return true;