Basic sonar handling

Not 100% happy with it, but this will have to do.
Calibration and control!

Whuuuut, this thing is flying pretty nicely now.