new_point[8] = q_norm[1]
new_point[9] = q_norm[2]
new_point[10] = q_norm[3]
return new_point
# Load the data
headers = None
initialised = False
initial_time = 0.0
MAX_THROTTLE = 25000.0
nmpc.setup(
state_weights=[1, 1, 1, 1, 1, 1, 1, 1, 1e1, 7e-1, 7e-1, 1e1],
control_weights=[1e-1, 1e3, 1e3],
terminal_weights=[1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1],
upper_control_bound=[1.0, 1.0, 1.0],
lower_control_bound=[0, 0, 0])
nmpc.initialise_horizon()
xplane_reference_points = []
attitude = euler_to_q(0, 0, 0)
out = [
0.0, # t
0.0, 0.0, 0.0, # N, E, D
20.0, 0, 0.0, # N, E, D
attitude[0],
attitude[1],
attitude[2],
attitude[3],
q_norm = q.normalize()
new_point[7] = q_norm[0]
new_point[8] = q_norm[1]
new_point[9] = q_norm[2]
new_point[10] = q_norm[3]
return new_point
# Load the data
headers = None
initialised = False
initial_time = 0.0
nmpc.setup(
state_weights=[1e-1, 1e-1, 1e-1, 1, 1, 1, 1e1, 1e1, 1e1, 1e1, 1e1, 1e1],
control_weights=[1e-10, 1e-2, 1e-2],
terminal_weights=[1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1],
upper_control_bound=[25000, 1.0, 1.0],
lower_control_bound=[0, -1.0, -1.0])
nmpc.initialise_horizon()
xplane_reference_points = []
fields = ["time",
"pos_x", "pos_y", "pos_z"
"vel_n", "vel_e", "vel_d",
"att_x", "att_y", "att_z", "att_w",
"angvel_p", "angvel_q", "angvel_r"]
# Load all the X-Plane data in one go
for line in sys.stdin:
if line.strip() == "":
continue
