bhi160.py 4.23 KB
Newer Older
1
2
import sys_bhi160
import interrupt
koalo's avatar
koalo committed
3
4
5
import ucollections

DataVector = ucollections.namedtuple("DataVector", ["x", "y", "z", "status"])
6
7


8
9
10
11
12
13
14
15
16
def disable_all_sensors():
    """
    Disable all sensor also if they are already deactivated.

    :returns: None
    """
    sys_bhi160.disable_all_sensors()


koalo's avatar
koalo committed
17
18
19
20
21
22
class BHI160:
    def enable_sensor(self):
        interrupt.disable_callback(self.interrupt_id)
        interrupt.set_callback(self.interrupt_id, self._interrupt)
        self.stream_id = sys_bhi160.enable_sensor(
            self.sensor_id, self.sample_buffer_len, self.sample_rate, self.dynamic_range
23
        )
koalo's avatar
koalo committed
24
25
26
27
28
29
30
31

        if self.stream_id < 0:
            raise ValueError("Enable sensor returned %i", self.stream_id)

        self.active = True

        if self._callback:
            interrupt.enable_callback(self.interrupt_id)
32
33
34
35
36
37
38
39

    def __enter__(self):
        return self

    def __exit__(self, _et, _ev, _t):
        self.close()

    def close(self):
koalo's avatar
koalo committed
40
41
        if self.active:
            self.active = False
koalo's avatar
koalo committed
42
43
44
45
46
            ret = sys_bhi160.disable_sensor(self.sensor_id)

            if ret < 0:
                raise ValueError("Disable sensor returned %i", ret)

koalo's avatar
koalo committed
47
48
            interrupt.disable_callback(self.interrupt_id)
            interrupt.set_callback(self.interrupt_id, None)
49
50
51

    def read(self):
        result = []
koalo's avatar
koalo committed
52
53
54
        if self.active:
            for sample in sys_bhi160.read_sensor(self.stream_id):
                result.append(self.convert(sample))
55
56
        return result

koalo's avatar
koalo committed
57
58
59
    def _interrupt(self, _):
        if self.active:
            data = self.read()
60
61
            if self._callback:
                self._callback(data)
koalo's avatar
koalo committed
62

koalo's avatar
koalo committed
63
64
65
66
67
68
69
70
    def convert_data_vector(self, sample):
        return DataVector(
            self.convert_single(sample[0]),
            self.convert_single(sample[1]),
            self.convert_single(sample[2]),
            sample[3],
        )

koalo's avatar
koalo committed
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88

class BHI160Accelerometer(BHI160):
    def __init__(
        self, sample_rate=4, dynamic_range=2, callback=None, sample_buffer_len=200
    ):
        self.sample_rate = sample_rate
        self.dynamic_range = dynamic_range
        self.callback = callback
        self.sample_buffer_len = sample_buffer_len
        self.sensor_id = 0
        self.interrupt_id = interrupt.BHI160_ACCELEROMETER
        self._callback = callback
        self.enable_sensor()

    def convert_single(self, value):
        return 2 * value / 32768.0

    def convert(self, sample):
koalo's avatar
koalo committed
89
        return self.convert_data_vector(sample)
koalo's avatar
koalo committed
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108


class BHI160Gyroscope(BHI160):
    def __init__(
        self, sample_rate=4, dynamic_range=2, callback=None, sample_buffer_len=200
    ):
        self.sample_rate = sample_rate
        self.dynamic_range = dynamic_range
        self.callback = callback
        self.sample_buffer_len = sample_buffer_len
        self.sensor_id = 3
        self.interrupt_id = interrupt.BHI160_GYROSCOPE
        self._callback = callback
        self.enable_sensor()

    def convert_single(self, value):
        return 360 * value / 32768.0

    def convert(self, sample):
koalo's avatar
koalo committed
109
        return self.convert_data_vector(sample)
koalo's avatar
koalo committed
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128


class BHI160Orientation(BHI160):
    def __init__(
        self, sample_rate=4, dynamic_range=2, callback=None, sample_buffer_len=200
    ):
        self.sample_rate = sample_rate
        self.dynamic_range = dynamic_range
        self.callback = callback
        self.sample_buffer_len = sample_buffer_len
        self.sensor_id = 2
        self.interrupt_id = interrupt.BHI160_ORIENTATION
        self._callback = callback
        self.enable_sensor()

    def convert_single(self, value):
        return 360 * value / 32768.0

    def convert(self, sample):
koalo's avatar
koalo committed
129
        return self.convert_data_vector(sample)
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149


class BHI160Magnetometer(BHI160):
    def __init__(
        self, sample_rate=4, dynamic_range=1, callback=None, sample_buffer_len=200
    ):
        self.sample_rate = sample_rate
        self.dynamic_range = dynamic_range
        self.callback = callback
        self.sample_buffer_len = sample_buffer_len
        self.sensor_id = 1
        self.interrupt_id = interrupt.BHI160_MAGNETOMETER
        self._callback = callback
        self.enable_sensor()

    def convert_single(self, value):
        return 1000 * value / 32768.0

    def convert(self, sample):
        return self.convert_data_vector(sample)