Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Proximity: only send distance_sensor messages if valid data seen

Browse Source 
In the case you only have a forward-pointing LIDAR we'd send messages
for each of the other orientations from proximty's horizontal-distances
array, chewing up bandwidth and processing time.
This commit is contained in:
Peter Barker 2021-02-17 19:13:03 +11:00 committed by Randy Mackay
parent fcd35ba0f8
commit 9a66a1564c
2 changed files with 11 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_Proximity/AP_Proximity.h
View File

@ -64,6 +64,12 @@ public:
struct Proximity_Distance_Array {
uint8_t orientation[PROXIMITY_MAX_DIRECTION]; // orientation (i.e. rough direction) of the distance (see MAV_SENSOR_ORIENTATION)
float distance[PROXIMITY_MAX_DIRECTION]; // distance in meters
bool valid(uint8_t offset) const {
// returns true if the distance stored at offset is valid
return (offset < 8 && (offset_valid & (1U<<offset)));
};
uint8_t offset_valid; // bitmask
};
// detect and initialise any available proximity sensors

5
libraries/AP_Proximity/AP_Proximity_Backend.cpp
View File

@ -31,16 +31,21 @@ AP_Proximity_Backend::AP_Proximity_Backend(AP_Proximity &_frontend, AP_Proximity
{
}
static_assert(PROXIMITY_MAX_DIRECTION <= 8,
"get_horizontal_distances assumes 8-bits is enough for validity bitmask");
// get distances in PROXIMITY_MAX_DIRECTION directions horizontally. used for sending distances to ground station
bool AP_Proximity_Backend::get_horizontal_distances(AP_Proximity::Proximity_Distance_Array &prx_dist_array) const
{
// 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;
prx_dist_array.offset_valid = 0;
for (uint8_t i=0; i<PROXIMITY_MAX_DIRECTION; i++) {
prx_dist_array.orientation[i] = i;
const AP_Proximity_Boundary_3D::Face face(PROXIMITY_MIDDLE_LAYER, i);
if (boundary.get_distance(face, prx_dist_array.distance[i])) {
prx_dist_array.offset_valid |= (1U << i);
valid_distances = true;
} else {
prx_dist_array.distance[i] = distance_max();