Fixed error: Publish the vtol status message inside the main loop, not ouside