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'