SENS: SF45: Improved Datahandling and fixes

[Claudio-Chies]

Changes:

1. Moved to the MAV_SENSOR_ORIENTATION ENUM to be consistent with the orientations used in DistanceSensor.msg

2. Removed the publishing to distance_sensor.

3. The data in each bin is now the smallest measured value, and not the last measured value.

4. Added obstacle_distance_fused and distance_sensor to the logged vision and avoidance topics

Changelog Entry

For release notes:

Bugfix SF45 rotary lidar driver for Collision Prevention
Documentation: docs.px4.io

Alternatives

for the distance sensor, we could also calculate a quaternion for each measurement, and then set the orientation quaternion, via that. Then we could remove the obstacle_distance publishing.
This would add some complexity in the driver here, but offload all data handling to Collision prevention.
As the obstacle_distance publishing was already in place, i opted to stay with this approach.

Relevant PR's

• added sf45 as a seperate rangefinder PX4-user_guide#3448

Test coverage

• Tested on Hardware (Flightreview) please note here an offset was hardcoded which is not necessary, it just aligns the array differently for easier debugging

[Claudio-Chies]

@dirksavage88 Would be nice if you could also do a quick review.

[dirksavage88]

This should be non-zero. There has to be some offset since the obstacle distance array element 0 starts and moves clockwise, however with an increment of 10 degrees there won't be coverage immediately to the left of the actual obstacle distance data

[Claudio-Chies]

It's a bit of a question on the definition of the angle offset.
as the original algorithm was the Vector Field Histogram, I assumed that the angle was also defined the same way, meaning the corresponding angle is in the centre of the bin.

Which would also make sense as on 1D distance sensor pointed to the front with a fov of zero, gets only mapped to the bin with index zero.

I mean, we can agree on a definition here, and then also properly document it in the message.

[Claudio-Chies]

I did a quick test where I set the fov of a 1D sensor pointing to the front to be 5 deg.

and then the adjacent bins are empty and only the bin with index 0 is populated. Which would support the statement above.

[dirksavage88]

Maybe a better test of this is a 1D sensor 20 degree field of view sensor forward facing. If 20 degrees is split down the middle would the bins occupied be 0, 1 and 71, 72? (assuming 5 degree increment).

I guess this could be changed and defined to include that wrapping effect: the angle offset is super non-intuitive to most users, specifically to those implementing new drivers where there is sparse info on the sign of the angle and what it means.

It doesn't make a huge difference on the rotating lidars since the degree resolution is small, but would make a difference on 1D lidars with a significant field of view.

[dirksavage88]

There's some confusion as to how the resolution of collision prevention can be used. I think in collision prevention code it's assumed to be an increment of 10, however the obstacle distance array can support an increment set to a minimum of 5 deg for the "resolution", which works out to 360 degree resolution coverage wrapped in 72 bins (should be default imo).

This was probably done this way since the original collision prevention impl leveraged point-lidars and the point-sample resolution was in the 10's of degrees: https://discuss.px4.io/t/question-on-distances-array-in-obstacle-distance/12911

In the case of the SF45: https://support.lightware.co.za/sf45b/#/specs

TLDR; setting it to 10 is not taking full advantage of the sensor's resolution, and we should probably change it in collision prevention as well

[Claudio-Chies]

Yea, as we discussed in the CP PR, let's switch here to 5° Degrees.
I also discussed with Matthias yesterday that if computation is not the limitation, it should also be possible to decrease the internal resolution to 1 deg, then we could publish distance_sensor messages with orientation set via the quaternion which would result in even better resolution. Maybe something to consider in the future if there is need for that.

[dirksavage88]

We can remove the "/* SF45/B (50M) */" comment now that the rangefinder min/max values are no longer relevant

[dirksavage88]

I never actually had the chance to test these on the bench. The non-standard Yaw cfg as well.