Guidance feature for Collision Prevention

[baumanta]

Add guidance feature to the collision prevention library. Enable the algorithm to slightly change the setpoint direction to help navigate in cluttered environments and fly through narrow gaps.

SITL and flight tested

[hamishwillee]

@baumanta Let me know if this needs any docs - when you're ready to submit.

[baumanta]

@mhkabir it is a bit of a long story related to overlapping long and short range sensors. The correction for pitch was quite minimal, so I don't think that will impair the breaking maneuver. But doing the adaption had some negative effects on recognizing valid sensor data. E.g. if the sensor registers no obstacle, it sends a value just above the sensor range. Then we know, we should limit flight speed, such we can break at that distance, but we do not actually have any sensor data. If that sensor overlaps with a longer range sensor, we will want to use the data of that one, because the other value is not actually valid. If we now do the pitch adaption and the value gets smaller to the point that is is inside sensor range, we cannot know anymore whether it is an actual obstacle reading or not. We could solve that by using another array stating whether data is valid or not, but I don't think it is worth the effort.

The light green bin which is constantly around 7.9 is the distance sensor, the other bins are filled with camera data. You can see that distance sensor bin goes inside range (which was 7.9m) all the time due to pitch correction even though the raw signal is constantly at 7.9m (no obstacle). If we receive a valid signal (inside range) from a short range sensor, this has priority over the long range sensor. So we were constantly limiting the flight speed to be able to break in 7.9m even though we did not need to.

[mhkabir]

How about we perform the pitch correction in the distance sensor callback, and only if the sensor is within range?

When testing in simulation, I had found that the pitch correction helped greatly when accelerating (pitch down) towards obstacles.

[baumanta]

@mhkabir with distance sensor callback do you refer to the mavlink receiver? that would actually be an option that should work I think, the question is just if that would be ok for all other modules etc. using this data

otherwise, I might be able to put it back in with an if statement as well, such that we just leave the data at max range without correction and only perform the step for close data

[baumanta]

This has been tested extensively and flies quite nice. here a link to some of the flight logs:
https://logs.px4.io/plot_app?log=a4a2c795-3993-4ea5-bd0e-f9272ea7c9ee
https://logs.px4.io/plot_app?log=54a8fe23-5832-44b1-8337-e313cdf8eb11
https://logs.px4.io/plot_app?log=16cf619a-3855-4f0c-838d-e28c7c4f0f6e

[dagar]

Suggested change

	float sp_angle_body_frame = atan2(setpoint_dir(1), setpoint_dir(0));
	float sp_angle_body_frame = atan2f(setpoint_dir(1), setpoint_dir(0));

[jkflying]

Replaced by #13017 due to branch permissions