aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Proximity: simplify get_horizontal_distances

Browse Source 
no need to fill in missing orientations.  This is only used for reporting to the ground stations
This commit is contained in:
Randy Mackay 2020-10-01 16:10:59 +09:00 committed by Andrew Tridgell
parent e081d83185
commit 4381eff482
1 changed files with 7 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
libraries/AP_Proximity/AP_Proximity_Backend.cpp
View File

@ -78,51 +78,20 @@ bool AP_Proximity_Backend::get_object_angle_and_distance(uint8_t object_number,
// get distances in PROXIMITY_MAX_DIRECTION directions. used for sending distances to ground station // get distances in PROXIMITY_MAX_DIRECTION directions. used for sending distances to ground station
bool AP_Proximity_Backend::get_horizontal_distances(AP_Proximity::Proximity_Distance_Array &prx_dist_array) const bool AP_Proximity_Backend::get_horizontal_distances(AP_Proximity::Proximity_Distance_Array &prx_dist_array) const
{ {
// exit immediately if we have no good ranges // cycle through all sectors filling in distances and orientations
// see MAV_SENSOR_ORIENTATION for orientations (0 = forward, 1 = 45 degree clockwise from north, etc)
bool valid_distances = false; bool valid_distances = false;
for (uint8_t i=0; i<PROXIMITY_NUM_SECTORS; i++) {
if (_distance_valid[i]) {
valid_distances = true;
break;
}
}
if (!valid_distances) {
return false;
}
// initialise orientations and directions
// see MAV_SENSOR_ORIENTATION for orientations (0 = forward, 1 = 45 degree clockwise from north, etc)
// distances initialised to maximum distances
bool dist_set[PROXIMITY_MAX_DIRECTION]{};
for (uint8_t i=0; i<PROXIMITY_MAX_DIRECTION; i++) { for (uint8_t i=0; i<PROXIMITY_MAX_DIRECTION; i++) {
prx_dist_array.orientation[i] = i; prx_dist_array.orientation[i] = i;
prx_dist_array.distance[i] = distance_max();
}
// cycle through all sectors filling in distances
for (uint8_t i=0; i<PROXIMITY_NUM_SECTORS; i++) {
if (_distance_valid[i]) { if (_distance_valid[i]) {
// convert angle to orientation valid_distances = true;
int16_t orientation = static_cast<int16_t>((_angle[i]+PROXIMITY_SECTOR_WIDTH_DEG*0.5) * (PROXIMITY_MAX_DIRECTION / 360.0f)); prx_dist_array.distance[i] = _distance[i];
orientation %= PROXIMITY_MAX_DIRECTION; } else {
if ((orientation >= 0) && (orientation < PROXIMITY_MAX_DIRECTION) && (_distance[i] < prx_dist_array.distance[orientation])) { prx_dist_array.distance[i] = distance_max();
prx_dist_array.distance[orientation] = _distance[i];
dist_set[orientation] = true;
}
} }
} }
// fill in missing orientations with average of adjacent orientations if necessary and possible return valid_distances;
for (uint8_t i=0; i<PROXIMITY_MAX_DIRECTION; i++) {
if (!dist_set[i]) {
uint8_t orient_before = (i==0) ? (PROXIMITY_MAX_DIRECTION - 1) : (i-1);
uint8_t orient_after = (i==(PROXIMITY_MAX_DIRECTION - 1)) ? 0 : (i+1);
if (dist_set[orient_before] && dist_set[orient_after]) {
prx_dist_array.distance[i] = (prx_dist_array.distance[orient_before] + prx_dist_array.distance[orient_after]) / 2.0f;
}
}
}
return true;
} }
// get boundary points around vehicle for use by avoidance // get boundary points around vehicle for use by avoidance