zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_Proximity: refactor Boundary_3D 

rename stack to layer
swap order of layer and sector within arrays
rename Boundary_Location to Face
rename get_sector to get_face
rename set_attributes to set_face_attributes
get_distance returns bool and fills in argument instead of returning distance
zr-v5.1
Randy Mackay 4 years ago
parent
bd37eab3af
commit
5a0cc08dcc
2 changed files with 206 additions and 172 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 245
      libraries/AP_Proximity/AP_Proximity_Boundary_3D.cpp
  2. 133
      libraries/AP_Proximity/AP_Proximity_Boundary_3D.h

245
libraries/AP_Proximity/AP_Proximity_Boundary_3D.cpp
Unescape View File

@ -8,60 +8,75 @@
AP_Proximity_Boundary_3D::AP_Proximity_Boundary_3D() AP_Proximity_Boundary_3D::AP_Proximity_Boundary_3D()
{ {
// initialise sector edge vector used for building the boundary fence // initialise sector edge vector used for building the boundary fence
init_boundary(); init();
} }
// initialise the boundary and sector_edge_vector array used for object avoidance // initialise the boundary and sector_edge_vector array used for object avoidance
// should be called if the sector_middle_deg or _sector_width_deg arrays are changed // should be called if the sector_middle_deg or _sector_width_deg arrays are changed
void AP_Proximity_Boundary_3D::init_boundary() void AP_Proximity_Boundary_3D::init()
{ {
for (uint8_t stack = 0; stack < PROXIMITY_NUM_LAYERS; stack ++) { for (uint8_t layer=0; layer < PROXIMITY_NUM_LAYERS; layer++) {
for (uint8_t sector=0; sector < PROXIMITY_NUM_SECTORS; sector++) { for (uint8_t sector=0; sector < PROXIMITY_NUM_SECTORS; sector++) {
float angle_rad = ((float)_sector_middle_deg[sector]+(PROXIMITY_SECTOR_WIDTH_DEG/2.0f)); float angle_rad = ((float)_sector_middle_deg[sector]+(PROXIMITY_SECTOR_WIDTH_DEG/2.0f));
float pitch = ((float)_pitch_middle_deg[stack]); float pitch = ((float)_pitch_middle_deg[layer]);
_sector_edge_vector[sector][stack].offset_bearing(angle_rad, pitch, 100.0f); _sector_edge_vector[layer][sector].offset_bearing(angle_rad, pitch, 100.0f);
_boundary_points[sector][stack] = _sector_edge_vector[sector][stack] * PROXIMITY_BOUNDARY_DIST_DEFAULT; _boundary_points[layer][sector] = _sector_edge_vector[layer][sector] * PROXIMITY_BOUNDARY_DIST_DEFAULT;
} }
} }
} }
// returns Boundary_Location object consisting of appropriate stack and sector corresponding to the yaw and pitch. // returns face corresponding to the provided yaw and (optionally) pitch
// Pitch defaults to zero if only yaw is passed to this method // 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 the detected object makes with 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)
// Pitch is the vertical body-frame angle the detected object makes with the vehicle AP_Proximity_Boundary_3D::Face AP_Proximity_Boundary_3D::get_face(float pitch, float yaw) const
boundary_location AP_Proximity_Boundary_3D::get_sector(float yaw, float pitch)
{ {
const uint8_t sector = wrap_360(yaw + (PROXIMITY_SECTOR_WIDTH_DEG * 0.5f)) / 45.0f; const uint8_t sector = wrap_360(yaw + (PROXIMITY_SECTOR_WIDTH_DEG * 0.5f)) / 45.0f;
const float pitch_degrees = constrain_float(pitch, -75.0f, 74.9f); const float pitch_limited = constrain_float(pitch, -75.0f, 74.9f);
const uint8_t stack = (pitch_degrees + 75.0f)/PROXIMITY_PITCH_WIDTH_DEG; const uint8_t layer = (pitch_limited + 75.0f)/PROXIMITY_PITCH_WIDTH_DEG;
return boundary_location{sector, stack}; return Face(layer, sector);
} }
// Set the actual body-frame angle(yaw), pitch, and distance of the detected object. // Set the actual body-frame angle(yaw), pitch, and distance of the detected object.
// This method will also mark the sector and stack to be "valid", so this distance can be used for Obstacle Avoidance // This method will also mark the sector and layer to be "valid", so this distance can be used for Obstacle Avoidance
void AP_Proximity_Boundary_3D::set_attributes(const Boundary_Location& bnd_loc, float angle, float pitch, float distance) void AP_Proximity_Boundary_3D::set_face_attributes(Face face, float angle, float pitch, float distance)
{ {
const uint8_t sector = bnd_loc.sector; if (!face.valid()) {
const uint8_t stack = bnd_loc.stack; return;
_angle[sector][stack] = angle; }
_pitch[sector][stack] = pitch;
_distance[sector][stack] = distance; _angle[face.layer][face.sector] = angle;
_distance_valid[sector][stack] = true; _pitch[face.layer][face.sector] = pitch;
_distance[face.layer][face.sector] = distance;
_distance_valid[face.layer][face.sector] = true;
// update boundary used for simple avoidance
update_boundary(face);
}
// add a distance to the boundary if it is shorter than any other provided distance since the last time the boundary was reset
// pitch and yaw are in degrees, distance is in meters
void AP_Proximity_Boundary_3D::add_distance(float pitch, float yaw, float distance)
{
Face face = get_face(pitch, yaw);
if (!_distance_valid[face.layer][face.sector] || (distance < _distance[face.layer][face.sector])) {
_distance[face.layer][face.sector] = distance;
_distance_valid[face.layer][face.sector] = true;
}
} }
// update boundary points used for object avoidance based on a single sector and pitch distance changing // update boundary points used for object avoidance based on a single sector and pitch distance changing
// the boundary points lie on the line between sectors meaning two boundary points may be updated based on a single sector's distance changing // the boundary points lie on the line between sectors meaning two boundary points may be updated based on a single sector's distance changing
// the boundary point is set to the shortest distance found in the two adjacent sectors, this is a conservative boundary around the vehicle // the boundary point is set to the shortest distance found in the two adjacent sectors, this is a conservative boundary around the vehicle
void AP_Proximity_Boundary_3D::update_boundary(const Boundary_Location& bnd_loc) void AP_Proximity_Boundary_3D::update_boundary(const Face face)
{ {
const uint8_t sector = bnd_loc.sector;
const uint8_t layer = bnd_loc.stack;
// sanity check // sanity check
if (sector >= PROXIMITY_NUM_SECTORS) { if (!face.valid()) {
return; return;
} }
const uint8_t layer = face.layer;
const uint8_t sector = face.sector;
// find adjacent sector (clockwise) // find adjacent sector (clockwise)
uint8_t next_sector = sector + 1; uint8_t next_sector = sector + 1;
if (next_sector >= PROXIMITY_NUM_SECTORS) { if (next_sector >= PROXIMITY_NUM_SECTORS) {
@ -70,84 +85,100 @@ void AP_Proximity_Boundary_3D::update_boundary(const Boundary_Location& bnd_loc)
// boundary point lies on the line between the two sectors at the shorter distance found in the two sectors // boundary point lies on the line between the two sectors at the shorter distance found in the two sectors
float shortest_distance = PROXIMITY_BOUNDARY_DIST_DEFAULT; float shortest_distance = PROXIMITY_BOUNDARY_DIST_DEFAULT;
if (_distance_valid[sector][layer] && _distance_valid[next_sector][layer]) { if (_distance_valid[layer][sector] && _distance_valid[layer][next_sector]) {
shortest_distance = MIN(_distance[sector][layer], _distance[next_sector][layer]); shortest_distance = MIN(_distance[layer][sector], _distance[layer][next_sector]);
} else if (_distance_valid[sector][layer]) { } else if (_distance_valid[layer][sector]) {
shortest_distance = _distance[sector][layer]; shortest_distance = _distance[layer][sector];
} else if (_distance_valid[next_sector][layer]) { } else if (_distance_valid[layer][next_sector]) {
shortest_distance = _distance[next_sector][layer]; shortest_distance = _distance[layer][next_sector];
} }
if (shortest_distance < PROXIMITY_BOUNDARY_DIST_MIN) { if (shortest_distance < PROXIMITY_BOUNDARY_DIST_MIN) {
shortest_distance = PROXIMITY_BOUNDARY_DIST_MIN; shortest_distance = PROXIMITY_BOUNDARY_DIST_MIN;
} }
_boundary_points[sector][layer] = _sector_edge_vector[sector][layer] * shortest_distance; _boundary_points[layer][sector] = _sector_edge_vector[layer][sector] * shortest_distance;
// if the next sector (clockwise) has an invalid distance, set boundary to create a cup like boundary // if the next sector (clockwise) has an invalid distance, set boundary to create a cup like boundary
if (!_distance_valid[next_sector][layer]) { if (!_distance_valid[layer][next_sector]) {
_boundary_points[next_sector][layer] = _sector_edge_vector[next_sector][layer] * shortest_distance; _boundary_points[layer][next_sector] = _sector_edge_vector[layer][next_sector] * shortest_distance;
} }
// repeat for edge between sector and previous sector // repeat for edge between sector and previous sector
uint8_t prev_sector = (sector == 0) ? PROXIMITY_NUM_SECTORS-1 : sector-1; uint8_t prev_sector = (sector == 0) ? PROXIMITY_NUM_SECTORS-1 : sector-1;
shortest_distance = PROXIMITY_BOUNDARY_DIST_DEFAULT; shortest_distance = PROXIMITY_BOUNDARY_DIST_DEFAULT;
if (_distance_valid[prev_sector][layer] && _distance_valid[sector][layer]) { if (_distance_valid[layer][prev_sector] && _distance_valid[layer][sector]) {
shortest_distance = MIN(_distance[prev_sector][layer], _distance[sector][layer]); shortest_distance = MIN(_distance[layer][prev_sector], _distance[layer][sector]);
} else if (_distance_valid[prev_sector][layer]) { } else if (_distance_valid[layer][prev_sector]) {
shortest_distance = _distance[prev_sector][layer]; shortest_distance = _distance[layer][prev_sector];
} else if (_distance_valid[sector][layer]) { } else if (_distance_valid[layer][sector]) {
shortest_distance = _distance[sector][layer]; shortest_distance = _distance[layer][sector];
} }
_boundary_points[prev_sector][layer] = _sector_edge_vector[prev_sector][layer] * shortest_distance; _boundary_points[layer][prev_sector] = _sector_edge_vector[layer][prev_sector] * shortest_distance;
// if the sector counter-clockwise from the previous sector has an invalid distance, set boundary to create a cup like boundary // if the sector counter-clockwise from the previous sector has an invalid distance, set boundary to create a cup like boundary
uint8_t prev_sector_ccw = (prev_sector == 0) ? PROXIMITY_NUM_SECTORS - 1 : prev_sector - 1; uint8_t prev_sector_ccw = (prev_sector == 0) ? PROXIMITY_NUM_SECTORS - 1 : prev_sector - 1;
if (!_distance_valid[prev_sector_ccw][layer]) { if (!_distance_valid[layer][prev_sector_ccw]) {
_boundary_points[prev_sector_ccw][layer] = _sector_edge_vector[prev_sector_ccw][layer] * shortest_distance; _boundary_points[layer][prev_sector_ccw] = _sector_edge_vector[layer][prev_sector_ccw] * shortest_distance;
} }
} }
// Reset this location, specified by Boundary_Location object, back to default // update middle layer boundary points
// i.e Distance is marked as not-valid, and set to a large number. void AP_Proximity_Boundary_3D::update_middle_boundary()
void AP_Proximity_Boundary_3D::reset_sector(const Boundary_Location& bnd_loc)
{ {
_distance[bnd_loc.sector][bnd_loc.stack] = DISTANCE_MAX; for (uint8_t sector=0; sector < PROXIMITY_NUM_SECTORS; sector++) {
_distance_valid[bnd_loc.sector][bnd_loc.stack] = false; update_boundary(Face(PROXIMITY_MIDDLE_LAYER, sector));
}
} }
// Reset all horizontal sectors // reset boundary. marks all distances as invalid
// i.e Distance is marked as not-valid, and set to a large number for all horizontal sectors. void AP_Proximity_Boundary_3D::reset()
void AP_Proximity_Boundary_3D::reset_all_horizontal_sectors()
{ {
for (uint8_t i=0; i < PROXIMITY_NUM_SECTORS; i++) { for (uint8_t layer=0; layer < PROXIMITY_NUM_LAYERS; layer++) {
const Boundary_Location bnd_loc{i}; for (uint8_t sector=0; sector < PROXIMITY_NUM_SECTORS; sector++) {
reset_sector(bnd_loc); _distance_valid[layer][sector] = false;
}
} }
} }
// Reset all stacks and sectors // Reset this location, specified by Face object, back to default
// i.e Distance is marked as not-valid, and set to a large number for all stacks and sectors. // i.e Distance is marked as not-valid, and set to a large number.
void AP_Proximity_Boundary_3D::reset_all_sectors_and_stacks() void AP_Proximity_Boundary_3D::reset_face(Face face)
{ {
for (uint8_t j=0; j < PROXIMITY_NUM_LAYERS; j++) { if (!face.valid()) {
for (uint8_t i=0; i < PROXIMITY_NUM_SECTORS; i++) { return;
const Boundary_Location bnd_loc{i, j};
reset_sector(bnd_loc);
}
} }
_distance_valid[face.layer][face.sector] = false;
// update simple avoidance boundary
update_boundary(face);
}
// get distance for a face. returns true on success and fills in distance argument with distance in meters
bool AP_Proximity_Boundary_3D::get_distance(Face face, float &distance) const
{
if (!face.valid()) {
return false;
}
if (_distance_valid[face.layer][face.sector]) {
distance = _distance[face.layer][face.sector];
return true;
}
return false;
} }
// get the total number of obstacles // get the total number of obstacles
// this method iterates through the entire 3-D boundary and checks which layer has atleast one valid distance // this method iterates through the entire 3-D boundary and checks which layer has at least one valid distance
uint8_t AP_Proximity_Boundary_3D::get_obstacle_count() uint8_t AP_Proximity_Boundary_3D::get_obstacle_count()
{ {
uint8_t obstacle_count = 0; uint8_t obstacle_count = 0;
// reset entire array to false // reset entire array to false
memset(_active_layer, 0, sizeof(_active_layer)); memset(_active_layer, 0, sizeof(_active_layer));
// check if this layer has atleast one valid sector // check if this layer has atleast one valid sector
for (uint8_t j=0; j<PROXIMITY_NUM_LAYERS; j++) { for (uint8_t layer=0; layer<PROXIMITY_NUM_LAYERS; layer++) {
for (uint8_t i=0; i<PROXIMITY_NUM_SECTORS; i++ ) { for (uint8_t sector=0; sector<PROXIMITY_NUM_SECTORS; sector++ ) {
if (_distance_valid[i][j]) { if (_distance_valid[layer][sector]) {
_active_layer[j] = true; _active_layer[layer] = true;
obstacle_count += PROXIMITY_NUM_SECTORS; obstacle_count += PROXIMITY_NUM_SECTORS;
break; break;
} }
@ -156,60 +187,60 @@ uint8_t AP_Proximity_Boundary_3D::get_obstacle_count()
return obstacle_count; return obstacle_count;
} }
// Converts obstacle_num passed from avoidance library into appropriate stack and sector // Converts obstacle_num passed from avoidance library into appropriate layer and sector
// This is packed into a Boundary Location object and returned // This is packed into a Boundary Location object and returned
boundary_location AP_Proximity_Boundary_3D::convert_obstacle_num_to_boundary_loc(uint8_t obstacle_num) const AP_Proximity_Boundary_3D::Face AP_Proximity_Boundary_3D::convert_obstacle_num_to_face(uint8_t obstacle_num) const
{ {
const uint8_t active_layer = obstacle_num / PROXIMITY_NUM_SECTORS; const uint8_t active_layer = obstacle_num / PROXIMITY_NUM_SECTORS;
uint8_t layer_count = 0; uint8_t layer_count = 0;
uint8_t stack = 0; uint8_t layer = 0;
for (uint8_t i=0; i < PROXIMITY_NUM_LAYERS; i++) { for (uint8_t i=0; i < PROXIMITY_NUM_LAYERS; i++) {
if (_active_layer[i]) { if (_active_layer[i]) {
layer_count++; layer_count++;
} }
if (layer_count == (active_layer + 1)) { if (layer_count == (active_layer + 1)) {
stack = i; layer = i;
break; break;
} }
} }
const uint8_t sector = obstacle_num % PROXIMITY_NUM_SECTORS; const uint8_t sector = obstacle_num % PROXIMITY_NUM_SECTORS;
return boundary_location{sector, stack}; return AP_Proximity_Boundary_3D::Face(layer, sector);
} }
// WARNING: This requires get_obstacle_count() to be called before calling this method // WARNING: This requires get_obstacle_count() to be called before calling this method
// Appropriate stack and sector are found from the passed obstacle_num // Appropriate layer and sector are found from the passed obstacle_num
// This function then draws a line between this sector, and sector + 1 at the given stack // This function then draws a line between this sector, and sector + 1 at the given layer
// Then returns the closest point on this line from vehicle, in body-frame. // Then returns the closest point on this line from vehicle, in body-frame.
// Used by GPS based Simple Avoidance // Used by GPS based Simple Avoidance
void AP_Proximity_Boundary_3D::get_obstacle(uint8_t obstacle_num, Vector3f& vec_to_obstacle) const void AP_Proximity_Boundary_3D::get_obstacle(uint8_t obstacle_num, Vector3f& vec_to_obstacle) const
{ {
const boundary_location bnd_loc = convert_obstacle_num_to_boundary_loc(obstacle_num); const AP_Proximity_Boundary_3D::Face face = convert_obstacle_num_to_face(obstacle_num);
const uint8_t sector_end = bnd_loc.sector; const uint8_t sector_end = face.sector;
uint8_t sector_start = bnd_loc.sector + 1; uint8_t sector_start = face.sector + 1;
if (sector_start >= PROXIMITY_NUM_SECTORS) { if (sector_start >= PROXIMITY_NUM_SECTORS) {
sector_start = 0; sector_start = 0;
} }
const Vector3f start = _boundary_points[sector_start][bnd_loc.stack]; const Vector3f start = _boundary_points[face.layer][sector_start];
const Vector3f end = _boundary_points[sector_end][bnd_loc.stack]; const Vector3f end = _boundary_points[face.layer][sector_end];
vec_to_obstacle = Vector3f::closest_point_between_line_and_point(start, end, Vector3f{0.0f, 0.0f, 0.0f}); vec_to_obstacle = Vector3f::closest_point_between_line_and_point(start, end, Vector3f{0.0f, 0.0f, 0.0f});
} }
// WARNING: This requires get_obstacle_count() to be called before calling this method // WARNING: This requires get_obstacle_count() to be called before calling this method
// Appropriate stack and sector are found from the passed obstacle_num // Appropriate layer and sector are found from the passed obstacle_num
// This function then draws a line between this sector, and sector + 1 at the given stack // This function then draws a line between this sector, and sector + 1 at the given layer
// Then returns the closest point on this line from the segment that was passed, in body-frame. // Then returns the closest point on this line from the segment that was passed, in body-frame.
// Used by GPS based Simple Avoidance - for "brake mode" // Used by GPS based Simple Avoidance - for "brake mode"
float AP_Proximity_Boundary_3D::distance_to_obstacle(uint8_t obstacle_num, const Vector3f& seg_start, const Vector3f& seg_end, Vector3f& closest_point) const float AP_Proximity_Boundary_3D::distance_to_obstacle(uint8_t obstacle_num, const Vector3f& seg_start, const Vector3f& seg_end, Vector3f& closest_point) const
{ {
const boundary_location bnd_loc = convert_obstacle_num_to_boundary_loc(obstacle_num); const AP_Proximity_Boundary_3D::Face face = convert_obstacle_num_to_face(obstacle_num);
const uint8_t sector_end = bnd_loc.sector; const uint8_t sector_end = face.sector;
uint8_t sector_start = bnd_loc.sector + 1; uint8_t sector_start = face.sector + 1;
if (sector_start >= PROXIMITY_NUM_SECTORS) { if (sector_start >= PROXIMITY_NUM_SECTORS) {
sector_start = 0; sector_start = 0;
} }
const Vector3f start = _boundary_points[sector_start][bnd_loc.stack]; const Vector3f start = _boundary_points[face.layer][sector_start];
const Vector3f end = _boundary_points[sector_end][bnd_loc.stack]; const Vector3f end = _boundary_points[face.layer][sector_end];
return Vector3f::segment_to_segment_dist(seg_start, seg_end, start, end, closest_point); return Vector3f::segment_to_segment_dist(seg_start, seg_end, start, end, closest_point);
} }
@ -217,30 +248,30 @@ float AP_Proximity_Boundary_3D::distance_to_obstacle(uint8_t obstacle_num, const
// returns true on success, false if no valid readings // returns true on success, false if no valid readings
bool AP_Proximity_Boundary_3D::get_closest_object(float& angle_deg, float &distance) const bool AP_Proximity_Boundary_3D::get_closest_object(float& angle_deg, float &distance) const
{ {
bool sector_found = false; bool closest_found = false;
uint8_t sector = 0; uint8_t closest_sector = 0;
uint8_t stack = 0; uint8_t closest_layer = 0;
// check boundary for shortest distance // check boundary for shortest distance
// only check for middle layers and higher // only check for middle layers and higher
// lower layers might contain ground, which will give false pre-arm failure // lower layers might contain ground, which will give false pre-arm failure
for (uint8_t j=PROXIMITY_MIDDLE_LAYER; j<PROXIMITY_NUM_LAYERS; j++) { for (uint8_t layer=PROXIMITY_MIDDLE_LAYER; layer<PROXIMITY_NUM_LAYERS; layer++) {
for (uint8_t i=0; i<PROXIMITY_NUM_SECTORS; i++) { for (uint8_t sector=0; sector<PROXIMITY_NUM_SECTORS; sector++) {
if (_distance_valid[i][j]) { if (_distance_valid[layer][sector]) {
if (!sector_found || (_distance[i][j] < _distance[sector][stack])) { if (!closest_found || (_distance[layer][sector] < _distance[closest_layer][closest_sector])) {
sector = i; closest_layer = layer;
stack = j; closest_sector = sector;
sector_found = true; closest_found = true;
} }
} }
} }
} }
if (sector_found) { if (closest_found) {
angle_deg = _angle[sector][stack]; angle_deg = _angle[closest_layer][closest_sector];
distance = _distance[sector][stack]; distance = _distance[closest_layer][closest_sector];
} }
return sector_found; return closest_found;
} }
// get number of objects, used for non-GPS avoidance // get number of objects, used for non-GPS avoidance
@ -253,9 +284,9 @@ uint8_t AP_Proximity_Boundary_3D::get_horizontal_object_count() const
// returns false if no angle or distance could be returned for some reason // returns false if no angle or distance could be returned for some reason
bool AP_Proximity_Boundary_3D::get_horizontal_object_angle_and_distance(uint8_t object_number, float& angle_deg, float &distance) const bool AP_Proximity_Boundary_3D::get_horizontal_object_angle_and_distance(uint8_t object_number, float& angle_deg, float &distance) const
{ {
if (object_number < PROXIMITY_NUM_SECTORS && _distance_valid[object_number][PROXIMITY_MIDDLE_LAYER]) { if ((object_number < PROXIMITY_NUM_SECTORS) && _distance_valid[PROXIMITY_MIDDLE_LAYER][object_number]) {
angle_deg = _angle[object_number][PROXIMITY_MIDDLE_LAYER]; angle_deg = _angle[PROXIMITY_MIDDLE_LAYER][object_number];
distance = _distance[object_number][PROXIMITY_MIDDLE_LAYER]; distance = _distance[PROXIMITY_MIDDLE_LAYER][object_number];
return true; return true;
} }
return false; return false;

133
libraries/AP_Proximity/AP_Proximity_Boundary_3D.h
Unescape View File

@ -16,13 +16,12 @@
#pragma once #pragma once
#define PROXIMITY_NUM_SECTORS 8 // number of sectors #define PROXIMITY_NUM_SECTORS 8 // number of sectors
#define PROXIMITY_NUM_LAYERS 5 // num of stacks in a sector #define PROXIMITY_NUM_LAYERS 5 // num of layers in a sector
#define PROXIMITY_MIDDLE_LAYER 2 // middle stack #define PROXIMITY_MIDDLE_LAYER 2 // middle layer
#define PROXIMITY_PITCH_WIDTH_DEG 30 // width between each stack in degrees #define PROXIMITY_PITCH_WIDTH_DEG 30 // width between each layer in degrees
#define PROXIMITY_SECTOR_WIDTH_DEG 45.0f // width of sectors in degrees #define PROXIMITY_SECTOR_WIDTH_DEG 45.0f // width of sectors in degrees
#define PROXIMITY_BOUNDARY_DIST_MIN 0.6f // minimum distance for a boundary point. This ensures the object avoidance code doesn't think we are outside the boundary. #define PROXIMITY_BOUNDARY_DIST_MIN 0.6f // minimum distance for a boundary point. This ensures the object avoidance code doesn't think we are outside the boundary.
#define PROXIMITY_BOUNDARY_DIST_DEFAULT 100 // if we have no data for a sector, boundary is placed 100m out #define PROXIMITY_BOUNDARY_DIST_DEFAULT 100 // if we have no data for a sector, boundary is placed 100m out
#define DISTANCE_MAX 999999.0f // arbritary "large" distance
class AP_Proximity_Boundary_3D class AP_Proximity_Boundary_3D
{ {
@ -30,69 +29,75 @@ public:
// constructor. This incorporates initialisation as well. // constructor. This incorporates initialisation as well.
AP_Proximity_Boundary_3D(); AP_Proximity_Boundary_3D();
// This class is used to store the stack and sector as a single packet to access and modify the 3-D boundary // stores the layer and sector as a single object to access and modify the 3-D boundary
class Boundary_Location class Face
{ {
public: public:
// constructor when both stack and sector are passed
Boundary_Location(uint8_t Sector, uint8_t Stack) { sector = Sector; stack = Stack; } // constructor, invalidate id and distance
// constructor defaults to "middle(horizontal) layer" if only sector is passed Face() { layer = sector = UINT8_MAX; }
Boundary_Location(uint8_t Sector) { sector = Sector; stack = PROXIMITY_MIDDLE_LAYER; } Face(uint8_t _layer, uint8_t _sector) { layer = _layer; sector = _sector; }
uint8_t stack; // vertical "steps" on the 3D Boundary // return true if face has valid layer and sector values
bool valid() const { return ((layer < PROXIMITY_NUM_LAYERS) && (sector < PROXIMITY_NUM_SECTORS)); }
// comparison operator
bool operator ==(const Face &other) const { return ((layer == other.layer) && (sector == other.sector)); }
bool operator !=(const Face &other) const { return ((layer != other.layer) || (sector != other.sector)); }
uint8_t layer; // vertical "steps" on the 3D Boundary
uint8_t sector; // horizontal "steps" on the 3D Boundary uint8_t sector; // horizontal "steps" on the 3D Boundary
}; };
// returns Boundary_Location object consisting of appropriate stack and sector // returns face corresponding to the provided yaw and (optionally) pitch
// corresponding to the yaw and pitch. // pitch is the vertical body-frame angle (in degrees) to the obstacle (0=directly ahead, 90 is above the vehicle?)
// Pitch defaults to zero if only yaw is passed to this method // 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)
// Yaw is the horizontal body-frame angle the detected object makes with the vehicle Face get_face(float pitch, float yaw) const;
// Pitch is the vertical body-frame angle the detected object makes with the vehicle Face get_face(float yaw) const { return get_face(0, yaw); }
Boundary_Location get_sector(float yaw, float pitch = 0.0f);
// Set the actual body-frame angle(yaw), pitch, and distance of the detected object. // Set the actual body-frame angle(yaw), pitch, and distance of the detected object.
// This method will also mark the sector and stack to be "valid" // This method will also mark the sector and layer to be "valid",
// This distance can then be used for Obstacle Avoidance
void set_attributes(const Boundary_Location& bnd_loc, float angle, float pitch, float distance);
// Set the actual body-frame angle(yaw), pitch, and distance of the detected object.
// This method will also mark the sector and stack to be "valid",
// This distance can then be used for Obstacle Avoidance // This distance can then be used for Obstacle Avoidance
// Assume detected obstacle is horizontal (zero pitch), if no pitch is passed // Assume detected obstacle is horizontal (zero pitch), if no pitch is passed
void set_attributes(const Boundary_Location& bnd_loc, float angle, float distance) { set_attributes(bnd_loc, angle, 0.0f, distance); } void set_face_attributes(Face face, float pitch, float yaw, float distance);
void set_face_attributes(Face face, float yaw, float distance) { set_face_attributes(face, 0, yaw, distance); }
// add a distance to the boundary if it is shorter than any other provided distance since the last time the boundary was reset
// pitch and yaw are in degrees, distance is in meters
void add_distance(float pitch, float yaw, float distance);
void add_distance(float yaw, float distance) { add_distance(0, yaw, distance); }
// update boundary points used for object avoidance based on a single sector and pitch distance changing // update boundary points used for simple avoidance based on a single sector and pitch distance changing
// the boundary points lie on the line between sectors meaning two boundary points may be updated based on a single sector's distance changing // the boundary points lie on the line between sectors meaning two boundary points may be updated based on a single sector's distance changing
// the boundary point is set to the shortest distance found in the two adjacent sectors, this is a conservative boundary around the vehicle // the boundary point is set to the shortest distance found in the two adjacent sectors, this is a conservative boundary around the vehicle
void update_boundary(const Boundary_Location& bnd_loc); void update_boundary(Face face);
// Reset this location, specified by Boundary_Location object, back to default // update middle layer boundary points
void update_middle_boundary();
// reset boundary. marks all distances as invalid
void reset();
// Reset this location, specified by Face object, back to default
// i.e Distance is marked as not-valid, and set to a large number. // i.e Distance is marked as not-valid, and set to a large number.
void reset_sector(const Boundary_Location& bnd_loc); void reset_face(Face face);
// Reset all horizontal sectors
void reset_all_horizontal_sectors(); // get distance for a face. returns true on success and fills in distance argument with distance in meters
// Reset all stacks and sectors bool get_distance(Face face, float &distance) const;
void reset_all_sectors_and_stacks();
// Get values given the stack and sector as a Boundary_Location object
float get_angle(const Boundary_Location& bnd_loc) const { return _angle[bnd_loc.sector][bnd_loc.stack]; }
float get_pitch(const Boundary_Location& bnd_loc) const { return _pitch[bnd_loc.sector][bnd_loc.stack]; }
float get_distance(const Boundary_Location& bnd_loc) const { return _distance[bnd_loc.sector][bnd_loc.stack]; }
bool check_distance_valid(const Boundary_Location& bnd_loc) const { return _distance_valid[bnd_loc.sector][bnd_loc.stack]; }
// Get the total number of obstacles // Get the total number of obstacles
// This method iterates through the entire 3-D boundary and checks which layer has atleast one valid distance // This method iterates through the entire 3-D boundary and checks which layer has at least one valid distance
uint8_t get_obstacle_count(); uint8_t get_obstacle_count();
// WARNING: This requires get_obstacle_count() to be called before calling this method // WARNING: This requires get_obstacle_count() to be called before calling this method
// Appropriate stack and sector are found from the passed obstacle_num // Appropriate layer and sector are found from the passed obstacle_num
// This function then draws a line between this sector, and sector + 1 at the given stack // This function then draws a line between this sector, and sector + 1 at the given layer
// Then returns the closest point on this line from vehicle, in body-frame. // Then returns the closest point on this line from vehicle, in body-frame.
void get_obstacle(uint8_t obstacle_num, Vector3f& vec_to_boundary) const; void get_obstacle(uint8_t obstacle_num, Vector3f& vec_to_boundary) const;
// WARNING: This requires get_obstacle_count() to be called before calling this method // WARNING: This requires get_obstacle_count() to be called before calling this method
// Appropriate stack and sector are found from the passed obstacle_num // Appropriate layer and sector are found from the passed obstacle_num
// This function then draws a line between this sector, and sector + 1 at the given stack // This function then draws a line between this sector, and sector + 1 at the given layer
// Then returns the closest point on this line from the segment that was passed, in body-frame. // Then returns the closest point on this line from the segment that was passed, in body-frame.
// Used by GPS based Simple Avoidance - for "brake mode" // Used by GPS based Simple Avoidance - for "brake mode"
float distance_to_obstacle(uint8_t obstacle_num, const Vector3f& seg_start, const Vector3f& seg_end, Vector3f& closest_point) const; float distance_to_obstacle(uint8_t obstacle_num, const Vector3f& seg_start, const Vector3f& seg_end, Vector3f& closest_point) const;
@ -111,22 +116,20 @@ public:
const int16_t _pitch_middle_deg[PROXIMITY_NUM_LAYERS] {-60, -30, 0, 30, 60}; const int16_t _pitch_middle_deg[PROXIMITY_NUM_LAYERS] {-60, -30, 0, 30, 60};
private: private:
// initialise the boundary and sector_edge_vector array used for object avoidance // initialise the boundary and sector_edge_vector array used for object avoidance
void init_boundary(); void init();
// Converts obstacle_num passed from avoidance library into appropriate stack and sector // Converts obstacle_num passed from avoidance library into appropriate face
// This is packed into a Boundary Location object and returned Face convert_obstacle_num_to_face(uint8_t obstacle_num) const;
Boundary_Location convert_obstacle_num_to_boundary_loc(uint8_t obstacle_num) const;
Vector3f _sector_edge_vector[PROXIMITY_NUM_LAYERS][PROXIMITY_NUM_SECTORS];
Vector3f _sector_edge_vector[PROXIMITY_NUM_SECTORS][PROXIMITY_NUM_LAYERS]; Vector3f _boundary_points[PROXIMITY_NUM_LAYERS][PROXIMITY_NUM_SECTORS];
Vector3f _boundary_points[PROXIMITY_NUM_SECTORS][PROXIMITY_NUM_LAYERS];
float _angle[PROXIMITY_NUM_LAYERS][PROXIMITY_NUM_SECTORS]; // yaw angle in degrees to closest object within each sector and layer
// sensor data float _pitch[PROXIMITY_NUM_LAYERS][PROXIMITY_NUM_SECTORS]; // pitch angle in degrees to the closest object within each sector and layer
float _angle[PROXIMITY_NUM_SECTORS][PROXIMITY_NUM_LAYERS]; // angle to closest object within each sector and stack float _distance[PROXIMITY_NUM_LAYERS][PROXIMITY_NUM_SECTORS]; // distance to closest object within each sector and layer
float _pitch[PROXIMITY_NUM_SECTORS][PROXIMITY_NUM_LAYERS]; // pitch to the closest object within each sector and stack bool _distance_valid[PROXIMITY_NUM_LAYERS][PROXIMITY_NUM_SECTORS]; // true if a valid distance received for each sector and layer
float _distance[PROXIMITY_NUM_SECTORS][PROXIMITY_NUM_LAYERS]; // distance to closest object within each sector and stack bool _active_layer[PROXIMITY_NUM_LAYERS]; // layers which have at least one valid distance are marked true
bool _distance_valid[PROXIMITY_NUM_SECTORS][PROXIMITY_NUM_LAYERS]; // true if a valid distance received for each sector and stack
bool _active_layer[PROXIMITY_NUM_LAYERS]; // layers which have atleast one valid distance are marked true
}; };
typedef AP_Proximity_Boundary_3D::Boundary_Location boundary_location;

Write Preview
Loading…
Cancel
Save