Fixed PX4 sonar units and added mm resolution support.

The Multiwii originally works in cm resolution (with integers). Some of
the PID code was changed to allow precise PID control up to mm
resolution.