Control Algorithm for Quadrotor