Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-05 23:43:58 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Proximity: Convert Proximity Backend drivers to use 3D Boundary methods

Browse Source
This commit is contained in:
Rishabh 2020-12-06 17:53:55 +05:30 committed by Randy Mackay
parent 4fce715a9a
commit 36bba2e02c
12 changed files with 159 additions and 119 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
libraries/AP_Proximity/AP_Proximity.cpp
View File

@ -359,21 +359,32 @@ bool AP_Proximity::get_horizontal_distances(Proximity_Distance_Array &prx_dist_a
return drivers[primary_instance]->get_horizontal_distances(prx_dist_array);
}
// get boundary points around vehicle for use by avoidance
// returns nullptr and sets num_points to zero if no boundary can be returned
const Vector2f* AP_Proximity::get_boundary_points(uint8_t instance, uint16_t& num_points) const
{
if (!valid_instance(instance)) {
num_points = 0;
return nullptr;
// get total number of obstacles, used in GPS based Simple Avoidance
uint8_t AP_Proximity::get_obstacle_count() const
{
if (!valid_instance(primary_instance)) {
return 0;
}
// get boundary from backend
return drivers[instance]->get_boundary_points(num_points);
return drivers[primary_instance]->get_obstacle_count();
}
const Vector2f* AP_Proximity::get_boundary_points(uint16_t& num_points) const
// get vector to obstacle based on obstacle_num passed, used in GPS based Simple Avoidance
void AP_Proximity::get_obstacle(uint8_t obstacle_num, Vector3f& vec_to_obstacle) const
{
return get_boundary_points(primary_instance, num_points);
if (!valid_instance(primary_instance)) {
return;
}
return drivers[primary_instance]->get_obstacle(obstacle_num, vec_to_obstacle);
}
// returns shortest distance to "obstacle_num" obstacle, from a line segment formed between "seg_start" and "seg_end"
// used in GPS based Simple Avoidance
float AP_Proximity::distance_to_obstacle(uint8_t obstacle_num, const Vector3f& seg_start, const Vector3f& seg_end, Vector3f& closest_point) const
{
if (!valid_instance(primary_instance)) {
return FLT_MAX;
}
return drivers[primary_instance]->distance_to_obstacle(obstacle_num, seg_start, seg_end, closest_point);
}
// get distance and angle to closest object (used for pre-arm check)
@ -394,7 +405,7 @@ uint8_t AP_Proximity::get_object_count() const
return 0;
}
// get count from backend
return drivers[primary_instance]->get_object_count();
return drivers[primary_instance]->get_horizontal_object_count();
}
// get an object's angle and distance, used for non-GPS avoidance
@ -405,7 +416,7 @@ bool AP_Proximity::get_object_angle_and_distance(uint8_t object_number, float& a
return false;
}
// get angle and distance from backend
return drivers[primary_instance]->get_object_angle_and_distance(object_number, angle_deg, distance);
return drivers[primary_instance]->get_horizontal_object_angle_and_distance(object_number, angle_deg, distance);
}
// get maximum and minimum distances (in meters) of primary sensor

13
libraries/AP_Proximity/AP_Proximity.h
View File

@ -24,6 +24,7 @@
#define PROXIMITY_MAX_IGNORE 6 // up to six areas can be ignored
#define PROXIMITY_MAX_DIRECTION 8
#define PROXIMITY_SENSOR_ID_START 10
#define PROXIMITY_NUM_LAYERS 5
class AP_Proximity_Backend;
@ -88,10 +89,14 @@ public:
// get distances in PROXIMITY_MAX_DIRECTION directions. used for sending distances to ground station
bool get_horizontal_distances(Proximity_Distance_Array &prx_dist_array) const;
// get boundary points around vehicle for use by avoidance
// returns nullptr and sets num_points to zero if no boundary can be returned
const Vector2f* get_boundary_points(uint8_t instance, uint16_t& num_points) const;
const Vector2f* get_boundary_points(uint16_t& num_points) const;
// get total number of obstacles, used in GPS based Simple Avoidance
uint8_t get_obstacle_count() const;
// get vector to obstacle based on obstacle_num passed, used in GPS based Simple Avoidance
void get_obstacle(uint8_t obstacle_num, Vector3f& vec_to_obstacle) const;
// returns shortest distance to "obstacle_num" obstacle, from a line segment formed between "seg_start" and "seg_end"
float distance_to_obstacle(uint8_t obstacle_num, const Vector3f& seg_start, const Vector3f& seg_end, Vector3f& closest_point) const;
// get distance and angle to closest object (used for pre-arm check)
// returns true on success, false if no valid readings

