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 your FlyingF3?
We measure the distance "height_in" and then why there is this "height_out"?