mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-05 07:28:29 -04:00
AP_Proximity: only send distance_sensor messages if valid data seen
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:
parent
fcd35ba0f8
commit
9a66a1564c
@ -64,6 +64,12 @@ public:
|
|||||||
struct Proximity_Distance_Array {
|
struct Proximity_Distance_Array {
|
||||||
uint8_t orientation[PROXIMITY_MAX_DIRECTION]; // orientation (i.e. rough direction) of the distance (see MAV_SENSOR_ORIENTATION)
|
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
|
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
|
// detect and initialise any available proximity sensors
|
||||||
|
@ -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
|
// 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
|
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
|
// cycle through all sectors filling in distances and orientations
|
||||||
// see MAV_SENSOR_ORIENTATION for orientations (0 = forward, 1 = 45 degree clockwise from north, etc)
|
// see MAV_SENSOR_ORIENTATION for orientations (0 = forward, 1 = 45 degree clockwise from north, etc)
|
||||||
bool valid_distances = false;
|
bool valid_distances = false;
|
||||||
|
prx_dist_array.offset_valid = 0;
|
||||||
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;
|
||||||
const AP_Proximity_Boundary_3D::Face face(PROXIMITY_MIDDLE_LAYER, i);
|
const AP_Proximity_Boundary_3D::Face face(PROXIMITY_MIDDLE_LAYER, i);
|
||||||
if (boundary.get_distance(face, prx_dist_array.distance[i])) {
|
if (boundary.get_distance(face, prx_dist_array.distance[i])) {
|
||||||
|
prx_dist_array.offset_valid |= (1U << i);
|
||||||
valid_distances = true;
|
valid_distances = true;
|
||||||
} else {
|
} else {
|
||||||
prx_dist_array.distance[i] = distance_max();
|
prx_dist_array.distance[i] = distance_max();
|
||||||
|
Loading…
Reference in New Issue
Block a user