bhi160.py 8.36 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):
40
41
42
        """
        Close the connection to the sensor
        """
koalo's avatar
koalo committed
43
44
        if self.active:
            self.active = False
koalo's avatar
koalo committed
45
46
47
48
49
            ret = sys_bhi160.disable_sensor(self.sensor_id)

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

koalo's avatar
koalo committed
50
51
            interrupt.disable_callback(self.interrupt_id)
            interrupt.set_callback(self.interrupt_id, None)
52
53

    def read(self):
54
55
56
57
58
59
60
61
62
63
64
65
        """
        Read sensor values

        :returns: The recent collected sensor values as a list. If no data is
            available the list contains no elements. Maximum length of the list
            is ``sample_buffer_len``. The last element contains the most recent
            data. The elements contains a sensor specific named tuple. See the
            documentation of the sensor class for more information.

        .. warning::
            Weird behaviour ahead: If the internal buffer overflows, the new samples will be dropped.
        """
66
        result = []
koalo's avatar
koalo committed
67
68
69
        if self.active:
            for sample in sys_bhi160.read_sensor(self.stream_id):
                result.append(self.convert(sample))
70
71
        return result

koalo's avatar
koalo committed
72
73
74
    def _interrupt(self, _):
        if self.active:
            data = self.read()
75
76
            if self._callback:
                self._callback(data)
koalo's avatar
koalo committed
77

koalo's avatar
koalo committed
78
79
80
81
82
83
84
85
    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
86
87

class BHI160Accelerometer(BHI160):
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
    """
    Accelerometer of the BHI160.

    This sensors sample data named tuple contains the following fields:

    - ``x``: Acceleration along the x axis
    - ``y``: Acceleration along the y axis
    - ``z``: Acceleration along the z axis
    - ``status``: accuracy / "confidence" value of the sensor (0 being worst and 3 being best)

    .. todo::
        These values are not scaled correctly

    :param int sample_rate: Sample rate (optional, default is 4, range is 1 - 200 in *Hz*)
    :param int dynamic_range: Dynamic range (optional, default is 2)
    :param callback: Call this callback when enough data is collected (optional, default is None)

            .. todo::
               The callback functionality is untested, so do not be confused if it does not work.
    :param int sample_buffer_len: Length of sample buffer (optional, default is 200)
    """

koalo's avatar
koalo committed
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
    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
126
        return self.convert_data_vector(sample)
koalo's avatar
koalo committed
127
128
129


class BHI160Gyroscope(BHI160):
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
    """
    Gyroscope of the BHI160.

    This sensors sample data named tuple contains the following fields:

    - ``x``: Rotation around the x axis
    - ``y``: Rotation around the y axis
    - ``z``: Rotation around the z axis
    - ``status``: accuracy / "confidence" value of the sensor (0 being worst and 3 being best)

    .. todo::
        These values are not scaled correctly

    :param int sample_rate: Sample rate (optional, default is 4, range is 1 - 200 in *Hz*)
    :param int dynamic_range: Dynamic range (optional, default is 2)
    :param callback: Call this callback when enough data is collected (optional, default is None)

            .. todo::
               The callback functionality is untested, so do not be confused if it does not work.
    :param int sample_buffer_len: Length of sample buffer (optional, default is 200)
    """

koalo's avatar
koalo committed
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
    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
168
        return self.convert_data_vector(sample)
koalo's avatar
koalo committed
169
170
171


class BHI160Orientation(BHI160):
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
    """
    Orientation of the BHI160. Orientation is a virtual sensor that combines
    Accelerometer, Magnetometer and Gyroscope using the IMU Algorithm to
    calculate an absolute orientation.

    This sensors sample data named tuple contains the following fields:

    - ``x``: azimuth
    - ``y``: pitch
    - ``z``: roll
    - ``status``: accuracy / "confidence" value of the sensor (0 being worst and 3 being best)

    .. todo::
        These values are not scaled correctly

    :param int sample_rate: Sample rate (optional, default is 4, range is 1 - 200 in *Hz*)
    :param int dynamic_range: This parameter is unused for the Orientation.
    :param callback: Call this callback when enough data is collected (optional, default is None)

            .. todo::
               The callback functionality is untested, so do not be confused if it does not work.
    :param int sample_buffer_len: Length of sample buffer (optional, default is 200)
    """

koalo's avatar
koalo committed
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
    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
212
        return self.convert_data_vector(sample)
213
214
215


class BHI160Magnetometer(BHI160):
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
    """
    Magnetometer of the BHI160

    This sensors sample data named tuple contains the following fields:

    - ``x``: Magnetic field along the x axis
    - ``y``: Magnetic field along the y axis
    - ``z``: Magnetic field along the z axis
    - ``status``: accuracy / "confidence" value of the sensor (0 being worst and 3 being best)

    .. todo::
        These values are not scaled correctly

    :param int sample_rate: Sample rate (optional, default is 4, range is 1 - 200 in *Hz*)
    :param int dynamic_range: Dynamic range (optional, default is 1)
    :param callback: Call this callback when enough data is collected (optional, default is None)

            .. todo::
               The callback functionality is untested, so do not be confused if it does not work.
    :param int sample_buffer_len: Length of sample buffer (optional, default is 200)

   .. versionadded:: 1.11
    """

240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
    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)