Added test and function to convert from AGL to MSL