SITL bug fixes