Added support for Sonar altitude hold and PX4FLOW

Code needs to be tested/debugged but all the structure is there. To
test, we will have to work on connecting the hardware.