AP_Proximity: boundary uses shortest dist from multiple backends

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

libraries/AP_Proximity/AP_Proximity_Boundary_3D.cpp

@@ -53,17 +53,30 @@ AP_Proximity_Boundary_3D::Face AP_Proximity_Boundary_3D::get_face(float pitch, f
 }
 
 // Set the actual body-frame angle(yaw), pitch, and distance of the detected object.
-// This method will also mark the sector and layer to be "valid", so this distance can be used for Obstacle Avoidance
-void AP_Proximity_Boundary_3D::set_face_attributes(const Face &face, float pitch, float angle, float distance)
+// This method will also mark the sector and layer to be "valid",
+// This distance can then be used for Obstacle Avoidance
+// Assume detected obstacle is horizontal (zero pitch), if no pitch is passed
+// prx_instance should be set to the proximity sensor backend instance number
+void AP_Proximity_Boundary_3D::set_face_attributes(const Face &face, float pitch, float angle, float distance, uint8_t prx_instance)
 {
     if (!face.valid()) {
         return;
     }
 
+    // ignore update if another instance has provided a shorter distance within the last 0.2 seconds
+    if ((prx_instance != _prx_instance[face.layer][face.sector]) && _distance_valid[face.layer][face.sector] && (_filtered_distance[face.layer][face.sector].get() < distance)) {
+        // check if recent
+        const uint32_t now_ms = AP_HAL::millis();
+        if (now_ms - _last_update_ms[face.layer][face.sector] < PROXIMITY_FACE_RESET_MS) {
+            return;
+        }
+    }
+
     _angle[face.layer][face.sector] = angle;
     _pitch[face.layer][face.sector] = pitch;
     _distance[face.layer][face.sector] = distance;
     _distance_valid[face.layer][face.sector] = true;
+    _prx_instance[face.layer][face.sector] = prx_instance;
 
     // apply filter
     set_filtered_distance(face, distance);
@@ -170,11 +183,26 @@ void AP_Proximity_Boundary_3D::reset()
 
 // Reset this location, specified by Face object, back to default
 // i.e Distance is marked as not-valid, and set to a large number.
-void AP_Proximity_Boundary_3D::reset_face(const Face &face)
+// prx_instance should be set to the proximity sensor's backend instance number
+void AP_Proximity_Boundary_3D::reset_face(const Face &face, uint8_t prx_instance)
 {
     if (!face.valid()) {
         return;
     }
+
+    // return immediately if face already has no valid distance
+    if (!_distance_valid[face.layer][face.sector]) {
+        return;
+    }
+
+    // ignore reset if another instance provided this face's distance within the last 0.2 seconds
+    if (prx_instance != _prx_instance[face.layer][face.sector]) {
+        const uint32_t now_ms = AP_HAL::millis();
+        if (now_ms - _last_update_ms[face.layer][face.sector] < 200) {
+            return;
+        }
+    }
+
     _distance_valid[face.layer][face.sector] = false;
 
     // update simple avoidance boundary
@@ -194,10 +222,10 @@ void AP_Proximity_Boundary_3D::check_face_timeout()
     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]) {
-                if ((AP_HAL::millis() - _last_update_ms[layer][sector]) > PROXIMITY_FACE_RESET_MS) {
+                if ((now_ms - _last_update_ms[layer][sector]) > PROXIMITY_FACE_RESET_MS) {
                     // this face has a valid distance but wasn't updated for a long time, reset it
-                    AP_Proximity_Boundary_3D::Face face{layer, sector};
-                    reset_face(face);
+                    _distance_valid[layer][sector] = false;
+                    update_boundary(AP_Proximity_Boundary_3D::Face{layer, sector});
                 }
             }
         }
@@ -417,13 +445,14 @@ void AP_Proximity_Temp_Boundary::add_distance(const AP_Proximity_Boundary_3D::Fa
 }
 
 // fill the original 3D boundary with the contents of this temporary boundary
