This paper presents an idea for controlling the formation flight of unmanned aerial vehicles. Formation flight control with a leader–follower configuration based on position and velocity errors has proven to be problematic, especially during the activation of formation flight mode with large distances between leader and follower, and with large differences in leader and follower heading angles. Proposed formation flight control scheme is composed of two stages – guidance on a leader and precise leader following. Synthesis of the two stage switching controller is described, implemented and presented. Proposed control system was checked and verified during simulation tests. Leader and follower flight parameters were presented and analyzed. Proposed control algorithm improves formation flight organization.