Fix LEAVE_HEADROOM_FOR_MOTORS issue; Adjust failsafe