/ adafruit_bno055.py
adafruit_bno055.py
1 # The MIT License (MIT) 2 # 3 # Copyright (c) 2017 Radomir Dopieralski for Adafruit Industries. 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 13 # all 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 21 # THE SOFTWARE. 22 23 24 """ 25 ``adafruit_bno055`` - Adafruit 9-DOF Absolute Orientation IMU Fusion Breakout - BNO055 26 ======================================================================================= 27 28 This is a CircuitPython driver for the Bosch BNO055 nine degree of freedom 29 inertial measurement unit module with sensor fusion. 30 31 * Author(s): Radomir Dopieralski 32 """ 33 import time 34 import struct 35 36 from micropython import const 37 from adafruit_bus_device.i2c_device import I2CDevice 38 from adafruit_register.i2c_struct import Struct, UnaryStruct 39 40 __version__ = "0.0.0-auto.0" 41 __repo__ = "https://github.com/adafruit/Adafruit_CircuitPython_BNO055.git" 42 43 _CHIP_ID = const(0xA0) 44 45 CONFIG_MODE = const(0x00) 46 ACCONLY_MODE = const(0x01) 47 MAGONLY_MODE = const(0x02) 48 GYRONLY_MODE = const(0x03) 49 ACCMAG_MODE = const(0x04) 50 ACCGYRO_MODE = const(0x05) 51 MAGGYRO_MODE = const(0x06) 52 AMG_MODE = const(0x07) 53 IMUPLUS_MODE = const(0x08) 54 COMPASS_MODE = const(0x09) 55 M4G_MODE = const(0x0A) 56 NDOF_FMC_OFF_MODE = const(0x0B) 57 NDOF_MODE = const(0x0C) 58 59 ACCEL_2G = const(0x00) # For accel_range property 60 ACCEL_4G = const(0x01) # Default 61 ACCEL_8G = const(0x02) 62 ACCEL_16G = const(0x03) 63 ACCEL_7_81HZ = const(0x00) # For accel_bandwidth property 64 ACCEL_15_63HZ = const(0x04) 65 ACCEL_31_25HZ = const(0x08) 66 ACCEL_62_5HZ = const(0x0C) # Default 67 ACCEL_125HZ = const(0x10) 68 ACCEL_250HZ = const(0x14) 69 ACCEL_500HZ = const(0x18) 70 ACCEL_1000HZ = const(0x1C) 71 ACCEL_NORMAL_MODE = const(0x00) # Default. For accel_mode property 72 ACCEL_SUSPEND_MODE = const(0x20) 73 ACCEL_LOWPOWER1_MODE = const(0x40) 74 ACCEL_STANDBY_MODE = const(0x60) 75 ACCEL_LOWPOWER2_MODE = const(0x80) 76 ACCEL_DEEPSUSPEND_MODE = const(0xA0) 77 78 GYRO_2000_DPS = const(0x00) # Default. For gyro_range property 79 GYRO_1000_DPS = const(0x01) 80 GYRO_500_DPS = const(0x02) 81 GYRO_250_DPS = const(0x03) 82 GYRO_125_DPS = const(0x04) 83 GYRO_523HZ = const(0x00) # For gyro_bandwidth property 84 GYRO_230HZ = const(0x08) 85 GYRO_116HZ = const(0x10) 86 GYRO_47HZ = const(0x18) 87 GYRO_23HZ = const(0x20) 88 GYRO_12HZ = const(0x28) 89 GYRO_64HZ = const(0x30) 90 GYRO_32HZ = const(0x38) # Default 91 GYRO_NORMAL_MODE = const(0x00) # Default. For gyro_mode property 92 GYRO_FASTPOWERUP_MODE = const(0x01) 93 GYRO_DEEPSUSPEND_MODE = const(0x02) 94 GYRO_SUSPEND_MODE = const(0x03) 95 GYRO_ADVANCEDPOWERSAVE_MODE = const(0x04) 96 97 MAGNET_2HZ = const(0x00) # For magnet_rate property 98 MAGNET_6HZ = const(0x01) 99 MAGNET_8HZ = const(0x02) 100 MAGNET_10HZ = const(0x03) 101 MAGNET_15HZ = const(0x04) 102 MAGNET_20HZ = const(0x05) # Default 103 MAGNET_25HZ = const(0x06) 104 MAGNET_30HZ = const(0x07) 105 MAGNET_LOWPOWER_MODE = const(0x00) # For magnet_operation_mode property 106 MAGNET_REGULAR_MODE = const(0x08) # Default 107 MAGNET_ENHANCEDREGULAR_MODE = const(0x10) 108 MAGNET_ACCURACY_MODE = const(0x18) 109 MAGNET_NORMAL_MODE = const(0x00) # for magnet_power_mode property 110 MAGNET_SLEEP_MODE = const(0x20) 111 MAGNET_SUSPEND_MODE = const(0x40) 112 MAGNET_FORCEMODE_MODE = const(0x60) # Default 113 114 _POWER_NORMAL = const(0x00) 115 _POWER_LOW = const(0x01) 116 _POWER_SUSPEND = const(0x02) 117 118 _MODE_REGISTER = const(0x3D) 119 _PAGE_REGISTER = const(0x07) 120 _ACCEL_CONFIG_REGISTER = const(0x08) 121 _MAGNET_CONFIG_REGISTER = const(0x09) 122 _GYRO_CONFIG_0_REGISTER = const(0x0A) 123 _GYRO_CONFIG_1_REGISTER = const(0x0B) 124 _CALIBRATION_REGISTER = const(0x35) 125 _OFFSET_ACCEL_REGISTER = const(0x55) 126 _OFFSET_MAGNET_REGISTER = const(0x5B) 127 _OFFSET_GYRO_REGISTER = const(0x61) 128 _RADIUS_ACCEL_REGISTER = const(0x67) 129 _RADIUS_MAGNET_REGISTER = const(0x69) 130 _TRIGGER_REGISTER = const(0x3F) 131 _POWER_REGISTER = const(0x3E) 132 _ID_REGISTER = const(0x00) 133 134 135 class _ScaledReadOnlyStruct(Struct): # pylint: disable=too-few-public-methods 136 def __init__(self, register_address, struct_format, scale): 137 super(_ScaledReadOnlyStruct, self).__init__(register_address, struct_format) 138 self.scale = scale 139 140 def __get__(self, obj, objtype=None): 141 result = super(_ScaledReadOnlyStruct, self).__get__(obj, objtype) 142 return tuple(self.scale * v for v in result) 143 144 def __set__(self, obj, value): 145 raise NotImplementedError() 146 147 148 class _ReadOnlyUnaryStruct(UnaryStruct): # pylint: disable=too-few-public-methods 149 def __set__(self, obj, value): 150 raise NotImplementedError() 151 152 153 class _ModeStruct(Struct): # pylint: disable=too-few-public-methods 154 def __init__(self, register_address, struct_format, mode): 155 super().__init__(register_address, struct_format) 156 self.mode = mode 157 158 def __get__(self, obj, objtype=None): 159 last_mode = obj.mode 160 obj.mode = self.mode 161 result = super().__get__(obj, objtype) 162 obj.mode = last_mode 163 # single value comes back as a one-element tuple 164 return result[0] if isinstance(result, tuple) and len(result) == 1 else result 165 166 def __set__(self, obj, value): 167 last_mode = obj.mode 168 obj.mode = self.mode 169 # underlying __set__() expects a tuple 170 set_val = value if isinstance(value, tuple) else (value,) 171 super().__set__(obj, set_val) 172 obj.mode = last_mode 173 174 175 class BNO055: # pylint: disable=too-many-public-methods 176 """ 177 Base class for the BNO055 9DOF IMU sensor. 178 """ 179 180 def __init__(self): 181 chip_id = self._read_register(_ID_REGISTER) 182 if chip_id != _CHIP_ID: 183 raise RuntimeError("bad chip id (%x != %x)" % (chip_id, _CHIP_ID)) 184 self._reset() 185 self._write_register(_POWER_REGISTER, _POWER_NORMAL) 186 self._write_register(_PAGE_REGISTER, 0x00) 187 self._write_register(_TRIGGER_REGISTER, 0x00) 188 self.accel_range = ACCEL_4G 189 self.gyro_range = GYRO_2000_DPS 190 self.magnet_rate = MAGNET_20HZ 191 time.sleep(0.01) 192 self.mode = NDOF_MODE 193 time.sleep(0.01) 194 195 def _reset(self): 196 """Resets the sensor to default settings.""" 197 self.mode = CONFIG_MODE 198 try: 199 self._write_register(_TRIGGER_REGISTER, 0x20) 200 except OSError: # error due to the chip resetting 201 pass 202 # wait for the chip to reset (650 ms typ.) 203 time.sleep(0.7) 204 205 @property 206 def mode(self): 207 """ 208 legend: x=on, -=off 209 210 +------------------+-------+---------+------+----------+ 211 | Mode | Accel | Compass | Gyro | Absolute | 212 +==================+=======+=========+======+==========+ 213 | CONFIG_MODE | - | - | - | - | 214 +------------------+-------+---------+------+----------+ 215 | ACCONLY_MODE | X | - | - | - | 216 +------------------+-------+---------+------+----------+ 217 | MAGONLY_MODE | - | X | - | - | 218 +------------------+-------+---------+------+----------+ 219 | GYRONLY_MODE | - | - | X | - | 220 +------------------+-------+---------+------+----------+ 221 | ACCMAG_MODE | X | X | - | - | 222 +------------------+-------+---------+------+----------+ 223 | ACCGYRO_MODE | X | - | X | - | 224 +------------------+-------+---------+------+----------+ 225 | MAGGYRO_MODE | - | X | X | - | 226 +------------------+-------+---------+------+----------+ 227 | AMG_MODE | X | X | X | - | 228 +------------------+-------+---------+------+----------+ 229 | IMUPLUS_MODE | X | - | X | - | 230 +------------------+-------+---------+------+----------+ 231 | COMPASS_MODE | X | X | - | X | 232 +------------------+-------+---------+------+----------+ 233 | M4G_MODE | X | X | - | - | 234 +------------------+-------+---------+------+----------+ 235 | NDOF_FMC_OFF_MODE| X | X | X | X | 236 +------------------+-------+---------+------+----------+ 237 | NDOF_MODE | X | X | X | X | 238 +------------------+-------+---------+------+----------+ 239 240 The default mode is ``NDOF_MODE``. 241 242 | You can set the mode using the line below: 243 | ``sensor.mode = adafruit_bno055.ACCONLY_MODE`` 244 | replacing ``ACCONLY_MODE`` with the mode you want to use 245 246 .. data:: CONFIG_MODE 247 248 This mode is used to configure BNO, wherein all output data is reset to zero and sensor 249 fusion is halted. 250 251 .. data:: ACCONLY_MODE 252 253 In this mode, the BNO055 behaves like a stand-alone acceleration sensor. In this mode the 254 other sensors (magnetometer, gyro) are suspended to lower the power consumption. 255 256 .. data:: MAGONLY_MODE 257 258 In MAGONLY mode, the BNO055 behaves like a stand-alone magnetometer, with acceleration 259 sensor and gyroscope being suspended. 260 261 .. data:: GYRONLY_MODE 262 263 In GYROONLY mode, the BNO055 behaves like a stand-alone gyroscope, with acceleration 264 sensor and magnetometer being suspended. 265 266 .. data:: ACCMAG_MODE 267 268 Both accelerometer and magnetometer are switched on, the user can read the data from 269 these two sensors. 270 271 .. data:: ACCGYRO_MODE 272 273 Both accelerometer and gyroscope are switched on; the user can read the data from these 274 two sensors. 275 276 .. data:: MAGGYRO_MODE 277 278 Both magnetometer and gyroscope are switched on, the user can read the data from these 279 two sensors. 280 281 .. data:: AMG_MODE 282 283 All three sensors accelerometer, magnetometer and gyroscope are switched on. 284 285 .. data:: IMUPLUS_MODE 286 287 In the IMU mode the relative orientation of the BNO055 in space is calculated from the 288 accelerometer and gyroscope data. The calculation is fast (i.e. high output data rate). 289 290 .. data:: COMPASS_MODE 291 292 The COMPASS mode is intended to measure the magnetic earth field and calculate the 293 geographic direction. 294 295 .. data:: M4G_MODE 296 297 The M4G mode is similar to the IMU mode, but instead of using the gyroscope signal to 298 detect rotation, the changing orientation of the magnetometer in the magnetic field is 299 used. 300 301 .. data:: NDOF_FMC_OFF_MODE 302 303 This fusion mode is same as NDOF mode, but with the Fast Magnetometer Calibration turned 304 ‘OFF’. 305 306 .. data:: NDOF_MODE 307 308 This is a fusion mode with 9 degrees of freedom where the fused absolute orientation data 309 is calculated from accelerometer, gyroscope and the magnetometer. 310 311 """ 312 return self._read_register(_MODE_REGISTER) 313 314 @mode.setter 315 def mode(self, new_mode): 316 self._write_register(_MODE_REGISTER, CONFIG_MODE) # Empirically necessary 317 time.sleep(0.02) # Datasheet table 3.6 318 if new_mode != CONFIG_MODE: 319 self._write_register(_MODE_REGISTER, new_mode) 320 time.sleep(0.01) # Table 3.6 321 322 @property 323 def calibration_status(self): 324 """Tuple containing sys, gyro, accel, and mag calibration data.""" 325 calibration_data = self._read_register(_CALIBRATION_REGISTER) 326 sys = (calibration_data >> 6) & 0x03 327 gyro = (calibration_data >> 4) & 0x03 328 accel = (calibration_data >> 2) & 0x03 329 mag = calibration_data & 0x03 330 return sys, gyro, accel, mag 331 332 @property 333 def calibrated(self): 334 """Boolean indicating calibration status.""" 335 sys, gyro, accel, mag = self.calibration_status 336 return sys == gyro == accel == mag == 0x03 337 338 @property 339 def external_crystal(self): 340 """Switches the use of external crystal on or off.""" 341 last_mode = self.mode 342 self.mode = CONFIG_MODE 343 self._write_register(_PAGE_REGISTER, 0x00) 344 value = self._read_register(_TRIGGER_REGISTER) 345 self.mode = last_mode 346 return value == 0x80 347 348 @external_crystal.setter 349 def use_external_crystal(self, value): 350 last_mode = self.mode 351 self.mode = CONFIG_MODE 352 self._write_register(_PAGE_REGISTER, 0x00) 353 self._write_register(_TRIGGER_REGISTER, 0x80 if value else 0x00) 354 self.mode = last_mode 355 time.sleep(0.01) 356 357 @property 358 def temperature(self): 359 """Measures the temperature of the chip in degrees Celsius.""" 360 return self._temperature 361 362 @property 363 def _temperature(self): 364 raise NotImplementedError("Must be implemented.") 365 366 @property 367 def acceleration(self): 368 """Gives the raw accelerometer readings, in m/s. 369 Returns an empty tuple of length 3 when this property has been disabled by the current mode. 370 """ 371 if self.mode not in [0x00, 0x02, 0x03, 0x06]: 372 return self._acceleration 373 return (None, None, None) 374 375 @property 376 def _acceleration(self): 377 raise NotImplementedError("Must be implemented.") 378 379 @property 380 def magnetic(self): 381 """Gives the raw magnetometer readings in microteslas. 382 Returns an empty tuple of length 3 when this property has been disabled by the current mode. 383 """ 384 if self.mode not in [0x00, 0x03, 0x05, 0x08]: 385 return self._magnetic 386 return (None, None, None) 387 388 @property 389 def _magnetic(self): 390 raise NotImplementedError("Must be implemented.") 391 392 @property 393 def gyro(self): 394 """Gives the raw gyroscope reading in radians per second. 395 Returns an empty tuple of length 3 when this property has been disabled by the current mode. 396 """ 397 if self.mode not in [0x00, 0x01, 0x02, 0x04, 0x09, 0x0A]: 398 return self._gyro 399 return (None, None, None) 400 401 @property 402 def _gyro(self): 403 raise NotImplementedError("Must be implemented.") 404 405 @property 406 def euler(self): 407 """Gives the calculated orientation angles, in degrees. 408 Returns an empty tuple of length 3 when this property has been disabled by the current mode. 409 """ 410 if self.mode in [0x09, 0x0B, 0x0C]: 411 return self._euler 412 return (None, None, None) 413 414 @property 415 def _euler(self): 416 raise NotImplementedError("Must be implemented.") 417 418 @property 419 def quaternion(self): 420 """Gives the calculated orientation as a quaternion. 421 Returns an empty tuple of length 3 when this property has been disabled by the current mode. 422 """ 423 if self.mode in [0x09, 0x0B, 0x0C]: 424 return self._quaternion 425 return (None, None, None, None) 426 427 @property 428 def _quaternion(self): 429 raise NotImplementedError("Must be implemented.") 430 431 @property 432 def linear_acceleration(self): 433 """Returns the linear acceleration, without gravity, in m/s. 434 Returns an empty tuple of length 3 when this property has been disabled by the current mode. 435 """ 436 if self.mode in [0x09, 0x0B, 0x0C]: 437 return self._linear_acceleration 438 return (None, None, None) 439 440 @property 441 def _linear_acceleration(self): 442 raise NotImplementedError("Must be implemented.") 443 444 @property 445 def gravity(self): 446 """Returns the gravity vector, without acceleration in m/s. 447 Returns an empty tuple of length 3 when this property has been disabled by the current mode. 448 """ 449 if self.mode in [0x09, 0x0B, 0x0C]: 450 return self._gravity 451 return (None, None, None) 452 453 @property 454 def _gravity(self): 455 raise NotImplementedError("Must be implemented.") 456 457 @property 458 def accel_range(self): 459 """ Switch the accelerometer range and return the new range. Default value: +/- 4g 460 See table 3-8 in the datasheet. 461 """ 462 self._write_register(_PAGE_REGISTER, 0x01) 463 value = self._read_register(_ACCEL_CONFIG_REGISTER) 464 self._write_register(_PAGE_REGISTER, 0x00) 465 return 0b00000011 & value 466 467 @accel_range.setter 468 def accel_range(self, rng=ACCEL_4G): 469 self._write_register(_PAGE_REGISTER, 0x01) 470 value = self._read_register(_ACCEL_CONFIG_REGISTER) 471 masked_value = 0b11111100 & value 472 self._write_register(_ACCEL_CONFIG_REGISTER, masked_value | rng) 473 self._write_register(_PAGE_REGISTER, 0x00) 474 475 @property 476 def accel_bandwidth(self): 477 """ Switch the accelerometer bandwidth and return the new bandwidth. Default value: 62.5 Hz 478 See table 3-8 in the datasheet. 479 """ 480 self._write_register(_PAGE_REGISTER, 0x01) 481 value = self._read_register(_ACCEL_CONFIG_REGISTER) 482 self._write_register(_PAGE_REGISTER, 0x00) 483 return 0b00011100 & value 484 485 @accel_bandwidth.setter 486 def accel_bandwidth(self, bandwidth=ACCEL_62_5HZ): 487 if self.mode in [0x08, 0x09, 0x0A, 0x0B, 0x0C]: 488 raise RuntimeError("Mode must not be a fusion mode") 489 self._write_register(_PAGE_REGISTER, 0x01) 490 value = self._read_register(_ACCEL_CONFIG_REGISTER) 491 masked_value = 0b11100011 & value 492 self._write_register(_ACCEL_CONFIG_REGISTER, masked_value | bandwidth) 493 self._write_register(_PAGE_REGISTER, 0x00) 494 495 @property 496 def accel_mode(self): 497 """ Switch the accelerometer mode and return the new mode. Default value: Normal 498 See table 3-8 in the datasheet. 499 """ 500 self._write_register(_PAGE_REGISTER, 0x01) 501 value = self._read_register(_ACCEL_CONFIG_REGISTER) 502 self._write_register(_PAGE_REGISTER, 0x00) 503 return 0b11100000 & value 504 505 @accel_mode.setter 506 def accel_mode(self, mode=ACCEL_NORMAL_MODE): 507 if self.mode in [0x08, 0x09, 0x0A, 0x0B, 0x0C]: 508 raise RuntimeError("Mode must not be a fusion mode") 509 self._write_register(_PAGE_REGISTER, 0x01) 510 value = self._read_register(_ACCEL_CONFIG_REGISTER) 511 masked_value = 0b00011111 & value 512 self._write_register(_ACCEL_CONFIG_REGISTER, masked_value | mode) 513 self._write_register(_PAGE_REGISTER, 0x00) 514 515 @property 516 def gyro_range(self): 517 """ Switch the gyroscope range and return the new range. Default value: 2000 dps 518 See table 3-9 in the datasheet. 519 """ 520 self._write_register(_PAGE_REGISTER, 0x01) 521 value = self._read_register(_GYRO_CONFIG_0_REGISTER) 522 self._write_register(_PAGE_REGISTER, 0x00) 523 return 0b00000111 & value 524 525 @gyro_range.setter 526 def gyro_range(self, rng=GYRO_2000_DPS): 527 if self.mode in [0x08, 0x09, 0x0A, 0x0B, 0x0C]: 528 raise RuntimeError("Mode must not be a fusion mode") 529 self._write_register(_PAGE_REGISTER, 0x01) 530 value = self._read_register(_GYRO_CONFIG_0_REGISTER) 531 masked_value = 0b00111000 & value 532 self._write_register(_GYRO_CONFIG_0_REGISTER, masked_value | rng) 533 self._write_register(_PAGE_REGISTER, 0x00) 534 535 @property 536 def gyro_bandwidth(self): 537 """ Switch the gyroscope bandwidth and return the new bandwidth. Default value: 32 Hz 538 See table 3-9 in the datasheet. 539 """ 540 self._write_register(_PAGE_REGISTER, 0x01) 541 value = self._read_register(_GYRO_CONFIG_0_REGISTER) 542 self._write_register(_PAGE_REGISTER, 0x00) 543 return 0b00111000 & value 544 545 @gyro_bandwidth.setter 546 def gyro_bandwidth(self, bandwidth=GYRO_32HZ): 547 if self.mode in [0x08, 0x09, 0x0A, 0x0B, 0x0C]: 548 raise RuntimeError("Mode must not be a fusion mode") 549 self._write_register(_PAGE_REGISTER, 0x01) 550 value = self._read_register(_GYRO_CONFIG_0_REGISTER) 551 masked_value = 0b00000111 & value 552 self._write_register(_GYRO_CONFIG_0_REGISTER, masked_value | bandwidth) 553 self._write_register(_PAGE_REGISTER, 0x00) 554 555 @property 556 def gyro_mode(self): 557 """ Switch the gyroscope mode and return the new mode. Default value: Normal 558 See table 3-9 in the datasheet. 559 """ 560 self._write_register(_PAGE_REGISTER, 0x01) 561 value = self._read_register(_GYRO_CONFIG_1_REGISTER) 562 self._write_register(_PAGE_REGISTER, 0x00) 563 return 0b00000111 & value 564 565 @gyro_mode.setter 566 def gyro_mode(self, mode=GYRO_NORMAL_MODE): 567 if self.mode in [0x08, 0x09, 0x0A, 0x0B, 0x0C]: 568 raise RuntimeError("Mode must not be a fusion mode") 569 self._write_register(_PAGE_REGISTER, 0x01) 570 value = self._read_register(_GYRO_CONFIG_1_REGISTER) 571 masked_value = 0b00000000 & value 572 self._write_register(_GYRO_CONFIG_1_REGISTER, masked_value | mode) 573 self._write_register(_PAGE_REGISTER, 0x00) 574 575 @property 576 def magnet_rate(self): 577 """ Switch the magnetometer data output rate and return the new rate. Default value: 20Hz 578 See table 3-10 in the datasheet. 579 """ 580 self._write_register(_PAGE_REGISTER, 0x01) 581 value = self._read_register(_MAGNET_CONFIG_REGISTER) 582 self._write_register(_PAGE_REGISTER, 0x00) 583 return 0b00000111 & value 584 585 @magnet_rate.setter 586 def magnet_rate(self, rate=MAGNET_20HZ): 587 if self.mode in [0x08, 0x09, 0x0A, 0x0B, 0x0C]: 588 raise RuntimeError("Mode must not be a fusion mode") 589 self._write_register(_PAGE_REGISTER, 0x01) 590 value = self._read_register(_MAGNET_CONFIG_REGISTER) 591 masked_value = 0b01111000 & value 592 self._write_register(_MAGNET_CONFIG_REGISTER, masked_value | rate) 593 self._write_register(_PAGE_REGISTER, 0x00) 594 595 @property 596 def magnet_operation_mode(self): 597 """ Switch the magnetometer operation mode and return the new mode. Default value: Regular 598 See table 3-10 in the datasheet. 599 """ 600 self._write_register(_PAGE_REGISTER, 0x01) 601 value = self._read_register(_MAGNET_CONFIG_REGISTER) 602 self._write_register(_PAGE_REGISTER, 0x00) 603 return 0b00011000 & value 604 605 @magnet_operation_mode.setter 606 def magnet_operation_mode(self, mode=MAGNET_REGULAR_MODE): 607 if self.mode in [0x08, 0x09, 0x0A, 0x0B, 0x0C]: 608 raise RuntimeError("Mode must not be a fusion mode") 609 self._write_register(_PAGE_REGISTER, 0x01) 610 value = self._read_register(_MAGNET_CONFIG_REGISTER) 611 masked_value = 0b01100111 & value 612 self._write_register(_MAGNET_CONFIG_REGISTER, masked_value | mode) 613 self._write_register(_PAGE_REGISTER, 0x00) 614 615 @property 616 def magnet_mode(self): 617 """ Switch the magnetometer power mode and return the new mode. Default value: Forced 618 See table 3-10 in the datasheet. 619 """ 620 self._write_register(_PAGE_REGISTER, 0x01) 621 value = self._read_register(_MAGNET_CONFIG_REGISTER) 622 self._write_register(_PAGE_REGISTER, 0x00) 623 return 0b01100000 & value 624 625 @magnet_mode.setter 626 def magnet_mode(self, mode=MAGNET_FORCEMODE_MODE): 627 if self.mode in [0x08, 0x09, 0x0A, 0x0B, 0x0C]: 628 raise RuntimeError("Mode must not be a fusion mode") 629 self._write_register(_PAGE_REGISTER, 0x01) 630 value = self._read_register(_MAGNET_CONFIG_REGISTER) 631 masked_value = 0b00011111 & value 632 self._write_register(_MAGNET_CONFIG_REGISTER, masked_value | mode) 633 self._write_register(_PAGE_REGISTER, 0x00) 634 635 def _write_register(self, register, value): 636 raise NotImplementedError("Must be implemented.") 637 638 def _read_register(self, register): 639 raise NotImplementedError("Must be implemented.") 640 641 642 class BNO055_I2C(BNO055): 643 """ 644 Driver for the BNO055 9DOF IMU sensor via I2C. 645 """ 646 647 _temperature = _ReadOnlyUnaryStruct(0x34, "b") 648 _acceleration = _ScaledReadOnlyStruct(0x08, "<hhh", 1 / 100) 649 _magnetic = _ScaledReadOnlyStruct(0x0E, "<hhh", 1 / 16) 650 _gyro = _ScaledReadOnlyStruct(0x14, "<hhh", 0.001090830782496456) 651 _euler = _ScaledReadOnlyStruct(0x1A, "<hhh", 1 / 16) 652 _quaternion = _ScaledReadOnlyStruct(0x20, "<hhhh", 1 / (1 << 14)) 653 _linear_acceleration = _ScaledReadOnlyStruct(0x28, "<hhh", 1 / 100) 654 _gravity = _ScaledReadOnlyStruct(0x2E, "<hhh", 1 / 100) 655 656 offsets_accelerometer = _ModeStruct(_OFFSET_ACCEL_REGISTER, "<hhh", CONFIG_MODE) 657 """Calibration offsets for the accelerometer""" 658 offsets_magnetometer = _ModeStruct(_OFFSET_MAGNET_REGISTER, "<hhh", CONFIG_MODE) 659 """Calibration offsets for the magnetometer""" 660 offsets_gyroscope = _ModeStruct(_OFFSET_GYRO_REGISTER, "<hhh", CONFIG_MODE) 661 """Calibration offsets for the gyroscope""" 662 663 radius_accelerometer = _ModeStruct(_RADIUS_ACCEL_REGISTER, "<h", CONFIG_MODE) 664 """Radius for accelerometer (cm?)""" 665 radius_magnetometer = _ModeStruct(_RADIUS_MAGNET_REGISTER, "<h", CONFIG_MODE) 666 """Radius for magnetometer (cm?)""" 667 668 def __init__(self, i2c, address=0x28): 669 self.buffer = bytearray(2) 670 self.i2c_device = I2CDevice(i2c, address) 671 super().__init__() 672 673 def _write_register(self, register, value): 674 self.buffer[0] = register 675 self.buffer[1] = value 676 with self.i2c_device as i2c: 677 i2c.write(self.buffer) 678 679 def _read_register(self, register): 680 self.buffer[0] = register 681 with self.i2c_device as i2c: 682 i2c.write_then_readinto(self.buffer, self.buffer, out_end=1, in_start=1) 683 return self.buffer[1] 684 685 686 class BNO055_UART(BNO055): 687 """ 688 Driver for the BNO055 9DOF IMU sensor via UART. 689 """ 690 691 def __init__(self, uart): 692 self._uart = uart 693 self._uart.baudrate = 115200 694 super().__init__() 695 696 def _write_register(self, register, data): # pylint: disable=arguments-differ 697 if not isinstance(data, bytes): 698 data = bytes([data]) 699 self._uart.write(bytes([0xAA, 0x00, register, len(data)]) + data) 700 now = time.monotonic() 701 while self._uart.in_waiting < 2 and time.monotonic() - now < 0.25: 702 pass 703 resp = self._uart.read(self._uart.in_waiting) 704 if len(resp) < 2: 705 raise OSError("UART access error.") 706 if resp[0] != 0xEE or resp[1] != 0x01: 707 raise RuntimeError("UART write error: {}".format(resp[1])) 708 709 def _read_register(self, register, length=1): # pylint: disable=arguments-differ 710 i = 0 711 while i < 3: 712 self._uart.write(bytes([0xAA, 0x01, register, length])) 713 now = time.monotonic() 714 while self._uart.in_waiting < length + 2 and time.monotonic() - now < 0.1: 715 pass 716 resp = self._uart.read(self._uart.in_waiting) 717 if len(resp) >= 2 and resp[0] == 0xBB: 718 break 719 i += 1 720 if len(resp) < 2: 721 raise OSError("UART access error.") 722 if resp[0] != 0xBB: 723 raise RuntimeError("UART read error: {}".format(resp[1])) 724 if length > 1: 725 return resp[2:] 726 return int(resp[2]) 727 728 @property 729 def _temperature(self): 730 return self._read_register(0x34) 731 732 @property 733 def _acceleration(self): 734 resp = struct.unpack("<hhh", self._read_register(0x08, 6)) 735 return tuple([x / 100 for x in resp]) 736 737 @property 738 def _magnetic(self): 739 resp = struct.unpack("<hhh", self._read_register(0x0E, 6)) 740 return tuple([x / 16 for x in resp]) 741 742 @property 743 def _gyro(self): 744 resp = struct.unpack("<hhh", self._read_register(0x14, 6)) 745 return tuple([x * 0.001090830782496456 for x in resp]) 746 747 @property 748 def _euler(self): 749 resp = struct.unpack("<hhh", self._read_register(0x1A, 6)) 750 return tuple([x / 16 for x in resp]) 751 752 @property 753 def _quaternion(self): 754 resp = struct.unpack("<hhhh", self._read_register(0x20, 8)) 755 return tuple([x / (1 << 14) for x in resp]) 756 757 @property 758 def _linear_acceleration(self): 759 resp = struct.unpack("<hhh", self._read_register(0x28, 6)) 760 return tuple([x / 100 for x in resp]) 761 762 @property 763 def _gravity(self): 764 resp = struct.unpack("<hhh", self._read_register(0x2E, 6)) 765 return tuple([x / 100 for x in resp]) 766 767 @property 768 def offsets_accelerometer(self): 769 """Calibration offsets for the accelerometer""" 770 return struct.unpack("<hhh", self._read_register(_OFFSET_ACCEL_REGISTER, 6)) 771 772 @offsets_accelerometer.setter 773 def offsets_accelerometer(self, offsets): 774 data = bytearray(6) 775 struct.pack_into("<hhh", data, 0, *offsets) 776 self._write_register(_OFFSET_ACCEL_REGISTER, bytes(data)) 777 778 @property 779 def offsets_magnetometer(self): 780 """Calibration offsets for the magnetometer""" 781 return struct.unpack("<hhh", self._read_register(_OFFSET_MAGNET_REGISTER, 6)) 782 783 @offsets_magnetometer.setter 784 def offsets_magnetometer(self, offsets): 785 data = bytearray(6) 786 struct.pack_into("<hhh", data, 0, *offsets) 787 self._write_register(_OFFSET_MAGNET_REGISTER, bytes(data)) 788 789 @property 790 def offsets_gyroscope(self): 791 """Calibration offsets for the gyroscope""" 792 return struct.unpack("<hhh", self._read_register(_OFFSET_GYRO_REGISTER, 6)) 793 794 @offsets_gyroscope.setter 795 def offsets_gyroscope(self, offsets): 796 data = bytearray(6) 797 struct.pack_into("<hhh", data, 0, *offsets) 798 self._write_register(_OFFSET_GYRO_REGISTER, bytes(data)) 799 800 @property 801 def radius_accelerometer(self): 802 """Radius for accelerometer (cm?)""" 803 return struct.unpack("<h", self._read_register(_RADIUS_ACCEL_REGISTER, 2))[0] 804 805 @radius_accelerometer.setter 806 def radius_accelerometer(self, radius): 807 data = bytearray(2) 808 struct.pack_into("<h", data, 0, radius) 809 self._write_register(_RADIUS_ACCEL_REGISTER, bytes(data)) 810 811 @property 812 def radius_magnetometer(self): 813 """Radius for magnetometer (cm?)""" 814 return struct.unpack("<h", self._read_register(_RADIUS_MAGNET_REGISTER, 2))[0] 815 816 @radius_magnetometer.setter 817 def radius_magnetometer(self, radius): 818 data = bytearray(2) 819 struct.pack_into("<h", data, 0, radius) 820 self._write_register(_RADIUS_MAGNET_REGISTER, bytes(data))