diff --git a/libraries/AC_Fence/AC_Fence.cpp b/libraries/AC_Fence/AC_Fence.cpp index 91acc11dd3..5c3a2422da 100644 --- a/libraries/AC_Fence/AC_Fence.cpp +++ b/libraries/AC_Fence/AC_Fence.cpp @@ -129,6 +129,11 @@ bool AC_Fence::pre_arm_check_polygon(const char* &fail_msg) const return false; } + if (!_poly_loader.check_inclusion_circle_margin(_margin)) { + fail_msg = "Margin is less than inclusion circle radius"; + return false; + } + return true; } @@ -139,6 +144,11 @@ bool AC_Fence::pre_arm_check_circle(const char* &fail_msg) const fail_msg = "Invalid FENCE_RADIUS value"; return false; } + if (_circle_radius < _margin) { + fail_msg = "FENCE_MARGIN is less than FENCE_RADIUS"; + return false; + } + return true; } diff --git a/libraries/AC_Fence/AC_PolyFence_loader.cpp b/libraries/AC_Fence/AC_PolyFence_loader.cpp index fc8fdb8684..972f3d8100 100644 --- a/libraries/AC_Fence/AC_PolyFence_loader.cpp +++ b/libraries/AC_Fence/AC_PolyFence_loader.cpp @@ -865,6 +865,19 @@ bool AC_PolyFence_loader::get_inclusion_circle(uint8_t index, Vector2f ¢er_p return true; } +bool AC_PolyFence_loader::check_inclusion_circle_margin(float margin) const +{ + // check circular includes + for (uint8_t i=0; i<_num_loaded_circle_inclusion_boundaries; i++) { + const InclusionCircle &circle = _loaded_circle_inclusion_boundary[i]; + if (circle.radius < margin) { + // circle radius should never be less than margin + return false; + } + } + return true; +} + bool AC_PolyFence_loader::validate_fence(const AC_PolyFenceItem *new_items, uint16_t count) const { // validate the fence items... diff --git a/libraries/AC_Fence/AC_PolyFence_loader.h b/libraries/AC_Fence/AC_PolyFence_loader.h index 9521e21c6a..0c4235a27f 100644 --- a/libraries/AC_Fence/AC_PolyFence_loader.h +++ b/libraries/AC_Fence/AC_PolyFence_loader.h @@ -110,6 +110,9 @@ public: /// center is offsets in cm from EKF origin in NE frame, radius is in meters bool get_inclusion_circle(uint8_t index, Vector2f ¢er_pos_cm, float &radius) const; + // false if margin < fence radius + bool check_inclusion_circle_margin(float margin) const; + /// /// mavlink ///