def Basic_Controller():
    global states, euler_max, max_yaw_rate, pub_ctrl,K,traj

    Ctrl        = Controls()
    Ctrl.Obj    = [Control()]*1
    Ctrl.header.stamp = states.header.stamp
    g = 9.80665 # average value of earth's gravitational constant m/s^2
    m = 1.282 #  IRIS mass in kg

        #===================================#
        #    Get State Trajectory Errors    #
        #===================================#

    if states.Obj[0].visible:
        X = np.asmatrix(np.zeros((7,1)))
        X[0] = traj.Obj[0].x-states.Obj[0].x
        X[1] = traj.Obj[0].y-states.Obj[0].y
        X[2] = traj.Obj[0].z-states.Obj[0].z
        X[3] = traj.Obj[0].xdot-states.Obj[0].u
        X[4] = traj.Obj[0].ydot-states.Obj[0].v
        X[5] = traj.Obj[0].zdot-states.Obj[0].w
        X[6] = traj.Obj[0].psi-states.Obj[0].psi*np.pi/180

        #============================================#
        #     Differential Flatness Control Input    #
        #============================================#

        # LQR input
        utilde = -K*X
        # required input
        u_r = np.asmatrix(np.zeros((4,1)))
        u = utilde+u_r-np.matrix([[0],[0],[9.81],[0]])

        #==================================#
        #     Rotate to Vehicle 1 Frame    #
        #==================================#

        psi = states.Obj[0].psi*np.pi/180
        rotZ = np.matrix([[cos(psi), sin(psi), 0],[-sin(psi), cos(psi), 0],[0, 0, 1]])
        Cart = np.matrix([[1, 0, 0],[0, -1, 0],[0, 0, -1]])
        u[:-1] = Cart*rotZ*u[:-1]

        #===================================#
        #     Normalize given the Thrust    #
        #===================================#

        T = sqrt(u[0:3].T*u[0:3])
        u[:-1] = np.divide(u[:-1],-T)

        #==================#
        #   Set Controls   #
        #==================#

        # Controls for Ardrone
        # -phi = right... +phi = left
        # -theta = back... +theta = forward
        # -psi = right...  +psi = left
        global phi_trim,theta_trim,phi_scale,theta_scale
        phi_d = (asin(u[1,-1]))
        theta_d = (-asin(u[0,-1]))
        ctrl        = Control()
        ctrl.name   = states.Obj[0].name
        ctrl.phi    = phi_trim + phi_scale*phi_d
        ctrl.theta  = theta_trim + theta_scale*theta_d
        ctrl.psi    = -u[3,-1]/max_yaw_rate
        global nominal_thrust
        T_d = nominal_thrust+(T-g)/g
        ctrl.T      = T_d
        Ctrl.Obj[0] = ctrl
        Ctrl.header = states.header
        #rospy.loginfo("latency = %f",states.header.stamp.to_sec()-rospy.get_time())
        pub_ctrl.publish(Ctrl)

    #===================#
    #       Main        #
    #===================#