AP_Proximity: Add parameter to allow manually setting range to sensors

libraries/AP_Proximity/AP_Proximity.cpp

@@ -174,6 +174,22 @@ const AP_Param::GroupInfo AP_Proximity::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("_FILT", 18, AP_Proximity, _filt_freq, 0.25f),
 
+    // @Param: _MIN
+    // @DisplayName: Proximity minimum range
+    // @Description: Minimum expected range for Proximity Sensor. Setting this to 0 will set value to manufacturer reported range.
+    // @Units: m
+    // @Range: 0 500
+    // @User: Advanced
+    AP_GROUPINFO("_MIN", 19, AP_Proximity, _min_m, 0.0f),
+
+    // @Param: _MAX
+    // @DisplayName: Proximity maximum range
+    // @Description: Maximum expected range for Proximity Sensor. Setting this to 0 will set value to manufacturer reported range.
+    // @Units: m
+    // @Range: 0 500
+    // @User: Advanced
+    AP_GROUPINFO("_MAX", 20, AP_Proximity, _max_m, 0.0f),
+
     AP_GROUPEND
 };
 

libraries/AP_Proximity/AP_Proximity.h

@@ -191,6 +191,8 @@ private:
     AP_Int8 _raw_log_enable;                            // enable logging raw distances
     AP_Int8 _ign_gnd_enable;                           // true if land detection should be enabled
     AP_Float _filt_freq;                               // cutoff frequency for low pass filter
+    AP_Float _max_m;                                   // Proximity maximum range
+    AP_Float _min_m;                                   // Proximity minimum range
 
     void detect_instance(uint8_t instance);
 };

libraries/AP_Proximity/AP_Proximity_Backend.cpp