-void AP_Proximity_Temp_Boundary::update_3D_boundary(AP_Proximity_Boundary_3D &boundary)
+// prx_instance should be set to the proximity sensor's backend instance number
+void AP_Proximity_Temp_Boundary::update_3D_boundary(uint8_t prx_instance, AP_Proximity_Boundary_3D &boundary)
 {
     for (uint8_t layer=0; layer < PROXIMITY_NUM_LAYERS; layer++) {
         for (uint8_t sector=0; sector < PROXIMITY_NUM_SECTORS; sector++) {
             if (_distances[layer][sector] < FLT_MAX) {
                 AP_Proximity_Boundary_3D::Face face{layer, sector};
-                boundary.set_face_attributes(face, _pitch[layer][sector], _angle[layer][sector], _distances[layer][sector]);
+                boundary.set_face_attributes(face, _pitch[layer][sector], _angle[layer][sector], _distances[layer][sector], prx_instance);
             }
         }
     }

libraries/AP_Proximity/AP_Proximity_Boundary_3D.h

@@ -79,8 +79,9 @@ public:
     // This method will also mark the sector and layer to be "valid",
     // This distance can then be used for Obstacle Avoidance
     // Assume detected obstacle is horizontal (zero pitch), if no pitch is passed
-    void set_face_attributes(const Face &face, float pitch, float yaw, float distance);
-    void set_face_attributes(const Face &face, float yaw, float distance) { set_face_attributes(face, 0, yaw, distance); }
+    // prx_instance should be set to the proximity sensor backend instance number
+    void set_face_attributes(const Face &face, float pitch, float yaw, float distance, uint8_t prx_instance);
+    void set_face_attributes(const Face &face, float yaw, float distance, uint8_t prx_instance) { set_face_attributes(face, 0, yaw, distance, prx_instance); }
 
     // update boundary points used for simple avoidance based on a single sector and pitch distance changing
     //   the boundary points lie on the line between sectors meaning two boundary points may be updated based on a single sector's distance changing
@@ -92,7 +93,8 @@ public:
 
     // Reset this location, specified by Face object, back to default
     // i.e Distance is marked as not-valid
-    void reset_face(const Face &face);
+    // prx_instance should be set to the proximity sensor's backend instance number
+    void reset_face(const Face &face, uint8_t prx_instance);
 
     // check if a face has valid distance even if it was updated a long time back
     void check_face_timeout();
@@ -170,6 +172,7 @@ private:
     float _distance[PROXIMITY_NUM_LAYERS][PROXIMITY_NUM_SECTORS];       // distance to closest object within each sector and layer
     bool _distance_valid[PROXIMITY_NUM_LAYERS][PROXIMITY_NUM_SECTORS];  // true if a valid distance received for each sector and layer
     uint32_t _last_update_ms[PROXIMITY_NUM_LAYERS][PROXIMITY_NUM_SECTORS]; // time when distance was last updated
+    uint8_t _prx_instance[PROXIMITY_NUM_LAYERS][PROXIMITY_NUM_SECTORS]; // proximity sensor backend instance that provided the distance
     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
@@ -193,7 +196,8 @@ public:
     void add_distance(const AP_Proximity_Boundary_3D::Face &face, float yaw, float distance) { add_distance(face, 0.0f, yaw, distance); }
 
     // fill the original 3D boundary with the contents of this temporary boundary
-    void update_3D_boundary(AP_Proximity_Boundary_3D &boundary);
+    // prx_instance should be set to the proximity sensor's backend instance number
+    void update_3D_boundary(uint8_t prx_instance, AP_Proximity_Boundary_3D &boundary);
 
 private:
 

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(frontend.boundary);
+        _temp_boundary.update_3D_boundary(state.instance, frontend.boundary);
         reset();
         return true;
     }

libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp

