Beispiel #1
0
    horizon_point = [a for a in interpolate_reference(
        i*nmpc.STEP_LENGTH, xplane_reference_points)]
    horizon_point.extend([15000, 0, 0])
    nmpc.set_reference(horizon_point[1:], i)

# Set up terminal reference. No need for control values as they'd be ignored.
terminal_point = [a for a in interpolate_reference(
    nmpc.HORIZON_LENGTH*nmpc.STEP_LENGTH, xplane_reference_points)]
nmpc.set_reference(terminal_point[1:], nmpc.HORIZON_LENGTH)

initial_point = interpolate_reference(0, xplane_reference_points)
_cnmpc.nmpc_fixedwingdynamics_set_position(*initial_point[1:4])
_cnmpc.nmpc_fixedwingdynamics_set_velocity(*initial_point[4:7])
_cnmpc.nmpc_fixedwingdynamics_set_attitude(
    initial_point[10],
    initial_point[7],
    initial_point[8],
    initial_point[9])
_cnmpc.nmpc_fixedwingdynamics_set_angular_velocity(*initial_point[11:14])
_cnmpc.nmpc_fixedwingdynamics_get_state(state)

for i in xrange(500):
    nmpc.prepare()
    nmpc.solve(
        list(state.position) +
        list(state.velocity) +
        list(state.attitude) +
        list(state.angular_velocity))
    print "%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f" \
        % (i*nmpc.STEP_LENGTH,
            state.position[0],
Beispiel #2
0
time.sleep(1.0)
response = sock.recv(1024)
for line in response.split("\n"):
    if line.find("local_x") >= 0:
        position_offset[1] = float(line.split(" ")[-1])
    elif line.find("local_y") >= 0:
        position_offset[2] = -float(line.split(" ")[-1])
    elif line.find("local_z") >= 0:
        position_offset[0] = -float(line.split(" ")[-1])

print position_offset

TIMESTEP = 1.0/500.0  # 50Hz updates.
_cnmpc.nmpc_fixedwingdynamics_set_position(0, 0, 0)
_cnmpc.nmpc_fixedwingdynamics_set_velocity(20, 0, 0)
_cnmpc.nmpc_fixedwingdynamics_set_attitude(1, 0, 0, 0)
_cnmpc.nmpc_fixedwingdynamics_set_angular_velocity(0, 0, 0)
_cnmpc.nmpc_fixedwingdynamics_get_state(state)

control_vec = [0, 0, 0]

while 1:
    keys = pygame.event.get()
    axes = [joystick.get_axis(i) for i in range(joystick.get_numaxes())]
    axes[0] = -0.5*(axes[0]+0.239)
    axes[1] = -0.5*(axes[1]+0.239)
    control_vec = [
        (-(axes[3] - 0.35) * 0.8) * 19000,
        axes[1] - axes[0],
        axes[1] + axes[0]]
Beispiel #3
0
time.sleep(1.0)
response = sock.recv(1024)
for line in response.split("\n"):
    if line.find("local_x") >= 0:
        position_offset[1] = float(line.split(" ")[-1])
    elif line.find("local_y") >= 0:
        position_offset[2] = -float(line.split(" ")[-1])
    elif line.find("local_z") >= 0:
        position_offset[0] = -float(line.split(" ")[-1])

print position_offset

TIMESTEP = 1.0 / 500.0  # 50Hz updates.
_cnmpc.nmpc_fixedwingdynamics_set_position(0, 0, 0)
_cnmpc.nmpc_fixedwingdynamics_set_velocity(20, 0, 0)
_cnmpc.nmpc_fixedwingdynamics_set_attitude(1, 0, 0, 0)
_cnmpc.nmpc_fixedwingdynamics_set_angular_velocity(0, 0, 0)
_cnmpc.nmpc_fixedwingdynamics_get_state(state)

control_vec = [0, 0, 0]

while 1:
    keys = pygame.event.get()
    axes = [joystick.get_axis(i) for i in range(joystick.get_numaxes())]
    axes[0] = -0.5 * (axes[0] + 0.239)
    axes[1] = -0.5 * (axes[1] + 0.239)
    control_vec = [(-(axes[3] - 0.35) * 0.8) * 19000, axes[1] - axes[0],
                   axes[1] + axes[0]]

    # print control_vec