import math

time_step = 1		# Measurement time step
mag_num = 1		# Magnetometer number

# Функция mag_calibrated function will adjust the magnetometer’s readings considering the calibration factors
def mag_calibrated(magx,magy,magz):
	magx_cal = 1.06*(magx + -7.49) + -0.01*(magy + -23.59) + 0.07*(magz + -108.24)
	magy_cal = -0.01*(magx + -7.49) + 1.11*(magy + -23.59) + 0.09*(magz + -108.24)
	magz_cal = 0.07*(magx + -7.49) + 0.09*(magy + -23.59) + 1.00*(magz + -108.24)
	return magx_cal, magy_cal, magz_cal
	
def initialize_all():
	print "Enable magnetometer", mag_num
	magnetometer_turn_on(mag_num)
	sleep(1)
	
def switch_off_all():
	print "Disable magnetometer", mag_num
	magnetometer_turn_off(mag_num)
	
def control(): # Program’s main function which is calling all other functions main function
	initialize_all()
	mag_state = 0 		# Initialize magnetometer’s status
	alpha_goal = 0		# Target angle
	omega_goal = 0 		# Target angular velocity 
	
	for i in range(60):
		# magnetometer data request
		mag_state, magx_raw, magy_raw, magz_raw = magnetometer_request_raw(mag_num)
		
		if not mag_state: # if magnetometer returned error code 0 (no error)
			magx_cal, magy_cal, magz_cal = mag_calibrated(magx_raw,magy_raw,magz_raw)
			mag_alpha = math.atan2(magy_cal, magx_cal)/math.pi*180
			print "mag_alpha atan2= ", mag_alpha

		elif mag_state == 1:
			print "Fail because of access error, check the connection"

		elif mag_state == 2:
			print "Fail because of interface error, check your code"
							
		sleep(time_step)

	switch_off_all()