Add altitude hold debugging value
Add functions for calculating accelerometer height