SF45: Adjust _handle_missed_bins() logic

[mahimayoga]

Solved Problem

When flying with the SF45 rotary lidar, the _handle_missed_bins function in lightware_sf45_serial.cpp can fill bins in the obstacle map that are outside the sensor's FOV. This has negative consequences for the setpoint constraints that are set when flying with CollisionPrevention. (For example: losing ability to move in direction outside of FOV)

Data being added to bins outside the FOV & consequenctly not being able to pitch-up (move backwards):

Solution

When filling the missed bins, we loop thruough the indices always starting from the first bin within the FOV. (see figure). We loop through these indices, but fill the obstacle map at the original bin indices.

This way we can handle cases in which the current_bin (in this illustration bin 6) and previous_bin (in this illustration bin 10) are near the bins which are outside the FOV. It also simplifies the logic such that we don't need to consider the wrap-around case.

Relevant functions added to ObstacleMath library.

Test coverage

• Unit/integration test: Unit tests added in ObstacleMathTest.cpp
• Bench tests:

1. Wrap around case.
   

2. Bin jumps from 24 to 47. Shortest path to fill the missed bins would be through the bins not in the FOV (~ bins 32 - 40). But we can see that now we fill in the opposite direction. (Not all bins shown as it's messy)

• Flight test: https://review.px4.io/plot_app?log=8013463b-fdce-47ac-af60-80c8b461a3d8

[mahimayoga]

@MaEtUgR do you think this is conceptually better logic? If you have a better idea let me know! Otherwise I can flight test it in the next days.

[MaEtUgR]

You're onto something. I have to continue reviewing through, I didn't get through all the logic yet.

[sfuhrer]

Good find, and nice cleanup along the way!

When filling the missed bins, the bin indices are temporarily re-numbered such that the 0th bin corresponds to the first bin which is within the FOV

That confused me for a moment, maybe you could instead just say that you loop from the indices always starting from the first bin within the FOV.

Minor comments. Please use const wherever applicable.

[sfuhrer]

Looks good now. Please clean up the commits into stand-alone functional changes or refactors, then good to go!