import math time_step = 1 # Measurement time step, sec mag_num = 1 # Magnetometer number def control(): # Program’s main function print "Enable magnetometer", mag_num magnetometer_turn_on(mag_num) sleep(1) mag_state = 0 # Initialize magnetometer status alpha_goal = 0 # Target angle omega_goal = 0 # Target angular velocity for i in range(60): mag_state, magx_raw, magy_raw, magz_raw = magnetometer_request_raw(mag_num) # magnetometer data request if not mag_state: # if error code is 0, no error 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)