aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Proximity: Add parameter to allow manually setting range to sensors

Browse Source
This commit is contained in:
Rishabh 2021-12-29 16:46:34 +05:30 committed by Randy Mackay
parent 3598724378
commit 6ceee23b85
9 changed files with 71 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_Proximity/AP_Proximity.cpp
View File

@ -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
};

2
libraries/AP_Proximity/AP_Proximity.h
View File

@ -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);
};

56
libraries/AP_Proximity/AP_Proximity_Backend.cpp
View File

@ -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;
}

17
libraries/AP_Proximity/AP_Proximity_Backend.h
View File

@ -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 &current_pos, Matrix3f &body_to_ned);

13
libraries/AP_Proximity/AP_Proximity_MAV.cpp
View File

@ -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());

2
libraries/AP_Proximity/AP_Proximity_RangeFinder.cpp
View File

@ -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);

2
libraries/AP_Proximity/AP_Proximity_SITL.cpp
View File

@ -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;
}

4
libraries/AP_Proximity/AP_Proximity_TeraRangerTower.cpp
View File

@ -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);

2
libraries/AP_Proximity/AP_Proximity_TeraRangerTowerEvo.cpp
View File

@ -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);