def test1(): """ Example of use of the Fox Sink dongle """ # instanciate dongle foxdongle = fox_dongle.FoxDongle() print 'Enter acquisition loop (type ^ to stop).' init = False while True: try: # handle dongle initialization if not init and foxdongle.init_dongle(test_acq_callback): init = True print 'Device is connected to %s.' % (foxdongle.line()) if foxdongle.is_running(): time.sleep(5) val = foxdongle.read_all() print "READ_ALL:", val elif init: init = False print 'Device is disconnected. Please reconnect.' except KeyboardInterrupt: print "\nStopped." break except Exception as e: logging.error('exception reached:' + str(e)) # must close to kill read thread (fox_pedometer) foxdongle.close_dongle() print 'Done'
def test2(): """ Example of use of the Fox Sink dongle """ # instanciate dongle foxdongle = fox_dongle.FoxDongle() print 'Enter acquisition loop (type ^C to stop and save tmp_imu.csv).' init = False while True: try: # handle dongle initialization if not init and foxdongle.init_dongle(test_acq_callback): init = True print 'Device is connected to %s.' % (foxdongle.line()) time.sleep(5) except KeyboardInterrupt: print "\nStopped and Record in tmp_imu.csv." resp = fox.save_foxsignals_csvfile(foxdongle.data[:, 1], foxdongle.data[:, 2:5],\ foxdongle.data[:, 5:8],\ foxdongle.data[:, 8:11], "tmp_imu.csv") break except Exception as e: logging.error('exception reached:' + str(e)) # must close to kill read thread (fox_sink) foxdongle.close_dongle() print 'Done'
def real_time_madgwick(): rt = RT_Madgwick() # instanciate dongle foxdongle = fdongle.FoxDongle() # init 3d visu rt.f_3D_plot_init() print 'Enter acquisition loop (type ^C to stop).' init = False calib_mag_bool = False scale = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]) offset = np.array([0, 0, 0]) while True: try: # handle dongle initialization if not init and foxdongle.init_dongle(): init = True print 'Device is connected to %s.' % (foxdongle.line()) if foxdongle.is_running(): data = foxdongle.read() if data is not None and data.shape == (11, ): data.reshape(1, 11) data[5:8] = np.transpose( np.dot( scale, np.transpose((data[5:8] - np.transpose(offset))))) if not calib_mag_bool: # record 40s of data for the magnetometer calibration print 'Calib magneto, 20 seconds \n' data_calib = np.zeros((4000, 11)) for i in range(1, 4000): data_calib[i] = foxdongle.read().reshape(1, 11) time.sleep(0.005) offset, scale = calib_mag.compute(data_calib[:, 5:8]) print scale print offset print 'Calib magneto over' print '-----------------\n' print 'Hold the IMU as on the VPython visualization (initial position)' time.sleep(6) print 'START' rt.text.height = 0 calib_mag_bool = True else: rt.update(data[2:12]) time.sleep(0.005) except KeyboardInterrupt: print "\nStopped" break except Exception as e: logging.error('exception reached:' + str(e)) # must close to kill read thread (fox_sink) foxdongle.close_dongle() print 'Done'
def real_time_martin(): rt = RT_Martin() # instanciate dongle foxdongle = fdongle.FoxDongle() # init 3d visu rt.f_3D_plot_init() print 'Enter acquisition loop (type ^C to stop).' init = False init_observer = False init_frame = False calib_mag_bool = False scale = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]) offset = np.array([0, 0, 0]) while True: try: # handle dongle initialization if not init and foxdongle.init_dongle(): init = True print 'Device is connected to %s.' % (foxdongle.line()) if foxdongle.is_running(): data = foxdongle.read() if data is not None and data.shape == (11, ): data = data.reshape(1, 11) data[0, 5:8] = np.transpose( np.dot( scale, np.transpose( (data[0, 5:8] - np.transpose(offset))))) if not calib_mag_bool: # record 40s of data for the magnetometer calibration print 'Calib magneto, 20 seconds \n' data_calib = np.zeros((4000, 11)) for i in range(1, 4000): data_calib[i] = foxdongle.read().reshape(1, 11) time.sleep(0.005) offset, scale = calib_mag.compute(data_calib[:, 5:8]) print scale print offset print 'Calib magneto over' print 'Initializing the observer...' calib_mag_bool = True if not init_observer and calib_mag_bool: # record data for initializing the observer print 'Please stay without moving in the initial position (as VPython)' time.sleep(4) data_obs = np.zeros((1000, 11)) for i in range(0, 1000): data = foxdongle.read().reshape(1, 11) data[0, 5:8] = np.transpose( np.dot( scale, np.transpose((data[0, 5:8] - np.transpose(offset))))) data_obs[i, :] = data[:] time.sleep(0.005) print('.'), rt.init_observer(np.mean(data_obs[:, 2:12], 0)) init_observer = True print 'Start animation...' rt.text.height = 0 if init_observer and not init_frame: for i in range( 0, 1000): # waits that the algorithm converges data = foxdongle.read().reshape(1, 11) data[0, 5:8] = np.transpose( np.dot( scale, np.transpose((data[0, 5:8] - np.transpose(offset))))) rt.update(data) time.sleep(0.005) rt.quat_offset = nq.conjugate(rt.quaternion) init_frame = True else: rt.update(data) time.sleep(0.005) except KeyboardInterrupt: print "\nStopped" break except Exception as e: logging.error('exception reached:' + str(e)) # must close to kill read thread (fox_sink) foxdongle.close_dongle() print 'Done'