Added clamping to altitude update code (fixes #22)