This is a similar point to below. Sonar measures range, not altitude. In order to convert the range into altitude, the sensor's angle w.r.t. ground needs to be accounted for. This is a combination of two factors: mounting angle and vehicle attitude. So to do this properly, 

1) We would need new UAVO fields for sonar settings, which would include only the RPY mounting angle. According to our naming conventions this should go in the SensorSettings UAVO.
2) We would need the rotations to be applied. Since the sensor is in its own frame, this is just the application of two successive rotations. This is identical to what we do with the inertial sensors when first rotating them from board to body and then from body to inertial. At this point, only the third member of the array, altitude, could be extracted; or we could even provide a 3D solution, which IMHO would be more useful and would only cost 8 extra bytes of memory. 

@solidgoldbomb Sorry, I should have noticed this earlier. I realize that this is a little outside what you were meaning to do by refactoring this PR. If you want some help with the math, let me know.