import math

time_step = 1		# Временной шаг измерений
mag_num = 1		# Номер магнитометра

# Функция mag_calibrated вносит поправки в показания магнитометра с учетом калибровочных коэффициентов
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(): # основная функция программы, в которой вызываются остальные функции
	initialize_all()
	mag_state = 0 		# Инициализируем статус магнитометра
	alpha_goal = 0		# Целевой угол
	omega_goal = 0 		# Целевая угловая скорость
	
	for i in range(60):
		# опрос датчиков и маховика
		mag_state, magx_raw, magy_raw, magz_raw = magnetometer_request_raw(mag_num)
		
		if not mag_state: # если магнитометр вернул код ошибки 0, т.е. ошибки нет
			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()