/
data_fuser.py
72 lines (54 loc) · 2.14 KB
/
data_fuser.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
from ctypes import c_void_p, cast, POINTER
from mbientlab.metawear import MetaWear, libmetawear, parse_value, cbindings
from time import sleep
from threading import Event
from sys import argv
states = []
class State:
def __init__(self, device):
self.device = device
self.callback = cbindings.FnVoid_VoidP_DataP(self.data_handler)
self.processor = None
def data_handler(self, ctx, data):
values = parse_value(data, n_elem = 2)
print("acc: (%.4f,%.4f,%.4f), gyro; (%.4f,%.4f,%.4f)" % (values[0].x, values[0].y, values[0].z, values[1].x, values[1].y, values[1].z))
def setup(self):
libmetawear.mbl_mw_settings_set_connection_parameters(self.device.board, 7.5, 7.5, 0, 6000)
sleep(1.5)
e = Event()
def processor_created(context, pointer):
self.processor = pointer
e.set()
fn_wrapper = cbindings.FnVoid_VoidP_VoidP(processor_created)
acc = libmetawear.mbl_mw_acc_get_acceleration_data_signal(self.device.board)
gyro = libmetawear.mbl_mw_gyro_bmi160_get_rotation_data_signal(self.device.board)
signals = (c_void_p * 1)()
signals[0] = gyro
libmetawear.mbl_mw_dataprocessor_fuser_create(acc, signals, 1, None, fn_wrapper)
e.wait()
libmetawear.mbl_mw_datasignal_subscribe(self.processor, None, self.callback)
def start(self):
libmetawear.mbl_mw_gyro_bmi160_enable_rotation_sampling(self.device.board)
libmetawear.mbl_mw_acc_enable_acceleration_sampling(self.device.board)
libmetawear.mbl_mw_gyro_bmi160_start(self.device.board)
libmetawear.mbl_mw_acc_start(self.device.board)
for i in range(len(argv) - 1):
d = MetaWear(argv[i + 1])
d.connect()
print("Connected to " + d.address)
states.append(State(d))
for s in states:
print("configuring %s" % (s.device.address))
s.setup()
for s in states:
s.start()
sleep(10.0)
print("resetting devices")
events = []
for s in states:
e = Event()
events.append(e)
s.device.on_disconnect = lambda s: e.set()
libmetawear.mbl_mw_debug_reset(s.device.board)
for e in events:
e.wait()