AP_Proximity: Filter out land detected by sensors

libraries/AP_Proximity/AP_Proximity.cpp

@@ -150,37 +150,19 @@ const AP_Param::GroupInfo AP_Proximity::var_info[] = {
     // @User: Standard
     AP_GROUPINFO("_IGN_WID6", 15, AP_Proximity, _ignore_width_deg[5], 0),
 
-#if PROXIMITY_MAX_INSTANCES > 1
+    // @Param{Copter}: _IGN_GND
-    // @Param: 2_TYPE
+    // @DisplayName: Proximity sensor land detection
-    // @DisplayName: Second Proximity type
+    // @Description: Ignore proximity data that is within 1 meter of the ground below the vehicle. This requires a downward facing rangefinder
-    // @Description: What type of proximity sensor is connected
+    // @Values: 0:Disabled, 1:Enabled
-    // @Values: 0:None,7:LightwareSF40c,1:LightWareSF40C-legacy,2:MAVLink,3:TeraRangerTower,4:RangeFinder,5:RPLidarA2,6:TeraRangerTowerEvo,8:LightwareSF45B,10:SITL,12:AirSimSITL
-    // @User: Advanced
-    // @RebootRequired: True
-    AP_GROUPINFO("2_TYPE", 16, AP_Proximity, _type[1], 0),
-
-    // @Param: 2_ORIENT
-    // @DisplayName: Second Proximity sensor orientation
-    // @Description: Second Proximity sensor orientation
-    // @Values: 0:Default,1:Upside Down
     // @User: Standard
-    AP_GROUPINFO("2_ORIENT", 17, AP_Proximity, _orientation[1], 0),
+    AP_GROUPINFO_FRAME("_IGN_GND", 16, AP_Proximity, _ign_gnd_enable, 1, AP_PARAM_FRAME_COPTER | AP_PARAM_FRAME_HELI | AP_PARAM_FRAME_TRICOPTER),
-
-    // @Param: 2_YAW_CORR
-    // @DisplayName: Second Proximity sensor yaw correction
-    // @Description: Second Proximity sensor yaw correction
-    // @Units: deg
-    // @Range: -180 180
-    // @User: Standard
-    AP_GROUPINFO("2_YAW_CORR", 18, AP_Proximity, _yaw_correction[1], 0),
-#endif
 
     // @Param: _LOG_RAW
     // @DisplayName: Proximity raw distances log
     // @Description: Set this parameter to one if logging unfiltered(raw) distances from sensor should be enabled
     // @Values: 0:Off, 1:On
     // @User: Advanced
-    AP_GROUPINFO("_LOG_RAW", 19, AP_Proximity, _raw_log_enable, 0),
+    AP_GROUPINFO("_LOG_RAW", 17, AP_Proximity, _raw_log_enable, 0),
 
     AP_GROUPEND
 };
@@ -499,6 +481,17 @@ 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)
+{
+    if (!valid_instance(primary_instance)) {
+        return;
+    }
+    // store alt at the backend
+    drivers[primary_instance]->set_rangefinder_alt(use, healthy, alt_cm);
+}
+
+
 AP_Proximity *AP_Proximity::_singleton;
 
 namespace AP {

libraries/AP_Proximity/AP_Proximity.h

@@ -155,6 +155,9 @@ public:
     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);
+
 private:
     static AP_Proximity *_singleton;
     Proximity_State state[PROXIMITY_MAX_INSTANCES];
@@ -176,6 +179,7 @@ private:
     AP_Int16 _ignore_angle_deg[PROXIMITY_MAX_IGNORE];   // angle (in degrees) of area that should be ignored by sensor (i.e. leg shows up)
     AP_Int8 _ignore_width_deg[PROXIMITY_MAX_IGNORE];    // width of beam (in degrees) that should be ignored
     AP_Int8 _raw_log_enable;                            // enable logging raw distances
+    AP_Int8 _ign_gnd_enable;                           // true if land detection should be enabled
 
     void detect_instance(uint8_t instance);
 };

