bhi160.py 8.52 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
class BHI160:
    def enable_sensor(self):
        interrupt.disable_callback(self.interrupt_id)
        interrupt.set_callback(self.interrupt_id, self._interrupt)
21
22
23
24
25

        # If the sensor is already enabled, disable it first.
        # The call is allowed to silently fail.
        sys_bhi160.disable_sensor(self.sensor_id)

koalo's avatar
koalo committed
26
27
        self.stream_id = sys_bhi160.enable_sensor(
            self.sensor_id, self.sample_buffer_len, self.sample_rate, self.dynamic_range
28
        )
koalo's avatar
koalo committed
29
30
31
32
33
34
35
36

        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)
37
38
39
40
41
42
43
44

    def __enter__(self):
        return self

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

    def close(self):
45
46
47
        """
        Close the connection to the sensor
        """
koalo's avatar
koalo committed
48
49
        if self.active:
            self.active = False
koalo's avatar
koalo committed
50
51
52
53
54
            ret = sys_bhi160.disable_sensor(self.sensor_id)

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

koalo's avatar
koalo committed
55
56
            interrupt.disable_callback(self.interrupt_id)
            interrupt.set_callback(self.interrupt_id, None)
57
58

    def read(self):
59
60
61
62
63
64
65
66
67
68
69
70
        """
        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.
        """
71
        result = []
koalo's avatar
koalo committed
72
73
74
        if self.active:
            for sample in sys_bhi160.read_sensor(self.stream_id):
                result.append(self.convert(sample))
75
76
        return result

koalo's avatar
koalo committed
77
78
79
    def _interrupt(self, _):
        if self.active:
            data = self.read()
80
81
            if self._callback:
                self._callback(data)
koalo's avatar
koalo committed
82

koalo's avatar
koalo committed
83
84
85
86
87
88
89
90
    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
91
92

class BHI160Accelerometer(BHI160):
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
    """
    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
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
    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
131
        return self.convert_data_vector(sample)
koalo's avatar
koalo committed
132
133
134


class BHI160Gyroscope(BHI160):
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
    """
    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
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
    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
173
        return self.convert_data_vector(sample)
koalo's avatar
koalo committed
174
175
176


class BHI160Orientation(BHI160):
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
    """
    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
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
    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
217
        return self.convert_data_vector(sample)
218
219
220


class BHI160Magnetometer(BHI160):
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
    """
    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
    """

245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
    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)