示例#1
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

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],
示例#2
0
    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