
#!/usr/bin/python

import time
import math
import mpu6050
import sqlite3 as lite
import thread

size = 0

def compass(nn):
	# Sensor initialization
	mpu = mpu6050.MPU6050()
	mpu.dmpInitialize()
	mpu.setDMPEnabled(True)
	global size	
	# get expected DMP packet size for later comparison
	packetSize = mpu.dmpGetFIFOPacketSize() 
	while True:
		# Get INT_STATUS byte
		mpuIntStatus = mpu.getIntStatus()

		if mpuIntStatus >= 2: # check for DMP data ready interrupt (this should happen frequently) 
			# get current FIFO count
			fifoCount = mpu.getFIFOCount()

			# check for overflow (this should never happen unless our code is too inefficient)
			if fifoCount == 1024:
				mpu.resetFIFO()
				
				
			# wait for correct available data length, should be a VERY short wait
			fifoCount = mpu.getFIFOCount()
			while fifoCount < packetSize:
				fifoCount = mpu.getFIFOCount()

			result = mpu.getFIFOBytes(packetSize)
			q = mpu.dmpGetQuaternion(result)
			g = mpu.dmpGetGravity(q)
			ypr = mpu.dmpGetYawPitchRoll(q, g)

			size = ypr['yaw'] * 180 / math.pi		
			
			# track FIFO count here in case there is > 1 packet available
			# (this lets us immediately read more without waiting for an interrupt)        
			fifoCount -= packetSize

con = lite.connect('parameters.db')
con.isolation_level = None
thread.start_new_thread( compass, ("go",) )
while True:
	degree = int(size)
	if(degree < 0):
		degree = 360 + degree
	cur = con.cursor()
	cur.execute("INSERT INTO direction(degree ) VALUES({0})".format(degree))
	print degree
	time.sleep(0.5)