@@ -77,20 +77,41 @@ 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 or if obstacle is near land
+// check if a reading should be ignored because it falls into an ignore area (check_for_ign_area should be sent as false if this check is not needed)
-bool AP_Proximity_Backend::ignore_reading(uint16_t angle_deg, float distance_m) const
+// pitch is the vertical body-frame angle (in degrees) to the obstacle (0=directly ahead, 90 is above the vehicle)
+// yaw is the horizontal body-frame angle (in degrees) to the obstacle (0=directly ahead of the vehicle, 90 is to the right of the vehicle)
+// Also checks if obstacle is near land or out of range
+// angles should be in degrees and in the range of 0 to 360, distance should be in meteres
+bool AP_Proximity_Backend::ignore_reading(float pitch, float yaw, float distance_m, bool check_for_ign_area) const
 {
-    // check angle vs each ignore area
+    // check if distances are supposed to be in a particular range
-    for (uint8_t i=0; i < PROXIMITY_MAX_IGNORE; i++) {
+    if (!is_zero(frontend._max_m)) {
-        if (frontend._ignore_width_deg[i] != 0) {
+        if (distance_m > frontend._max_m) {
-            if (abs(angle_deg - frontend._ignore_angle_deg[i]) <= (frontend._ignore_width_deg[i]/2)) {
+            // too far away
-                return true;
+            return true;
+        }
+    }
+
+    if (!is_zero(frontend._min_m)) {
+        if (distance_m < frontend._min_m) {
+            // too close
+            return true;
+        }
+    }
+
+    if (check_for_ign_area) {
+        // check angle vs each ignore area
+        for (uint8_t i=0; i < PROXIMITY_MAX_IGNORE; i++) {
+            if (frontend._ignore_width_deg[i] != 0) {
+                if (abs(yaw - frontend._ignore_angle_deg[i]) <= (frontend._ignore_width_deg[i]/2)) {
+                    return true;
+                }
             }
         }
     }
 
    // check if obstacle is near land
-   return check_obstacle_near_ground(angle_deg, distance_m);
+   return check_obstacle_near_ground(pitch, yaw, distance_m);
 }
 
 // store rangefinder values
@@ -121,7 +142,7 @@ bool AP_Proximity_Backend::get_rangefinder_alt(float &alt_m) const
 }
 
 // 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
+bool AP_Proximity_Backend::check_obstacle_near_ground(float pitch, float yaw, float distance) const
 {
     if (!frontend._ign_gnd_enable) {
         return false;
@@ -139,28 +160,15 @@ bool AP_Proximity_Backend::check_obstacle_near_ground(float yaw, float pitch, fl
     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) {
+    if (rotated_object_3D.z > -0.5f) {
         // obstacle is at the most 0.5 meters above vehicle
-        if ((alt - PROXIMITY_GND_DETECT_THRESHOLD) < obstacle.z) {
+        if ((alt - PROXIMITY_GND_DETECT_THRESHOLD) < rotated_object_3D.z) {
             // obstacle is near or below ground
             return true;
         }

libraries/AP_Proximity/AP_Proximity_Backend.h

@@ -90,18 +90,19 @@ protected:
     // correct an angle (in degrees) based on the orientation and yaw correction parameters
     float correct_angle_for_orientation(float angle_degrees) const;
 
-    // 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 (check_for_ign_area should be sent as false if this check is not needed)
-    // angles should be in degrees and in the range of 0 to 360
+    // pitch is the vertical body-frame angle (in degrees) to the obstacle (0=directly ahead, 90 is above the vehicle)
-    bool ignore_reading(uint16_t angle_deg, float distance_m) const;
+    // yaw is the horizontal body-frame angle (in degrees) to the obstacle (0=directly ahead of the vehicle, 90 is to the right of the vehicle)
+    // Also checks if obstacle is near land or out of range
+    // angles should be in degrees and in the range of 0 to 360, distance should be in meteres
+    bool ignore_reading(float pitch, float yaw, float distance_m, bool check_for_ign_area = true) const;
+    bool ignore_reading(float yaw, float distance_m, bool check_for_ign_area = true) const { return ignore_reading(0.0f, yaw, distance_m, check_for_ign_area); }
 
     // 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 pitch, float yaw, 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);

libraries/AP_Proximity/AP_Proximity_MAV.cpp

@@ -99,7 +99,7 @@ void AP_Proximity_MAV::handle_distance_sensor_msg(const mavlink_message_t &msg)
         _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)) {
+        if (in_range && !ignore_reading(yaw_angle_deg, distance, false)) {
             temp_boundary.add_distance(face, yaw_angle_deg, distance);
             // update OA database
             database_push(yaw_angle_deg, distance);
@@ -168,7 +168,7 @@ void AP_Proximity_MAV::handle_obstacle_distance_msg(const mavlink_message_t &msg
 
         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)) {
+        if (range_check || ignore_reading(mid_angle, packet_distance_m, false)) {
             // sanity check failed, ignore this distance value
             continue;
         }
@@ -249,10 +249,6 @@ void AP_Proximity_MAV::handle_obstacle_distance_3d_msg(const mavlink_message_t &
         // 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);
@@ -261,6 +257,11 @@ void AP_Proximity_MAV::handle_obstacle_distance_3d_msg(const mavlink_message_t &
     const float yaw = wrap_360(degrees(atan2f(obstacle.y, obstacle.x)));
     const float pitch = wrap_180(degrees(M_PI_2 - atan2f(obstacle.xy().length(), obstacle.z))); 
 
+    if (ignore_reading(pitch, yaw, obstacle_distance, false)) {
+        // obstacle is probably near ground or out of range
+        return;
+    }
+
     // allot to correct layer and sector based on calculated pitch and yaw
     const AP_Proximity_Boundary_3D::Face face = boundary.get_face(pitch, yaw);
     temp_boundary.add_distance(face, pitch, yaw, obstacle.length());

libraries/AP_Proximity/AP_Proximity_RangeFinder.cpp

@@ -50,7 +50,7 @@ void AP_Proximity_RangeFinder::update(void)
                 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) && !check_obstacle_near_ground(angle, distance)) {
+                if ((distance <= _distance_max) && (distance >= _distance_min) && !ignore_reading(angle, distance, false)) {
                     boundary.set_face_attributes(face, angle, distance);
                     // update OA database
                     database_push(angle, distance);

libraries/AP_Proximity/AP_Proximity_SITL.cpp

@@ -101,7 +101,7 @@ bool AP_Proximity_SITL::get_distance_to_fence(float angle_deg, float &distance)
         }
     }
     distance = min_dist;
-    if (check_obstacle_near_ground(angle_deg, distance)) {
+    if (ignore_reading(angle_deg, distance, false)) {
         // obstacle near land, lets ignore it
         return false;
     }

libraries/AP_Proximity/AP_Proximity_TeraRangerTower.cpp

@@ -96,7 +96,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) && !check_obstacle_near_ground(angle_deg, distance_cm * 0.001f)) {
+    if ((distance_cm != 0xffff) && !ignore_reading(angle_deg, distance_cm * 0.001f, false)) {
         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

@@ -151,7 +151,7 @@ void AP_Proximity_TeraRangerTowerEvo::update_sector_data(int16_t angle_deg, uint
     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 > 0x0001);
-    if (valid && !check_obstacle_near_ground(angle_deg, distance_cm * 0.001f)) {
+    if (valid && !ignore_reading(angle_deg, distance_cm * 0.001f, false)) {
         boundary.set_face_attributes(face, angle_deg, ((float) distance_cm) / 1000);
         // update OA database
         database_push(angle_deg, ((float) distance_cm) / 1000);