SENS: SF45: Scale the measured distance with pitch and roll

[Claudio-Chies]

To prevent oscillations caused by the coupling of pitch and roll, which increase the sensor's measured distance, we account for these effects by introducing scaling based on pitch and roll into the measurements. This adjustment ensures accurate collision prevention.
Issue was solved inside of collision prevention by @mahimayoga in #24107, whereby this implementation is aequivalent.

Pitch

Roll

Solution

Pitch

Roll

Test coverage

• Rough test on Hardware, should be flight tested before Merging.

[bresch]

What about extracting this function to a library so we don't have 2 times the exact same implementation?

[Claudio-Chies]

This would be an option, in the long run, i would move away from publishing the obstacle_distance from the sensor, and just publish each measurement with its orientation set as the quaternion, but for this we need a callback based collision prevention map logic, which it is currently not.

If its worth it for the time inbetween i cant say, there you would probably know better.