33
libraries/AP_Proximity/AP_Proximity_AirSimSITL.cpp
View File

@ -33,8 +33,8 @@ void AP_Proximity_AirSimSITL::update(void)
}
set_status(AP_Proximity::Status::Good);
memset(_distance_valid, 0, sizeof(_distance_valid));
// reset all horizontal sectors to default, so that it can be filled with the fresh lidar data
boundary.reset_all_horizontal_sectors();
for (uint16_t i=0; i<points.length; i++) {
Vector3f &point = points.data[i];
@ -42,42 +42,37 @@ void AP_Proximity_AirSimSITL::update(void)
continue;
}
const float angle_deg = wrap_360(degrees(atan2f(point.y, point.x)));
const uint8_t sector = convert_angle_to_sector(angle_deg);
// Get location on 3-D boundary based on angle to the object
const boundary_location bnd_loc = boundary.get_sector(angle_deg);
const Vector2f v = Vector2f(point.x, point.y);
const float distance_m = v.length();
if (distance_m > distance_min()) {
if (_last_sector == sector) {
if (_last_sector == bnd_loc.sector) {
if (_distance_m_last > distance_m) {
_distance_m_last = distance_m;
_angle_deg_last = angle_deg;
}
} else {
// new sector started, previous one can be updated
_distance_valid[_last_sector] = true;
_angle[_last_sector] = _angle_deg_last;
_distance[_last_sector] = _distance_m_last;
// create a boundary location object
const boundary_location set_loc{_last_sector};
boundary.set_attributes(set_loc, _angle_deg_last, _distance_m_last);
// update boundary
update_boundary_for_sector(_last_sector, true);
boundary.update_boundary(set_loc);
// update OA database
database_push(_angle_deg_last, _distance_m_last);
// initialize new sector
_last_sector = sector;
_last_sector = bnd_loc.sector;
_distance_m_last = INT16_MAX;
_angle_deg_last = angle_deg;
}
} else {
_distance_valid[sector] = false;
// reset data back to defaults at this location
boundary.reset_sector(bnd_loc);
}
}
#if 0
printf("npoints=%u\n", points.length);
for (uint16_t i=0; i<PROXIMITY_NUM_SECTORS; i++) {
printf("sector[%u] ang=%.1f dist=%.1f\n", i, _angle[i], _distance[i]);
}
#endif
}
// get maximum and minimum distances (in meters) of primary sensor

21
libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp
View File

