There are three parts presented in this thesis: derivation of quadrotor’s equation of motion and linearization, displacement controller design. In displacement controller design, used PD controller to control whole system. Finally, the open source flight control program in APM flight control board is rewritten to implement the displacement controller.