AP_Proximity: add temp boundary class

libraries/AP_Proximity/AP_Proximity_AirSimSITL.cpp

@@ -46,6 +46,8 @@ void AP_Proximity_AirSimSITL::update(void)
     const float accuracy_sq = sq(PROXIMITY_ACCURACY);
     bool prev_pos_valid = false;
     Vector2f prev_pos;
+    // clear temp boundary since we have a new message
+    temp_boundary.reset();
 
     for (uint16_t i=0; i<points.length; i++) {
         Vector3f &point = points.data[i];
@@ -60,7 +62,9 @@ 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)));
-            boundary.add_distance(yaw_angle_deg, safe_sqrt(distance_sq));
+            const AP_Proximity_Boundary_3D::Face face = 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));
 
             // check distance from previous point to reduce amount of data sent to object database
             if (!prev_pos_valid || ((new_pos - prev_pos).length_squared() >= accuracy_sq)) {
@@ -72,9 +76,8 @@ void AP_Proximity_AirSimSITL::update(void)
             }
         }
     }
-
-    // update middle boundary
-    boundary.update_middle_boundary();
+    // copy temp boundary to real boundary
+    temp_boundary.update_3D_boundary(boundary);
 }
 
 // get maximum and minimum distances (in meters) of primary sensor

libraries/AP_Proximity/AP_Proximity_AirSimSITL.h

@@ -41,6 +41,7 @@ public:
 
 private:
     SITL::SITL *sitl = AP::sitl();
+    AP_Proximity_Temp_Boundary temp_boundary;
 
 };
 #endif // CONFIG_HAL_BOARD

libraries/AP_Proximity/AP_Proximity_Boundary_3D.cpp

@@ -73,24 +73,6 @@ void AP_Proximity_Boundary_3D::set_face_attributes(const Face &face, float pitch
     update_boundary(face);
 }
 
-// add a distance to the boundary if it is shorter than any other provided distance since the last time the boundary was reset
-// pitch and yaw are in degrees, distance is in meters
-void AP_Proximity_Boundary_3D::add_distance(float pitch, float yaw, float distance)
-{
-    Face face = get_face(pitch, yaw);
-    if (!face.valid()) {
-        return;
-    }
-    
-    if (!_distance_valid[face.layer][face.sector] || (distance < _distance[face.layer][face.sector])) {
-        _distance[face.layer][face.sector] = distance;
-        _distance_valid[face.layer][face.sector] = true;
-        _angle[face.layer][face.sector] = yaw;
-        _pitch[face.layer][face.sector] = pitch;
-        set_filtered_distance(face, distance);
-    }
-}
-
 // apply a new cutoff_freq to low-pass filter
 void AP_Proximity_Boundary_3D::apply_filter_freq(float cutoff_freq)
 {
@@ -177,14 +159,6 @@ void AP_Proximity_Boundary_3D::update_boundary(const Face &face)
     }
 }
 
-// update middle layer boundary points
-void AP_Proximity_Boundary_3D::update_middle_boundary()
-{
-    for (uint8_t sector=0; sector < PROXIMITY_NUM_SECTORS; sector++) {
-        update_boundary(Face{PROXIMITY_MIDDLE_LAYER, sector});
-    }
-}
-
 // reset boundary.  marks all distances as invalid
 void AP_Proximity_Boundary_3D::reset()
 {
@@ -404,4 +378,38 @@ bool AP_Proximity_Boundary_3D::get_layer_distances(uint8_t layer_number, float d
     return valid_distances;
 }
 
+// reset the temporary boundary. This fills in distances with FLT_MAX
+void AP_Proximity_Temp_Boundary::reset()
+{
+    for (uint8_t layer=0; layer < PROXIMITY_NUM_LAYERS; layer++) {
+        for (uint8_t sector=0; sector < PROXIMITY_NUM_SECTORS; sector++) {
+            _distances[layer][sector] = FLT_MAX;
+        }
+    }
+}
+
+// add a distance to the temp boundary if it is shorter than any other provided distance since the last time the boundary was reset
+// pitch and yaw are in degrees, distance is in meters
+void AP_Proximity_Temp_Boundary::add_distance(const AP_Proximity_Boundary_3D::Face &face, float pitch, float yaw, float distance)
+{
+    if (face.valid() && distance < _distances[face.layer][face.sector]) {
+        _distances[face.layer][face.sector] = distance;
+        _angle[face.layer][face.sector] = yaw;
+        _pitch[face.layer][face.sector] = pitch;
+    }
+}
+
+// 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)
+{
+    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]);
+            }
+        }
+    }
+}
+
 #endif // HAL_PROXIMITY_ENABLED

