Added ground avoidance
Fixed odd boundary condition with gimble lock straight up.
DO A BARREL ROLL!