def main():
global initQ
#* initialize pyGame and create a window
pygame.init()
video_flags = OPENGL | DOUBLEBUF
screen = pygame.display.set_mode((640, 480), video_flags)
pygame.display.set_caption("Press Esc To Quit")
GL.resize(640, 480)
GL.init()
frames = 0
ticks = pygame.time.get_ticks()
#* initial attitude, identity
initQ = np.array([1.0, 0, 0, 0])
while 1:
event = pygame.event.poll()
#* click the x button on top of the window or hit Esc key to quit
if event.type == pygame.QUIT or \
(event.type == KEYDOWN and event.key == K_ESCAPE):
pygame.quit() #* quit pygame properly
break
#* reading data from Arduino
read_data()
#* implementing algorithm
gyro_integration()
#print("initQ is ...", initQ)
#* applying complementary filter
tcQ = tilt_correction()
filteredQ = Quat.multiplication(tcQ, initQ)
#* convert quaternion to angl-axis representation
angleAxis = Quat.quatToRodrigues(filteredQ)
#* pygame & OpenGL rendering
#* Note: holding the IMU board such that IMU coordinate system is
#* same as the OpenGL coordinate system
GL.draw(angleAxis)
#* update entire display
pygame.display.flip()
frames = frames + 1
print("fps: %d" % ((frames * 1000) / (pygame.time.get_ticks() - ticks)))
ser.close()
