# Set up the NMPC reference trajectory using correct interpolation.
for i in xrange(0, nmpc.HORIZON_LENGTH):
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))
position_offset = [0, 0, 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]]
position_offset = [0, 0, 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
