AP_TECS : modifed to use inertial nav height and vertical velocity if available