Question on distances array in OBSTACLE_DISTANCE

Hello. I am trying to setup collision prevention using distance information from ultrasonic sensors. I have a flight management unit (FMU) running PX4 and a microcontroller sending MAVLink messages to the FMU.

I’ve got a question regarding how I should fill the OBSTACLE_DISTANCE message. I have 6 ultrasonic sensors, and I plan to distribute them evenly around the drone. So I assume increment should be 60. I notice that the array distances is of size 72 but I only have 6 sensors. If I put the measurements from ultrasonic sensors in the first 6 elements, what values should I assign to the rest? Would PX4 firmware only consider the first 6 elements of the array and ignore the rest?

Any help will be appreciated.

it depends on the field of view of your sensors, at which angles do you get sensor coverage? And how is the data reported, e.g do you get one value per sensor at each timestep or do you actually get a range scan per sensor (and what would be the resolution of that).

But as an example:
I assume you have 6 sensors with fov 10deg, which all report one value per timestep. sensor 1 is located to the front of the drone and they are evenly spaced.

then you should fill the message as follows:
msg.frame = MAV_FRAME_BODY_FRD
msg.increment = 10 (degrees, resolution)
msg. angle_offset = -5 (degrees to shift, such the the fov of the first sensor is centered to the front of the drone)
msg.distances = [data1, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX
data2, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX,
data3, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX,
data4, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX,
data5, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX,
data6, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX]

put UINT16_MAX where you do not have any sensor coverage. The drone will refuse to fly these directions
msg.

@baumanta Thank you so much for the detailed explanation. I am planning to place 6 sensors evenly around. So assuming the front is at 0deg, they will be at 0deg, 60deg, 120deg, 180deg, 240deg, and 300deg. I will need to poll the 6 sensors and each time step I will one distance value from one sensor.

I actually don’t quite know the fov of each sensor…For ultrasonic sensor, how do I usually obtain that information?

From your description, it seems that we need to provide the array with the number of elements equal (360/resolution), right? In your example, as the resolution is 10, the number of elements is 36. For the rest of the array, I could put any value there, right?

So I would choose the resolution to be the fov of your sensor (but not lower than 5deg, as the message does not support than 72 values). That way you can keep the message as lightweight as possible. The FOV should be somewhere in the sensor specs, otherwise, you’ll have to try and measure it or guess roughly. It is not too crucial probably and should be accurate to a few degrees.

The array values that are not inside the fov of any sensor, you should set to UINT16_MAX. This means that there is no sensor coverage and the vehicle will not move into those directions. Alternatively you could put any number, e.g. 300. That way the autopilot would assume an obstacle 3m form the drone and just limit the speed to be able to break in time.
To conclude, it depends on the behavior you want. If you want an absolutely safe system, put UINT16_MAX, so you can only fly where you have sensor coverage. Other wise it is up to you to tune the max speed in those directions. If you put any high value in there, the vehicle will move full speed where there are no sensors.