# bno055.py MicroPython driver for Bosch cls nine degree of freedom inertial # measurement unit module with sensor fusion. # The MIT License (MIT) # # Copyright (c) 2017 Radomir Dopieralski for Adafruit Industries. # # Permission is hereby granted, free of charge, to any person obtaining a copy # of this software and associated documentation files (the "Software"), to deal # in the Software without restriction, including without limitation the rights # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell # copies of the Software, and to permit persons to whom the Software is # furnished to do so, subject to the following conditions: # # The above copyright notice and this permission notice shall be included in # all copies or substantial portions of the Software. # # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN # THE SOFTWARE. # This is a port of the Adafruit CircuitPython driver to MicroPython, with # modified/enhanced functionality. # Original Author: Radomir Dopieralski # Ported to MicroPython and extended by Peter Hinch # This port copyright (c) Peter Hinch 2019 from micropython import const from bno055_base import BNO055_BASE CONFIG_MODE = 0x00 ACCONLY_MODE = 0x01 MAGONLY_MODE = 0x02 GYRONLY_MODE = 0x03 ACCMAG_MODE = 0x04 ACCGYRO_MODE = 0x05 MAGGYRO_MODE = 0x06 AMG_MODE = 0x07 IMUPLUS_MODE = 0x08 COMPASS_MODE = 0x09 M4G_MODE = 0x0a NDOF_FMC_OFF_MODE = 0x0b NDOF_MODE = 0x0c ACC = 0x08 # Registers for configuration (page 1) MAG = 0x09 GYRO = 0x0a ACC_DATA = 0x08 # Data regsiters (page 0) MAG_DATA = 0x0e GYRO_DATA = 0x14 GRAV_DATA = 0x2e LIN_ACC_DATA = 0x28 EULER_DATA = 0x1a QUAT_DATA = 0x20 _PAGE_REGISTER = const(0x07) _AXIS_MAP_SIGN = const(0x42) _AXIS_MAP_CONFIG = const(0x41) class BNO055(BNO055_BASE): acc_range = (2, 4, 8, 16) # G acc_bw = (8, 16, 31, 62, 125, 250, 500, 1000) gyro_range = (2000, 1000, 500, 250, 125) # dps gyro_bw = (523, 230, 116, 47, 23, 12, 64, 32) # bandwidth (Hz) mag_rate = (2, 6, 8, 10, 15, 20, 25, 30) # rate (Hz) @classmethod def _tuple_to_int(cls, dev, v): # Convert (range, bw) to register value try: if dev == ACC: msg = 'Illegal accel range {} or bandwidth {}' return cls.acc_range.index(v[0]) | (cls.acc_bw.index(v[1]) << 2) elif dev == GYRO: msg = 'Illegal gyro range {} or bandwidth {}' return cls.gyro_range.index(v[0]) | (cls.gyro_bw.index(v[1]) << 3) elif dev == MAG: msg = 'Illegal magnetometer rate {}' return cls.mag_rate.index(v[0]) except ValueError: raise ValueError(msg.format(*v)) # Return the current config in human readable form @classmethod def _int_to_tuple(cls, dev, v): try: if dev == ACC: return (cls.acc_range[v & 3], cls.acc_bw[v >> 2]) elif dev == GYRO: return (cls.gyro_range[v & 7], cls.gyro_bw[v >> 3]) elif dev == MAG: return (cls.mag_rate[v],) except IndexError: return False # Can occur e.g. initial config of magnetometer raise ValueError('Unknown device.', dev) # Convert two bytes to signed integer (little endian) Can be used in an interrupt handler @staticmethod def _bytes_toint(lsb, msb): if not msb & 0x80: return msb << 8 | lsb # +ve return - (((msb ^ 255) << 8) | (lsb ^ 255) + 1) @staticmethod def _argcheck(arg, name): if len(arg) != 3 or not (isinstance(arg, list) or isinstance(arg, tuple)): raise ValueError(name + ' must be a 3 element list or tuple') # Transposition (x, y, z) 0 == x 1 == y 2 == z hence (0, 1, 2) is no change # Scaling (x, y, z) 0 == normal 1 == invert def __init__(self, i2c, address=0x28, crystal=True, transpose=(0, 1, 2), sign=(0, 0, 0)): self._argcheck(sign, 'Sign') if [x for x in sign if x not in (0, 1)]: raise ValueError('Sign values must be 0 or 1') self.sign = sign self._argcheck(transpose, 'Transpose') if set(transpose) != {0, 1, 2}: raise ValueError('Transpose indices must be unique and in range 0-2') self.transpose = transpose super().__init__(i2c, address, crystal) self.buf6 = bytearray(6) self.buf8 = bytearray(8) self.w = 0 self.x = 0 self.y = 0 self.z = 0 def orient(self): if self.transpose != (0, 1, 2): a = self.transpose self._write(_AXIS_MAP_CONFIG, (a[2] << 4) + (a[1] << 2) + a[0]) if self.sign != (0, 0, 0): a = self.sign self._write(_AXIS_MAP_SIGN, a[2] + (a[1] << 1) + (a[0] << 2)) # Configuration: if a tuple is passed, convert to int using function from bno055_help.py def config(self, dev, value=None): if dev not in (ACC, MAG, GYRO): raise ValueError('Unknown device:', dev) if isinstance(value, tuple): value = self._tuple_to_int(dev, value) # Convert tuple to register value elif value is not None: raise ValueError('value must be a tuple or None.') last_mode = self.mode(CONFIG_MODE) self._write(_PAGE_REGISTER, 1) old_val = self._read(dev) if value is not None: self._write(dev, value) self._write(_PAGE_REGISTER, 0) self.mode(last_mode) return self._int_to_tuple(dev, old_val) # For use in ISR def iget(self, reg): if reg == 0x20: n = 4 buf = self.buf8 else: n = 3 buf = self.buf6 self._i2c.readfrom_mem_into(self.address, reg, buf) if n == 4: self.w = self._bytes_toint(buf[0], buf[1]) i = 2 else: self.w = 0 i = 0 self.x = self._bytes_toint(buf[i], buf[i+1]) self.y = self._bytes_toint(buf[i+2], buf[i+3]) self.z = self._bytes_toint(buf[i+4], buf[i+5])