quadrocopter.py 文件源码

def _circle_reference(state,
                      time,
                      finished,
                      radius=None,
                      speed=None,
                      init_angle=None,
                      z_vel=None):
    ref = StateVector()
    angle = init_angle + speed / radius * time

    ref.pos = array([radius * cos(angle),
                     radius * sin(angle),
                     z_vel * time])
    ref.vel[:] = [-speed * sin(angle), speed * cos(angle), z_vel]
    ref.euler[2] = pi + np.arctan2(state.pos[1], state.pos[0])
    # reference.omega_b[2] = speed / radius
    return ref


# private stationary reference function