def objective(params):
cost = 0
for env in environment:
states = compute_states(init_state,
params,
env,
plane_specs,
time_step=time_step)
for i in range(6, len(states), 6):
px, py, v, h, a, w = states[i:i + 6]
cost += centerline_weight * np.abs(
signed_rejection_dist(center_line, px, py))
cost += velocity_weight * np.abs(desired_v - v)
cost += heading_weight * np.abs(desired_h - h)
return cost / len(environment)
def apply_takeoff_controls():
# set wind environment
wind_speed, wind_heading = set_winds()
throttle_controller = PID(2.0, 0.0, 1.0, 10.0, sample_time)
rudder_controller = PID(0.3, 0.4, 1.5, 10.0, sample_time)
read_drefs = XPlaneDefs.control_dref + XPlaneDefs.position_dref
maximum_cle = 0
controls = None
start = time.time()
for t in range(int(simulation_steps // receding_horizon)):
gs, psi, throttle, x, _, z = xp_client.getDREFs(read_drefs)
gs, psi, throttle, x, z = gs[0], psi[0], throttle[0], x[0], z[0]
if TAKEOFF:
break
# cle = rejection_dist(desired_x, desired_z, x - origin_x, z - origin_z)
# maximum_cle = max(cld, maximum_cle)
dist = signed_rejection_dist(center_line, x - origin_x, z - origin_z)
dist = confine_bins(dist, c_bins)
psi = confine_bins(psi - runway_heading, h_bins)
gs = confine_bins(gs, v_bins)
wind_speed = confine_bins(wind_speed, wsp_bins)
wind_heading = confine_bins(wind_heading, whe_bins)
controls, cost = table[(dist, psi, gs, wind_speed, wind_heading)]
# change wind conditions every second
if time.time() - start > 1:
wind_speed, wind_heading = set_winds()
start = time.time()
# let PID controller take over
apply_lookup_controls(xp_client, throttle_controller,
rudder_controller, controls, sample_time,
receding_horizon)
return maximum_cle
def kalman_solver(client, queues, kalman, init_states, config, runway,
center_line):
controls_queue, conditions_queue, states_queue, data_queue, stop_queue = queues
run_time = config['satisfy_steps']
abort_time = config['abort_steps']
time_step, sim_size = config['time_step'], config['simulation_size']
receding_horizon = config['receding_horizon']
terminal_velocity = config['terminal_velocity'] + 20
plane_specs = [
config['plane_cs'], config['plane_mass'], config['plane_half_length']
]
acceleration_constraint = config['acceleration_constraint']
turning_constraint = config['turning_constraint']
solver_constraints = (acceleration_constraint, turning_constraint)
runway_heading = runway['runway_heading']
end_x, end_z = runway['terminate_X'], runway['terminate_Z']
origin_x, origin_z = runway['origin_X'], runway['origin_Z']
desired_states = [runway_heading, terminal_velocity]
winds = [[0, 0]]
if not conditions_queue.empty():
winds = conditions_queue.get()
controls, cost = solve_states2(init_states,
desired_states,
center_line,
winds,
plane_specs,
solver_constraints,
time_step=time_step,
sim_time=sim_size)
c_controls = [[c[0], c[1]] for c in controls]
accs = [c[2] for c in controls]
omegas = [c[3] for c in controls]
controls_queue.put(c_controls)
need_abort_takeoff = True
breaks_set = False
start, next_input = time.time(), time.time()
while time.time() - start < run_time + abort_time and need_abort_takeoff:
pred_heading = init_states[-1]
for i in range(int(receding_horizon / time_step)):
ax, az = find_kalman_controls(accs[i], pred_heading, omegas[i],
winds, plane_specs, time_step)
kalman.predict(np.array([ax, az]))
_, _, pred_ground_speed, pred_heading = kalman.get_state()
x, z, pred_ground_speed, pred_heading = kalman.get_state()
# print("----------------------------------------------")
# print("KALMAN PREDICTION:")
# print("X, Z: {}, {}".format(x, z))
# print("ground speed, heading: {}, {}".format(pred_ground_speed, pred_heading))
# print("----------------------------------------------\n")
dist = signed_rejection_dist(center_line, x, z)
init_states = [dist, pred_ground_speed, pred_heading]
if not conditions_queue.empty():
winds = conditions_queue.get()
controls, cost = solve_states2(init_states,
desired_states,
center_line,
winds,
plane_specs,
solver_constraints,
time_step=time_step,
sim_time=sim_size)
c_controls = [[c[0], c[1]] for c in controls]
accs = [c[2] for c in controls]
omegas = [c[3] for c in controls]
time.sleep(max(0, receding_horizon - (time.time() - next_input)))
controls_queue.put(c_controls)
while not states_queue.empty():
measurement = states_queue.get()
x, z, vx, vz = measurement
me_gs = math.sqrt(vx**2 + vz**2)
me_head = math.degrees(math.atan(vz / vx)) % 360 - 270
# print("----------------------------------------------")
# print("MEASUREMENTS:")
# print("X, Z: {}, {}".format(x, z))
# print("ground speed, heading: {}, {}".format(me_gs, me_head))
# print("----------------------------------------------\n")
kalman.update(measurement)
check_dist = abs(signed_rejection_dist(center_line, x, z))
check_heading = abs(runway_heading - me_head)
check_velocity = me_gs - terminal_velocity
data_queue.put((time.time() - start, cost, me_head, me_gs, x, z,
winds[0][0], winds[0][1]))
if check_dist < 3 and check_heading < 2 and check_velocity > -20:
# takeoff presumably successful
print("Takeoff success with conditions:")
print(
f"groundspeed: {me_gs}, heading: {me_head}, cte: {check_dist}")
need_abort_takeoff = False
break
if time.time() - start > run_time:
# set abort takeoff
# set parking break when velocity is low
if not breaks_set:
print("Aborting takeoff, conditions not satisfied")
print(
f"groundspeed: {me_gs}, heading: {me_head}, cte: {check_dist}"
)
breaks_set = True
client.sendDREF(XPlaneDefs.park_dref, 1)
if abs(me_gs) < 0.3:
break
desired_states = [runway_heading, 0]
next_input = time.time()
if need_abort_takeoff:
data_queue.put(0)
else:
# takeoff should be successful
data_queue.put(1)
stop_queue.put(1)
choose_num_samples = [0, 5, 8]
takeoff_successes = [0, 0, 0]
log = open("sample_0.csv", "w")
log.write("sample_no,data,prediction\n")
log2 = open("sample_5.csv", "w")
log2.write("sample_no,data,prediction\n")
log3 = open("sample_8.csv", "w")
log3.write("sample_no,data,prediction\n")
logs = [log, log2, log3]
for i in range(no_sim_runs):
curr_log = logs[i % 3]
num_samples = choose_num_samples[i % 3]
gs, psi, x, z = setup(xp_client, runway, init_phi, init_theta,
fuel_tank)
dist = signed_rejection_dist(center_line, x - origin_x, z - origin_z)
init_states = [dist, gs, psi]
vx, vz = find_kalman_controls(gs, psi)
kf.reset()
kf.set_state(np.array([x - origin_x, z - origin_z, vx, vz]))
sim_results, takeoff_decision = run_controller()
print(
f"({num_samples}) Simulation number: {i+1} with results: {takeoff_decision}"
)
takeoff_successes[i % 3] += takeoff_decision
print(f"Number of successful takeoffs: {takeoff_successes[i%3]}")
curr_log.write(f"{i},{sim_results},{takeoff_decision}\n")
log.close()
log2.close()
log3.close()
time.sleep(1)
controls = None
throttle_controller = PID(2.0, 0.0, 1.0, 10.0, sample_time)
rudder_controller = PID(0.3, 0.4, 1.5, 10.0, sample_time)
read_drefs = XPlaneDefs.control_dref + XPlaneDefs.position_dref
winds = [[0, runway_heading]]
cl = np.array([desired_x, desired_z])
for t in range(50):
gs, psi, throttle, x, _, z = xp_client.getDREFs(read_drefs)
gs, psi, throttle, x, z = gs[0], psi[0], throttle[0], x[0], z[0]
desired_states = [runway_heading, desired_velocity]
dist = signed_rejection_dist(cl, x - origin_x, z - origin_z)
init_states = [dist, gs, psi]
controls, _ = solve_states(init_states,
desired_states,
cl,
winds,
plane_specs,
acceleration_constraint,
turning_constraint,
time_step=time_step,
sim_time=num_steps)
controls = [[c[0], c[1]] for c in controls]
apply_controls(xp_client, throttle_controller, rudder_controller, controls,
sample_time, time_step, receding_horizon)