diff --git a/libraries/AP_Proximity/AP_Proximity.cpp b/libraries/AP_Proximity/AP_Proximity.cpp
index a90780efb1..1521929258 100644
--- a/libraries/AP_Proximity/AP_Proximity.cpp
+++ b/libraries/AP_Proximity/AP_Proximity.cpp
@@ -103,7 +103,7 @@ AP_Proximity::AP_Proximity()
 
 // initialise the Proximity class. We do detection of attached sensors here
 // we don't allow for hot-plugging of sensors (i.e. reboot required)
-void AP_Proximity::init(void)
+void AP_Proximity::init()
 {
     if (num_instances != 0) {
         // init called a 2nd time?
@@ -123,7 +123,7 @@ void AP_Proximity::init(void)
 }
 
 // update Proximity state for all instances. This should be called at a high rate by the main loop
-void AP_Proximity::update(void)
+void AP_Proximity::update()
 {
     for (uint8_t i=0; i<num_instances; i++) {
         if (!valid_instance(i)) {
@@ -141,6 +141,14 @@ void AP_Proximity::update(void)
     }
 }
 
+AP_Proximity::Type AP_Proximity::get_type(uint8_t instance) const
+{
+    if (instance < PROXIMITY_MAX_INSTANCES) {
+        return (Type)((uint8_t)params[instance].type);
+    }
+    return Type::None;
+}
+
 // return sensor orientation
 uint8_t AP_Proximity::get_orientation(uint8_t instance) const
 {
@@ -177,107 +185,24 @@ AP_Proximity::Status AP_Proximity::get_status() const
     return get_status(primary_instance);
 }
 
-// handle mavlink DISTANCE_SENSOR messages
-void AP_Proximity::handle_msg(const mavlink_message_t &msg)
+// get maximum and minimum distances (in meters) of primary sensor
+float AP_Proximity::distance_max() const
 {
-    for (uint8_t i=0; i<num_instances; i++) {
-        if (valid_instance(i)) {
-            drivers[i]->handle_msg(msg);
-        }
+    if (!valid_instance(primary_instance)) {
+        return 0.0f;
     }
+    // get maximum distance from backend
+    return drivers[primary_instance]->distance_max();
 }
-
-// return true if the given instance exists
-bool AP_Proximity::valid_instance(uint8_t i) const
+float AP_Proximity::distance_min() const
 {
-    if (i >= PROXIMITY_MAX_INSTANCES) {
-        return false;
-    }
-
-    if (drivers[i] == nullptr) {
-        return false;
+    if (!valid_instance(primary_instance)) {
+        return 0.0f;
     }
-    return (Type)params[i].type.get() != Type::None;
+    // get minimum distance from backend
+    return drivers[primary_instance]->distance_min();
 }
 
-//  detect if an instance of a proximity sensor is connected.
-void AP_Proximity::detect_instance(uint8_t instance)
-{
-    switch (get_type(instance)) {
-    case Type::None:
-        return;
-    case Type::RPLidarA2:
-        if (AP_Proximity_RPLidarA2::detect(instance)) {
-            state[instance].instance = instance;
-            drivers[instance] = new AP_Proximity_RPLidarA2(*this, state[instance], params[instance]);
-            return;
-        }
-        break;
-    case Type::MAV:
-        state[instance].instance = instance;
-        drivers[instance] = new AP_Proximity_MAV(*this, state[instance], params[instance]);
-        return;
-
-    case Type::TRTOWER:
-        if (AP_Proximity_TeraRangerTower::detect(instance)) {
-            state[instance].instance = instance;
-            drivers[instance] = new AP_Proximity_TeraRangerTower(*this, state[instance], params[instance]);
-            return;
-        }
-        break;
-    case Type::TRTOWEREVO:
-        if (AP_Proximity_TeraRangerTowerEvo::detect(instance)) {
-            state[instance].instance = instance;
-            drivers[instance] = new AP_Proximity_TeraRangerTowerEvo(*this, state[instance], params[instance]);
-            return;
-        }
-        break;
-
-    case Type::RangeFinder:
-        state[instance].instance = instance;
-        drivers[instance] = new AP_Proximity_RangeFinder(*this, state[instance], params[instance]);
-        return;
-
-    case Type::SF40C:
-        if (AP_Proximity_LightWareSF40C::detect(instance)) {
-            state[instance].instance = instance;
-            drivers[instance] = new AP_Proximity_LightWareSF40C(*this, state[instance], params[instance]);
-            return;
-        }
-        break;
-
-    case Type::SF45B:
-        if (AP_Proximity_LightWareSF45B::detect(instance)) {
-            state[instance].instance = instance;
-            drivers[instance] = new AP_Proximity_LightWareSF45B(*this, state[instance], params[instance]);
-            return;
-        }
-        break;
-
-    case Type::CYGBOT_D1:
-#if AP_PROXIMITY_CYGBOT_ENABLED
-    if (AP_Proximity_Cygbot_D1::detect(instance)) {
-        state[instance].instance = instance;
-        drivers[instance] = new AP_Proximity_Cygbot_D1(*this, state[instance], params[instance]);
-        return;
-    }
-# endif
-    break;
-
-#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
-    case Type::SITL:
-        state[instance].instance = instance;
-        drivers[instance] = new AP_Proximity_SITL(*this, state[instance], params[instance]);
-        return;
-
-    case Type::AirSimSITL:
-        state[instance].instance = instance;
-        drivers[instance] = new AP_Proximity_AirSimSITL(*this, state[instance], params[instance]);
-        return;
-
-#endif
-    }
-}
 
 // 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
@@ -289,6 +214,15 @@ bool AP_Proximity::get_horizontal_distances(Proximity_Distance_Array &prx_dist_a
     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
 {
@@ -308,15 +242,6 @@ uint8_t AP_Proximity::get_obstacle_count() const
     return drivers[primary_instance]->get_obstacle_count();
 }
 
-// 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 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
 {
@@ -368,22 +293,29 @@ bool AP_Proximity::get_object_angle_and_distance(uint8_t object_number, float& a
     return drivers[primary_instance]->get_horizontal_object_angle_and_distance(object_number, angle_deg, distance);
 }
 
-// get maximum and minimum distances (in meters) of primary sensor
-float AP_Proximity::distance_max() const
+// handle mavlink DISTANCE_SENSOR messages
+void AP_Proximity::handle_msg(const mavlink_message_t &msg)
 {
-    if (!valid_instance(primary_instance)) {
-        return 0.0f;
+    for (uint8_t i=0; i<num_instances; i++) {
+        if (valid_instance(i)) {
+            drivers[i]->handle_msg(msg);
+        }
     }
-    // get maximum distance from backend
-    return drivers[primary_instance]->distance_max();
 }
-float AP_Proximity::distance_min() const
+
+// methods for mavlink SYS_STATUS message (send_sys_status)
+// these methods cover only the primary instance
+bool AP_Proximity::sensor_present() const
 {
-    if (!valid_instance(primary_instance)) {
-        return 0.0f;
-    }
-    // get minimum distance from backend
-    return drivers[primary_instance]->distance_min();
+    return get_status() != Status::NotConnected;
+}
+bool AP_Proximity::sensor_enabled() const
+{
+    return get_type(primary_instance) != Type::None;
+}
+bool AP_Proximity::sensor_failed() const
+{
+    return get_status() != Status::Good;
 }
 
 // get distance in meters upwards, returns true on success
@@ -401,27 +333,6 @@ bool AP_Proximity::get_upward_distance(float &distance) const
     return get_upward_distance(primary_instance, distance);
 }
 
-AP_Proximity::Type AP_Proximity::get_type(uint8_t instance) const
-{
-    if (instance < PROXIMITY_MAX_INSTANCES) {
-        return (Type)((uint8_t)params[instance].type);
-    }
-    return Type::None;
-}
-
-bool AP_Proximity::sensor_present() const
-{
-    return get_status() != Status::NotConnected;
-}
-bool AP_Proximity::sensor_enabled() const
-{
-    return get_type(primary_instance) != Type::None;
-}
-bool AP_Proximity::sensor_failed() const
-{
-    return get_status() != Status::Good;
-}
-
 // set alt as read from dowward facing rangefinder. Tilt is already adjusted for.
 void AP_Proximity::set_rangefinder_alt(bool use, bool healthy, float alt_cm)
 {
@@ -498,6 +409,100 @@ void AP_Proximity::log()
 }
 #endif
 
+// return true if the given instance exists
+bool AP_Proximity::valid_instance(uint8_t i) const
+{
+    if (i >= PROXIMITY_MAX_INSTANCES) {
+        return false;
+    }
+
+    if (drivers[i] == nullptr) {
+        return false;
+    }
+    return (Type)params[i].type.get() != Type::None;
+}
+
+//  detect if an instance of a proximity sensor is connected.
+void AP_Proximity::detect_instance(uint8_t instance)
+{
+    switch (get_type(instance)) {
+    case Type::None:
+        return;
+    case Type::RPLidarA2:
+        if (AP_Proximity_RPLidarA2::detect(instance)) {
+            state[instance].instance = instance;
+            drivers[instance] = new AP_Proximity_RPLidarA2(*this, state[instance], params[instance]);
+            return;
+        }
+        break;
+    case Type::MAV:
+        state[instance].instance = instance;
+        drivers[instance] = new AP_Proximity_MAV(*this, state[instance], params[instance]);
+        return;
+
+    case Type::TRTOWER:
+        if (AP_Proximity_TeraRangerTower::detect(instance)) {
+            state[instance].instance = instance;
+            drivers[instance] = new AP_Proximity_TeraRangerTower(*this, state[instance], params[instance]);
+            return;
+        }
+        break;
+    case Type::TRTOWEREVO:
+        if (AP_Proximity_TeraRangerTowerEvo::detect(instance)) {
+            state[instance].instance = instance;
+            drivers[instance] = new AP_Proximity_TeraRangerTowerEvo(*this, state[instance], params[instance]);
+            return;
+        }
+        break;
+
+    case Type::RangeFinder:
+        state[instance].instance = instance;
+        drivers[instance] = new AP_Proximity_RangeFinder(*this, state[instance], params[instance]);
+        return;
+
+    case Type::SF40C:
+        if (AP_Proximity_LightWareSF40C::detect(instance)) {
+            state[instance].instance = instance;
+            drivers[instance] = new AP_Proximity_LightWareSF40C(*this, state[instance], params[instance]);
+            return;
+        }
+        break;
+
+    case Type::SF45B:
+        if (AP_Proximity_LightWareSF45B::detect(instance)) {
+            state[instance].instance = instance;
+            drivers[instance] = new AP_Proximity_LightWareSF45B(*this, state[instance], params[instance]);
+            return;
+        }
+        break;
+
+    case Type::CYGBOT_D1:
+#if AP_PROXIMITY_CYGBOT_ENABLED
+    if (AP_Proximity_Cygbot_D1::detect(instance)) {
+        state[instance].instance = instance;
+        drivers[instance] = new AP_Proximity_Cygbot_D1(*this, state[instance], params[instance]);
+        return;
+    }
+# endif
+    break;
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    case Type::SITL:
+        state[instance].instance = instance;
+        drivers[instance] = new AP_Proximity_SITL(*this, state[instance], params[instance]);
+        return;
+
+    case Type::AirSimSITL:
+        state[instance].instance = instance;
+        drivers[instance] = new AP_Proximity_AirSimSITL(*this, state[instance], params[instance]);
+        return;
+
+#endif
+    }
+}
+
+
+
 AP_Proximity *AP_Proximity::_singleton;
 
 namespace AP {
diff --git a/libraries/AP_Proximity/AP_Proximity.h b/libraries/AP_Proximity/AP_Proximity.h
index 4e8301e697..717ce94e3a 100644
--- a/libraries/AP_Proximity/AP_Proximity.h
+++ b/libraries/AP_Proximity/AP_Proximity.h
@@ -64,23 +64,17 @@ public:
         Good
     };
 
-    // 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
-    };
-
     // detect and initialise any available proximity sensors
-    void init(void);
+    void init();
 
     // update state of all proximity sensors. Should be called at high rate from main loop
-    void update(void);
+    void update();
+
+    // return the number of proximity sensor backends
+    uint8_t num_sensors() const { return num_instances; }
+
+    // return sensor type of a given instance
+    Type get_type(uint8_t instance) const;
 
     // return sensor orientation and yaw correction
     uint8_t get_orientation(uint8_t instance) const;
@@ -91,14 +85,32 @@ public:
     Status get_status(uint8_t instance) const;
     Status get_status() const;
 
-    // Return the number of proximity sensors
-    uint8_t num_sensors(void) const {
-        return num_instances;
-    }
+    // get maximum and minimum distances (in meters) of primary sensor
+    float distance_max() const;
+    float distance_min() const;
+
+    //
+    // 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;
 
@@ -120,50 +132,44 @@ public:
     uint8_t get_object_count() const;
     bool get_object_angle_and_distance(uint8_t object_number, float& angle_deg, float &distance) const;
 
-    // get number of layers
-    uint8_t get_num_layers() const;
-
-    // get maximum and minimum distances (in meters) of primary sensor
-    float distance_max() const;
-    float distance_min() const;
+    //
+    // mavlink related methods
+    //
 
     // handle mavlink DISTANCE_SENSOR messages
     void handle_msg(const mavlink_message_t &msg);
 
-    // The Proximity_State structure is filled in by the backend driver
-    struct Proximity_State {
-        uint8_t                 instance;   // the instance number of this proximity sensor
-        Status   status;     // sensor status
-    };
+    // methods for mavlink SYS_STATUS message (send_sys_status)
+    // these methods cover only the primary instance
+    bool sensor_present() const;
+    bool sensor_enabled() const;
+    bool sensor_failed() const;
 
     //
-    // support for upward facing sensors
+    // support for upwards and downwards facing sensors
     //
 
     // get distance upwards in meters. returns true on success
     bool get_upward_distance(uint8_t instance, float &distance) const;
     bool get_upward_distance(float &distance) const;
 
-    Type get_type(uint8_t instance) const;
+    // set alt as read from downward facing rangefinder. Tilt is already adjusted for
+    void set_rangefinder_alt(bool use, bool healthy, float alt_cm);
+
+    // method called by vehicle to have AP_Proximity write onboard log messages
+    void log();
+
+    // The Proximity_State structure is filled in by the backend driver
+    struct Proximity_State {
+        uint8_t instance;   // the instance number of this proximity sensor
+        Status status;      // sensor status
+    };
 
     // parameter list
     static const struct AP_Param::GroupInfo var_info[];
 
     static AP_Proximity *get_singleton(void) { return _singleton; };
 
-    // methods for mavlink SYS_STATUS message (send_sys_status)
-    // these methods cover only the primary instance
-    bool sensor_present() const;
-    bool sensor_enabled() const;
-    bool sensor_failed() const;
-
-    // set alt as read from downward facing rangefinder. Tilt is already adjusted for
-    void set_rangefinder_alt(bool use, bool healthy, float alt_cm);
-
-    // method called by vehicle to have AP_Proximity write onboard log
-    // messages:
-    void log();
-
 protected:
 
     // parameters for backends