libraries/AP_Proximity/AP_Proximity_Backend.cpp

@@ -20,6 +20,9 @@
 #include <AP_HAL/AP_HAL.h>
 #include "AP_Proximity.h"
 #include "AP_Proximity_Backend.h"
+#include <AP_HAL/AP_HAL.h>
+
+extern const AP_HAL::HAL& hal;
 
 /*
   base class constructor. 
@@ -59,8 +62,8 @@ float AP_Proximity_Backend::correct_angle_for_orientation(float angle_degrees) c
     return wrap_360(angle_degrees * angle_sign + frontend.get_yaw_correction(state.instance));
 }
 
-// check if a reading should be ignored because it falls into an ignore area
+// check if a reading should be ignored because it falls into an ignore area or if obstacle is near land
-bool AP_Proximity_Backend::ignore_reading(uint16_t angle_deg) const
+bool AP_Proximity_Backend::ignore_reading(uint16_t angle_deg, float distance_m) const
 {
     // check angle vs each ignore area
     for (uint8_t i=0; i < PROXIMITY_MAX_IGNORE; i++) {
@@ -70,6 +73,83 @@ bool AP_Proximity_Backend::ignore_reading(uint16_t angle_deg) const
             }
         }
     }
+
+   // check if obstacle is near land
+   return check_obstacle_near_ground(angle_deg, distance_m);
+}
+
+// store rangefinder values
+void AP_Proximity_Backend::set_rangefinder_alt(bool use, bool healthy, float alt_cm)
+{
+    _last_downward_update_ms = AP_HAL::millis();
+    _rangefinder_use = use;
+    _rangefinder_healthy = healthy;
+    _rangefinder_alt = alt_cm * 0.01f;
+}
+
+// get alt from rangefinder in meters
+bool AP_Proximity_Backend::get_rangefinder_alt(float &alt_m) const
+{
+    if (!_rangefinder_use || !_rangefinder_healthy) {
+        // range finder is not healthy
+        return false;
+    }
+
+    const uint32_t dt = AP_HAL::millis() - _last_downward_update_ms;
+    if (dt > PROXIMITY_ALT_DETECT_TIMEOUT_MS) {
+        return false;
+    }
+
+    // readings are healthy
+    alt_m = _rangefinder_alt;
+    return true;
+}
+
+// Check if Obstacle defined by body-frame yaw and pitch is near ground
+bool AP_Proximity_Backend::check_obstacle_near_ground(float yaw, float pitch, float distance) const
+{
+    if (!frontend._ign_gnd_enable) {
+        return false;
+    }
+    if (!hal.util->get_soft_armed()) {
+        // don't run this feature while vehicle is disarmed, otherwise proximity data will not show up on GCS
+        return false;
+    }
+    if ((pitch > 90.0f) || (pitch < -90.0f)) {
+        // sanity check on pitch
+        return false;
+    }
+    // Assume object is yaw and pitch bearing and distance meters away from the vehicle
+    Vector3f object_3D;
+    object_3D.offset_bearing(wrap_180(yaw), (pitch * -1.0f), distance);
+    const Matrix3f body_to_ned = AP::ahrs().get_rotation_body_to_ned();
+    const Vector3f rotated_object_3D = body_to_ned * object_3D;
+    return check_obstacle_near_ground(rotated_object_3D);
+}
+
+// Check if Obstacle defined by Vector3f is near ground. The vector is assumed to be body frame FRD
+bool AP_Proximity_Backend::check_obstacle_near_ground(const Vector3f &obstacle) const
+{
+    if (!frontend._ign_gnd_enable) {
+        return false;
+    }
+    if (!hal.util->get_soft_armed()) {
+        // don't run this feature while vehicle is disarmed, otherwise proximity data will not show up on GCS
+        return false;
+    }
+
+    float alt = FLT_MAX;
+    if (!get_rangefinder_alt(alt)) {
+        return false;
+    }
+
+    if (obstacle.z > -0.5f) {
+        // obstacle is at the most 0.5 meters above vehicle
+        if ((alt - PROXIMITY_GND_DETECT_THRESHOLD) < obstacle.z) {
+            // obstacle is near or below ground
+            return true;
+        }
+    }
     return false;
 }
 
@@ -109,10 +189,13 @@ void AP_Proximity_Backend::database_push(float angle, float pitch, float distanc
     if (oaDb == nullptr || !oaDb->healthy()) {
         return;
     }
-
+    if ((pitch > 90.0f) || (pitch < -90.0f)) {
+        // sanity check on pitch
+        return;
+    }
     //Assume object is angle and pitch bearing and distance meters away from the vehicle 
     Vector3f object_3D;
-    object_3D.offset_bearing(wrap_180(angle), wrap_180(pitch * -1.0f), distance);
+    object_3D.offset_bearing(wrap_180(angle), (pitch * -1.0f), distance);
     const Vector3f rotated_object_3D = body_to_ned * object_3D;
 
     //Calculate the position vector from origin

libraries/AP_Proximity/AP_Proximity_Backend.h

@@ -20,6 +20,9 @@
 #include <AP_Common/Location.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
+
 class AP_Proximity_Backend
 {
 public:
@@ -70,6 +73,9 @@ public:
     // 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);
+
 protected:
 
     // set status and update valid_count
@@ -80,7 +86,16 @@ protected:
     
     // check if a reading should be ignored because it falls into an ignore area
     // angles should be in degrees and in the range of 0 to 360
-    bool ignore_reading(uint16_t angle_deg) const;
+    bool ignore_reading(uint16_t angle_deg, float distance_m) const;
+
+    // get alt from rangefinder in meters. This reading is corrected for vehicle tilt
+    bool get_rangefinder_alt(float &alt_m) const;
+
+    // Check if Obstacle defined by body-frame yaw and pitch is near ground
+    bool check_obstacle_near_ground(float yaw, float pitch, float distance) const;
+    bool check_obstacle_near_ground(float yaw, float distance) const { return check_obstacle_near_ground(yaw, 0.0f, distance); };
+    // Check if Obstacle defined by Vector3f is near ground. The vector is assumed to be body frame FRD
+    bool check_obstacle_near_ground(const Vector3f &obstacle) const;
 
     // database helpers. All angles are in degrees
     static bool database_prepare_for_push(Vector3f &current_pos, Matrix3f &body_to_ned);
@@ -91,6 +106,12 @@ protected:
     };
     static void database_push(float angle, float pitch, float distance, uint32_t timestamp_ms, const Vector3f &current_pos, const Matrix3f &body_to_ned);
 
+    // used for ground detection
+    uint32_t _last_downward_update_ms;
+    bool     _rangefinder_use;
+    bool     _rangefinder_healthy;
+    float    _rangefinder_alt;
+
     AP_Proximity &frontend;
     AP_Proximity::Proximity_State &state;   // reference to this instances state
 

libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp

@@ -335,11 +335,10 @@ void AP_Proximity_LightWareSF40C::process_message()
             }
 
             // check reading is not within an ignore zone
-            if (!ignore_reading(angle_deg)) {
+            const float dist_m = dist_cm * 0.01f;
+            if (!ignore_reading(angle_deg, dist_m)) {
                 // check distance reading is valid
                 if ((dist_cm >= dist_min_cm) && (dist_cm <= dist_max_cm)) {
-                    const float dist_m = dist_cm * 0.01f;
-
                     // update shortest distance for this face
                     if (!_face_distance_valid || dist_m < _face_distance) {
                         _face_distance = dist_m;

libraries/AP_Proximity/AP_Proximity_LightWareSF40C_v09.cpp

@@ -323,7 +323,7 @@ bool AP_Proximity_LightWareSF40C_v09::process_reply()
         {
             float angle_deg = strtof(element_buf[0], NULL);
             float distance_m = strtof(element_buf[1], NULL);
-            if (!ignore_reading(angle_deg)) {
+            if (!ignore_reading(angle_deg, distance_m)) {
                 _last_distance_received_ms = AP_HAL::millis();
                 success = true;
                 // Get location on 3-D boundary based on angle to the object

libraries/AP_Proximity/AP_Proximity_LightWareSF45B.cpp

@@ -168,7 +168,7 @@ void AP_Proximity_LightWareSF45B::process_message()
         }
 
         // check reading is valid
-        if (!ignore_reading(angle_deg) && (distance_m >= distance_min()) && (distance_m <= distance_max())) {
+        if (!ignore_reading(angle_deg, distance_m) && (distance_m >= distance_min()) && (distance_m <= distance_max())) {
             // update shortest distance for this face
             if (!_face_distance_valid || (distance_m < _face_distance)) {
                 _face_yaw_deg = angle_deg;

libraries/AP_Proximity/AP_Proximity_MAV.cpp

@@ -79,7 +79,8 @@ void AP_Proximity_MAV::handle_distance_sensor_msg(const mavlink_message_t &msg)
         const float distance = packet.current_distance * 0.01f;
         _distance_min = packet.min_distance * 0.01f;
         _distance_max = packet.max_distance * 0.01f;
-        if (distance <= _distance_max && distance >= _distance_min) {
+        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);
             // update OA database
             database_push(yaw_angle_deg, distance);
@@ -137,25 +138,26 @@ void AP_Proximity_MAV::handle_obstacle_distance_msg(const mavlink_message_t &msg
 
     // variables to calculate closest angle and distance for each face
     AP_Proximity_Boundary_3D::Face face;
-    float face_distance;
+    float face_distance = FLT_MAX;
-    float face_yaw_deg;
+    float face_yaw_deg = 0.0f;
     bool face_distance_valid = false;
 
+    // reset this  boundary to fill with new data
+    boundary.reset();
+
     // iterate over message's sectors
     for (uint8_t j = 0; j < total_distances; j++) {
         const uint16_t distance_cm = packet.distances[j];
-        if (distance_cm == 0 ||
+        const float packet_distance_m = distance_cm * 0.01f;
-            distance_cm == 65535 ||
+        const float mid_angle = wrap_360((float)j * increment + yaw_correction);
-            distance_cm < packet.min_distance ||
+
-            distance_cm > packet.max_distance)
+        const bool range_check = distance_cm == 0 || distance_cm == 65535 || distance_cm < packet.min_distance ||
-        {
+                                 distance_cm > packet.max_distance;
+        if (range_check || check_obstacle_near_ground(mid_angle, packet_distance_m)) {
             // sanity check failed, ignore this distance value
             continue;
         }
 
-        const float packet_distance_m = distance_cm * 0.01f;
-        const float mid_angle = wrap_360((float)j * increment + yaw_correction);
-
         // get face for this latest reading
         AP_Proximity_Boundary_3D::Face latest_face = boundary.get_face(mid_angle);
         if (latest_face != face) {
@@ -226,11 +228,20 @@ void AP_Proximity_MAV::handle_obstacle_distance_3d_msg(const mavlink_message_t &
     Matrix3f body_to_ned;
     const bool database_ready = database_prepare_for_push(current_pos, body_to_ned);
 
-    const Vector3f obstacle(packet.x, packet.y, packet.z * -1.0f);
+    const Vector3f obstacle_FRD(packet.x, packet.y, packet.z);
-    if (obstacle.length() < _distance_min || obstacle.length() > _distance_max || obstacle.is_zero()) {
+    const float obstacle_distance = obstacle_FRD.length();
+    if (obstacle_distance < _distance_min || obstacle_distance > _distance_max || is_zero(obstacle_distance)) {
         // message isn't healthy
         return;
     }
+    if (check_obstacle_near_ground(obstacle_FRD)) {
+        // obstacle is probably near ground
+        return;
+    }
+
+    // convert to FRU
+    const Vector3f obstacle(obstacle_FRD.x, obstacle_FRD.y, obstacle_FRD.z * -1.0f);
+
     // extract yaw and pitch from Obstacle Vector
     const float yaw = wrap_360(degrees(atan2f(obstacle.y, obstacle.x)));
     const float pitch = wrap_180(degrees(M_PI_2 - atan2f(norm(obstacle.x, obstacle.y), obstacle.z))); 

libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp

@@ -311,7 +311,7 @@ void AP_Proximity_RPLidarA2::parse_response_data()
                 Debug(2, "                                       D%02.2f A%03.1f Q%02d", distance_m, angle_deg, quality);
 #endif
                 _last_distance_received_ms = AP_HAL::millis();
-                if (!ignore_reading(angle_deg)) {
+                if (!ignore_reading(angle_deg, distance_m)) {
                     const AP_Proximity_Boundary_3D::Face face = boundary.get_face(angle_deg);
 
                     if (face != _last_face) {

libraries/AP_Proximity/AP_Proximity_RangeFinder.cpp

@@ -48,7 +48,7 @@ void AP_Proximity_RangeFinder::update(void)
                 const float distance_m = sensor->distance_cm() * 0.01f;
                 _distance_min = sensor->min_distance_cm() * 0.01f;
                 _distance_max = sensor->max_distance_cm() * 0.01f;
-                if ((distance_m <= _distance_max) && (distance_m >= _distance_min)) {
+                if ((distance_m <= _distance_max) && (distance_m >= _distance_min) && !check_obstacle_near_ground(angle, distance_m)) {
                     boundary.set_face_attributes(face, angle, distance_m);
                     // update OA database
                     database_push(angle, distance_m);

libraries/AP_Proximity/AP_Proximity_SITL.cpp

@@ -52,13 +52,13 @@ void AP_Proximity_SITL::update(void)
         // only called to prompt polyfence to reload fence if required
     }
     if (AP::fence()->polyfence().inclusion_boundary_available()) {
+        set_status(AP_Proximity::Status::Good);
         // 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);
             float fence_distance;
             if (get_distance_to_fence(yaw_angle_deg, fence_distance)) {
-                set_status(AP_Proximity::Status::Good);
                 boundary.set_face_attributes(face, yaw_angle_deg, fence_distance);
                 // update OA database
                 database_push(yaw_angle_deg, fence_distance);
@@ -98,6 +98,10 @@ bool AP_Proximity_SITL::get_distance_to_fence(float angle_deg, float &distance)
         }
     }
     distance = min_dist;
+    if (check_obstacle_near_ground(angle_deg, distance)) {
+        // obstacle near land, lets ignore it
+        return false;
+    }
     return true;
 }
 

libraries/AP_Proximity/AP_Proximity_TeraRangerTower.cpp

@@ -94,7 +94,7 @@ 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 = boundary.get_face(angle_deg);
-    if (distance_cm != 0xffff) {
+    if ((distance_cm != 0xffff) && !check_obstacle_near_ground(angle_deg, distance_cm * 0.001f)) {
         boundary.set_face_attributes(face, angle_deg, ((float) distance_cm) / 1000);
         // update OA database
         database_push(angle_deg, ((float) distance_cm) / 1000);

libraries/AP_Proximity/AP_Proximity_TeraRangerTowerEvo.cpp

@@ -148,8 +148,8 @@ void AP_Proximity_TeraRangerTowerEvo::update_sector_data(int16_t angle_deg, uint
     // Get location on 3-D boundary based on angle to the object
     const AP_Proximity_Boundary_3D::Face face = boundary.get_face(angle_deg);
     //check for target too far, target too close and sensor not connected
-    const bool valid = distance_cm != 0xffff && distance_cm != 0x0000 && distance_cm != 0x0001;
+    const bool valid = (distance_cm != 0xffff) && (distance_cm != 0x0000) && (distance_cm != 0x0001);
-    if (valid) {
+    if (valid && !check_obstacle_near_ground(angle_deg, distance_cm * 0.001f)) {
         boundary.set_face_attributes(face, angle_deg, ((float) distance_cm) / 1000);
         // update OA database
         database_push(angle_deg, ((float) distance_cm) / 1000);