-
Notifications
You must be signed in to change notification settings - Fork 2
/
bno055.py
113 lines (97 loc) · 3.64 KB
/
bno055.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
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
from micropython import const
import ustruct
from functools import partial
import time
_CHIP_ID = const(0xa0)
CONFIG_MODE = const(0x00)
ACCONLY_MODE = const(0x01)
MAGONLY_MODE = const(0x02)
GYRONLY_MODE = const(0x03)
ACCMAG_MODE = const(0x04)
ACCGYRO_MODE = const(0x05)
MAGGYRO_MODE = const(0x06)
AMG_MODE = const(0x07)
IMUPLUS_MODE = const(0x08)
COMPASS_MODE = const(0x09)
M4G_MODE = const(0x0a)
NDOF_FMC_OFF_MODE = const(0x0b)
NDOF_MODE = const(0x0c)
_POWER_NORMAL = const(0x00)
_POWER_LOW = const(0x01)
_POWER_SUSPEND = const(0x02)
class BNO055:
"""
Driver for the BNO055 9DOF IMU sensor.
Example::
import bno055
from machine import I2C, Pin
i2c = I2C(-1, Pin(5), Pin(4))
s = bno055.BNO055(i2c)
print(s.temperature())
print(s.euler())
"""
def __init__(self, i2c, address=0x28):
self.i2c = i2c
self.address = address
self.init()
def _registers(self, register, struct, value=None, scale=1):
if value is None:
size = ustruct.calcsize(struct)
data = self.i2c.readfrom_mem(self.address, register, size)
value = ustruct.unpack(struct, data)
if scale != 1:
value = tuple(v * scale for v in value)
return value
if scale != 1:
value = tuple(v / scale for v in value)
data = ustruct.pack(struct, *value)
self.i2c.writeto_mem(self.address, register, data)
def _register(self, register, value=None, struct='B'):
if value is None:
return self._registers(register, struct=struct)[0]
self._registers(register, struct=struct, value=(value,))
_chip_id = partial(_register, register=0x00, value=None)
_power_mode = partial(_register, register=0x3e)
_system_trigger = partial(_register, register=0x3f)
_page_id = partial(_register, register=0x07)
operation_mode = partial(_register, register=0x3d)
temperature = partial(_register, register=0x34, value=None)
accelerometer = partial(_registers, register=0x08, struct='<hhh',
value=None, scale=1/100)
magnetometer = partial(_registers, register=0x0e, struct='<hhh',
value=None, scale=1/16)
gyroscope = partial(_registers, register=0x14, struct='<hhh',
value=None, scale=1/900)
euler = partial(_registers, register=0x1a, struct='<hhh',
value=None, scale=1/16)
quaternion = partial(_registers, register=0x20, struct='<hhhh',
value=None, scale=1/(1<<14))
linear_acceleration = partial(_registers, register=0x28, struct='<hhh',
value=None, scale=1/100)
gravity = partial(_registers, register=0x2e, struct='<hhh',
value=None, scale=1/100)
def init(self, mode=NDOF_MODE):
chip_id = self._chip_id()
if chip_id != _CHIP_ID:
raise RuntimeError("bad chip id (%x != %x)" % (chip_id, _CHIP_ID))
self.reset()
self._power_mode(_POWER_NORMAL)
time.sleep_ms(10)
self._page_id(0)
self._system_trigger(0x00)
time.sleep_ms(10)
self.operation_mode(mode)
time.sleep_ms(10)
def reset(self):
self.operation_mode(CONFIG_MODE)
self._system_trigger(0x20)
time.sleep_ms(700)
def use_external_crystal(self, value):
last_mode = self.operation_mode()
self.operation_mode(config_mode)
time.sleep_ms(10)
self._page_id(0)
self._system_trigger(0x80 if value else 0x00)
time.sleep_ms(10)
self.operation_mode(last_mode)
time.sleep_ms(10)