From 5fe16d52504d29f98795541230ef8982be0326e2 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Wed, 22 May 2019 15:03:47 +1000
Subject: [PATCH] AP_Proximity: cope with polyfence holding boundary points

---
 libraries/AP_Proximity/AP_Proximity_SITL.cpp | 37 +++++---------------
 libraries/AP_Proximity/AP_Proximity_SITL.h   |  6 ----
 2 files changed, 9 insertions(+), 34 deletions(-)

diff --git a/libraries/AP_Proximity/AP_Proximity_SITL.cpp b/libraries/AP_Proximity/AP_Proximity_SITL.cpp
index 595d81c107..d198a2b33d 100644
--- a/libraries/AP_Proximity/AP_Proximity_SITL.cpp
+++ b/libraries/AP_Proximity/AP_Proximity_SITL.cpp
@@ -18,6 +18,7 @@
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
 #include <AP_Param/AP_Param.h>
 #include "AP_Proximity_SITL.h"
+#include <AC_Fence/AC_Fence.h>
 #include <stdio.h>
 
 extern const AP_HAL::HAL& hal;
@@ -34,10 +35,6 @@ AP_Proximity_SITL::AP_Proximity_SITL(AP_Proximity &_frontend,
     sitl(AP::sitl())
 {
     ap_var_type ptype;
-    fence_count = (AP_Int8 *)AP_Param::find("FENCE_TOTAL", &ptype);
-    if (fence_count == nullptr || ptype != AP_PARAM_INT8) {
-        AP_HAL::panic("Proximity_SITL: Failed to find FENCE_TOTAL");
-    }
     fence_alt_max = (AP_Float *)AP_Param::find("FENCE_ALT_MAX", &ptype);
     if (fence_alt_max == nullptr || ptype != AP_PARAM_FLOAT) {
         AP_HAL::panic("Proximity_SITL: Failed to find FENCE_ALT_MAX");
@@ -47,11 +44,14 @@ AP_Proximity_SITL::AP_Proximity_SITL(AP_Proximity &_frontend,
 // update the state of the sensor
 void AP_Proximity_SITL::update(void)
 {
-    load_fence();
     current_loc.lat = sitl->state.latitude * 1.0e7;
     current_loc.lng = sitl->state.longitude * 1.0e7;
     current_loc.alt = sitl->state.altitude * 1.0e2;
-    if (fence && fence_loader.boundary_valid(fence_count->get(), fence)) {
+
+    if (!AP::fence()->polyfence().breached()) {
+        // only called to prompt polyfence to reload fence if required
+    }
+    if (AP::fence()->polyfence().valid()) {
         // update distance in one sector
         if (get_distance_to_fence(_sector_middle_deg[last_sector], _distance[last_sector])) {
             set_status(AP_Proximity::Proximity_Good);
@@ -70,29 +70,10 @@ void AP_Proximity_SITL::update(void)
     }
 }
 
-void AP_Proximity_SITL::load_fence(void)
-{
-    uint32_t now = AP_HAL::millis();
-    if (now - last_load_ms < 1000) {
-        return;
-    }
-    last_load_ms = now;
-    
-    if (fence == nullptr) {
-        fence = (Vector2l *)fence_loader.create_point_array(sizeof(Vector2l));
-    }
-    if (fence == nullptr) {
-        return;
-    }
-    for (uint8_t i=0; i<fence_count->get(); i++) {
-        fence_loader.load_point_from_eeprom(i, fence[i]);
-    }
-}
-
 // get distance in meters to fence in a particular direction in degrees (0 is forward, angles increase in the clockwise direction)
 bool AP_Proximity_SITL::get_distance_to_fence(float angle_deg, float &distance) const
 {
-    if (!fence_loader.boundary_valid(fence_count->get(), fence)) {
+    if (!AP::fence()->polyfence().valid()) {
         return false;
     }
 
@@ -106,10 +87,10 @@ bool AP_Proximity_SITL::get_distance_to_fence(float angle_deg, float &distance)
     float min_dist = 0, max_dist = PROXIMITY_MAX_RANGE;
     while (max_dist - min_dist > PROXIMITY_ACCURACY) {
         float test_dist = (max_dist+min_dist)*0.5f;
+
         Location loc = current_loc;
         loc.offset_bearing(angle_deg, test_dist);
-        Vector2l vecloc(loc.lat, loc.lng);
-        if (fence_loader.boundary_breached(vecloc, fence_count->get(), fence)) {
+        if (AP::fence()->polyfence().breached(loc)) {
             max_dist = test_dist;
         } else {
             min_dist = test_dist;
diff --git a/libraries/AP_Proximity/AP_Proximity_SITL.h b/libraries/AP_Proximity/AP_Proximity_SITL.h
index baa2a4b8c4..bad2cc7d2f 100644
--- a/libraries/AP_Proximity/AP_Proximity_SITL.h
+++ b/libraries/AP_Proximity/AP_Proximity_SITL.h
@@ -26,18 +26,12 @@ public:
 
 private:
     SITL::SITL *sitl;
-    Vector2l *fence;
-    AP_Int8 *fence_count;
     AP_Float *fence_alt_max;
-    uint32_t last_load_ms;
-    AC_PolyFence_loader fence_loader;
     Location current_loc;
 
     // latest sector updated
     uint8_t last_sector;
 
-    void load_fence(void);
-
     // get distance in meters to fence in a particular direction in degrees (0 is forward, angles increase in the clockwise direction)
     bool get_distance_to_fence(float angle_deg, float &distance) const;