├── .github
└── ISSUE_TEMPLATE
│ ├── bug_report.md
│ └── feature_request.md
├── LICENSE
├── README.md
├── mpu6050.mpy
└── mpu6050.py
/.github/ISSUE_TEMPLATE/bug_report.md:
--------------------------------------------------------------------------------
1 | ---
2 | name: Bug report
3 | about: Create a report to help us improve
4 | title: ''
5 | labels: ''
6 | assignees: ''
7 |
8 | ---
9 |
10 | **Describe the bug**
11 | A clear and concise description of what the bug is.
12 |
13 | **To Reproduce**
14 | Steps to reproduce the behavior:
15 | 1. Go to '...'
16 | 2. Click on '....'
17 | 3. Scroll down to '....'
18 | 4. See error
19 |
20 | **Expected behavior**
21 | A clear and concise description of what you expected to happen.
22 |
23 | **Screenshots**
24 | If applicable, add screenshots to help explain your problem.
25 |
26 | **Desktop (please complete the following information):**
27 | - OS: [e.g. iOS]
28 | - Browser [e.g. chrome, safari]
29 | - Version [e.g. 22]
30 |
31 | **Smartphone (please complete the following information):**
32 | - Device: [e.g. iPhone6]
33 | - OS: [e.g. iOS8.1]
34 | - Browser [e.g. stock browser, safari]
35 | - Version [e.g. 22]
36 |
37 | **Additional context**
38 | Add any other context about the problem here.
39 |
--------------------------------------------------------------------------------
/.github/ISSUE_TEMPLATE/feature_request.md:
--------------------------------------------------------------------------------
1 | ---
2 | name: Feature request
3 | about: Suggest an idea for this project
4 | title: ''
5 | labels: ''
6 | assignees: ''
7 |
8 | ---
9 |
10 | **Is your feature request related to a problem? Please describe.**
11 | A clear and concise description of what the problem is. Ex. I'm always frustrated when [...]
12 |
13 | **Describe the solution you'd like**
14 | A clear and concise description of what you want to happen.
15 |
16 | **Describe alternatives you've considered**
17 | A clear and concise description of any alternative solutions or features you've considered.
18 |
19 | **Additional context**
20 | Add any other context or screenshots about the feature request here.
21 |
--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
1 | MIT License
2 |
3 | Copyright (c) 2021 Michael Guidry
4 |
5 | Permission is hereby granted, free of charge, to any person obtaining a copy
6 | of this software and associated documentation files (the "Software"), to deal
7 | in the Software without restriction, including without limitation the rights
8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
9 | copies of the Software, and to permit persons to whom the Software is
10 | furnished to do so, subject to the following conditions:
11 |
12 | The above copyright notice and this permission notice shall be included in all
13 | copies or substantial portions of the Software.
14 |
15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
21 | SOFTWARE.
22 |
--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
1 | # upy-motion
2 |
3 |
4 | An MPU6050 driver written in micropython. This driver should be compatible with any micropython device. This driver does not support quaternion
5 |
6 | ### Features:
7 |
8 | 1) Auto-calibration if the `ofs` argument is omitted
9 | 2) After auto-calibration the `ofs` argument is supplied to you
10 | 3) Automatic FIFO if an interrupt pin and callback are supplied
11 | 4) Kalman and complimentary filters are built in and automatically applied to data based on flags you set
12 | 5) Data can be retrieved as raw gyroscope and accelerometer data or as angles (roll, pitch only)
13 | 6) Temperature can be retrieved as Celsius or Fahrenheit
14 | 7) Numerous print options available that format the data into a very neat and easy-to-read display
15 | 8) Self-test is built in and using just one property will tell you if your device is functioning properly
16 | 9) Everything you can do is well-documented below
17 |
18 |
19 | ### Community:
20 |
21 | _To officially file a bug report or feature request you can use these templates:_ [bug report](https://github.com/OneMadGypsy/upy-motion/blob/main/.github/ISSUE_TEMPLATE/bug_report.md) | [feature request](https://github.com/OneMadGypsy/upy-motion/blob/main/.github/ISSUE_TEMPLATE/feature_request.md)
22 |
23 | _To discus features, bugs or share your own project that utilize code in this repo:_ [join the discussion](https://github.com/OneMadGypsy/upy-motion/discussions/1)
24 |
25 |
26 |
27 | -------
28 |
29 |
30 |
31 | ## Ports:
32 |
33 | ### MPU6050.py
34 | >This can be uploaded directly to the board, but is intended to be used as a frozen module. For information regarding how to setup the sdk and freeze a module you can refer to [this post](https://www.raspberrypi.org/forums/viewtopic.php?f=146&t=306449#p1862108) on the Raspberry Pi forum.
35 |
36 |
37 | ### MPU6050.mpy
38 | >This is a cross-compiled version of `MPU6050.py`. It is intended to be uploaded to your board as you would any normal `.py` script.
39 |
40 |
41 |
42 |
43 | -------
44 |
45 |
46 |
47 | ## Docs:
48 |
49 |
50 | **MPU6050(`bus`, `sda`, `scl`, `ofs`, `intr`, `callback`, `angles`, `clock`, `gyro`, `accel`, `dlpf`, `rate`, `filtered`, `anglefilter`, `R`, `Q`, `A`, `addr`, `freq`)**
51 |
52 | >The argument for this constructor may seem daunting, but it can be broken up into sections that make it far easier to manage. From `bus` through `scl` sets up I2C connection with the device. `ofs` is the configuration offsets that help your device function accurately. Examples are provided later in this document that explain how to get this value. From `intr` through `angles` are interrupt related and only used if you want to use FIFO. From `clock` through `rate` are device specific settings. From `filtered` through `A` are filter specific settings. `addr` and `freq` are the device address and the frequency it should run at. There is very little reason why you should ever have to change these, which is why they are the very last arguments.
53 |
54 |
55 | >Most things are evidenced later in this document, but there are a couple of things that are easier to simply explain right here. Providing an interrupt pin and a callback will automatically trigger the script to use FIFO. The `rate` argument has 2 purposes. Whatever value you supply will be the divisor for gyroscope clock output, which is it's main intended purpose. However, my driver also uses half of this number to determine how many samples to average when using a complementary filter.
56 |
57 |
58 | Arg | Type | Description | Default
59 | ----------------|------------|------------------------------------------------|-------------
60 | **bus** | int | I2C bus id | **REQUIRED**
61 | **sda** | int or Pin | data pin | **REQUIRED**
62 | **scl** | int or Pin | clock pin | **REQUIRED**
63 | **ofs** | tuple | axis offsets | None
64 | **intr** | int or Pin | interrupt pin (FIFO only) | None
65 | **callback** | function | function to call on interrupt (FIFO only) | None
66 | **angles** | int | return `angles` instead of `data` (FIFO only) | False
67 | **clock** | int | clock source to use | CLK_PLL_XGYRO
68 | **gyro** | int | gyroscope full scale range | GYRO_FS_500
69 | **accel** | int | accelerometer full scale range | ACCEL_FS_2
70 | **dlpf** | int | digital low-pass filter | DLPF_BW_188
71 | **rate** | int | sample rate | 4
72 | **filtered** | int | which properties to filter | NONE
73 | **anglefilter** | int | which filters to apply to angles | NONE
74 | **R** | float | Kalman filter measure | 0.003
75 | **Q** | float | Kalman filter bias | 0.001
76 | **A** | float | complementary filter alpha | .8
77 | **addr** | int | device I2C address | 0x68
78 | **freq** | int | I2C frequency | 400000
79 |
80 |
81 |
82 | -------
83 |
84 |
85 |
86 | ## Constants:
87 |
88 |
89 |
90 | #### clock
91 | >Possible values for the `clock` constructor argument are the following. The default clock is `CLK_PLL_XGYRO`. The documents recommend that you use one of the gyro clocks. All clocks (except external) have their typical frequency listed. Actual frequency may vary +/- 3 Khz.
92 |
93 | Const | Value | Frequency
94 | ------------------|-------|--------
95 | **CLK_INTERNAL** | 0x00 | 8 Mhz
96 | **CLK_PLL_XGYRO** | 0x01 | 33 Khz
97 | **CLK_PLL_YGYRO** | 0x02 | 30 Khz
98 | **CLK_PLL_ZGYRO** | 0x03 | 27 Khz
99 | **CLK_PLL_EXT32K**| 0x04 | 32.768 Khz
100 | **CLK_PLL_EXT19M**| 0x05 | 19.2 Mhz
101 | **CLK_KEEP_RESET**| 0x07 | 0
102 |
103 |
104 |
105 | #### gyro
106 | >Possible values for the `gyro` constructor argument are the following. The default gyro is `GYRO_FS_500`.
107 |
108 | Const | Value
109 | ----------------|-------
110 | **GYRO_FS_250** | 0x00
111 | **GYRO_FS_500** | 0x01
112 | **GYRO_FS_1000**| 0x02
113 | **GYRO_FS_2000**| 0x03
114 |
115 |
116 |
117 | #### accel
118 | >Possible values for the `accel` constructor argument are the following. The default accel is `ACCEL_FS_2`
119 |
120 | Const | Value
121 | ----------------|-------
122 | **ACCEL_FS_2** | 0x00
123 | **ACCEL_FS_4** | 0x01
124 | **ACCEL_FS_8** | 0x02
125 | **ACCEL_FS_16** | 0x03
126 |
127 |
128 |
129 | #### dlpf
130 | >Possible values for the `dlpf` constructor argument include the following. The default dlpf is `DLPF_BW_188`. Headers marked **ms** below represent the milliseconds of delay a DLPF will create.
131 |
132 | Const | Value | Accel(ms) | Gyro(ms) | FS (Khz)
133 | ----------------|-------|-----------|----------|---------
134 | **DLPF_BW_256** | 0x00 | 0 | 0.98 | 8
135 | **DLPF_BW_188** | 0x01 | 2.0 | 1.9 | 1
136 | **DLPF_BW_98** | 0x02 | 3.0 | 2.8 | 1
137 | **DLPF_BW_42** | 0x03 | 4.9 | 4.8 | 1
138 | **DLPF_BW_20** | 0x04 | 8.5 | 8.3 | 1
139 | **DLPF_BW_10** | 0x05 | 13.8 | 13.4 | 1
140 | **DLPF_BW_5** | 0x06 | 19.0 | 18.6 | 1
141 |
142 |
143 |
144 | #### filtered
145 | >Possible values for the `filtered` constructor argument include the following. The default filter is `NONE`. Applying one or more of these flags tells the driver which data to filter. For accel and gyro Kalman filters will be applied. For angles another flag must be used to determine which filters you want applied.
146 |
147 | Flag | Value
148 | -----------------|-------
149 | **NONE** | 0x00
150 | **FILTER_ACCEL** | 0x01
151 | **FILTER_GYRO** | 0x02
152 | **FILTER_ANGLES**| 0x04
153 | **FILTER_ALL** | 0x07
154 |
155 |
156 |
157 | #### anglefilter
158 | >Possible values for the `anglefilter` constructor argument include the following. The default anglefilter is `NONE`.
159 |
160 | Flag | Value
161 | -----------------|-------
162 | **NONE** | 0x00
163 | **ANGLE_KAL** | 0x01
164 | **ANGLE_COMP** | 0x02
165 | **ANGLE_BOTH** | 0x03
166 |
167 |
168 |
169 | -----------------
170 |
171 |
172 |
173 | ### Properties:
174 |
175 |
176 |
177 | **.device_id**
178 | >The id of the device
179 |
180 |
181 |
182 | **.connected**
183 | >True or False device is connected
184 |
185 |
186 |
187 | **.data**
188 | >Returns gyroscope and accelerometer data. This data may be filtered with a Kalman filter if the appropriate flag is supplied to the `filtered` argument in the constructor. The [filters](https://github.com/OneMadGypsy/upy-motion/blob/main/README.md#filters) section contains more information on how to use filters. This is a `namedtuple` with the following fields
189 |
190 | Field | Type | Description
191 | ------------|-------|-----------------
192 | **.acc_x** | float | accelerometer x
193 | **.acc_y** | float | accelerometer y
194 | **.acc_z** | float | accelerometer z
195 | **.gyro_x** | float | gyroscope x
196 | **.gyro_y** | float | gyroscope y
197 | **.gyro_z** | float | gyroscope z
198 |
199 |
200 |
201 | **.angles**
202 | >Returns angles concocted from accelerometer data. These angles may be filtered (with Kalman, complementar or both) according to the flag supplied for the `anglefilter` argument in the constructor. The [filters](https://github.com/OneMadGypsy/upy-motion/blob/main/README.md#filters) section contains more information on how to use filters. This is a `namedtuple` with the following fields
203 |
204 | Field | Type | Description
205 | ------------|-------|-----------------
206 | **.roll** | float | roll angle
207 | **.pitch** | float | pitch angle
208 |
209 |
210 |
211 | **.int_angles**
212 | >Returns angles concocted from accelerometer data as _ints_. These angles may be filtered (with Kalman, complementar or both) according to the flag supplied for the `anglefilter` argument in the constructor. The [filters](https://github.com/OneMadGypsy/upy-motion/blob/main/README.md#filters) section contains more information on how to use filters. This is a `namedtuple` with the following fields
213 |
214 | Field | Type | Description
215 | ------------|-------|-----------------
216 | **.roll** | int | roll angle
217 | **.pitch** | int | pitch angle
218 |
219 |
220 |
221 | **.passed_self_test**
222 | >True or False passed system self-test
223 |
224 |
225 |
226 | **.celsius**
227 | >Returns the temperature in Celsius
228 |
229 |
230 |
231 | **.fahrenheit**
232 | >Returns the temperature in Fahrenheit
233 |
234 |
235 |
236 | -----------------
237 |
238 |
239 |
240 | ### Methods:
241 |
242 |
243 |
244 | **.start()**
245 | >If an interrupt pin and callback were supplied to the constructor, this will start FIFO interrupts
246 |
247 |
248 |
249 | **.stop()**
250 | >If an interrupt pin and callback were supplied to the constructor, this will stop FIFO interrupts
251 |
252 |
253 |
254 | **.print_data()**
255 | >Prints the gyroscope and accelerometer data with any flagged filters automatically applied. The [filters](https://github.com/OneMadGypsy/upy-motion/blob/main/README.md#filters) section contains more information on how to use filters.
256 |
257 |
258 |
259 | **.print_from_data(`data:tuple`)**
260 | >Prints the gyroscope and accelerometer data that was passed to it
261 |
262 |
263 |
264 | **.print_angles(`asint:bool=False`)**
265 | >Prints the roll and pitch angles with any flagged filters automatically applied. If `asint` is `True` angles will be gathered from `.int_angles`. The [filters](https://github.com/OneMadGypsy/upy-motion/blob/main/README.md#filters) section contains more information on how to use filters.
266 |
267 |
268 |
269 | **.print_from_angles(`angles:tuple`)**
270 | >Prints the angle data that was passed to it
271 |
272 |
273 |
274 | **.print_celsius()**
275 | >Prints the temperature in Celsius
276 |
277 |
278 |
279 | **.print_fahrenheit()**
280 | >Prints the temperature in Fahrenheit
281 |
282 |
283 |
284 | **.print_offsets()**
285 | >Prints the offsets as a line of code to be used as the `ofs` argument when instantiating `MPU6050`
286 |
287 |
288 |
289 | **.print_all()**
290 | >Prints everything except offsets
291 |
292 |
293 |
294 | -------
295 |
296 |
297 |
298 | ## Usage
299 |
300 | All the below examples can be copy/pasted, but you must make sure to provide the `bus`, `sda` and `scl` pins (or pin numbers) that apply to your wiring scheme. If an interrupt pin or calibration offsets are used in an example, those too must be replaced with the data that applies to you.
301 |
302 |
303 |
304 | #### calibration
305 |
306 | >The very first thing you should do is calibrate the device. When it completes it will print a small line of code that you need to copy and paste for use with the `ofs` argument. Failure to provide an `ofs` argument will result in your device auto-calibrating every time you instance it. Make sure your device is as flat and level as you can get it before running calibration. Only run calibration from a fresh power-up of the device. If you do a good job calibrating, the numbers this returns can be used constantly, and you should not need to calibrate again unless you customize any of the device configuration arguments (ie. `clock`, `dlpf`, `rate`, `gyro` or `accel`). If you do intend to change any of the configuration arguments, you should add those changes to the script below before running it.
307 |
308 | ```python
309 | from mpu6050 import MPU6050
310 |
311 | MPU6050(1, 6, 7)
312 | ```
313 |
314 |
315 | #### FIFO
316 |
317 | >Supplying an interrupt pin and a callback will trigger the driver to use FIFO automatically. You must also call `start()` for interrupts to begin. The `data` argument in the handler will contain all of the accelerometer and gyroscope data, unless you set the `angles` constructor argument to `True`, in which case `data` will then contain angles values.
318 |
319 | ```python
320 | from mpu6050 import MPU6050
321 |
322 | def handler(data:tuple):
323 | if 'mpu' in globals():
324 | print('[{:<16}] {:<10.2f}'.format('TEMPERATURE', mpu.fahrenheit))
325 | mpu.print_from_data(data)
326 |
327 | mpu = MPU6050(1, 6, 7, (1314, -1629, 410, 28, -17, 51), 2, handler)
328 | if mpu.passed_self_test:
329 | mpu.start()
330 | ```
331 |
332 |
333 |
334 | #### polling
335 |
336 | >If an interrupt pin and callback are not supplied the driver assumes you want to manage your own polling
337 |
338 | ```python
339 | from mpu6050 import MPU6050
340 | import utime
341 |
342 | mpu = MPU6050(1, 6, 7, (1314, -1629, 410, 28, -17, 51))
343 |
344 | if mpu.passed_self_test:
345 | while True:
346 | print('[{:<16}] {:<10.2f}'.format('TEMPERATURE', mpu.fahrenheit))
347 | mpu.print_data()
348 | utime.sleep_ms(100)
349 | ```
350 |
351 |
352 |
353 | #### accessing data
354 |
355 | >data is a `namedtuple` and can be used like any other `namedtuple`. You can either unpack the properties or use them directly. The below examples illustrate both of these methods.
356 |
357 | ```python
358 | from mpu6050 import MPU6050
359 | import utime
360 |
361 | mpu = MPU6050(1, 6, 7, (1314, -1629, 410, 28, -17, 51))
362 |
363 | if mpu.passed_self_test:
364 | while True:
365 | ax, ay, az, gx, gy, gz = mpu.data
366 | ```
367 |
368 |
369 | _or_
370 |
371 |
372 | ```python
373 | from mpu6050 import MPU6050
374 |
375 | def handler(data:tuple):
376 | if 'mpu' in globals():
377 | asum = data.acc_x + data.acc_y + data.acc_z
378 | gsum = data.gyro_x + data.gyro_y + data.gyro_z
379 |
380 | mpu = MPU6050(1, 6, 7, (1314, -1629, 410, 28, -17, 51), 2, handler)
381 | if mpu.passed_self_test:
382 | mpu.start()
383 | ```
384 |
385 |
386 |
387 | #### angles
388 |
389 | >`angles` are handled no different than `data`
390 |
391 | ```python
392 | from mpu6050 import MPU6050
393 |
394 | def handler(data:tuple):
395 | if 'mpu' in globals():
396 | roll, pitch = mpu.angles
397 |
398 | mpu = MPU6050(1, 6, 7, (1314, -1629, 410, 28, -17, 51), 2, handler)
399 | if mpu.passed_self_test:
400 | mpu.start()
401 | ```
402 |
403 |
404 | _just like `data`, `angles` has its own print method, as well_
405 |
406 |
407 | ```python
408 | from mpu6050 import MPU6050
409 | import utime
410 |
411 | mpu = MPU6050(1, 6, 7, (1314, -1629, 410, 28, -17, 51))
412 |
413 | if mpu.passed_self_test:
414 | while True:
415 | mpu.print_angles()
416 | utime.sleep_ms(100)
417 | ```
418 |
419 | _when using fifo you can tell the script to send `angles` instead of axis `data` to the handler callback by setting the `angles` constructor argument to `True`_
420 |
421 |
422 | ```python
423 | from mpu6050 import MPU6050
424 |
425 | def handler(data:tuple):
426 | if 'mpu' in globals():
427 | roll, pitch = data
428 | mpu.print_from_angles(data)
429 |
430 | mpu = MPU6050(1, 6, 7, (1314, -1629, 410, 28, -17, 51), 2, handler, True)
431 | if mpu.passed_self_test:
432 | mpu.start()
433 |
434 | ```
435 |
436 |
437 |
438 | #### filters
439 |
440 | >This driver supports 2 different types of filters (Kalman and complementary). Complementary filters can only be applied to angles. If a complementary filter is flagged on angles it will return the average of all the samples taken. The amount of samples that are taken will be half of the `rate` argument that was supplied to the constructor.
441 |
442 |
443 | ```python
444 | from mpu6050 import MPU6050, FILTER_GYRO, FILTER_ANGLES, ANGLE_COMP
445 |
446 | def handler(data:tuple):
447 | if 'mpu' in globals():
448 | mpu.print_from_angles(data)
449 |
450 | cfg = dict(
451 | rate = 20, #MPU6050_SPLRTDIV ~ comp filter samples at half of this number
452 | filtered = FILTER_GYRO | FILTER_ANGLES, #wont filter accelerometer raw readings
453 | anglefilter = ANGLE_COMP, #apply only complementary filter to angles
454 | angles = True #send data to handler as angles
455 | )
456 | mpu = MPU6050(1, 6, 7, (1368, -1684, 416, 20, -6, 49), 2, handler, **cfg)
457 |
458 | if mpu.passed_self_test:
459 | mpu.start()
460 |
461 | ```
462 |
--------------------------------------------------------------------------------
/mpu6050.mpy:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/OneMadGypsy/upy-motion/3e473878515e101c8d4e06f72456265ca2a7a731/mpu6050.mpy
--------------------------------------------------------------------------------
/mpu6050.py:
--------------------------------------------------------------------------------
1 | from machine import Pin, I2C
2 | from micropython import const
3 | from collections import namedtuple
4 | import struct, utime, math
5 |
6 | _R2D = 180/math.pi
7 |
8 | NONE = const(0x00)
9 |
10 | #filter type flags
11 | ANGLE_KAL = const(0x01)
12 | ANGLE_COMP = const(0x02)
13 | ANGLE_BOTH = const(0x03)
14 |
15 | #filter 'which' flags
16 | FILTER_ACCEL = const(0x01)
17 | FILTER_GYRO = const(0x02)
18 | FILTER_ANGLES = const(0x04)
19 | FILTER_ALL = const(0x07)
20 |
21 | #accel
22 | ACCEL_FS_2 = const(0x00)
23 | ACCEL_FS_4 = const(0x01)
24 | ACCEL_FS_8 = const(0x02)
25 | ACCEL_FS_16 = const(0x03)
26 |
27 | #gyro
28 | GYRO_FS_250 = const(0x00)
29 | GYRO_FS_500 = const(0x01)
30 | GYRO_FS_1000 = const(0x02)
31 | GYRO_FS_2000 = const(0x03)
32 |
33 | #dlpf
34 | DLPF_BW_256 = const(0x00)
35 | DLPF_BW_188 = const(0x01)
36 | DLPF_BW_98 = const(0x02)
37 | DLPF_BW_42 = const(0x03)
38 | DLPF_BW_20 = const(0x04)
39 | DLPF_BW_10 = const(0x05)
40 | DLPF_BW_5 = const(0x06)
41 |
42 | #clock
43 | CLK_INTERNAL = const(0x00)
44 | CLK_PLL_XGYRO = const(0x01)
45 | CLK_PLL_YGYRO = const(0x02)
46 | CLK_PLL_ZGYRO = const(0x03)
47 | CLK_PLL_EXT32K = const(0x04)
48 | CLK_PLL_EXT19M = const(0x05)
49 | CLK_KEEP_RESET = const(0x07)
50 |
51 |
52 | # Data Structure
53 | _D = namedtuple('D', ('acc_x', 'acc_y', 'acc_z', 'gyro_x', 'gyro_y', 'gyro_z'))
54 | _A = namedtuple('A', ('roll', 'pitch'))
55 |
56 | # Data Formatting
57 | _SEP = '-'*60
58 | _W = (' ', '-')
59 | _C = '{}\n{}\n{}'
60 | _F = '\t{} failed Self-Test'
61 | _OUT = '[{:<16}] x: {}{:<10.2f} y: {}{:<10.2f} z: {}{:<10.2f}'
62 |
63 |
64 | #__> I2C
65 | class __I2CHelper(object):
66 | def __init__(self, bus:int, sda, scl, addr:int, freq:int=400000) -> None:
67 | if isinstance(sda, int): sda = Pin(sda)
68 | if isinstance(scl, int): scl = Pin(scl)
69 | self.__addr = addr
70 | self.__bus = I2C(bus, scl=scl, sda=sda, freq=freq)
71 | self.__buffer = bytearray(16)
72 | self.__data = [memoryview(self.__buffer[0:n]) for n in range(1, 17)]
73 |
74 | def __writeBytes(self, reg:int, buff) -> None:
75 | self.__bus.writeto_mem(self.__addr, reg, buff)
76 |
77 | def __writeWords(self, reg:int, length:int, val) -> None:
78 | if isinstance(val, int):
79 | L = int(length * 2)
80 | val = bytearray(val.to_bytes(L, 'big'))
81 |
82 | if isinstance(val, (list, tuple)):
83 | val = bytearray(val)
84 |
85 | if isinstance(val, (bytearray, bytes, memoryview)):
86 | self.__writeBytes(reg, val)
87 |
88 | def __writeByte(self, reg:int, val) -> None:
89 | if isinstance(val, int):
90 | val = bytearray([val])
91 |
92 | if isinstance(val, (bytearray, bytes, memoryview)):
93 | self.__writeBytes(reg, val)
94 |
95 | def __writeBit(self, reg:int, bit:int, data:int) -> None:
96 | b = self.__readByte(reg)
97 | self.__data[0][0] = (b | (1 << bit)) if data else (b & ~(1 << bit))
98 | self.__writeByte(reg, self.__data[0][0])
99 |
100 | def __writeBits(self, reg:int, bitstart:int, length:int, data:int) -> None:
101 | shift = (bitstart - length + 1)
102 | mask = ((1 << length) - 1) << shift
103 | self.__readByte(reg)
104 | data <<= shift
105 | data &= mask
106 | self.__data[0][0] &= ~(mask)
107 | self.__data[0][0] |= data
108 | self.__writeByte(reg, self.__data[0])
109 |
110 | def __readBytes(self, reg:int, length:int) -> int:
111 | if length > 0:
112 | if length in range(1, 17):
113 | self.__bus.readfrom_mem_into(self.__addr, reg, self.__data[length-1])
114 | return self.__data[length-1]
115 | else:
116 | buff = bytearray([0x00]*length)
117 | self.__bus.readfrom_mem_into(self.__addr, reg, buff)
118 | return buff
119 | else:
120 | return bytearray()
121 |
122 | def __readWords(self, reg:int, length:int) -> int:
123 | fmt = '>{}'.format('h'*length)
124 | return struct.unpack(fmt, self.__readBytes(reg, length*2))
125 |
126 | def __readByte(self, reg:int) -> int:
127 | return self.__readBytes(reg, 1)[0]
128 |
129 | def __readBit(self, reg:int, bit:int) -> int:
130 | return (self.__readByte(reg) & (1 << bit))
131 |
132 | def __readBits(self, reg:int, bitstart:int, length:int) -> int:
133 | shift = (bitstart - length + 1)
134 | mask = ((1 << length) - 1) << shift
135 | return ((self.__readByte(reg) & mask) >> shift)
136 |
137 |
138 | #__> Kalman Filter
139 | class __Filters(object):
140 | def __init__(self, R:float, Q:float, alpha:float) -> None:
141 | self.__cov = float('nan')
142 | self.__x = float('nan')
143 | self.__c = float('nan')
144 | self.__A, self.__B, self.__C = 1, 0, 1
145 | self.__R, self.__Q = R, Q
146 |
147 | self.__alpha = alpha
148 | self.__time = utime.ticks_us()
149 | self.__delta = utime.ticks_diff(utime.ticks_us(), self.__time)/1000000
150 |
151 | def kalman(self, angle:float) -> float:
152 | u = 0
153 | if math.isnan(self.__x):
154 | self.__x = (1 / self.__C) * angle
155 | self.__cov = (1 / self.__C) * self.__Q * (1 / self.__C)
156 | else:
157 | px = (self.__A * self.__x) + (self.__B * u)
158 | pc = ((self.__A * self.__cov) * self.__A) + self.__R
159 | K = pc * self.__C * (1 / ((self.__C * pc * self.__C) + self.__Q))
160 | self.__x = px + K * (angle - (self.__C * px))
161 | self.__cov = pc - (K * self.__C * pc)
162 |
163 | return self.__x
164 |
165 | def complementary(self, rate:float, angle:float) -> float:
166 | if math.isnan(self.__c):
167 | self.__c = angle
168 |
169 | self.__delta = utime.ticks_diff(utime.ticks_us(), self.__time)/1000000
170 | self.__time = utime.ticks_us()
171 | self.__c = (1-self.__alpha) * (self.__c + rate * self.__delta) + self.__alpha * angle
172 | return self.__c
173 |
174 | #__> MPU6050
175 | class MPU6050(__I2CHelper):
176 |
177 | #__> PROPERTIES <__#
178 |
179 | @property
180 | def device_id(self) -> int:
181 | return self.__readBits(0x75, 0x6, 0x6)
182 |
183 | @property
184 | def data(self) -> namedtuple:
185 | data = None
186 | ax, ay, az, gx, gy, gz = 0, 0, 0, 0, 0, 0
187 | if self.__usefifo:
188 | data = self.__get_fifo_packet(12)
189 | if not data is None:
190 | ax = data[0] / self.__accfact
191 | ay = data[1] / self.__accfact
192 | az = data[2] / self.__accfact
193 | gx = data[3] / self.__gyrofact
194 | gy = data[4] / self.__gyrofact
195 | gz = data[5] / self.__gyrofact
196 |
197 | if (not self.__usefifo) or (data is None):
198 | data = struct.unpack(">hhhhhhh", self.__readBytes(0x3b, 14))
199 | ax = data[0] / self.__accfact
200 | ay = data[1] / self.__accfact
201 | az = data[2] / self.__accfact
202 | gx = data[4] / self.__gyrofact
203 | gy = data[5] / self.__gyrofact
204 | gz = data[6] / self.__gyrofact
205 |
206 | if self.__filtered & (FILTER_GYRO | FILTER_ACCEL):
207 | if self.__filtered & FILTER_GYRO:
208 | gx = self.__fil_gx.kalman(gx)
209 | gy = self.__fil_gy.kalman(gy)
210 | gz = self.__fil_gz.kalman(gz)
211 |
212 | if self.__filtered & FILTER_ACCEL:
213 | ax = self.__fil_ax.kalman(ax)
214 | ay = self.__fil_ay.kalman(ay)
215 | az = self.__fil_az.kalman(az)
216 |
217 | return _D(ax, ay, az, gx, gy, gz)
218 |
219 | @property
220 | def angles(self) -> tuple:
221 | if (self.__aftype & ANGLE_BOTH) and (self.__filtered & FILTER_ANGLES):
222 | return self.__filtered_angles()
223 |
224 | ax, ay, az, gx, gy, gz = self.data
225 | z2 = az**2
226 | roll = math.atan(ax/math.sqrt(ay**2+z2))*_R2D
227 | pitch = math.atan(ay/math.sqrt(ax**2+z2))*_R2D
228 |
229 | return _A(roll, pitch)
230 |
231 | @property
232 | def int_angles(self) -> tuple:
233 | if (self.__aftype & ANGLE_BOTH) and (self.__filtered & FILTER_ANGLES):
234 | return self.__filtered_angles(True)
235 |
236 | ax, ay, az, gx, gy, gz = self.data
237 | z2 = az**2
238 | roll = math.atan(ax/math.sqrt(ay**2+z2))*_R2D
239 | pitch = math.atan(ay/math.sqrt(ax**2+z2))*_R2D
240 |
241 | return _A(int(roll), int(pitch))
242 |
243 | @property
244 | def connected(self) -> bool:
245 | return self.device_id == 0x34
246 |
247 | @property
248 | def passed_self_test(self) -> bool:
249 | if self.connected:
250 | self.__enable_tests(True) #enable tests
251 | accel = self.__test(*self.__accel_st_data) #test accelerometer
252 | gyro = self.__test(*self.__gyro_st_data) #test gyroscope
253 | self.__enable_tests(False) #disble tests and revert to pre-test states
254 | if not (accel and gyro):
255 | if not gyro:
256 | print(_F.format('Gyroscope'))
257 | if not accel:
258 | print(_F.format('Accelerometer'))
259 | return False
260 | else:
261 | print('No Connection To Device')
262 | return False
263 | return True
264 |
265 | @property
266 | def celsius(self) -> float:
267 | return self.__temperature/340 + 36.53
268 |
269 | @property
270 | def fahrenheit(self) -> float:
271 | return self.celsius * 1.8 + 32
272 |
273 | #__> CONSTRUCTOR
274 | def __init__(self, bus:int, sda, scl, ofs:tuple=None, intr=None, callback=None, angles:bool=False, clock:int=CLK_PLL_XGYRO, gyro:int=GYRO_FS_500, accel:int=ACCEL_FS_2, dlpf:int=DLPF_BW_188, rate:int=4, filtered:int=NONE, anglefilter:int=NONE, R:float=0.003, Q:float=0.001, A:float=0.8, addr:int=0x68, freq:int=400000) -> None:
275 | super().__init__(bus, sda, scl, addr, freq)
276 | self.__accsense , self.__accfact , self.__accfs = 0, 0, accel
277 | self.__gyrosense, self.__gyrofact, self.__gyrofs = 0, 0, gyro
278 | self.__rate , self.__dlpf = rate, dlpf
279 | self.__intr , self.__usefifo = None, False
280 | self.__fil_r , self.__fil_p = None, None
281 | self.__fil_gx , self.__fil_gy , self.__fil_gz = None, None, None
282 | self.__fil_ax , self.__fil_ay , self.__fil_az = None, None, None
283 | self.__useangles, self.__filtered, self.__aftype = angles, filtered, anglefilter
284 |
285 | if filtered & FILTER_ANGLES:
286 | self.__fil_r , self.__fil_p = __Filters(R, Q, A), __Filters(R, Q, A)
287 | if filtered & FILTER_GYRO:
288 | self.__fil_gx, self.__fil_gy, self.__fil_gz = __Filters(R, Q, A), __Filters(R, Q, A), __Filters(R, Q, A)
289 | if filtered & FILTER_ACCEL:
290 | self.__fil_ax, self.__fil_ay, self.__fil_az = __Filters(R, Q, A), __Filters(R, Q, A), __Filters(R, Q, A)
291 |
292 | self.__enable_interrupts(False)
293 | self.__enable_fifo (False)
294 | self.__enable_tests(False)
295 | self.__enable_sleep(False)
296 |
297 | self.__set_clock(clock)
298 | self.__set_gyro (gyro )
299 | self.__set_accel(accel)
300 | self.__set_rate (rate )
301 | self.__set_dlpf (dlpf )
302 |
303 | utime.sleep_ms(100) #a moment to stabilize
304 | if (filtered & (FILTER_ANGLES | FILTER_GYRO)) or (anglefilter & ANGLE_KAL):
305 | for _ in range(50): self.angles #this primes the Kalman filters
306 | elif not anglefilter & NONE:
307 | for _ in range(10): self.angles #primes delta for complimentary filter
308 |
309 | self.__time = utime.ticks_us()
310 | self.__delta = utime.ticks_diff(utime.ticks_us(), self.__time)/1000000
311 | self.__cx, self.__cy = self.angles
312 |
313 | if isinstance(ofs, tuple):
314 | self.__set_offsets(*ofs) if (len(ofs) == 6) else self.__calibrate(6)
315 | else:
316 | self.__calibrate(6)
317 |
318 | if (not intr is None) and (not callback is None):
319 | self.__intr = Pin(intr, Pin.IN, Pin.PULL_DOWN) if isinstance(intr, int) else Pin(sda)
320 | self.__intr.irq(self.__handler, Pin.IRQ_RISING)
321 | self.__callback = callback
322 | self.__usefifo = True
323 | self.__reset_fifo()
324 | self.__enable_fifo(True)
325 |
326 | #__> PUBLIC METHODS <__#
327 |
328 | #INTERRUPT CONTROL____>
329 | def start(self) -> None:
330 | if not self.__usefifo is None:
331 | self.__enable_interrupts(True)
332 |
333 | def stop(self) -> None:
334 | if not self.__usefifo is None:
335 | self.__enable_interrupts(False)
336 |
337 | #PRINT________________>
338 | def print_offsets(self) -> None:
339 | ax, ay, az = self.__readWords(0x6 , 3)
340 | gx, gy, gz = self.__readWords(0x13, 3)
341 | print('ofs=({}, {}, {}, {}, {}, {})'.format(ax, ay, az, gx, gy, gz))
342 |
343 | def print_data(self):
344 | self.print_from_data(self.data)
345 |
346 | def print_from_data(self, data:tuple) -> None:
347 | ax, ay, az, gx, gy, gz = data
348 | a = _OUT.format('ACCELEROMETER', _W[ax<0], abs(ax), _W[ay<0], abs(ay), _W[az<0], abs(az))
349 | g = _OUT.format('GYROSCOPE' , _W[gx<0], abs(gx), _W[gy<0], abs(gy), _W[gz<0], abs(gz))
350 | print(_C.format(a, g, _SEP))
351 |
352 | def print_angles(self, asint:bool=False) -> None:
353 | if asint:
354 | self.print_from_angles(self.int_angles)
355 | return
356 | self.print_from_angles(self.angles)
357 |
358 | def print_from_angles(self, angles:tuple) -> None:
359 | r, p = angles[0:2]
360 | print('[{:<16}] roll: {}{:<10.2f} pitch: {}{:<10.2f}'.format('ANGLES', _W[r<0], abs(r), _W[p<0], abs(p)))
361 |
362 | def print_celsius(self) -> None:
363 | print('[{:<16}] {:>6.2f} C'.format('TEMPERATURE', self.celsius))
364 |
365 | def print_fahrenheit(self) -> None:
366 | print('[{:<16}] {:>6.2f} F'.format('TEMPERATURE', self.fahrenheit))
367 |
368 | def print_all(self):
369 | self.print_celsius()
370 | self.print_fahrenheit()
371 | self.print_angles()
372 | self.print_data()
373 |
374 | #__> PRIVATE PROPERTIES <__#
375 |
376 | @property
377 | def __temperature(self) -> int:
378 | return struct.unpack('>h', self.__readBytes(0x41, 2))[0]
379 |
380 | @property
381 | def __fifo_count(self) -> int:
382 | msb = self.__readByte(0x72)
383 | lsb = self.__readByte(0x73)
384 | return (msb << 8) | lsb
385 |
386 | @property
387 | def __accel_st_data(self) -> tuple: #self-test data
388 | a = self.__readBit(0x1c, 0x7)
389 | b = self.__readBit(0x1c, 0x6)
390 | c = self.__readBit(0x1c, 0x5)
391 | v = self.__readByte(0x10)
392 | x_ = self.__readByte(0xd)
393 | x = ((x_>>3) & 0x1C) | ((v>>4) & 0x03)
394 | y_ = self.__readByte(0xe)
395 | y = ((y_>>3) & 0x1C) | ((v>>2) & 0x03)
396 | z_ = self.__readByte(0xf)
397 | z = ((z_>>3) & 0x1C) | (v & 0x03)
398 | return (a, b, c), (x, y, z)
399 |
400 | @property
401 | def __gyro_st_data(self) -> tuple: #self-test data
402 | a = self.__readBit(0x1b, 0x7)
403 | b = self.__readBit(0x1b, 0x6)
404 | c = self.__readBit(0x1b, 0x5)
405 | x = self.__readByte(0xd) & 0x1F
406 | y = self.__readByte(0xe) & 0x1F
407 | z = self.__readByte(0xf) & 0x1F
408 | return (a, b, c), (x, y, z)
409 |
410 | #__> PRIVATE METHODS <__#
411 |
412 | #SETTERS______________>
413 | def __set_dlpf(self, dlpf:int):
414 | self.__writeBits(0x1a, 0x2, 0x3, dlpf)
415 |
416 | def __set_rate(self, rate:int) -> None:
417 | self.__writeByte(0x19, rate)
418 |
419 | def __set_gyro(self, rng:int) -> None:
420 | self.__gyrosense = [250, 500, 1000, 2000][rng]
421 | self.__gyrofact = [131, 66.5, 32.8, 16.4][rng]
422 | self.__writeBits(0x1b, 0x4, 0x2, rng)
423 |
424 | def __set_accel(self, rng:int) -> None:
425 | self.__accsense = [2, 4, 8, 16][rng]
426 | self.__accfact = 32768.0/self.__accsense
427 | self.__writeBits(0x1c, 0x4, 0x2, rng)
428 |
429 | def __set_clock(self, source:int) -> None:
430 | self.__writeBits(0x6b, 0x2, 0x3, source)
431 |
432 | #MISC_________________>
433 | def __handler(self, pin:Pin) -> None: #interrupt handler
434 | if (not self.__intr is None) and (not self.__callback is None):
435 | if self.__useangles:
436 | self.__callback(self.angles)
437 | return
438 | self.__callback(self.data)
439 |
440 | def __filtered_angles(self, asint:bool=False): #manages all angle filtering
441 | smps = self.__rate//2
442 | fx, fy = [0.00]*(smps), [0.00]*(smps)
443 | for s in range(smps):
444 | ax, ay, az, gx, gy, gz = self.data
445 | z2 = az**2
446 | ay2z2 = math.sqrt(ay**2+z2)
447 | ax2z2 = math.sqrt(ax**2+z2)
448 | roll = 0 if not ay2z2 else math.atan(ax/ay2z2)*_R2D
449 | pitch = 0 if not ax2z2 else math.atan(ay/ax2z2)*_R2D
450 | if self.__aftype & ANGLE_KAL:
451 | roll = self.__fil_r.kalman(roll)
452 | pitch = self.__fil_p.kalman(pitch)
453 | if self.__aftype & ANGLE_COMP:
454 | roll = self.__fil_r.complementary(gx, roll )
455 | pitch = self.__fil_p.complementary(gy, pitch)
456 | fx[s], fy[s] = roll, pitch
457 | utime.sleep_us(100)
458 | roll = sum(fx)/smps
459 | pitch = sum(fy)/smps
460 |
461 | if asint:
462 | return _A(int(roll), int(pitch))
463 |
464 | return _A(roll, pitch)
465 |
466 | def __enable_sleep(self, e:bool) -> None:
467 | self.__writeBit(0x6b, 0x6, e)
468 |
469 | def __enable_interrupts(self, e:bool) -> None:
470 | self.__writeByte(0x38, (0x11 if e else 0x00))
471 |
472 | #SELF-TEST____________>
473 | def __enable_tests(self, e:bool) -> None:
474 | #accelerometer test
475 | self.__set_accel(ACCEL_FS_8 if e else self.__accfs)
476 | self.__writeBit(0x1c, 0x7, e)
477 | self.__writeBit(0x1c, 0x6, e)
478 | self.__writeBit(0x1c, 0x5, e)
479 | #gyroscope test
480 | self.__set_gyro(GYRO_FS_500 if e else self.__gyrofs)
481 | self.__writeBit(0x1b, 0x7, e)
482 | self.__writeBit(0x1b, 0x6, e)
483 | self.__writeBit(0x1b, 0x5, e)
484 |
485 | def __test(self, st_data:tuple, trim:tuple) -> bool:
486 | spec = range(-14, 15) #factory min/max specs
487 | return sum([True if not t else ((s-t)//t in spec) for t, s in zip(trim, st_data)]) == 3
488 |
489 | #FIFO_________________>
490 | def __reset_fifo(self) -> None:
491 | self.__writeBit(0x6a, 0x6, False) #disable FIFO
492 | self.__writeBit(0x6a, 0x2, True) #reset FIFO
493 | self.__writeBit(0x6a, 0x6, True) #enable FIFO
494 |
495 | def __enable_fifo(self, e:bool) -> None:
496 | self.__writeBit(0x6a, 0x6, e) #enable FIFO
497 | self.__writeByte(0x23, (0x78 if e else 0x00)) #enable Gyro and Accel registers
498 |
499 | def __fifo_bytes(self, length:int) -> bytearray:
500 | #read from FIFO buffer
501 | return self.__readBytes(0x74, length) if length > 0 else None
502 |
503 | def __get_fifo_packet(self, length:int=12) -> bytearray:
504 | fifoC = self.__fifo_count
505 | extra = fifoC%length
506 | self.__fifo_bytes(extra)
507 | fifoC = self.__fifo_count
508 | return None if fifoC < length else struct.unpack(">hhhhhh", self.__fifo_bytes(fifoC)[-length:])
509 |
510 | #DMP__________________>
511 | def __reset_dmp(self) -> None:
512 | self.__writeBit(0x6a, 0x3, True)
513 |
514 | def __enable_dmp(self, e:bool) -> None:
515 | self.__writeBit(0x6a, 0x7, e)
516 |
517 | #CALIBRATE____________>
518 | def __calibrate(self, loops:int) -> None:
519 | x = (100 - int((loops - 1) * (0 - 20) / (5 - 1) + 20)) * .01
520 | kP, kI = 0.3*x, 90*x
521 | self.__pid(0x43, kP, kI, loops) #calibrate gyroscope
522 | kP, kI = 0.3*x, 20*x
523 | self.__pid(0x3b, kP, kI, loops) #calibrate accelerometer
524 | self.print_offsets()
525 |
526 | def __pid(self, readaddr:int, kP:float, kI:float, loops:int) -> None:
527 | saveaddr = 0x06 if readaddr == 0x3B else 0x13
528 | data , reading, bitzero = 0 , 0.0, [0]*3
529 | shift, esample, esum = 2 , 0 , 0
530 | error, pterm , iterm = 0.0, 0.0, [0.0]*3
531 |
532 | for i in range(3):
533 | data = self.__readWords(saveaddr + (i * shift), 1)[0]
534 | reading = data
535 | if not (saveaddr == 0x13):
536 | bitzero[i] = data & 1
537 | iterm[i] = reading * 8
538 | else:
539 | iterm[i] = reading * 4
540 |
541 | for L in range(loops):
542 | esample = 0
543 | for c in range(100):
544 | esum = 0
545 | for i in range(3):
546 | data = self.__readWords(readaddr + (i * 2), 1)[0]
547 | reading = data
548 |
549 | if ((readaddr == 0x3B) and (i == 2)): reading -= 16384
550 |
551 | error = -reading
552 | esum += abs(reading)
553 | pterm = kP * error
554 | iterm[i] += (error * 0.001) * kI
555 | data = (round((pterm + iterm[i]) / 8) & 0xFFFE) | bitzero[i] if saveaddr != 0x13 else round((pterm + iterm[i]) / 4)
556 |
557 | self.__writeWords(saveaddr + (i * shift), 1, data)
558 |
559 | c = 0 if (c == 99) and (esum > 1000) else c
560 |
561 | if (esum * (.05 if readaddr == 0x3B else 1)) < 5: esample+= 1
562 | if (esum < 100) and (c > 10) and (esample >= 10): break
563 | utime.sleep_ms(1)
564 |
565 | kP *= .75
566 | kI *= .75
567 | for i in range(3):
568 | data = (round(iterm[i] / 8) & 0xFFFE) | bitzero[i] if not (saveaddr == 0x13) else round(iterm[i] / 4)
569 | self.__writeWords(saveaddr + (i * shift), 1, data)
570 |
571 | self.__reset_fifo()
572 | self.__reset_dmp()
573 |
574 | def __set_offsets(self, ax:int, ay:int, az:int, gx:int, gy:int, gz:int) -> None:
575 | self.__writeWords(0x6 , 1, ax)
576 | self.__writeWords(0x8 , 1, ay)
577 | self.__writeWords(0xa , 1, az)
578 | self.__writeWords(0x13, 1, gx)
579 | self.__writeWords(0x15, 1, gy)
580 | self.__writeWords(0x17, 1, gz)
581 |
582 |
--------------------------------------------------------------------------------