AP_Proximity: move boundary to frontend

Co-authored-by: Rishabh <f20171602@hyderabad.bits-pilani.ac.in>

libraries/AP_Proximity/AP_Proximity.cpp

@@ -130,9 +130,14 @@ void AP_Proximity::update()
             continue;
         }
         drivers[i]->update();
-        drivers[i]->boundary_3D_checks();
     }
 
+    // set boundary cutoff freq for low pass filter
+    boundary.set_filter_freq(get_filter_freq());
+
+    // check if any face has valid distance when it should not
+    boundary.check_face_timeout();
+
     // work out primary instance - first sensor returning good data
     for (int8_t i=num_instances-1; i>=0; i--) {
         if (drivers[i] != nullptr && (state[i].status == Status::Good)) {
@@ -207,90 +212,46 @@ float AP_Proximity::distance_min() const
 // get distances in 8 directions. used for sending distances to ground station
 bool AP_Proximity::get_horizontal_distances(Proximity_Distance_Array &prx_dist_array) const
 {
-    if (!valid_instance(primary_instance)) {
+    Proximity_Distance_Array prx_filt_dist_array; // unused
-        return false;
+    return boundary.get_layer_distances(PROXIMITY_MIDDLE_LAYER, distance_max(), prx_dist_array, prx_filt_dist_array);
-    }
-    // get distances from backend
-    return drivers[primary_instance]->get_horizontal_distances(prx_dist_array);
-}
-
-// get number of layers.
-uint8_t AP_Proximity::get_num_layers() const
-{
-    if (!valid_instance(primary_instance)) {
-        return 0;
-    }
-    return drivers[primary_instance]->get_num_layers();
-}
-
-// get raw and filtered distances in 8 directions per layer. used for logging
-bool AP_Proximity::get_active_layer_distances(uint8_t layer, AP_Proximity::Proximity_Distance_Array &prx_dist_array, AP_Proximity::Proximity_Distance_Array &prx_filt_dist_array) const
-{
-    if (!valid_instance(primary_instance)) {
-        return false;
-    }
-    // get distances from backend
-    return drivers[primary_instance]->get_active_layer_distances(layer, prx_dist_array, prx_filt_dist_array);
 }
 
 // get total number of obstacles, used in GPS based Simple Avoidance
 uint8_t AP_Proximity::get_obstacle_count() const
-{   
+{
-    if (!valid_instance(primary_instance)) {
+    return boundary.get_obstacle_count();
-        return 0;
-    }
-    return drivers[primary_instance]->get_obstacle_count();
 }
 
 // get vector to obstacle based on obstacle_num passed, used in GPS based Simple Avoidance
 bool AP_Proximity::get_obstacle(uint8_t obstacle_num, Vector3f& vec_to_obstacle) const
 {
-    if (!valid_instance(primary_instance)) {
+    return boundary.get_obstacle(obstacle_num, vec_to_obstacle);
-        return false;
-    }
-    return drivers[primary_instance]->get_obstacle(obstacle_num, vec_to_obstacle);
 }
 
 // returns shortest distance to "obstacle_num" obstacle, from a line segment formed between "seg_start" and "seg_end"
-// used in GPS based Simple Avoidance
+// returns FLT_MAX if it's an invalid instance.
 bool AP_Proximity::closest_point_from_segment_to_obstacle(uint8_t obstacle_num, const Vector3f& seg_start, const Vector3f& seg_end, Vector3f& closest_point) const
 {
-    if (!valid_instance(primary_instance)) {
+    return boundary.closest_point_from_segment_to_obstacle(obstacle_num , seg_start, seg_end, closest_point);
-        return false;
-    }
-    return drivers[primary_instance]->closest_point_from_segment_to_obstacle(obstacle_num, seg_start, seg_end, closest_point);
 }
 
 // get distance and angle to closest object (used for pre-arm check)
 //   returns true on success, false if no valid readings
 bool AP_Proximity::get_closest_object(float& angle_deg, float &distance) const
 {
-    if (!valid_instance(primary_instance)) {
+    return boundary.get_closest_object(angle_deg, distance);
-        return false;
-    }
-    // get closest object from backend
-    return drivers[primary_instance]->get_closest_object(angle_deg, distance);
 }
 
-// get number of objects, used for non-GPS avoidance
+// get number of objects, angle and distance - used for non-GPS avoidance
 uint8_t AP_Proximity::get_object_count() const
 {
-    if (!valid_instance(primary_instance)) {
+    return boundary.get_horizontal_object_count();
-        return 0;
-    }
-    // get count from backend
-    return drivers[primary_instance]->get_horizontal_object_count();
 }
 
-// get an object's angle and distance, used for non-GPS avoidance
+// get number of objects, angle and distance - used for non-GPS avoidance
-// returns false if no angle or distance could be returned for some reason
 bool AP_Proximity::get_object_angle_and_distance(uint8_t object_number, float& angle_deg, float &distance) const
 {
-    if (!valid_instance(primary_instance)) {
+    return boundary.get_horizontal_object_angle_and_distance(object_number, angle_deg, distance);
-        return false;
-    }
-    // get angle and distance from backend
-    return drivers[primary_instance]->get_horizontal_object_angle_and_distance(object_number, angle_deg, distance);
 }
 
 // handle mavlink DISTANCE_SENSOR messages
@@ -355,8 +316,8 @@ void AP_Proximity::log()
     Proximity_Distance_Array dist_array{}; // raw distances stored here
     Proximity_Distance_Array filt_dist_array{}; //filtered distances stored here
     auto &logger { AP::logger() };
-    for (uint8_t i = 0; i < get_num_layers(); i++) {
+    for (uint8_t i = 0; i < boundary.get_num_layers(); i++) {
-        const bool active = get_active_layer_distances(i, dist_array, filt_dist_array);
+        const bool active = boundary.get_layer_distances(i, distance_max(), dist_array, filt_dist_array);
         if (!active) {
             // nothing on this layer
             continue;

libraries/AP_Proximity/AP_Proximity.h

@@ -23,9 +23,9 @@
 #include <AP_Math/AP_Math.h>
 #include <GCS_MAVLink/GCS_MAVLink.h>
 #include "AP_Proximity_Params.h"
+#include "AP_Proximity_Boundary_3D.h"
 
 #define PROXIMITY_MAX_INSTANCES             2   // Maximum number of proximity sensor instances available on this platform
-#define PROXIMITY_MAX_DIRECTION 8
 #define PROXIMITY_SENSOR_ID_START 10
 
 class AP_Proximity_Backend;
@@ -93,27 +93,9 @@ public:
     // 3D boundary related methods
     //
 
-    // structure holding distances in PROXIMITY_MAX_DIRECTION directions. used for sending distances to ground station
-    struct Proximity_Distance_Array {
-        uint8_t orientation[PROXIMITY_MAX_DIRECTION]; // orientation (i.e. rough direction) of the distance (see MAV_SENSOR_ORIENTATION)
-        float distance[PROXIMITY_MAX_DIRECTION];      // distance in meters
-        bool valid(uint8_t offset) const {
-            // returns true if the distance stored at offset is valid
-            return (offset < 8 && (offset_valid & (1U<<offset)));
-        };
-
-        uint8_t offset_valid; // bitmask
-    };
-
     // get distances in PROXIMITY_MAX_DIRECTION directions. used for sending distances to ground station
     bool get_horizontal_distances(Proximity_Distance_Array &prx_dist_array) const;
 
-    // get number of layers in the 3D boundary
-    uint8_t get_num_layers() const;
-
-    // get raw and filtered distances in 8 directions per layer. used for logging
-    bool get_active_layer_distances(uint8_t layer, AP_Proximity::Proximity_Distance_Array &prx_dist_array, AP_Proximity::Proximity_Distance_Array &prx_filt_dist_array) const;
-
     // get total number of obstacles, used in GPS based Simple Avoidance
     uint8_t get_obstacle_count() const;
 
@@ -170,6 +152,9 @@ public:
 
     static AP_Proximity *get_singleton(void) { return _singleton; };
 
+    // 3D boundary
+    AP_Proximity_Boundary_3D boundary;
+
 protected:
 
     // parameters for backends

libraries/AP_Proximity/AP_Proximity_AirSimSITL.cpp

@@ -37,7 +37,7 @@ void AP_Proximity_AirSimSITL::update(void)
     set_status(AP_Proximity::Status::Good);
 
     // reset all faces to default so that it can be filled with the fresh lidar data
-    boundary.reset();
+    frontend.boundary.reset();
 
     // precalculate sq of min distance
     const float distance_min_sq = sq(distance_min());
@@ -62,7 +62,7 @@ void AP_Proximity_AirSimSITL::update(void)
 
             // add distance to the 3D boundary
             const float yaw_angle_deg = wrap_360(degrees(atan2f(point.y, point.x)));
-            const AP_Proximity_Boundary_3D::Face face = boundary.get_face(yaw_angle_deg);
+            const AP_Proximity_Boundary_3D::Face face = frontend.boundary.get_face(yaw_angle_deg);
             // store the min distance in each face in a temp boundary
             temp_boundary.add_distance(face, yaw_angle_deg, safe_sqrt(distance_sq));
 
@@ -77,7 +77,7 @@ void AP_Proximity_AirSimSITL::update(void)
         }
     }
     // copy temp boundary to real boundary
-    temp_boundary.update_3D_boundary(boundary);
+    temp_boundary.update_3D_boundary(state.instance, frontend.boundary);
 }
 
 // get maximum and minimum distances (in meters) of primary sensor

libraries/AP_Proximity/AP_Proximity_Backend.cpp

@@ -38,39 +38,12 @@ AP_Proximity_Backend::AP_Proximity_Backend(AP_Proximity& _frontend, AP_Proximity
 static_assert(PROXIMITY_MAX_DIRECTION <= 8,
               "get_horizontal_distances assumes 8-bits is enough for validity bitmask");
 
-// get distances in PROXIMITY_MAX_DIRECTION directions horizontally. used for sending distances to ground station
-bool AP_Proximity_Backend::get_horizontal_distances(AP_Proximity::Proximity_Distance_Array &prx_dist_array) const
-{
-    AP_Proximity::Proximity_Distance_Array prx_filt_dist_array; // unused
-    return boundary.get_layer_distances(PROXIMITY_MIDDLE_LAYER, distance_max(), prx_dist_array, prx_filt_dist_array);
-}
-// get distances in PROXIMITY_MAX_DIRECTION directions at a layer. used for logging
-bool AP_Proximity_Backend::get_active_layer_distances(uint8_t layer, AP_Proximity::Proximity_Distance_Array &prx_dist_array, AP_Proximity::Proximity_Distance_Array &prx_filt_dist_array) const
-{
-    return boundary.get_layer_distances(layer, distance_max(), prx_dist_array, prx_filt_dist_array);
-}
-
 // set status and update valid count
 void AP_Proximity_Backend::set_status(AP_Proximity::Status status)
 {
     state.status = status;
 }
 
-// timeout faces that have not received data recently and update filter frequencies
-void AP_Proximity_Backend::boundary_3D_checks()
-{
-    // set the cutoff freq for low pass filter
-    boundary.set_filter_freq(frontend.get_filter_freq());
-
-    // check if any face has valid distance when it should not
-    const uint32_t now_ms = AP_HAL::millis();
-    // run this check every PROXIMITY_BOUNDARY_3D_TIMEOUT_MS
-    if ((now_ms - _last_timeout_check_ms) > PROXIMITY_BOUNDARY_3D_TIMEOUT_MS) {
-        _last_timeout_check_ms = now_ms;
-        boundary.check_face_timeout();
-    }
-}
-
 // correct an angle (in degrees) based on the orientation and yaw correction parameters
 float AP_Proximity_Backend::correct_angle_for_orientation(float angle_degrees) const
 {

libraries/AP_Proximity/AP_Proximity_Backend.h

@@ -18,11 +18,9 @@
 
 #if HAL_PROXIMITY_ENABLED
 #include <AP_Common/AP_Common.h>
-#include "AP_Proximity_Boundary_3D.h"
 
 #define PROXIMITY_GND_DETECT_THRESHOLD 1.0f // set ground detection threshold to be 1 meters
 #define PROXIMITY_ALT_DETECT_TIMEOUT_MS 500 // alt readings should arrive within this much time
-#define PROXIMITY_BOUNDARY_3D_TIMEOUT_MS 750 // we should check the 3D boundary faces after every this many ms
 
 class AP_Proximity_Backend
 {
@@ -37,9 +35,6 @@ public:
     // update the state structure
     virtual void update() = 0;
 
-    // timeout faces that have not received data recently and update filter frequencies
-    void boundary_3D_checks();
-
     // get maximum and minimum distances (in meters) of sensor
     virtual float distance_max() const = 0;
     virtual float distance_min() const = 0;
@@ -50,33 +45,6 @@ public:
     // handle mavlink DISTANCE_SENSOR messages
     virtual void handle_msg(const mavlink_message_t &msg) {}
 
-    // get total number of obstacles, used in GPS based Simple Avoidance
-    uint8_t get_obstacle_count() { return boundary.get_obstacle_count(); }
-    
-    // get vector to obstacle based on obstacle_num passed, used in GPS based Simple Avoidance
-    bool get_obstacle(uint8_t obstacle_num, Vector3f& vec_to_obstacle) const { return boundary.get_obstacle(obstacle_num, vec_to_obstacle); }
-    
-    // returns shortest distance to "obstacle_num" obstacle, from a line segment formed between "seg_start" and "seg_end"
-    // used in GPS based Simple Avoidance
-    bool closest_point_from_segment_to_obstacle(const uint8_t obstacle_num, const Vector3f& seg_start, const Vector3f& seg_end, Vector3f& closest_point) const { return boundary.closest_point_from_segment_to_obstacle(obstacle_num , seg_start, seg_end, closest_point); }
-
-    // get distance and angle to closest object (used for pre-arm check)
-    //   returns true on success, false if no valid readings
-    bool get_closest_object(float& angle_deg, float &distance) const { return boundary.get_closest_object(angle_deg, distance); }
-
-    // get number of objects, angle and distance - used for non-GPS avoidance
-    uint8_t get_horizontal_object_count() const {return boundary.get_horizontal_object_count(); }
-    bool get_horizontal_object_angle_and_distance(uint8_t object_number, float& angle_deg, float &distance) const { return boundary.get_horizontal_object_angle_and_distance(object_number, angle_deg, distance); }
-
-    // get distances in 8 directions. used for sending distances to ground station
-    bool get_horizontal_distances(AP_Proximity::Proximity_Distance_Array &prx_dist_array) const;
-
-    // get raw and filtered distances in 8 directions per layer. used for logging
-    bool get_active_layer_distances(uint8_t layer, AP_Proximity::Proximity_Distance_Array &prx_dist_array, AP_Proximity::Proximity_Distance_Array &prx_filt_dist_array) const;
-
-    // get number of layers
-    uint8_t get_num_layers() const { return boundary.get_num_layers(); }
-
     // store rangefinder values
     void set_rangefinder_alt(bool use, bool healthy, float alt_cm);
 
@@ -111,8 +79,6 @@ protected:
     };
     static void database_push(float angle, float pitch, float distance, uint32_t timestamp_ms, const Vector3f &current_pos, const Matrix3f &body_to_ned);
 
-    uint32_t _last_timeout_check_ms;  // time when boundary was checked for non-updated valid faces
-
     // used for ground detection
     uint32_t _last_downward_update_ms;
     bool     _rangefinder_use;
@@ -122,9 +88,6 @@ protected:
     AP_Proximity &frontend;
     AP_Proximity::Proximity_State &state;   // reference to this instances state
     AP_Proximity_Params &params;            // parameters for this backend
-
-    // Methods to manipulate 3D boundary in this class
-    AP_Proximity_Boundary_3D boundary;
 };
 
 #endif // HAL_PROXIMITY_ENABLED

libraries/AP_Proximity/AP_Proximity_Boundary_3D.cpp

@@ -15,8 +15,7 @@
 
 #include "AP_Proximity_Boundary_3D.h"
 
-#if HAL_PROXIMITY_ENABLED
+#define PROXIMITY_BOUNDARY_3D_TIMEOUT_MS 750 // we should check the 3D boundary faces after this many ms
-#include "AP_Proximity_Backend.h"
 
 /*
   Constructor. 
@@ -185,6 +184,13 @@ void AP_Proximity_Boundary_3D::reset_face(const Face &face)
 // check if a face has valid distance even if it was updated a long time back
 void AP_Proximity_Boundary_3D::check_face_timeout()
 {
+    // exit immediately if already checked recently
+    const uint32_t now_ms = AP_HAL::millis();
+    if ((now_ms - _last_check_face_timeout_ms) < PROXIMITY_BOUNDARY_3D_TIMEOUT_MS) {
+        return;
+    }
+    _last_check_face_timeout_ms = now_ms;
+
     for (uint8_t layer=0; layer < PROXIMITY_NUM_LAYERS; layer++) {
         for (uint8_t sector=0; sector < PROXIMITY_NUM_SECTORS; sector++) {
             if (_distance_valid[layer][sector]) {
@@ -363,7 +369,7 @@ bool AP_Proximity_Boundary_3D::get_filtered_distance(const Face &face, float &di
 }
 
 // Get raw and filtered distances in 8 directions per layer
-bool AP_Proximity_Boundary_3D::get_layer_distances(uint8_t layer_number, float dist_max, AP_Proximity::Proximity_Distance_Array &prx_dist_array, AP_Proximity::Proximity_Distance_Array &prx_filt_dist_array) const
+bool AP_Proximity_Boundary_3D::get_layer_distances(uint8_t layer_number, float dist_max, Proximity_Distance_Array &prx_dist_array, Proximity_Distance_Array &prx_filt_dist_array) const
 {
     // cycle through all sectors filling in distances and orientations
     // see MAV_SENSOR_ORIENTATION for orientations (0 = forward, 1 = 45 degree clockwise from north, etc)
@@ -423,4 +429,3 @@ void AP_Proximity_Temp_Boundary::update_3D_boundary(AP_Proximity_Boundary_3D &bo
     }
 }
 
-#endif // HAL_PROXIMITY_ENABLED

libraries/AP_Proximity/AP_Proximity_Boundary_3D.h

@@ -15,10 +15,8 @@
 
 #pragma once
 
-#include "AP_Proximity.h"
+#include <AP_Common/AP_Common.h>
-
+#include <AP_Math/AP_Math.h>
-#if HAL_PROXIMITY_ENABLED
-
 #include <Filter/LowPassFilter.h>
 
 #define PROXIMITY_NUM_SECTORS         8       // number of sectors
@@ -31,6 +29,19 @@
 #define PROXIMITY_FILT_RESET_TIME     1000    // reset filter if last distance was pushed more than this many ms away
 #define PROXIMITY_FACE_RESET_MS       1000    // face will be reset if not updated within this many ms
 
+// structure holding distances in PROXIMITY_MAX_DIRECTION directions. used for sending distances to ground station
+#define PROXIMITY_MAX_DIRECTION 8
+struct Proximity_Distance_Array {
+    uint8_t orientation[PROXIMITY_MAX_DIRECTION]; // orientation (i.e. rough direction) of the distance (see MAV_SENSOR_ORIENTATION)
+    float distance[PROXIMITY_MAX_DIRECTION];      // distance in meters
+    bool valid(uint8_t offset) const {
+        // returns true if the distance stored at offset is valid
+        return (offset < 8 && (offset_valid & (1U<<offset)));
+    };
+
+    uint8_t offset_valid; // bitmask
+};
+
 class AP_Proximity_Boundary_3D
 {
 public:
@@ -112,7 +123,7 @@ public:
     uint8_t get_num_layers() const { return PROXIMITY_NUM_LAYERS; }
 
     // get raw and filtered distances in 8 directions per layer.
-    bool get_layer_distances(uint8_t layer_number, float dist_max, AP_Proximity::Proximity_Distance_Array &prx_dist_array, AP_Proximity::Proximity_Distance_Array &prx_filt_dist_array) const;
+    bool get_layer_distances(uint8_t layer_number, float dist_max, Proximity_Distance_Array &prx_dist_array, Proximity_Distance_Array &prx_filt_dist_array) const;
 
     // pass down filter cut-off freq from params
     void set_filter_freq(float filt_freq) { _filter_freq = filt_freq; }
@@ -161,6 +172,7 @@ private:
     uint32_t _last_update_ms[PROXIMITY_NUM_LAYERS][PROXIMITY_NUM_SECTORS]; // time when distance was last updated
     LowPassFilterFloat _filtered_distance[PROXIMITY_NUM_LAYERS][PROXIMITY_NUM_SECTORS]; // low pass filter
     float _filter_freq;                                                 // cutoff freq of low pass filter
+    uint32_t _last_check_face_timeout_ms;                               // system time to throttle check_face_timeout method
 };
 
 // This class gives an easy way of making a temporary boundary, used for "sorting" distances.
@@ -189,5 +201,3 @@ private:
     float _angle[PROXIMITY_NUM_LAYERS][PROXIMITY_NUM_SECTORS];          // yaw angle in degrees to closest object within each sector and layer
     float _pitch[PROXIMITY_NUM_LAYERS][PROXIMITY_NUM_SECTORS];          // pitch angle in degrees to the closest object within each sector and layer
 };
-
-#endif // HAL_PROXIMITY_ENABLED

libraries/AP_Proximity/AP_Proximity_Cygbot_D1.cpp

@@ -118,7 +118,7 @@ bool AP_Proximity_Cygbot_D1::parse_byte(uint8_t data)
         // checksum is valid, parse payload
         _last_distance_received_ms = AP_HAL::millis();
         parse_payload();
-        _temp_boundary.update_3D_boundary(boundary);
+        _temp_boundary.update_3D_boundary(frontend.boundary);
         reset();
         return true;
     }
@@ -149,7 +149,8 @@ void AP_Proximity_Cygbot_D1::parse_payload()
                 continue;
             }
             // convert angle to face
-            const AP_Proximity_Boundary_3D::Face face = boundary.get_face(corrected_angle);
+            const AP_Proximity_Boundary_3D::Face face = frontend.boundary.get_face(corrected_angle);
+
             // push face to temp boundary
             _temp_boundary.add_distance(face, corrected_angle, distance_m);
             // push to OA_DB

libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp

@@ -320,16 +320,16 @@ void AP_Proximity_LightWareSF40C::process_message()
             const uint16_t idx = 14 + (i * 2);
             const int16_t dist_cm = (int16_t)buff_to_uint16(_msg.payload[idx], _msg.payload[idx+1]);
             const float angle_deg = wrap_360((point_start_index + i) * angle_inc_deg * angle_sign + angle_correction);
-            const AP_Proximity_Boundary_3D::Face face = boundary.get_face(angle_deg);
+            const AP_Proximity_Boundary_3D::Face face = frontend.boundary.get_face(angle_deg);
 
             // if point is on a new face then finish off previous face
             if (face != _face) {
                 // update boundary used for avoidance
                 if (_face_distance_valid) {
-                    boundary.set_face_attributes(_face, _face_yaw_deg, _face_distance);
+                    frontend.boundary.set_face_attributes(_face, _face_yaw_deg, _face_distance);
                 } else {
                     // mark previous face invalid
-                    boundary.reset_face(_face);
+                    frontend.boundary.reset_face(_face);
                 }
                 // init for new face
                 _face = face;

libraries/AP_Proximity/AP_Proximity_LightWareSF45B.cpp

@@ -141,13 +141,13 @@ void AP_Proximity_LightWareSF45B::process_message()
 
         // if distance is from a new face then update distance, angle and boundary for previous face
         // get face from 3D boundary based on yaw angle to the object
-        const AP_Proximity_Boundary_3D::Face face = boundary.get_face(angle_deg);
+        const AP_Proximity_Boundary_3D::Face face = frontend.boundary.get_face(angle_deg);
         if (face != _face) {
             if (_face_distance_valid) {
-                boundary.set_face_attributes(_face, _face_yaw_deg, _face_distance);
+                frontend.boundary.set_face_attributes(_face, _face_yaw_deg, _face_distance);
             } else {
                 // mark previous face invalid
-                boundary.reset_face(_face);
+                frontend.boundary.reset_face(_face);
             }
             // record updated face
             _face = face;

libraries/AP_Proximity/AP_Proximity_MAV.cpp

@@ -86,7 +86,7 @@ void AP_Proximity_MAV::handle_distance_sensor_msg(const mavlink_message_t &msg)
         // provided we got the new obstacle in less than PROXIMITY_TIMESTAMP_MSG_TIMEOUT_MS
         if ((previous_msg_timestamp != _last_msg_update_timestamp_ms) || (time_diff > PROXIMITY_TIMESTAMP_MSG_TIMEOUT_MS)) {
             // push data from temp boundary to the main 3-D proximity boundary
-            temp_boundary.update_3D_boundary(boundary);
+            temp_boundary.update_3D_boundary(frontend.boundary);
             // clear temp boundary for new data
             temp_boundary.reset();
         }
@@ -95,7 +95,7 @@ void AP_Proximity_MAV::handle_distance_sensor_msg(const mavlink_message_t &msg)
         const uint8_t sector = packet.orientation;
         // get the face for this sector
         const float yaw_angle_deg = sector * 45;
-        const AP_Proximity_Boundary_3D::Face face = boundary.get_face(yaw_angle_deg);
+        const AP_Proximity_Boundary_3D::Face face = frontend.boundary.get_face(yaw_angle_deg);
         _distance_min = packet.min_distance * 0.01f;
         _distance_max = packet.max_distance * 0.01f;
         const bool in_range = distance <= _distance_max && distance >= _distance_min;
@@ -158,7 +158,7 @@ void AP_Proximity_MAV::handle_obstacle_distance_msg(const mavlink_message_t &msg
     bool face_distance_valid = false;
 
     // reset this  boundary to fill with new data
-    boundary.reset();
+    frontend.boundary.reset();
 
     // iterate over message's sectors
     for (uint8_t j = 0; j < total_distances; j++) {
@@ -174,13 +174,13 @@ void AP_Proximity_MAV::handle_obstacle_distance_msg(const mavlink_message_t &msg
         }
 
         // get face for this latest reading
-        AP_Proximity_Boundary_3D::Face latest_face = boundary.get_face(mid_angle);
+        AP_Proximity_Boundary_3D::Face latest_face = frontend.boundary.get_face(mid_angle);
         if (latest_face != face) {
             // store previous face
             if (face_distance_valid) {
-                boundary.set_face_attributes(face, face_yaw_deg, face_distance);
+                frontend.boundary.set_face_attributes(face, face_yaw_deg, face_distance);
             } else {
-                boundary.reset_face(face);
+                frontend.boundary.reset_face(face);
             }
             // init for latest face
             face = latest_face;
@@ -202,9 +202,9 @@ void AP_Proximity_MAV::handle_obstacle_distance_msg(const mavlink_message_t &msg
 
     // process the last face
     if (face_distance_valid) {
-        boundary.set_face_attributes(face, face_yaw_deg, face_distance);
+        frontend.boundary.set_face_attributes(face, face_yaw_deg, face_distance);
     } else {
-        boundary.reset_face(face);
+        frontend.boundary.reset_face(face);
     }
     return;
 }
@@ -231,7 +231,7 @@ void AP_Proximity_MAV::handle_obstacle_distance_3d_msg(const mavlink_message_t &
 
     if ((previous_msg_timestamp != _last_msg_update_timestamp_ms) || (time_diff > PROXIMITY_TIMESTAMP_MSG_TIMEOUT_MS)) {
         // push data from temp boundary to the main 3-D proximity boundary because a new timestamp has arrived
-        temp_boundary.update_3D_boundary(boundary);
+        temp_boundary.update_3D_boundary(frontend.boundary);
         // clear temp boundary for new data
         temp_boundary.reset();
     }
@@ -263,7 +263,7 @@ void AP_Proximity_MAV::handle_obstacle_distance_3d_msg(const mavlink_message_t &
     }
 
     // allot to correct layer and sector based on calculated pitch and yaw
-    const AP_Proximity_Boundary_3D::Face face = boundary.get_face(pitch, yaw);
+    const AP_Proximity_Boundary_3D::Face face = frontend.boundary.get_face(pitch, yaw);
     temp_boundary.add_distance(face, pitch, yaw, obstacle.length());
 
     if (database_ready) {

libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp

@@ -314,15 +314,15 @@ void AP_Proximity_RPLidarA2::parse_response_data()
 #endif
                 _last_distance_received_ms = AP_HAL::millis();
                 if (!ignore_reading(angle_deg, distance_m)) {
-                    const AP_Proximity_Boundary_3D::Face face = boundary.get_face(angle_deg);
+                    const AP_Proximity_Boundary_3D::Face face = frontend.boundary.get_face(angle_deg);
 
                     if (face != _last_face) {
                         // distance is for a new face, the previous one can be updated now
                         if (_last_distance_valid) {
-                            boundary.set_face_attributes(_last_face, _last_angle_deg, _last_distance_m);
+                            frontend.boundary.set_face_attributes(_last_face, _last_angle_deg, _last_distance_m);
                         } else {
                             // reset distance from last face
-                            boundary.reset_face(face);
+                            frontend.boundary.reset_face(face);
                         }
 
                         // initialize the new face

libraries/AP_Proximity/AP_Proximity_RangeFinder.cpp

@@ -45,17 +45,17 @@ void AP_Proximity_RangeFinder::update(void)
             if (sensor->orientation() <= ROTATION_YAW_315) {
                 const uint8_t sector = (uint8_t)sensor->orientation();
                 const float angle = sector * 45;
-                const AP_Proximity_Boundary_3D::Face face = boundary.get_face(angle);
+                const AP_Proximity_Boundary_3D::Face face = frontend.boundary.get_face(angle);
                 // distance in meters
                 const float distance = sensor->distance();
                 _distance_min = sensor->min_distance_cm() * 0.01f;
                 _distance_max = sensor->max_distance_cm() * 0.01f;
                 if ((distance <= _distance_max) && (distance >= _distance_min) && !ignore_reading(angle, distance, false)) {
-                    boundary.set_face_attributes(face, angle, distance);
+                    frontend.boundary.set_face_attributes(face, angle, distance);
                     // update OA database
                     database_push(angle, distance);
                 } else {
-                    boundary.reset_face(face);
+                    frontend.boundary.reset_face(face);
                 }
                 _last_update_ms = now;
             }

libraries/AP_Proximity/AP_Proximity_SITL.cpp

@@ -61,14 +61,14 @@ void AP_Proximity_SITL::update(void)
         // update distance in each sector
         for (uint8_t sector=0; sector < PROXIMITY_NUM_SECTORS; sector++) {
             const float yaw_angle_deg = sector * 45.0f;
-            AP_Proximity_Boundary_3D::Face face = boundary.get_face(yaw_angle_deg);
+            AP_Proximity_Boundary_3D::Face face = frontend.boundary.get_face(yaw_angle_deg);
             float fence_distance;
             if (get_distance_to_fence(yaw_angle_deg, fence_distance)) {
-                boundary.set_face_attributes(face, yaw_angle_deg, fence_distance);
+                frontend.boundary.set_face_attributes(face, yaw_angle_deg, fence_distance);
                 // update OA database
                 database_push(yaw_angle_deg, fence_distance);
             } else {
-                boundary.reset_face(face);
+                frontend.boundary.reset_face(face);
             }
         }
     } else {

libraries/AP_Proximity/AP_Proximity_TeraRangerTower.cpp

@@ -95,13 +95,13 @@ bool AP_Proximity_TeraRangerTower::read_sensor_data()
 void AP_Proximity_TeraRangerTower::update_sector_data(int16_t angle_deg, uint16_t distance_mm)
 {
     // Get location on 3-D boundary based on angle to the object
-    const AP_Proximity_Boundary_3D::Face face = boundary.get_face(angle_deg);
+    const AP_Proximity_Boundary_3D::Face face = frontend.boundary.get_face(angle_deg);
     if ((distance_mm != 0xffff) && !ignore_reading(angle_deg, distance_mm * 0.001f, false)) {
-        boundary.set_face_attributes(face, angle_deg, ((float) distance_mm) / 1000);
+        frontend.boundary.set_face_attributes(face, angle_deg, ((float) distance_mm) / 1000);
         // update OA database
         database_push(angle_deg, ((float) distance_mm) / 1000);
     } else {
-        boundary.reset_face(face);
+        frontend.boundary.reset_face(face);
     }
     _last_distance_received_ms = AP_HAL::millis();
 }

libraries/AP_Proximity/AP_Proximity_TeraRangerTowerEvo.cpp

@@ -148,15 +148,15 @@ bool AP_Proximity_TeraRangerTowerEvo::read_sensor_data()
 void AP_Proximity_TeraRangerTowerEvo::update_sector_data(int16_t angle_deg, uint16_t distance_mm)
 {
     // Get location on 3-D boundary based on angle to the object
-    const AP_Proximity_Boundary_3D::Face face = boundary.get_face(angle_deg);
+    const AP_Proximity_Boundary_3D::Face face = frontend.boundary.get_face(angle_deg);
     //check for target too far, target too close and sensor not connected
     const bool valid = (distance_mm != 0xffff) && (distance_mm > 0x0001);
     if (valid && !ignore_reading(angle_deg, distance_mm * 0.001f, false)) {
-        boundary.set_face_attributes(face, angle_deg, ((float) distance_mm) / 1000);
+        frontend.boundary.set_face_attributes(face, angle_deg, ((float) distance_mm) / 1000);
         // update OA database
         database_push(angle_deg, ((float) distance_mm) / 1000);
     } else {
-        boundary.reset_face(face);
+        frontend.boundary.reset_face(face);
     }
     _last_distance_received_ms = AP_HAL::millis();
 }