for line in socket_readlines(sock):
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])
except socket.error:
pass
# Set up the NMPC reference trajectory using correct interpolation.
for i in xrange(0, nmpc.HORIZON_LENGTH+1):
horizon_point = [a for a in interpolate_reference(
i*nmpc.STEP_LENGTH, xplane_reference_points)]
horizon_point.extend([0.5, 0.5, 0.5])
nmpc.set_reference(horizon_point[1:], i)
# Set up initial attitude, velocity and angular velocity.
initial_point = interpolate_reference(0, xplane_reference_points)
update = ""
q = (initial_point[10], -initial_point[7], -initial_point[8], -initial_point[9])
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))
# Need to calculate the X-Plane quaternion to set the heading, pitch and roll
# as used by the physics model.
xplane_q = [0, 0, 0, 1]
psi = yaw / 2.0
theta = pitch / 2.0
phi = roll / 2.0
attitude[2],
attitude[3],
float(data["____P,rad/s"]),
float(data["____Q,rad/s"]),
float(data["____R,rad/s"])]
#print "\t".join(str(o) for o in out)
xplane_reference_points.append(map(float, out))
# 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])