added sonar check to change flight mode to loiter