From 6ceee23b8537af71d57130015cb617ec005729d6 Mon Sep 17 00:00:00 2001 From: Rishabh Date: Wed, 29 Dec 2021 16:46:34 +0530 Subject: [PATCH] AP_Proximity: Add parameter to allow manually setting range to sensors --- libraries/AP_Proximity/AP_Proximity.cpp | 16 ++++++ libraries/AP_Proximity/AP_Proximity.h | 2 + .../AP_Proximity/AP_Proximity_Backend.cpp | 56 +++++++++++-------- libraries/AP_Proximity/AP_Proximity_Backend.h | 17 +++--- libraries/AP_Proximity/AP_Proximity_MAV.cpp | 13 +++-- .../AP_Proximity/AP_Proximity_RangeFinder.cpp | 2 +- libraries/AP_Proximity/AP_Proximity_SITL.cpp | 2 +- .../AP_Proximity_TeraRangerTower.cpp | 4 +- .../AP_Proximity_TeraRangerTowerEvo.cpp | 2 +- 9 files changed, 71 insertions(+), 43 deletions(-) diff --git a/libraries/AP_Proximity/AP_Proximity.cpp b/libraries/AP_Proximity/AP_Proximity.cpp index c83f3263f9..70ba030652 100644 --- a/libraries/AP_Proximity/AP_Proximity.cpp +++ b/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 }; diff --git a/libraries/AP_Proximity/AP_Proximity.h b/libraries/AP_Proximity/AP_Proximity.h index d1531acf87..43db39d293 100644 --- a/libraries/AP_Proximity/AP_Proximity.h +++ b/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); }; diff --git a/libraries/AP_Proximity/AP_Proximity_Backend.cpp b/libraries/AP_Proximity/AP_Proximity_Backend.cpp index 4be559b3a6..55478aef1c 100644 --- a/libraries/AP_Proximity/AP_Proximity_Backend.cpp +++ b/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 -bool AP_Proximity_Backend::ignore_reading(uint16_t angle_deg, float distance_m) const +// 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) +// 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 - for (uint8_t i=0; i < PROXIMITY_MAX_IGNORE; i++) { - if (frontend._ignore_width_deg[i] != 0) { - if (abs(angle_deg - frontend._ignore_angle_deg[i]) <= (frontend._ignore_width_deg[i]/2)) { - return true; + // check if distances are supposed to be in a particular range + if (!is_zero(frontend._max_m)) { + if (distance_m > frontend._max_m) { + // too far away + 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; } diff --git a/libraries/AP_Proximity/AP_Proximity_Backend.h b/libraries/AP_Proximity/AP_Proximity_Backend.h index 470b3a799a..25603925df 100644 --- a/libraries/AP_Proximity/AP_Proximity_Backend.h +++ b/libraries/AP_Proximity/AP_Proximity_Backend.h @@ -89,19 +89,20 @@ 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 - // angles should be in degrees and in the range of 0 to 360 - bool ignore_reading(uint16_t angle_deg, float distance_m) const; + + // 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) + // 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 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 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; + bool check_obstacle_near_ground(float pitch, float yaw, float distance) const; // database helpers. All angles are in degrees static bool database_prepare_for_push(Vector3f ¤t_pos, Matrix3f &body_to_ned); diff --git a/libraries/AP_Proximity/AP_Proximity_MAV.cpp b/libraries/AP_Proximity/AP_Proximity_MAV.cpp index babe855f06..13375fbeae 100644 --- a/libraries/AP_Proximity/AP_Proximity_MAV.cpp +++ b/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()); diff --git a/libraries/AP_Proximity/AP_Proximity_RangeFinder.cpp b/libraries/AP_Proximity/AP_Proximity_RangeFinder.cpp index 4d3a4d335e..b7867913e8 100644 --- a/libraries/AP_Proximity/AP_Proximity_RangeFinder.cpp +++ b/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); diff --git a/libraries/AP_Proximity/AP_Proximity_SITL.cpp b/libraries/AP_Proximity/AP_Proximity_SITL.cpp index 07e77f45c9..730a8cb970 100644 --- a/libraries/AP_Proximity/AP_Proximity_SITL.cpp +++ b/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; } diff --git a/libraries/AP_Proximity/AP_Proximity_TeraRangerTower.cpp b/libraries/AP_Proximity/AP_Proximity_TeraRangerTower.cpp index aad39b6cca..ab82f90d98 100644 --- a/libraries/AP_Proximity/AP_Proximity_TeraRangerTower.cpp +++ b/libraries/AP_Proximity/AP_Proximity_TeraRangerTower.cpp @@ -93,10 +93,10 @@ bool AP_Proximity_TeraRangerTower::read_sensor_data() // process reply void AP_Proximity_TeraRangerTower::update_sector_data(int16_t angle_deg, uint16_t distance_cm) -{ +{ // 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); diff --git a/libraries/AP_Proximity/AP_Proximity_TeraRangerTowerEvo.cpp b/libraries/AP_Proximity/AP_Proximity_TeraRangerTowerEvo.cpp index c9fac09587..29a911b761 100644 --- a/libraries/AP_Proximity/AP_Proximity_TeraRangerTowerEvo.cpp +++ b/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);