@ -48,9 +48,6 @@ void AP_Proximity_LightWareSF40C::update(void)
// initialise sensor
void AP_Proximity_LightWareSF40C::initialise()
{
// initialise boundary
init_boundary();
// exit immediately if we've sent initialisation requests in the last second
uint32_t now_ms = AP_HAL::millis();
if ((now_ms - _last_request_ms) < 1000) {
@ -321,18 +318,20 @@ void AP_Proximity_LightWareSF40C::process_message()
const uint16_t idx = 14 + (i * 2);
const int16_t dist_cm = (int16_t)buff_to_uint16(_msg.payload[idx], _msg.payload[idx+1]);
const float angle_deg = wrap_360((point_start_index + i) * angle_inc_deg * angle_sign + angle_correction);
const uint8_t sector = convert_angle_to_sector(angle_deg);
// Get location on 3-D boundary based on angle to the object
const boundary_location bnd_loc = boundary.get_sector(angle_deg);
const uint8_t sector = bnd_loc.sector;
// if we've entered a new sector then finish off previous sector
if (sector != _last_sector) {
// update boundary used for avoidance
if (_last_sector != UINT8_MAX) {
update_boundary_for_sector(_last_sector, false);
// create a location package
const boundary_location last_loc{_last_sector};
boundary.update_boundary(last_loc);
}
// init for new sector
_last_sector = sector;
_distance[sector] = INT16_MAX;
_distance_valid[sector] = false;
boundary.reset_sector(bnd_loc);
}
// check reading is not within an ignore zone
@ -342,10 +341,8 @@ void AP_Proximity_LightWareSF40C::process_message()
const float dist_m = dist_cm * 0.01f;
// update shortest distance for this sector
if (dist_m < _distance[sector]) {
_angle[sector] = angle_deg;
_distance[sector] = dist_m;
_distance_valid[sector] = true;
if (dist_m < boundary.get_distance(bnd_loc)) {
boundary.set_attributes(bnd_loc, angle_deg, dist_m);
}
// calculate shortest of last few readings

20
libraries/AP_Proximity/AP_Proximity_LightWareSF40C_v09.cpp
View File

@ -79,9 +79,6 @@ bool AP_Proximity_LightWareSF40C_v09::initialise()
return false;
}
// initialise sectors
init_boundary();
return true;
}
@ -205,7 +202,7 @@ bool AP_Proximity_LightWareSF40C_v09::send_request_for_distance()
char request_str[16];
snprintf(request_str, sizeof(request_str), "?TS,%u,%u\r\n",
(unsigned int)PROXIMITY_SECTOR_WIDTH_DEG,
_sector_middle_deg[_last_sector]);
boundary._sector_middle_deg[_last_sector]);
_uart->write(request_str);
@ -327,14 +324,19 @@ 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)) {
const uint8_t sector = convert_angle_to_sector(angle_deg);
_angle[sector] = angle_deg;
_distance[sector] = distance_m;
_distance_valid[sector] = is_positive(distance_m);
_last_distance_received_ms = AP_HAL::millis();
success = true;
// Get location on 3-D boundary based on angle to the object
const boundary_location bnd_loc = boundary.get_sector(angle_deg);
// reset this sector back to default, so that it can be filled with the fresh sensor data
boundary.reset_sector(bnd_loc);
if (is_positive(distance_m)) {
boundary.set_attributes(bnd_loc, angle_deg, distance_m);
// update OA database
database_push(angle_deg, distance_m);
}
// update boundary used for avoidance
update_boundary_for_sector(sector, true);
boundary.update_boundary(bnd_loc);
}
break;
}

22
libraries/AP_Proximity/AP_Proximity_LightWareSF45B.cpp
View File

