import math
time_step = 1		# Временной шаг измерений, сек
mag_num = 1		# Номер магнитометра
def control(): # основная функция программы
	print "Enable magnetometer", mag_num	
	magnetometer_turn_on(mag_num)
	sleep(10)
	mag_state = 0 		# Инициализируем статус магнитометра
	alpha_goal = 0		# Целевой угол
	omega_goal = 0 		# Целевая угловая скорость
		for i in range(12):
		mag_state, magx_raw, magy_raw, magz_raw = magnetometer_request_raw(mag_num) # опрос магнитометра
		if not mag_state: # если код ошибки 0, то ошибки нет
			mag_alpha = math.atan2(magy_raw, magx_raw)/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)
	print "Disable magnetometer", mag_num
	magnetometer_turn_off(mag_num)