Sonar hcsr04 (rebase)

This PR rebases #612 onto next as well as fixes the issues with the original PR.

@scenkov: could you please retest this on you FlyingF3?