@ -69,9 +69,6 @@ void AP_Proximity_LightWareSF45B::initialise()
// request stream rate and contents
request_stream_start();
// initialise boundary
init_boundary();
}
// request start of streaming of distances
@ -141,16 +138,21 @@ void AP_Proximity_LightWareSF45B::process_message()
const float angle_deg = correct_angle_for_orientation((int16_t)UINT16_VALUE(_msg.payload[3], _msg.payload[2]) * 0.01f);
// if distance is from a new sector then update distance, angle and boundary for previous sector
const uint8_t sector = convert_angle_to_sector(angle_deg);
if (sector != _sector) {
// Get location on 3-D boundary based on angle to the object
const boundary_location bnd_loc = boundary.get_sector(angle_deg);
if (bnd_loc.sector != _sector) {
if (_sector != UINT8_MAX) {
_angle[_sector] = _sector_angle;
_distance[_sector] = _sector_distance;
_distance_valid[_sector] = _sector_distance_valid;
update_boundary_for_sector(_sector, false);
// create a location packet
const boundary_location loc{_sector};
boundary.reset_sector(loc);
if (_sector_distance_valid) {
boundary.set_attributes(loc, _sector_angle, _sector_distance);
}
// update boundary used for Obstacle Avoidance
boundary.update_boundary(loc);
}
// init for new sector
_sector = sector;
_sector = bnd_loc.sector;
_sector_angle = 0;
_sector_distance = INT16_MAX;
_sector_distance_valid = false;

46
libraries/AP_Proximity/AP_Proximity_MAV.cpp
View File

@ -21,7 +21,7 @@
extern const AP_HAL::HAL& hal;
#define PROXIMITY_MAV_TIMEOUT_MS 500 // distance messages must arrive within this many milliseconds
#define PROXIMITY_3D_MSG_TIMEOUT_MS 50 // mini-fence will be cleared if OBSTACLE_DISTANCE_3D message does not arrive within this many milliseconds
// update the state of the sensor
void AP_Proximity_MAV::update(void)
{
@ -53,14 +53,25 @@ void AP_Proximity_MAV::handle_msg(const mavlink_message_t &msg)
// store distance to appropriate sector based on orientation field
if (packet.orientation <= MAV_SENSOR_ROTATION_YAW_315) {
uint8_t sector = packet.orientation;
_angle[sector] = sector * 45;
_distance[sector] = packet.current_distance * 0.01f;
const uint8_t sector = packet.orientation;
// create a boundary location object based on this sector
const boundary_location bnd_loc{sector};
// store in meters
const uint16_t distance = packet.current_distance * 0.01f;
_distance_min = packet.min_distance * 0.01f;
_distance_max = packet.max_distance * 0.01f;
_distance_valid[sector] = (_distance[sector] >= _distance_min) && (_distance[sector] <= _distance_max);
// reset data on this sector, to be filled with new data
boundary.reset_sector(bnd_loc);
if (distance <= _distance_max && distance >= _distance_max) {
boundary.set_attributes(bnd_loc, sector * 45, distance);
// update OA database
database_push(boundary.get_angle(bnd_loc), distance);
}
_last_update_ms = AP_HAL::millis();
update_boundary_for_sector(sector, true);
// update boundary used for Obstacle Avoidance
boundary.update_boundary(bnd_loc);
}
// store upward distance
@ -88,7 +99,6 @@ void AP_Proximity_MAV::handle_msg(const mavlink_message_t &msg)
return;
}
const float MAX_DISTANCE = 9999.0f;
const uint8_t total_distances = MIN(((360.0f / fabsf(increment)) + 0.5f), MAVLINK_MSG_OBSTACLE_DISTANCE_FIELD_DISTANCES_LEN); // usually 72
// set distance min and max
@ -109,11 +119,8 @@ void AP_Proximity_MAV::handle_msg(const mavlink_message_t &msg)
// initialise updated array and proximity sector angles (to closest object) and distances
bool sector_updated[PROXIMITY_NUM_SECTORS];
for (uint8_t i = 0; i < PROXIMITY_NUM_SECTORS; i++) {
sector_updated[i] = false;
_angle[i] = _sector_middle_deg[i];
_distance[i] = MAX_DISTANCE;
}
memset(sector_updated, 0, sizeof(sector_updated));
boundary.reset_all_horizontal_sectors();
// iterate over message's sectors
for (uint8_t j = 0; j < total_distances; j++) {
@ -133,7 +140,7 @@ void AP_Proximity_MAV::handle_msg(const mavlink_message_t &msg)
// iterate over proximity sectors
for (uint8_t i = 0; i < PROXIMITY_NUM_SECTORS; i++) {
// update distance array sector with shortest distance from message
const float angle_diff = wrap_180(_sector_middle_deg[i] - mid_angle);
const float angle_diff = wrap_180(boundary._sector_middle_deg[i] - mid_angle);
if (fabsf(angle_diff) > PROXIMITY_SECTOR_WIDTH_DEG*0.5f) {
// not even in this sector
continue;
@ -146,15 +153,16 @@ void AP_Proximity_MAV::handle_msg(const mavlink_message_t &msg)
// safe.
continue;
}
if (packet_distance_m >= _distance[i]) {
if (packet_distance_m >= boundary.get_distance(i)) {
// this is no closer than a previous distance
// found from the pakcet
// found from the packet
continue;
}
// this is the shortest distance we've found in the packet so far
_distance[i] = packet_distance_m;
_angle[i] = mid_angle;
// create a location packet
const boundary_location bnd_loc{i};
boundary.set_attributes(bnd_loc, mid_angle, packet_distance_m);
sector_updated[i] = true;
}
@ -166,9 +174,9 @@ void AP_Proximity_MAV::handle_msg(const mavlink_message_t &msg)
// update proximity sectors validity and boundary point
for (uint8_t i = 0; i < PROXIMITY_NUM_SECTORS; i++) {
_distance_valid[i] = (_distance[i] >= _distance_min) && (_distance[i] <= _distance_max);
if (sector_updated[i]) {
update_boundary_for_sector(i, false);
const boundary_location bnd_loc{i};
boundary.update_boundary(bnd_loc);
}
}
}

19
libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp
View File

@ -103,9 +103,6 @@ float AP_Proximity_RPLidarA2::distance_min() const
bool AP_Proximity_RPLidarA2::initialise()
{
// initialise boundary
init_boundary();
if (!_initialised) {
reset_rplidar(); // set to a known state
Debug(1, "LIDAR initialised");
@ -315,7 +312,8 @@ void AP_Proximity_RPLidarA2::parse_response_data()
#endif
_last_distance_received_ms = AP_HAL::millis();
if (!ignore_reading(angle_deg)) {
const uint8_t sector = convert_angle_to_sector(angle_deg);
const boundary_location bnd_loc = boundary.get_sector(angle_deg);
const uint8_t sector = bnd_loc.sector;
if (distance_m > distance_min()) {
if (_last_sector == sector) {
if (_distance_m_last > distance_m) {
@ -324,18 +322,21 @@ void AP_Proximity_RPLidarA2::parse_response_data()
}
} else {
// a new sector started, the previous one can be updated now
_angle[_last_sector] = _angle_deg_last;
_distance[_last_sector] = _distance_m_last;
_distance_valid[_last_sector] = true;
// create a location packet
const boundary_location loc{_last_sector};
boundary.set_attributes(loc, _angle_deg_last, _distance_m_last);
// update boundary used for avoidance
update_boundary_for_sector(_last_sector, true);
boundary.update_boundary(loc);
// update OA database
database_push(_angle_deg_last, _distance_m_last);
// initialize the new sector
_last_sector = sector;
_distance_m_last = distance_m;
_angle_deg_last = angle_deg;
}
} else {
_distance_valid[sector] = false;
boundary.reset_sector(bnd_loc);
}
}
} else {

19
libraries/AP_Proximity/AP_Proximity_RangeFinder.cpp
View File

@ -41,14 +41,23 @@ void AP_Proximity_RangeFinder::update(void)
if (sensor->has_data()) {
// check for horizontal range finders
if (sensor->orientation() <= ROTATION_YAW_315) {
uint8_t sector = (uint8_t)sensor->orientation();
_angle[sector] = sector * 45;
_distance[sector] = sensor->distance_cm() * 0.01f;
const uint8_t sector = (uint8_t)sensor->orientation();
const boundary_location bnd_loc{sector};
boundary.reset_sector(bnd_loc);
// distance in meters
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;
_distance_valid[sector] = (_distance[sector] >= _distance_min) && (_distance[sector] <= _distance_max);
if ((distance_m <= _distance_max) && (distance_m >= _distance_min)) {
const float angle = sector * 45;
boundary.set_attributes(bnd_loc, angle, distance_m);
// update OA database
database_push(angle, distance_m);
}
_last_update_ms = now;
update_boundary_for_sector(sector, true);
boundary.update_boundary(bnd_loc);
}
// check upward facing range finder
if (sensor->orientation() == ROTATION_PITCH_90) {

14
libraries/AP_Proximity/AP_Proximity_SITL.cpp
View File

@ -53,13 +53,15 @@ void AP_Proximity_SITL::update(void)
}
if (AP::fence()->polyfence().inclusion_boundary_available()) {
// update distance in one sector
if (get_distance_to_fence(_sector_middle_deg[last_sector], _distance[last_sector])) {
boundary_location bnd_loc{last_sector};
boundary.reset_sector(bnd_loc);
float fence_distance;
if (get_distance_to_fence(boundary._sector_middle_deg[last_sector], fence_distance)) {
set_status(AP_Proximity::Status::Good);
_distance_valid[last_sector] = true;
_angle[last_sector] = _sector_middle_deg[last_sector];
update_boundary_for_sector(last_sector, true);
} else {
_distance_valid[last_sector] = false;
boundary.set_attributes(bnd_loc, boundary._sector_middle_deg[last_sector], fence_distance);
boundary.update_boundary(bnd_loc);
// update OA database
database_push(boundary._sector_middle_deg[last_sector], fence_distance);
}
last_sector++;
if (last_sector >= PROXIMITY_NUM_SECTORS) {

16
libraries/AP_Proximity/AP_Proximity_TeraRangerTower.cpp
View File

@ -91,12 +91,16 @@ bool AP_Proximity_TeraRangerTower::read_sensor_data()
// process reply
void AP_Proximity_TeraRangerTower::update_sector_data(int16_t angle_deg, uint16_t distance_cm)
{
const uint8_t sector = convert_angle_to_sector(angle_deg);
_angle[sector] = angle_deg;
_distance[sector] = ((float) distance_cm) / 1000;
_distance_valid[sector] = distance_cm != 0xffff;
{
// Get location on 3-D boundary based on angle to the object
const boundary_location bnd_loc = boundary.get_sector(angle_deg);
boundary.reset_sector(bnd_loc);
if (distance_cm != 0xffff) {
boundary.set_attributes(bnd_loc, angle_deg, ((float) distance_cm) / 1000);
// update OA database
database_push(angle_deg, ((float) distance_cm) / 1000);
}
_last_distance_received_ms = AP_HAL::millis();
// update boundary used for avoidance
update_boundary_for_sector(sector, true);
boundary.update_boundary(bnd_loc);
}

18
libraries/AP_Proximity/AP_Proximity_TeraRangerTowerEvo.cpp
View File

@ -144,14 +144,18 @@ bool AP_Proximity_TeraRangerTowerEvo::read_sensor_data()
// process reply
void AP_Proximity_TeraRangerTowerEvo::update_sector_data(int16_t angle_deg, uint16_t distance_cm)
{
const uint8_t sector = convert_angle_to_sector(angle_deg);
_angle[sector] = angle_deg;
_distance[sector] = ((float) distance_cm) / 1000;
{
// Get location on 3-D boundary based on angle to the object
const boundary_location bnd_loc = boundary.get_sector(angle_deg);
//check for target too far, target too close and sensor not connected
_distance_valid[sector] = distance_cm != 0xffff && distance_cm != 0x0000 && distance_cm != 0x0001;
const bool valid = distance_cm != 0xffff && distance_cm != 0x0000 && distance_cm != 0x0001;
boundary.reset_sector(bnd_loc);
if (valid) {
boundary.set_attributes(bnd_loc, angle_deg, ((float) distance_cm) / 1000);
// update OA database
database_push(angle_deg, ((float) distance_cm) / 1000);
}
_last_distance_received_ms = AP_HAL::millis();
// update boundary used for avoidance
update_boundary_for_sector(sector, true);
boundary.update_boundary(bnd_loc);
}