class Animation(object):
def __init__(self, filename, filename_r):
self.filename = filename
self.flight = ReadData(self.filename)
self.flight_data = self.flight.read_data();
self.app = QtGui.QApplication([])
self.win = QtGui.QMainWindow()
self.area = DockArea()
self.win.setCentralWidget(self.area)
self.win.showMaximized()
self.win.setWindowTitle('MPU Simulation')
self.win.setGeometry(10, 25, 1920, 1024)
#Reference Data
self.filename_r = filename_r
self.reference = ReadData(self.filename_r)
self.reference_data = self.reference.read_data();
# Create docks, place them into the window one at a time.
self.d_a = Dock("Animation", size=(500, 400))
self.d_n = Dock("North (Xn)", size=(500, 200))
self.d_e = Dock("East (Yn)", size=(500, 200))
self.d_h = Dock("Height (h)", size=(500, 200))
self.d_v = Dock("Velocity V", size=(500, 200))
self.d_u_ = Dock("Velocity u", size=(500, 200))
self.d_v_ = Dock("Velocity v", size=(500, 200))
self.d_w_ = Dock("Velocity w", size=(500, 200))
self.d_p = Dock("Velocity p", size=(500, 200))
self.d_q = Dock("Velocity q", size=(500, 200))
self.d_r = Dock("Velocity r", size=(500, 200))
self.d_phi = Dock("Roll", size=(500, 200))
self.d_theta = Dock("Pitch", size=(500,200))
self.d_psi = Dock("Yaw", size=(500,200))
self.d_w = Dock("Motors Rotation", size=(500, 200))
self.d_t = Dock("Motors Angle", size=(500,200))
self.d_deltae = Dock("Elevons", size=(500,200))
self.area.addDock(self.d_a, 'left')
self.area.addDock(self.d_n, 'right')
self.area.addDock(self.d_e, 'below', self.d_n)
self.area.addDock(self.d_h, 'below', self.d_e)
self.area.addDock(self.d_v, 'below', self.d_h)
self.area.addDock(self.d_u_, 'below', self.d_v)
self.area.addDock(self.d_v_, 'below', self.d_u_)
self.area.addDock(self.d_w_, 'below', self.d_v_)
self.area.addDock(self.d_phi, 'bottom', self.d_n)
self.area.addDock(self.d_theta, 'below', self.d_phi)
self.area.addDock(self.d_psi, 'below', self.d_theta)
self.area.addDock(self.d_p, 'below', self.d_psi)
self.area.addDock(self.d_q, 'below', self.d_p)
self.area.addDock(self.d_r, 'below', self.d_q)
self.area.addDock(self.d_w, 'bottom', self.d_phi)
self.area.addDock(self.d_t, 'below', self.d_w)
self.area.addDock(self.d_deltae, 'below', self.d_t)
#Animation Window
self.d_a.hideTitleBar()
self.w = gl.GLViewWidget()
self.zgrid = gl.GLGridItem()
self.w.addItem(self.zgrid)
self.w.setWindowTitle('Animation')
self.w.setCameraPosition(distance=20, azimuth=90, elevation=60)
self.x0 = self.flight_data[self.flight.i_north][0]
self.y0 = -self.flight_data[self.flight.i_east][0]
self.h0 = self.flight_data[self.flight.i_height][0]
#Fuselage Model
self.model_fus = Model("./obj_files/MPUFuselage.obj")
self.md_fus = gl.MeshData(vertexes=self.model_fus.returnMesh())
self.fuselage = gl.GLMeshItem(meshdata=self.md_fus, color=(1, 1, 1, 1),
smooth=False, shader='shaded', glOptions='opaque')
self.d_a.addWidget(self.w)
#Waypoint Model
self.model_wayp = Model("./obj_files/wayp.obj")
self.md_wayp = gl.MeshData(vertexes=self.model_wayp.returnMesh())
self.waypoint = gl.GLMeshItem(meshdata=self.md_wayp, color=(1, 1, 1, 1),
smooth=False, shader='shaded', glOptions='opaque')
self.d_a.addWidget(self.w)
#Plots Setup
#North Displacement
self.w_n = pg.PlotWidget(title="North Displacement")
self.w_n.setDownsampling(mode='peak')
self.w_n.setClipToView(True)
self.curve_x = self.w_n.plot()
self.curve_x_r = self.w_n.plot()
self.w_n.setLabel('bottom', 'Time', 's')
self.w_n.setLabel('left', 'Xn', 'm')
self.d_n.addWidget(self.w_n)
#East Displacement
self.w_e = pg.PlotWidget(title="East Displacement")
self.w_e.setDownsampling(mode='peak')
self.w_e.setClipToView(True)
self.curve_y = self.w_e.plot()
self.w_e.setLabel('bottom', 'Time', 's')
self.w_e.setLabel('left', 'Yn', 'm')
self.d_e.addWidget(self.w_e)
#Height
self.w_h = pg.PlotWidget(title="Height Displacement")
self.w_h.setDownsampling(mode='peak')
self.w_h.setClipToView(True)
self.curve_h = self.w_h.plot()
self.w_h.setLabel('bottom', 'Time', 's')
self.w_h.setLabel('left', 'h', 'm')
self.d_h.addWidget(self.w_h)
#Velocity V
self.w_v = pg.PlotWidget(title="Total Velocity")
self.w_v.setDownsampling(mode='peak')
self.w_v.setClipToView(True)
self.curve_V = self.w_v.plot()
self.w_v.setLabel('bottom', 'Time', 's')
self.w_v.setLabel('left', 'V', 'm/s')
self.d_v.addWidget(self.w_v)
#Velocity u
self.w_u_ = pg.PlotWidget(title="Velocity u")
self.w_u_.setDownsampling(mode='peak')
self.w_u_.setClipToView(True)
self.curve_u_ = self.w_u_.plot()
self.w_u_.setLabel('bottom', 'Time', 's')
self.w_u_.setLabel('left', 'u', 'm/s')
self.d_u_.addWidget(self.w_u_)
#Velocity v
self.w_v_ = pg.PlotWidget(title="Velocity v")
self.w_v_.setDownsampling(mode='peak')
self.w_v_.setClipToView(True)
self.curve_v_ = self.w_v_.plot()
self.w_v_.setLabel('bottom', 'Time', 's')
self.w_v_.setLabel('left', 'v', 'm/s')
self.d_v_.addWidget(self.w_v_)
#Velocity w
self.w_w_ = pg.PlotWidget(title="Velocity w")
self.w_w_.setDownsampling(mode='peak')
self.w_w_.setClipToView(True)
self.curve_w_ = self.w_w_.plot()
self.w_w_.setLabel('bottom', 'Time', 's')
self.w_w_.setLabel('left', 'w', 'm/s')
self.d_w_.addWidget(self.w_w_)
#Velocity p
self.w_p = pg.PlotWidget(title="Rotational Velocity p")
self.w_p.setDownsampling(mode='peak')
self.w_p.setClipToView(True)
self.curve_p = self.w_p.plot()
self.w_p.setLabel('bottom', 'Time', 's')
self.w_p.setLabel('left', 'p', 'rad/s')
self.d_p.addWidget(self.w_p)
#Velocity q
self.w_q = pg.PlotWidget(title="Rotational Velocity q")
self.w_q.setDownsampling(mode='peak')
self.w_q.setClipToView(True)
self.curve_q = self.w_q.plot()
self.w_q.setLabel('bottom', 'Time', 's')
self.w_q.setLabel('left', 'q', 'rad/s')
self.d_q.addWidget(self.w_q)
#Velocity q
self.w_r = pg.PlotWidget(title="Rotational Velocity r")
self.w_r.setDownsampling(mode='peak')
self.w_r.setClipToView(True)
self.curve_r = self.w_r.plot()
self.w_r.setLabel('bottom', 'Time', 's')
self.w_r.setLabel('left', 'r', 'rad/s')
self.d_r.addWidget(self.w_r)
#Roll
self.w_phi = pg.PlotWidget(title="Roll Angle")
self.w_phi.setDownsampling(mode='peak')
self.w_phi.setClipToView(True)
self.curve_phi = self.w_phi.plot()
self.w_phi.setLabel('bottom', 'Time', 's')
self.w_phi.setLabel('left', 'Roll Anlge', 'rad')
self.d_phi.addWidget(self.w_phi)
#Pitch
self.w_theta = pg.PlotWidget(title="Pitch Angle")
self.w_theta.setDownsampling(mode='peak')
self.w_theta.setClipToView(True)
self.curve_theta = self.w_theta.plot()
self.w_theta.setLabel('bottom', 'Time', 's')
self.w_theta.setLabel('left', 'Pitch Angle', 'rad')
self.d_theta.addWidget(self.w_theta)
#Yaw
self.w_psi = pg.PlotWidget(title="Yaw Angle")
self.w_psi.setDownsampling(mode='peak')
self.w_psi.setClipToView(True)
self.curve_psi = self.w_psi.plot()
self.w_psi.setLabel('bottom', 'Time', 's')
self.w_psi.setLabel('left', 'Yaw Angle', 'rad')
self.d_psi.addWidget(self.w_psi)
#Motors Rotation
self.w_w = pg.PlotWidget(title="Motors Rotational Velocity")
self.w_w.setDownsampling(mode='peak')
self.w_w.setClipToView(True)
self.curve_wl = self.w_w.plot(pen = (255,0,0))
self.curve_wr = self.w_w.plot(pen = (0,255,0))
self.curve_wb = self.w_w.plot(pen = (0,0,255))
self.w_w.setLabel('bottom', 'Time', 's')
self.w_w.setLabel('left', 'Rotational Velocity', 'rpm')
self.w_w.addLegend()
self.l_w = self.w_w.plotItem.legend
self.l_w.addItem(self.curve_wl, "Left Motor")
self.l_w.addItem(self.curve_wr, "Right Motor")
self.l_w.addItem(self.curve_wb, "Rear Motor")
self.d_w.addWidget(self.w_w)
#Motors Angle
self.w_t = pg.PlotWidget(title="Motors Angle")
self.w_t.setDownsampling(mode='peak')
self.w_t.setClipToView(True)
self.curve_tl = self.w_t.plot(pen = (255,0,0))
self.curve_tr = self.w_t.plot(pen = (0,255,0))
self.w_t.setLabel('bottom', 'Time', 's')
self.w_t.setLabel('left', 'Motors Angle', 'deg')
self.d_t.addWidget(self.w_t)
self.w_t.addLegend()
self.l_t = self.w_t.plotItem.legend
self.l_t.addItem(self.curve_tl, "Left Motor")
self.l_t.addItem(self.curve_tr, "Right Motor")
self.d_t.addWidget(self.w_t)
#Elevons Angle
self.w_deltae = pg.PlotWidget(title="Elevons Angle")
self.w_deltae.setDownsampling(mode='peak')
self.w_deltae.setClipToView(True)
self.curve_deltael = self.w_deltae.plot(pen = (255,0,0))
self.curve_deltaer = self.w_deltae.plot(pen = (0,255,0))
self.w_deltae.setLabel('bottom', 'Time', 's')
self.w_deltae.setLabel('left', 'Elevons Angle', 'deg')
self.d_deltae.addWidget(self.w_deltae)
self.w_deltae.addLegend()
self.l_deltae = self.w_deltae.plotItem.legend
self.l_deltae.addItem(self.curve_deltael, "Left Elevon")
self.l_deltae.addItem(self.curve_deltaer, "Right Elevon")
self.d_deltae.addWidget(self.w_deltae)
#Data Animation
self.pts = np.vstack([self.flight_data[self.flight.i_north],
-self.flight_data[self.flight.i_east],
self.flight_data[self.flight.i_height]]).transpose()
self.plt = gl.GLLinePlotItem(pos=self.pts, color=pg.glColor(('b')),
width=1, antialias=True)
self.w.addItem(self.plt)
#Reference
self.pts_r = np.stack([self.reference_data[self.reference.i_north_r],
-self.reference_data[self.reference.i_east_r],
self.reference_data[self.reference.i_height_r]]).transpose()
self.plt_r = gl.GLLinePlotItem(pos=self.pts_r, color=pg.glColor(('r')),
width=1, antialias=True)
# print(self.pts_r[0])
self.w.addItem(self.plt_r)
#Add Geometry
self.w.addItem(self.fuselage)
self.w.addItem(self.waypoint)
self.ti_in = 0
def update(self):
self.data_initiation()
if (self.ti_in < self.t_len):
self.geometry_update()
self.waypoints_update()
self.curves_update()
self.ti_in = self.ti_in + 1;
else:
self.fuselage.translate(0, 0, 0, local=False)
def data_initiation(self):
self.t = self.flight_data[self.flight.i_time];
self.t_len = len(self.t)
self.time_ = self.flight_data[self.flight.i_time]
self.north = self.flight_data[self.flight.i_north]
self.east = self.flight_data[self.flight.i_east]
self.height = self.flight_data[self.flight.i_height]
self.velocity = self.flight_data[self.flight.i_velocity]
self.u_ = self.flight_data[self.flight.i_u]
self.v_ = self.flight_data[self.flight.i_v]
self.w_ = self.flight_data[self.flight.i_w]
self.p = self.flight_data[self.flight.i_p]
self.q = self.flight_data[self.flight.i_q]
self.r = self.flight_data[self.flight.i_r]
self.phi = self.flight_data[self.flight.i_phi]
self.theta = self.flight_data[self.flight.i_theta]
self.psi = self.flight_data[self.flight.i_psi]
self.wl = self.flight_data[self.flight.i_wl]
self.wr = self.flight_data[self.flight.i_wr]
self.wb = self.flight_data[self.flight.i_wb]
self.tl = self.flight_data[self.flight.i_tl]
self.tr = self.flight_data[self.flight.i_tr]
self.deltael = self.flight_data[self.flight.i_deltael]
self.deltaer = self.flight_data[self.flight.i_deltaer]
self.w_i = self.flight_data[self.flight.i_w_i]
self.north_r = self.reference_data[self.reference.i_north_r]
self.east_r = self.reference_data[self.reference.i_east_r]
self.height_r = self.reference_data[self.reference.i_height_r]
def geometry_update(self):
# Fuselage
self.fuselage.resetTransform()
self.fuselage.rotate(np.degrees(self.psi[self.ti_in]), 0 ,0, -1, local=False)
self.fuselage.rotate(np.degrees(self.theta[self.ti_in]), 0, -1, 0, local=True)
self.fuselage.rotate(np.degrees(self.phi[self.ti_in]), 1, 0, 0, local=True)
self.fuselage.translate(self.north[self.ti_in], -self.east[self.ti_in],
self.height[self.ti_in], local=False)
self.camera_update()
def camera_update(self):
scale_xe = 0.01
scale_ye = 0.01
scale_ze = 0.01
# self.w.pan(scale_xe*self.north[self.ti_in] + 2,
# -scale_ye*self.east[self.ti_in] + 2,
# scale_ze*self.height[self.ti_in] + 2, relative=False)
print(self.w.opts['azimuth'])
def waypoints_update(self):
self.waypoint.resetTransform()
self.w_i_n = int(self.w_i[self.ti_in])
self.waypoint.translate(self.north_r[self.w_i_n],
-self.east_r[self.w_i_n], self.height_r[self.w_i_n], local=False)
def curves_update(self):
self.curve_x.setData(self.time_[:self.ti_in], self.north[:self.ti_in])
self.curve_y.setData(self.time_[:self.ti_in], self.east[:self.ti_in])
self.curve_h.setData(self.time_[:self.ti_in], self.height[:self.ti_in])
self.curve_V.setData(self.time_[:self.ti_in], self.velocity[:self.ti_in])
self.curve_u_.setData(self.time_[:self.ti_in], self.u_[:self.ti_in])
self.curve_v_.setData(self.time_[:self.ti_in], self.v_[:self.ti_in])
self.curve_w_.setData(self.time_[:self.ti_in], self.w_[:self.ti_in])
self.curve_p.setData(self.time_[:self.ti_in], self.p[:self.ti_in])
self.curve_q.setData(self.time_[:self.ti_in], self.q[:self.ti_in])
self.curve_r.setData(self.time_[:self.ti_in], self.r[:self.ti_in])
self.curve_phi.setData(self.time_[:self.ti_in], self.phi[:self.ti_in])
self.curve_theta.setData(self.time_[:self.ti_in], self.theta[:self.ti_in])
self.curve_psi.setData(self.time_[:self.ti_in], self.psi[:self.ti_in])
self.curve_wl.setData(self.time_[:self.ti_in], self.wl[:self.ti_in])
self.curve_wr.setData(self.time_[:self.ti_in], self.wr[:self.ti_in])
self.curve_wb.setData(self.time_[:self.ti_in], self.wb[:self.ti_in])
self.curve_tl.setData(self.time_[:self.ti_in], self.tl[:self.ti_in])
self.curve_tr.setData(self.time_[:self.ti_in], self.tr[:self.ti_in])
self.curve_deltael.setData(self.time_[:self.ti_in], self.deltael[:self.ti_in])
self.curve_deltaer.setData(self.time_[:self.ti_in], self.deltaer[:self.ti_in])
def start(self):
if (sys.flags.interactive != 1) or not hasattr(QtCore, 'PYQT_VERSION'):
QtGui.QApplication.instance().exec_()
def animation(self):
timer = QtCore.QTimer()
timer.timeout.connect(self.update)
timer.start(30)
self.start()
