readkeys = pandaapp.ReadKeys(ref, 1, panda3D_app)
# The main simulation loop
#########################################################
while readkeys.exitpressed is False:
#------------------------------------begin controller --------------------------------------------
if abs(t / att_controller.dt_ctrl_rate -
round(t / att_controller.dt_ctrl_rate)) < 0.000001:
# Get reference
if (args.ref_mode == "manual"):
omegab_ref = readkeys.ref[1:4]
else:
omegab_ref[0] = utils.give_signal(omegab_x_ref, t)
omegab_ref[1] = utils.give_signal(omegab_y_ref, t)
omegab_ref[2] = utils.give_signal(omegab_z_ref, t)
if (t > max(omegab_x_ref[-1, 0], omegab_y_ref[-1, 0],
omegab_z_ref[-1, 0])):
readkeys.exitpressed = True
# thrust comes alwyas from the keyboard
thrust_ref = readkeys.ref[0]
# Get Measurement (perfect measurement)
omegab_meas = qrb.omegab
tau_ref = att_controller.run_rate(omegab_ref, omegab_meas, qrb.I)
# Control allocation; use qftau_s model
panda3D_app = pandaapp.Panda3DApp(plus, qrb, ref, 3)
readkeys = pandaapp.ReadKeys(ref, 3, panda3D_app)
# The main simulation loop
#########################################################
while readkeys.exitpressed is False:
#------------------------------------begin controller --------------------------------------------
if abs(t / pos_controller.dt_ctrl_pos_p -
round(t / pos_controller.dt_ctrl_pos_p)) < 0.000001:
#reference
if (args.ref_mode == "manual"):
pos_ref = readkeys.ref
else:
pos_ref[0] = utils.give_signal(pos_x_ref, t)
pos_ref[1] = utils.give_signal(pos_y_ref, t)
pos_ref[2] = utils.give_signal(pos_z_ref, t)
if (t > max(pos_x_ref[-1, 0], pos_y_ref[-1, 0], pos_z_ref[-1, 0])):
readkeys.exitpressed = True
# perfect measurement
pos_meas = qrb.pos
ve_ref = pos_controller.run_pos(pos_ref, pos_meas)
if abs(t / pos_controller.dt_ctrl_pos_v -
round(t / pos_controller.dt_ctrl_pos_v)) < 0.000001:
ve_meas = qrb.ve
yaw_meas = qrb.rpy[2]
panda3D_app = pandaapp.Panda3DApp(plus, qrb, ref)
readkeys = pandaapp.ReadKeys(ref, 2, panda3D_app)
# The main simulation loop
#########################################################
while readkeys.exitpressed is False:
#------------------------------------begin controller --------------------------------------------
if abs(t / att_controller.dt_ctrl_angle -
round(t / att_controller.dt_ctrl_angle)) < 0.000001:
#reference
if (args.ref_mode == "manual"):
rpy_ref = readkeys.ref[1:4]
else:
rpy_ref[0] = utils.give_signal(roll_ref, t)
rpy_ref[1] = utils.give_signal(pitch_ref, t)
rpy_ref[2] = utils.give_signal(yaw_ref, t)
if (t > max(roll_ref[-1, 0], pitch_ref[-1, 0], yaw_ref[-1, 0])):
readkeys.exitpressed = True
# perfect measurement
meas_rpy = qrb.rpy
# controller call
omegab_ref = att_controller.run_angle(rpy_ref, meas_rpy)
if abs(t / att_controller.dt_ctrl_rate -
round(t / att_controller.dt_ctrl_rate)) < 0.000001:
# thrust comes alwyas from the keyboard