Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Proximity: Filter out land detected by sensors

Browse Source
This commit is contained in:
Rishabh 2021-02-20 00:25:28 +05:30 committed by Randy Mackay
parent b7c937592e
commit e9c0e50185
13 changed files with 168 additions and 53 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
libraries/AP_Proximity/AP_Proximity.cpp
View File

@ -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: 2_TYPE
// @DisplayName: Second Proximity type
// @Description: What type of proximity sensor is connected
// @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
// @Param{Copter}: _IGN_GND
// @DisplayName: Proximity sensor land detection
// @Description: Ignore proximity data that is within 1 meter of the ground below the vehicle. This requires a downward facing rangefinder
// @Values: 0:Disabled, 1:Enabled
// @User: Standard
AP_GROUPINFO("2_ORIENT", 17, AP_Proximity, _orientation[1], 0),
// @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
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: _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 {

4
libraries/AP_Proximity/AP_Proximity.h
View File

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

91
libraries/AP_Proximity/AP_Proximity_Backend.cpp
View File

@ -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
bool AP_Proximity_Backend::ignore_reading(uint16_t angle_deg) const
// 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 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

23
libraries/AP_Proximity/AP_Proximity_Backend.h
View File

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

5
libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp
View File

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

2
libraries/AP_Proximity/AP_Proximity_LightWareSF40C_v09.cpp
View File

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

2
libraries/AP_Proximity/AP_Proximity_LightWareSF45B.cpp
View File

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

37
libraries/AP_Proximity/AP_Proximity_MAV.cpp
View File

@ -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_yaw_deg;
float face_distance = FLT_MAX;
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 ||
distance_cm == 65535 ||
distance_cm < packet.min_distance ||
distance_cm > packet.max_distance)
{
const float packet_distance_m = distance_cm * 0.01f;
const float mid_angle = wrap_360((float)j * increment + yaw_correction);
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);
if (obstacle.length() < _distance_min || obstacle.length() > _distance_max || obstacle.is_zero()) {
const Vector3f obstacle_FRD(packet.x, packet.y, packet.z);
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)));

2
libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp
View File

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

2
libraries/AP_Proximity/AP_Proximity_RangeFinder.cpp
View File

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

6
libraries/AP_Proximity/AP_Proximity_SITL.cpp
View File

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

2
libraries/AP_Proximity/AP_Proximity_TeraRangerTower.cpp
View File

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

4
libraries/AP_Proximity/AP_Proximity_TeraRangerTowerEvo.cpp
View File

@ -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;
if (valid) {
const bool valid = (distance_cm != 0xffff) && (distance_cm != 0x0000) && (distance_cm != 0x0001);
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);