@@ -326,10 +326,10 @@ void AP_Proximity_LightWareSF40C::process_message()
             if (face != _face) {
                 // update boundary used for avoidance
                 if (_face_distance_valid) {
-                    frontend.boundary.set_face_attributes(_face, _face_yaw_deg, _face_distance);
+                    frontend.boundary.set_face_attributes(_face, _face_yaw_deg, _face_distance, state.instance);
                 } else {
                     // mark previous face invalid
-                    frontend.boundary.reset_face(_face);
+                    frontend.boundary.reset_face(_face, state.instance);
                 }
                 // init for new face
                 _face = face;

libraries/AP_Proximity/AP_Proximity_LightWareSF45B.cpp

@@ -144,10 +144,10 @@ void AP_Proximity_LightWareSF45B::process_message()
         const AP_Proximity_Boundary_3D::Face face = frontend.boundary.get_face(angle_deg);
         if (face != _face) {
             if (_face_distance_valid) {
-                frontend.boundary.set_face_attributes(_face, _face_yaw_deg, _face_distance);
+                frontend.boundary.set_face_attributes(_face, _face_yaw_deg, _face_distance, state.instance);
             } else {
                 // mark previous face invalid
-                frontend.boundary.reset_face(_face);
+                frontend.boundary.reset_face(_face, state.instance);
             }
             // 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(frontend.boundary);
+            temp_boundary.update_3D_boundary(state.instance, frontend.boundary);
             // clear temp boundary for new data
             temp_boundary.reset();
         }
@@ -178,9 +178,9 @@ void AP_Proximity_MAV::handle_obstacle_distance_msg(const mavlink_message_t &msg
         if (latest_face != face) {
             // store previous face
             if (face_distance_valid) {
-                frontend.boundary.set_face_attributes(face, face_yaw_deg, face_distance);
+                frontend.boundary.set_face_attributes(face, face_yaw_deg, face_distance, state.instance);
             } else {
-                frontend.boundary.reset_face(face);
+                frontend.boundary.reset_face(face, state.instance);
             }
             // 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) {
-        frontend.boundary.set_face_attributes(face, face_yaw_deg, face_distance);
+        frontend.boundary.set_face_attributes(face, face_yaw_deg, face_distance, state.instance);
     } else {
-        frontend.boundary.reset_face(face);
+        frontend.boundary.reset_face(face, state.instance);
     }
     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(frontend.boundary);
+        temp_boundary.update_3D_boundary(state.instance, frontend.boundary);
         // clear temp boundary for new data
         temp_boundary.reset();
     }

libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp

@@ -319,10 +319,10 @@ void AP_Proximity_RPLidarA2::parse_response_data()
                     if (face != _last_face) {
                         // distance is for a new face, the previous one can be updated now
                         if (_last_distance_valid) {
-                            frontend.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, state.instance);
                         } else {
                             // reset distance from last face
-                            frontend.boundary.reset_face(face);
+                            frontend.boundary.reset_face(face, state.instance);
                         }
 
                         // initialize the new face

libraries/AP_Proximity/AP_Proximity_RangeFinder.cpp

@@ -51,11 +51,11 @@ void AP_Proximity_RangeFinder::update(void)
                 _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)) {
-                    frontend.boundary.set_face_attributes(face, angle, distance);
+                    frontend.boundary.set_face_attributes(face, angle, distance, state.instance);
                     // update OA database
                     database_push(angle, distance);
                 } else {
-                    frontend.boundary.reset_face(face);
+                    frontend.boundary.reset_face(face, state.instance);
                 }
                 _last_update_ms = now;
             }

libraries/AP_Proximity/AP_Proximity_SITL.cpp

@@ -64,11 +64,11 @@ void AP_Proximity_SITL::update(void)
             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)) {
-                frontend.boundary.set_face_attributes(face, yaw_angle_deg, fence_distance);
+                frontend.boundary.set_face_attributes(face, yaw_angle_deg, fence_distance, state.instance);
                 // update OA database
                 database_push(yaw_angle_deg, fence_distance);
             } else {
-                frontend.boundary.reset_face(face);
+                frontend.boundary.reset_face(face, state.instance);
             }
         }
     } else {

libraries/AP_Proximity/AP_Proximity_TeraRangerTower.cpp

@@ -97,11 +97,11 @@ void AP_Proximity_TeraRangerTower::update_sector_data(int16_t angle_deg, uint16_
     // Get location on 3-D boundary based on angle to the object
     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)) {
-        frontend.boundary.set_face_attributes(face, angle_deg, ((float) distance_mm) / 1000);
+        frontend.boundary.set_face_attributes(face, angle_deg, ((float) distance_mm) / 1000, state.instance);
         // update OA database
         database_push(angle_deg, ((float) distance_mm) / 1000);
     } else {
-        frontend.boundary.reset_face(face);
+        frontend.boundary.reset_face(face, state.instance);
     }
     _last_distance_received_ms = AP_HAL::millis();
 }

libraries/AP_Proximity/AP_Proximity_TeraRangerTowerEvo.cpp

@@ -152,11 +152,11 @@ void AP_Proximity_TeraRangerTowerEvo::update_sector_data(int16_t angle_deg, uint
     //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)) {
-        frontend.boundary.set_face_attributes(face, angle_deg, ((float) distance_mm) / 1000);
+        frontend.boundary.set_face_attributes(face, angle_deg, ((float) distance_mm) / 1000, state.instance);
         // update OA database
         database_push(angle_deg, ((float) distance_mm) / 1000);
     } else {
-        frontend.boundary.reset_face(face);
+        frontend.boundary.reset_face(face, state.instance);
     }
     _last_distance_received_ms = AP_HAL::millis();
 }