measured_state["x"],
measured_state["y"],
measured_state["z"],
measured_state["vx"],
measured_state["vy"],
measured_state["vz"],
measured_state["qx"],
measured_state["qy"],
measured_state["qz"],
measured_state["qw"],
measured_state["wx"],
measured_state["wy"],
measured_state["wz"]]
print state
nmpc.solve(state)
control_vec = nmpc.get_controls()
print ("t: %.2f " % (i*nmpc.STEP_LENGTH)) + repr(control_vec)
update += "set sim/flightmodel/engine/ENGN_thro_use [%.6f,0,0,0,0,0,0,0]\n" % control_vec[0]
update += "set sim/flightmodel/controls/wing1l_ail1def %.6f\n" % math.degrees(control_vec[1] - 0.5)
update += "set sim/flightmodel/controls/wing1r_ail1def %.6f\n" % math.degrees(control_vec[2] - 0.5)
# Add one to the index because of the terminal point.
horizon_point = [a for a in interpolate_reference(
(i+1+nmpc.HORIZON_LENGTH)*nmpc.STEP_LENGTH, xplane_reference_points)]
horizon_point.extend([0.5, 0.5, 0.5])
nmpc.update_horizon(horizon_point[1:])
sock.sendall(update)
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],
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)]
