总的来讲就是算两个相邻扇区的最小距离,输出一个二维边界点,通过sin /cos × shortdistances 算出 。
// update boundary points used for object avoidance 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
void AP_Proximity_Backend::update_boundary_for_sector(uint8_t sector)
{
// 若是扇区大于最大扇区数量则直接退出
if (sector >= _num_sectors) {
return;
}
// find adjacent sector (clockwise)
uint8_t next_sector = sector + 1;
if (next_sector >= _num_sectors) {
next_sector = 0;
}
// 得到两个有效扇区之间的最小距离
float shortest_distance = PROXIMITY_BOUNDARY_DIST_DEFAULT;
if (_distance_valid[sector] && _distance_valid[next_sector]) {
shortest_distance = MIN(_distance[sector], _distance[next_sector]);
} else if (_distance_valid[sector]) {
shortest_distance = _distance[sector];
} else if (_distance_valid[next_sector]) {
shortest_distance = _distance[next_sector];
}
if (shortest_distance < PROXIMITY_BOUNDARY_DIST_MIN) {
shortest_distance = PROXIMITY_BOUNDARY_DIST_MIN;
}
_boundary_point[sector] = _sector_edge_vector[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]) {
_boundary_point[next_sector] = _sector_edge_vector[next_sector] * shortest_distance;
}
// 上一时刻的扇区和现在的扇区的比较得出最小的
uint8_t prev_sector = (sector == 0) ? _num_sectors-1 : sector-1;
shortest_distance = PROXIMITY_BOUNDARY_DIST_DEFAULT;
if (_distance_valid[prev_sector] && _distance_valid[sector]) {
shortest_distance = MIN(_distance[prev_sector], _distance[sector]);
} else if (_distance_valid[prev_sector]) {
shortest_distance = _distance[prev_sector];
} else if (_distance_valid[sector]) {
shortest_distance = _distance[sector];
}
_boundary_point[prev_sector] = _sector_edge_vector[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) ? _num_sectors-1 : prev_sector-1;
if (!_distance_valid[prev_sector_ccw]) {
_boundary_point[prev_sector_ccw] = _sector_edge_vector[prev_sector_ccw] * shortest_distance;
}
}