libraries/AP_Proximity/AP_Proximity_Boundary_3D.h

@@ -71,19 +71,11 @@ public:
     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); }
 
-    // add a distance to the boundary if it is shorter than any other provided distance since the last time the boundary was reset
-    // pitch and yaw are in degrees, distance is in meters
-    void add_distance(float pitch, float yaw, float distance);
-    void add_distance(float yaw, float distance) { add_distance(0, yaw, distance); }
-
     // 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
     //   the boundary point is set to the shortest distance found in the two adjacent sectors, this is a conservative boundary around the vehicle
     void update_boundary(const Face &face);
 
-    // update middle layer boundary points
-    void update_middle_boundary();
-
     // reset boundary.  marks all distances as invalid
     void reset();
 
@@ -171,4 +163,31 @@ private:
     float _filter_freq;                                                 // cutoff freq of low pass filter
 };
 
+// This class gives an easy way of making a temporary boundary, used for "sorting" distances.
+// When unkown number of distances at various orientations are sent we store the least distance in the temporary boundary.
+// After all the messages are received, we copy the contents of the temporary boundary and put it in the main 3-D boundary.
+class AP_Proximity_Temp_Boundary
+{
+public:
+    // constructor. This incorporates initialisation as well.
+	AP_Proximity_Temp_Boundary() { reset(); }
+
+    // reset the temporary boundary. This fills in distances with FLT_MAX
+    void reset();
+
+    // add a distance to the temp boundary if it is shorter than any other provided distance since the last time the boundary was reset
+    // pitch and yaw are in degrees, distance is in meters
+    void add_distance(const AP_Proximity_Boundary_3D::Face &face, float pitch, float yaw, float distance);
+    void add_distance(const AP_Proximity_Boundary_3D::Face &face, float yaw, float distance) { add_distance(face, 0.0f, PID_TUNING_YAW, distance); }
+
+    // fill the original 3D boundary with the contents of this temporary boundary
+    void update_3D_boundary(AP_Proximity_Boundary_3D &boundary);
+
+private:
+
+    float _distances[PROXIMITY_NUM_LAYERS][PROXIMITY_NUM_SECTORS];      // distance to closest object within each sector and layer. Will start with FLT_MAX, and then be changed to a valid distance if needed
+    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_MAV.cpp

@@ -23,7 +23,7 @@
 extern const AP_HAL::HAL& hal;
 
 #define PROXIMITY_MAV_TIMEOUT_MS    500 // distance messages must arrive within this many milliseconds
-#define PROXIMITY_3D_MSG_TIMEOUT_MS  50  // boundary will be reset if OBSTACLE_DISTANCE_3D message does not arrive within this many milliseconds
+#define PROXIMITY_TIMESTAMP_MSG_TIMEOUT_MS  50  // boundary will be reset if mavlink message does not arrive within this many milliseconds
 
 // update the state of the sensor
 void AP_Proximity_MAV::update(void)
@@ -73,24 +73,39 @@ void AP_Proximity_MAV::handle_distance_sensor_msg(const mavlink_message_t &msg)
 
     // store distance to appropriate sector based on orientation field
     if (packet.orientation <= MAV_SENSOR_ROTATION_YAW_315) {
+        const uint32_t previous_sys_time = _last_update_ms;
+        _last_update_ms = AP_HAL::millis();
+
+        // time_diff will check if the new message arrived significantly later than the last message
+        const uint32_t time_diff = _last_update_ms - previous_sys_time;
+
+        const uint32_t previous_msg_timestamp = _last_msg_update_timestamp_ms;
+        _last_msg_update_timestamp_ms = packet.time_boot_ms;
+
+        // we will add on to the last fence if the time stamp is the same
+        // 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)) {
+            // cleared fence back to defaults since we have a new timestamp
+            boundary.reset();
+            // push data from temp boundary to the main 3-D proximity boundary
+            temp_boundary.update_3D_boundary(boundary);
+            // clear temp boundary for new data
+            temp_boundary.reset();
+        }
+        // store in meters
+        const float distance = packet.current_distance * 0.01f;
         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);
