_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],
state.position[1],
state.position[2],
state.attitude[0],
state.attitude[1],
state.attitude[2],
state.attitude[3])
control_vec = nmpc.get_controls()
nmpc.integrate(nmpc.STEP_LENGTH, (ctypes.c_double * 3)(*control_vec))
horizon_point = [a for a in interpolate_reference(
(i+nmpc.HORIZON_LENGTH)*nmpc.STEP_LENGTH, xplane_reference_points)]
horizon_point.extend([15000, 0, 0])
nmpc.update_horizon(horizon_point[1:])
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
nmpc.integrate(TIMESTEP, (ctypes.c_double * 3)(*control_vec))
update = ""
update += "set sim/flightmodel/position/local_x %.9f\n" \
% (state.position[1] + position_offset[1])
update += "set sim/flightmodel/position/local_y %.9f\n" \
% -(state.position[2] + position_offset[2])
update += "set sim/flightmodel/position/local_z %.9f\n" \
% -(state.position[0] + position_offset[0])
print repr(state)
q = (state.attitude[3], -state.attitude[0], -state.attitude[1], -state.attitude[2])
yaw = math.atan2(2.0 * (q[0] * q[3] + q[1] * q[2]), 1.0 - 2.0 * (q[2] ** 2.0 + q[3] ** 2.0))
pitch = math.asin(2.0 * (q[0] * q[2] - q[3] * q[1]))
roll = math.atan2(2.0 * (q[0] * q[1] + q[2] * q[3]), 1.0 - 2.0 * (q[1] ** 2.0 + q[2] ** 2.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
nmpc.integrate(TIMESTEP, (ctypes.c_double * 3)(*control_vec))
update = ""
update += "set sim/flightmodel/position/local_x %.9f\n" \
% (state.position[1] + position_offset[1])
update += "set sim/flightmodel/position/local_y %.9f\n" \
% -(state.position[2] + position_offset[2])
update += "set sim/flightmodel/position/local_z %.9f\n" \
% -(state.position[0] + position_offset[0])
print repr(state)
q = (state.attitude[3], -state.attitude[0], -state.attitude[1],
-state.attitude[2])
yaw = math.atan2(2.0 * (q[0] * q[3] + q[1] * q[2]),
1.0 - 2.0 * (q[2]**2.0 + q[3]**2.0))
pitch = math.asin(2.0 * (q[0] * q[2] - q[3] * q[1]))
