mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-21 15:23:57 -04:00
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
This commit is contained in:
Randy Mackay
parent
bd37eab3af
commit
5a0cc08dcc
@ -8,60 +8,75 @@
|
||||
AP_Proximity_Boundary_3D::AP_Proximity_Boundary_3D()
|
||||
{
|
||||
// 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
|
||||
// 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++) {
|
||||
float angle_rad = ((float)_sector_middle_deg[sector]+(PROXIMITY_SECTOR_WIDTH_DEG/2.0f));
|
||||
float pitch = ((float)_pitch_middle_deg[stack]);
|
||||
_sector_edge_vector[sector][stack].offset_bearing(angle_rad, pitch, 100.0f);
|
||||
_boundary_points[sector][stack] = _sector_edge_vector[sector][stack] * PROXIMITY_BOUNDARY_DIST_DEFAULT;
|
||||
float pitch = ((float)_pitch_middle_deg[layer]);
|
||||
_sector_edge_vector[layer][sector].offset_bearing(angle_rad, pitch, 100.0f);
|
||||
_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.
|
||||
// Pitch defaults to zero if only yaw is passed to this method
|
||||
// Yaw is the horizontal body-frame angle the detected object makes with the vehicle
|
||||
// Pitch is the vertical body-frame angle the detected object makes with the vehicle
|
||||
boundary_location AP_Proximity_Boundary_3D::get_sector(float yaw, float pitch)
|
||||
// returns face corresponding to the provided yaw and (optionally) pitch
|
||||
// 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)
|
||||
AP_Proximity_Boundary_3D::Face AP_Proximity_Boundary_3D::get_face(float pitch, float yaw) const
|
||||
{
|
||||
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 uint8_t stack = (pitch_degrees + 75.0f)/PROXIMITY_PITCH_WIDTH_DEG;
|
||||
return boundary_location{sector, stack};
|
||||
const float pitch_limited = constrain_float(pitch, -75.0f, 74.9f);
|
||||
const uint8_t layer = (pitch_limited + 75.0f)/PROXIMITY_PITCH_WIDTH_DEG;
|
||||
return Face(layer, sector);
|
||||
}
|
||||
|
||||
// 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
|
||||
void AP_Proximity_Boundary_3D::set_attributes(const Boundary_Location& bnd_loc, float angle, float pitch, float distance)
|
||||
{
|
||||
const uint8_t sector = bnd_loc.sector;
|
||||
const uint8_t stack = bnd_loc.stack;
|
||||
_angle[sector][stack] = angle;
|
||||
_pitch[sector][stack] = pitch;
|
||||
_distance[sector][stack] = distance;
|
||||
_distance_valid[sector][stack] = true;
|
||||
// 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_face_attributes(Face face, float angle, float pitch, float distance)
|
||||
{
|
||||
if (!face.valid()) {
|
||||
return;
|
||||
}
|
||||
|
||||
_angle[face.layer][face.sector] = angle;
|
||||
_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
|
||||
// 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
|
||||
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
|
||||
if (sector >= PROXIMITY_NUM_SECTORS) {
|
||||
if (!face.valid()) {
|
||||
return;
|
||||
}
|
||||
|
||||
const uint8_t layer = face.layer;
|
||||
const uint8_t sector = face.sector;
|
||||
|
||||
// find adjacent sector (clockwise)
|
||||
uint8_t next_sector = sector + 1;
|
||||
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
|
||||
float shortest_distance = PROXIMITY_BOUNDARY_DIST_DEFAULT;
|
||||
if (_distance_valid[sector][layer] && _distance_valid[next_sector][layer]) {
|
||||
shortest_distance = MIN(_distance[sector][layer], _distance[next_sector][layer]);
|
||||
} else if (_distance_valid[sector][layer]) {
|
||||
shortest_distance = _distance[sector][layer];
|
||||
} else if (_distance_valid[next_sector][layer]) {
|
||||
shortest_distance = _distance[next_sector][layer];
|
||||
if (_distance_valid[layer][sector] && _distance_valid[layer][next_sector]) {
|
||||
shortest_distance = MIN(_distance[layer][sector], _distance[layer][next_sector]);
|
||||
} else if (_distance_valid[layer][sector]) {
|
||||
shortest_distance = _distance[layer][sector];
|
||||
} else if (_distance_valid[layer][next_sector]) {
|
||||
shortest_distance = _distance[layer][next_sector];
|
||||
}
|
||||
if (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 (!_distance_valid[next_sector][layer]) {
|
||||
_boundary_points[next_sector][layer] = _sector_edge_vector[next_sector][layer] * shortest_distance;
|
||||
if (!_distance_valid[layer][next_sector]) {
|
||||
_boundary_points[layer][next_sector] = _sector_edge_vector[layer][next_sector] * shortest_distance;
|
||||
}
|
||||
|
||||
// repeat for edge between sector and previous sector
|
||||
uint8_t prev_sector = (sector == 0) ? PROXIMITY_NUM_SECTORS-1 : sector-1;
|
||||
shortest_distance = PROXIMITY_BOUNDARY_DIST_DEFAULT;
|
||||
if (_distance_valid[prev_sector][layer] && _distance_valid[sector][layer]) {
|
||||
shortest_distance = MIN(_distance[prev_sector][layer], _distance[sector][layer]);
|
||||
} else if (_distance_valid[prev_sector][layer]) {
|
||||
shortest_distance = _distance[prev_sector][layer];
|
||||
} else if (_distance_valid[sector][layer]) {
|
||||
shortest_distance = _distance[sector][layer];
|
||||
if (_distance_valid[layer][prev_sector] && _distance_valid[layer][sector]) {
|
||||
shortest_distance = MIN(_distance[layer][prev_sector], _distance[layer][sector]);
|
||||
} else if (_distance_valid[layer][prev_sector]) {
|
||||
shortest_distance = _distance[layer][prev_sector];
|
||||
} else if (_distance_valid[layer][sector]) {
|
||||
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
|
||||
uint8_t prev_sector_ccw = (prev_sector == 0) ? PROXIMITY_NUM_SECTORS - 1 : prev_sector - 1;
|
||||
if (!_distance_valid[prev_sector_ccw][layer]) {
|
||||
_boundary_points[prev_sector_ccw][layer] = _sector_edge_vector[prev_sector_ccw][layer] * shortest_distance;
|
||||
if (!_distance_valid[layer][prev_sector_ccw]) {
|
||||
_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
|
||||
// i.e Distance is marked as not-valid, and set to a large number.
|
||||
void AP_Proximity_Boundary_3D::reset_sector(const Boundary_Location& bnd_loc)
|
||||
// update middle layer boundary points
|
||||
void AP_Proximity_Boundary_3D::update_middle_boundary()
|
||||
{
|
||||
_distance[bnd_loc.sector][bnd_loc.stack] = DISTANCE_MAX;
|
||||
_distance_valid[bnd_loc.sector][bnd_loc.stack] = false;
|
||||
}
|
||||
|
||||
// Reset all horizontal sectors
|
||||
// i.e Distance is marked as not-valid, and set to a large number for all horizontal sectors.
|
||||
void AP_Proximity_Boundary_3D::reset_all_horizontal_sectors()
|
||||
{
|
||||
for (uint8_t i=0; i < PROXIMITY_NUM_SECTORS; i++) {
|
||||
const Boundary_Location bnd_loc{i};
|
||||
reset_sector(bnd_loc);
|
||||
for (uint8_t sector=0; sector < PROXIMITY_NUM_SECTORS; sector++) {
|
||||
update_boundary(Face(PROXIMITY_MIDDLE_LAYER, sector));
|
||||
}
|
||||
}
|
||||
|
||||
// Reset all stacks and sectors
|
||||
// i.e Distance is marked as not-valid, and set to a large number for all stacks and sectors.
|
||||
void AP_Proximity_Boundary_3D::reset_all_sectors_and_stacks()
|
||||
// reset boundary. marks all distances as invalid
|
||||
void AP_Proximity_Boundary_3D::reset()
|
||||
{
|
||||
for (uint8_t j=0; j < PROXIMITY_NUM_LAYERS; j++) {
|
||||
for (uint8_t i=0; i < PROXIMITY_NUM_SECTORS; i++) {
|
||||
const Boundary_Location bnd_loc{i, j};
|
||||
reset_sector(bnd_loc);
|
||||
for (uint8_t layer=0; layer < PROXIMITY_NUM_LAYERS; layer++) {
|
||||
for (uint8_t sector=0; sector < PROXIMITY_NUM_SECTORS; sector++) {
|
||||
_distance_valid[layer][sector] = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Reset this location, specified by Face object, back to default
|
||||
// i.e Distance is marked as not-valid, and set to a large number.
|
||||
void AP_Proximity_Boundary_3D::reset_face(Face face)
|
||||
{
|
||||
if (!face.valid()) {
|
||||
return;
|
||||
}
|
||||
_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
|
||||
// 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 obstacle_count = 0;
|
||||
// reset entire array to false
|
||||
memset(_active_layer, 0, sizeof(_active_layer));
|
||||
// check if this layer has atleast one valid sector
|
||||
for (uint8_t j=0; j<PROXIMITY_NUM_LAYERS; j++) {
|
||||
for (uint8_t i=0; i<PROXIMITY_NUM_SECTORS; i++ ) {
|
||||
if (_distance_valid[i][j]) {
|
||||
_active_layer[j] = true;
|
||||
for (uint8_t layer=0; layer<PROXIMITY_NUM_LAYERS; layer++) {
|
||||
for (uint8_t sector=0; sector<PROXIMITY_NUM_SECTORS; sector++ ) {
|
||||
if (_distance_valid[layer][sector]) {
|
||||
_active_layer[layer] = true;
|
||||
obstacle_count += PROXIMITY_NUM_SECTORS;
|
||||
break;
|
||||
}
|
||||
@ -156,60 +187,60 @@ uint8_t AP_Proximity_Boundary_3D::get_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
|
||||
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;
|
||||
uint8_t layer_count = 0;
|
||||
uint8_t stack = 0;
|
||||
uint8_t layer = 0;
|
||||
for (uint8_t i=0; i < PROXIMITY_NUM_LAYERS; i++) {
|
||||
if (_active_layer[i]) {
|
||||
layer_count++;
|
||||
}
|
||||
if (layer_count == (active_layer + 1)) {
|
||||
stack = i;
|
||||
layer = i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
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
|
||||
// Appropriate stack 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
|
||||
// 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 layer
|
||||
// Then returns the closest point on this line from vehicle, in body-frame.
|
||||
// Used by GPS based Simple Avoidance
|
||||
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 uint8_t sector_end = bnd_loc.sector;
|
||||
uint8_t sector_start = bnd_loc.sector + 1;
|
||||
{
|
||||
const AP_Proximity_Boundary_3D::Face face = convert_obstacle_num_to_face(obstacle_num);
|
||||
const uint8_t sector_end = face.sector;
|
||||
uint8_t sector_start = face.sector + 1;
|
||||
if (sector_start >= PROXIMITY_NUM_SECTORS) {
|
||||
sector_start = 0;
|
||||
}
|
||||
const Vector3f start = _boundary_points[sector_start][bnd_loc.stack];
|
||||
const Vector3f end = _boundary_points[sector_end][bnd_loc.stack];
|
||||
const Vector3f start = _boundary_points[face.layer][sector_start];
|
||||
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});
|
||||
}
|
||||
|
||||
// WARNING: This requires get_obstacle_count() to be called before calling this method
|
||||
// Appropriate stack 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
|
||||
// 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 layer
|
||||
// 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"
|
||||
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 uint8_t sector_end = bnd_loc.sector;
|
||||
uint8_t sector_start = bnd_loc.sector + 1;
|
||||
const AP_Proximity_Boundary_3D::Face face = convert_obstacle_num_to_face(obstacle_num);
|
||||
const uint8_t sector_end = face.sector;
|
||||
uint8_t sector_start = face.sector + 1;
|
||||
if (sector_start >= PROXIMITY_NUM_SECTORS) {
|
||||
sector_start = 0;
|
||||
}
|
||||
const Vector3f start = _boundary_points[sector_start][bnd_loc.stack];
|
||||
const Vector3f end = _boundary_points[sector_end][bnd_loc.stack];
|
||||
const Vector3f start = _boundary_points[face.layer][sector_start];
|
||||
const Vector3f end = _boundary_points[face.layer][sector_end];
|
||||
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
|
||||
bool AP_Proximity_Boundary_3D::get_closest_object(float& angle_deg, float &distance) const
|
||||
{
|
||||
bool sector_found = false;
|
||||
uint8_t sector = 0;
|
||||
uint8_t stack = 0;
|
||||
bool closest_found = false;
|
||||
uint8_t closest_sector = 0;
|
||||
uint8_t closest_layer = 0;
|
||||
|
||||
// check boundary for shortest distance
|
||||
// only check for middle layers and higher
|
||||
// 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 i=0; i<PROXIMITY_NUM_SECTORS; i++) {
|
||||
if (_distance_valid[i][j]) {
|
||||
if (!sector_found || (_distance[i][j] < _distance[sector][stack])) {
|
||||
sector = i;
|
||||
stack = j;
|
||||
sector_found = true;
|
||||
for (uint8_t layer=PROXIMITY_MIDDLE_LAYER; layer<PROXIMITY_NUM_LAYERS; layer++) {
|
||||
for (uint8_t sector=0; sector<PROXIMITY_NUM_SECTORS; sector++) {
|
||||
if (_distance_valid[layer][sector]) {
|
||||
if (!closest_found || (_distance[layer][sector] < _distance[closest_layer][closest_sector])) {
|
||||
closest_layer = layer;
|
||||
closest_sector = sector;
|
||||
closest_found = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (sector_found) {
|
||||
angle_deg = _angle[sector][stack];
|
||||
distance = _distance[sector][stack];
|
||||
if (closest_found) {
|
||||
angle_deg = _angle[closest_layer][closest_sector];
|
||||
distance = _distance[closest_layer][closest_sector];
|
||||
}
|
||||
return sector_found;
|
||||
return closest_found;
|
||||
}
|
||||
|
||||
// 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
|
||||
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]) {
|
||||
angle_deg = _angle[object_number][PROXIMITY_MIDDLE_LAYER];
|
||||
distance = _distance[object_number][PROXIMITY_MIDDLE_LAYER];
|
||||
if ((object_number < PROXIMITY_NUM_SECTORS) && _distance_valid[PROXIMITY_MIDDLE_LAYER][object_number]) {
|
||||
angle_deg = _angle[PROXIMITY_MIDDLE_LAYER][object_number];
|
||||
distance = _distance[PROXIMITY_MIDDLE_LAYER][object_number];
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
|
||||
@ -16,13 +16,12 @@
|
||||
#pragma once
|
||||
|
||||
#define PROXIMITY_NUM_SECTORS 8 // number of sectors
|
||||
#define PROXIMITY_NUM_LAYERS 5 // num of stacks in a sector
|
||||
#define PROXIMITY_MIDDLE_LAYER 2 // middle stack
|
||||
#define PROXIMITY_PITCH_WIDTH_DEG 30 // width between each stack in degrees
|
||||
#define PROXIMITY_NUM_LAYERS 5 // num of layers in a sector
|
||||
#define PROXIMITY_MIDDLE_LAYER 2 // middle layer
|
||||
#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_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 DISTANCE_MAX 999999.0f // arbritary "large" distance
|
||||
|
||||
class AP_Proximity_Boundary_3D
|
||||
{
|
||||
@ -30,69 +29,75 @@ public:
|
||||
// constructor. This incorporates initialisation as well.
|
||||
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
|
||||
class Boundary_Location
|
||||
{
|
||||
// stores the layer and sector as a single object to access and modify the 3-D boundary
|
||||
class Face
|
||||
{
|
||||
public:
|
||||
// constructor when both stack and sector are passed
|
||||
Boundary_Location(uint8_t Sector, uint8_t Stack) { sector = Sector; stack = Stack; }
|
||||
// constructor defaults to "middle(horizontal) layer" if only sector is passed
|
||||
Boundary_Location(uint8_t Sector) { sector = Sector; stack = PROXIMITY_MIDDLE_LAYER; }
|
||||
|
||||
uint8_t stack; // vertical "steps" on the 3D Boundary
|
||||
|
||||
// constructor, invalidate id and distance
|
||||
Face() { layer = sector = UINT8_MAX; }
|
||||
Face(uint8_t _layer, uint8_t _sector) { layer = _layer; sector = _sector; }
|
||||
|
||||
// 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
|
||||
};
|
||||
|
||||
// returns Boundary_Location object consisting of appropriate stack and sector
|
||||
// corresponding to the yaw and pitch.
|
||||
// Pitch defaults to zero if only yaw is passed to this method
|
||||
// Yaw is the horizontal body-frame angle the detected object makes with the vehicle
|
||||
// Pitch is the vertical body-frame angle the detected object makes with the vehicle
|
||||
Boundary_Location get_sector(float yaw, float pitch = 0.0f);
|
||||
};
|
||||
|
||||
// returns face corresponding to the provided yaw and (optionally) pitch
|
||||
// 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)
|
||||
Face get_face(float pitch, float yaw) const;
|
||||
Face get_face(float yaw) const { return get_face(0, yaw); }
|
||||
|
||||
// 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
|
||||
// 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); }
|
||||
// Assume detected obstacle is horizontal (zero pitch), if no pitch is passed
|
||||
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); }
|
||||
|
||||
// update boundary points used for object avoidance based on a single sector and pitch distance changing
|
||||
// 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 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 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.
|
||||
void reset_sector(const Boundary_Location& bnd_loc);
|
||||
// Reset all horizontal sectors
|
||||
void reset_all_horizontal_sectors();
|
||||
// Reset all stacks and sectors
|
||||
void reset_all_sectors_and_stacks();
|
||||
void reset_face(Face face);
|
||||
|
||||
// get distance for a face. returns true on success and fills in distance argument with distance in meters
|
||||
bool get_distance(Face face, float &distance) const;
|
||||
|
||||
// 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
|
||||
// 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();
|
||||
|
||||
// WARNING: This requires get_obstacle_count() to be called before calling this method
|
||||
// Appropriate stack 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
|
||||
// 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 layer
|
||||
// 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;
|
||||
|
||||
// WARNING: This requires get_obstacle_count() to be called before calling this method
|
||||
// Appropriate stack 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
|
||||
// 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 layer
|
||||
// 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"
|
||||
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};
|
||||
|
||||
private:
|
||||
|
||||
// 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
|
||||
// This is packed into a Boundary Location object and returned
|
||||
Boundary_Location convert_obstacle_num_to_boundary_loc(uint8_t obstacle_num) const;
|
||||
// Converts obstacle_num passed from avoidance library into appropriate face
|
||||
Face convert_obstacle_num_to_face(uint8_t obstacle_num) const;
|
||||
|
||||
Vector3f _sector_edge_vector[PROXIMITY_NUM_SECTORS][PROXIMITY_NUM_LAYERS];
|
||||
Vector3f _boundary_points[PROXIMITY_NUM_SECTORS][PROXIMITY_NUM_LAYERS];
|
||||
|
||||
// sensor data
|
||||
float _angle[PROXIMITY_NUM_SECTORS][PROXIMITY_NUM_LAYERS]; // angle to closest object within each sector and stack
|
||||
float _pitch[PROXIMITY_NUM_SECTORS][PROXIMITY_NUM_LAYERS]; // pitch to the closest object within each sector and stack
|
||||
float _distance[PROXIMITY_NUM_SECTORS][PROXIMITY_NUM_LAYERS]; // distance to closest object within each sector and stack
|
||||
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
|
||||
Vector3f _sector_edge_vector[PROXIMITY_NUM_LAYERS][PROXIMITY_NUM_SECTORS];
|
||||
Vector3f _boundary_points[PROXIMITY_NUM_LAYERS][PROXIMITY_NUM_SECTORS];
|
||||
|
||||
float _angle[PROXIMITY_NUM_LAYERS][PROXIMITY_NUM_SECTORS]; // yaw angle in degrees to closest object within each sector and layer
|
||||
float _pitch[PROXIMITY_NUM_LAYERS][PROXIMITY_NUM_SECTORS]; // pitch angle in degrees to the closest object within each sector and layer
|
||||
float _distance[PROXIMITY_NUM_LAYERS][PROXIMITY_NUM_SECTORS]; // distance to closest object within each sector and layer
|
||||
bool _distance_valid[PROXIMITY_NUM_LAYERS][PROXIMITY_NUM_SECTORS]; // true if a valid distance received for each sector and layer
|
||||
bool _active_layer[PROXIMITY_NUM_LAYERS]; // layers which have at least one valid distance are marked true
|
||||
};
|
||||
|
||||
typedef AP_Proximity_Boundary_3D::Boundary_Location boundary_location;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user