TODO: Adopt to new SITL