-        // store in meters
-        const float distance = packet.current_distance * 0.01f;
         _distance_min = packet.min_distance * 0.01f;
         _distance_max = packet.max_distance * 0.01f;
         const bool in_range = distance <= _distance_max && distance >= _distance_min;
         if (in_range && !check_obstacle_near_ground(yaw_angle_deg, distance)) {
-            boundary.set_face_attributes(face, yaw_angle_deg, distance);
+            temp_boundary.add_distance(face, yaw_angle_deg, distance);
             // update OA database
             database_push(yaw_angle_deg, distance);
-        } else {
-            // reset distance for this face
-            boundary.reset_face(face);
         }
-        _last_update_ms = AP_HAL::millis();
     }
 
     // store upward distance
@@ -208,8 +223,8 @@ void AP_Proximity_MAV::handle_obstacle_distance_3d_msg(const mavlink_message_t &
     // time_diff will check if the new message arrived significantly later than the last message
     const uint32_t time_diff = _last_update_ms - previous_sys_time;
 
-    const uint32_t previous_msg_timestamp = _last_3d_msg_update_ms;
-    _last_3d_msg_update_ms = packet.time_boot_ms;
+    const uint32_t previous_msg_timestamp = _last_msg_update_timestamp_ms;
+    _last_msg_update_timestamp_ms = packet.time_boot_ms;
 
     if (packet.frame != MAV_FRAME_BODY_FRD) {
         // we do not support this frame of reference yet
@@ -217,10 +232,14 @@ void AP_Proximity_MAV::handle_obstacle_distance_3d_msg(const mavlink_message_t &
     }
 
     // we will add on to the last fence if the time stamp is the same
-    // provided we got the new obstacle in less than PROXIMITY_3D_MSG_TIMEOUT_MS  
-    if ((previous_msg_timestamp != _last_3d_msg_update_ms) || (time_diff > PROXIMITY_3D_MSG_TIMEOUT_MS)) {
+    // 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)) {
         // cleared fence back to defaults since we have a new timestamp
         boundary.reset();
+        // push data from temp boundary to the main 3-D proximity boundary
+        temp_boundary.update_3D_boundary(boundary);
+        // clear temp boundary for new data
+        temp_boundary.reset();
     }
 
     _distance_min = packet.min_distance;
@@ -250,13 +269,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);
-    float face_distance;
-    if (boundary.get_distance(face, face_distance) && (face_distance < obstacle.length())) {
-        // we already have a shorter distance in this layer and sector
-        return;
-    }
-
-    boundary.set_face_attributes(face, yaw, pitch, obstacle.length());
+    temp_boundary.add_distance(face, pitch, yaw, obstacle.length());
 
     if (database_ready) {
         database_push(yaw, pitch, obstacle.length(),_last_update_ms, current_pos, body_to_ned);

libraries/AP_Proximity/AP_Proximity_MAV.h

@@ -33,9 +33,11 @@ private:
     // handle mavlink OBSTACLE_DISTANCE_3D messages
     void handle_obstacle_distance_3d_msg(const mavlink_message_t &msg);
 
+   AP_Proximity_Temp_Boundary temp_boundary;
+
     // horizontal distance support
     uint32_t _last_update_ms;   // system time of last mavlink message received
-    uint32_t _last_3d_msg_update_ms;   // last stored OBSTACLE_DISTANCE_3D message timestamp
+    uint32_t _last_msg_update_timestamp_ms;   // last stored mavlink message timestamp
     float _distance_max;        // max range of sensor in meters
     float _distance_min;        // min range of sensor in meters