diff --git a/libraries/AP_Proximity/AP_Proximity_Boundary_3D.cpp b/libraries/AP_Proximity/AP_Proximity_Boundary_3D.cpp index a84fda1431..fbbef1b561 100644 --- a/libraries/AP_Proximity/AP_Proximity_Boundary_3D.cpp +++ b/libraries/AP_Proximity/AP_Proximity_Boundary_3D.cpp @@ -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; + for (uint8_t sector=0; sector < PROXIMITY_NUM_SECTORS; sector++) { + update_boundary(Face(PROXIMITY_MIDDLE_LAYER, sector)); + } } -// 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() +// reset boundary. marks all distances as invalid +void AP_Proximity_Boundary_3D::reset() { - for (uint8_t i=0; i < PROXIMITY_NUM_SECTORS; i++) { - const Boundary_Location bnd_loc{i}; - 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 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 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) { - 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); - } + 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_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