diff --git a/bno055/bno055.py b/bno055/bno055.py new file mode 100644 index 0000000..1407583 --- /dev/null +++ b/bno055/bno055.py @@ -0,0 +1,177 @@ +# 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]) diff --git a/bno055/bno055_base.py b/bno055/bno055_base.py new file mode 100644 index 0000000..fb726dc --- /dev/null +++ b/bno055/bno055_base.py @@ -0,0 +1,137 @@ +# bno055_base.py Minimal MicroPython driver for Bosch BNO055 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 + +import utime as time +import ustruct +from micropython import const + +_CHIP_ID = const(0xa0) + +_CONFIG_MODE = const(0) +_NDOF_MODE = const(0x0c) + +_POWER_NORMAL = const(0x00) +_POWER_LOW = const(0x01) +_POWER_SUSPEND = const(0x02) + +_MODE_REGISTER = const(0x3d) +_PAGE_REGISTER = const(0x07) +_CALIBRATION_REGISTER = const(0x35) +_TRIGGER_REGISTER = const(0x3f) +_POWER_REGISTER = const(0x3e) +_ID_REGISTER = const(0x00) + +class BNO055_BASE: + + def __init__(self, i2c, address=0x28, crystal=True, transpose=(0, 1, 2), sign=(0, 0, 0)): + self._i2c = i2c + self.address = address + self.crystal = crystal + self.mag = lambda : self.scaled_tuple(0x0e, 1/16) # microteslas (x, y, z) + self.accel = lambda : self.scaled_tuple(0x08, 1/100) # m.s^-2 + self.lin_acc = lambda : self.scaled_tuple(0x28, 1/100) # m.s^-2 + self.gravity = lambda : self.scaled_tuple(0x2e, 1/100) # m.s^-2 + self.gyro = lambda : self.scaled_tuple(0x14, 1/16) # deg.s^-1 + self.euler = lambda : self.scaled_tuple(0x1a, 1/16) # degrees (heading, roll, pitch) + self.quaternion = lambda : self.scaled_tuple(0x20, 1/(1<<14), bytearray(8), '> 6) & 0x03 # sys + s[1] = (cdata >> 4) & 0x03 # gyro + s[2] = (cdata >> 2) & 0x03 # accel + s[3] = cdata & 0x03 # mag + return s + + def calibrated(self): + s = self.cal_status() + # https://learn.adafruit.com/adafruit-bno055-absolute-orientation-sensor/device-calibration + return min(s[1:]) == 3 and s[0] > 0 + + # read byte from register, return int + def _read(self, memaddr, buf=bytearray(1)): # memaddr = memory location within the I2C device + self._i2c.readfrom_mem_into(self.address, memaddr, buf) + return buf[0] + + # write byte to register + def _write(self, memaddr, data, buf=bytearray(1)): + buf[0] = data + self._i2c.writeto_mem(self.address, memaddr, buf) + + # read n bytes, return buffer + def _readn(self, buf, memaddr): # memaddr = memory location within the I2C device + self._i2c.readfrom_mem_into(self.address, memaddr, buf) + return buf + + def mode(self, new_mode=None): + old_mode = self._read(_MODE_REGISTER) + if new_mode is not None: + self._write(_MODE_REGISTER, _CONFIG_MODE) # This is empirically necessary if the mode is to be changed + time.sleep_ms(20) # Datasheet table 3.6 + if new_mode != _CONFIG_MODE: + self._write(_MODE_REGISTER, new_mode) + time.sleep_ms(10) # Table 3.6 + return old_mode + + def external_crystal(self): + return bool(self._read(_TRIGGER_REGISTER) & 0x80) diff --git a/examples/bno055_test.py b/examples/bno055_test.py new file mode 100644 index 0000000..738eb22 --- /dev/null +++ b/examples/bno055_test.py @@ -0,0 +1,26 @@ +# bno055_test.py Simple test program for MicroPython bno055 driver + +# Copyright (c) Peter Hinch 2019 +# Released under the MIT licence. + +import machine +import time +from bno055 import * +# Pyboard hardware I2C +i2c = machine.I2C(1) +# ESP8266 soft I2C +# i2c = machine.I2C(-1, scl=machine.Pin(2), sda=machine.Pin(0)) +imu = BNO055(i2c) +calibrated = False +while True: + time.sleep(1) + if not calibrated: + calibrated = imu.calibrated() + print('Calibration required: sys {} gyro {} accel {} mag {}'.format(*imu.cal_status())) + print('Temperature {}°C'.format(imu.temperature())) + print('Mag x {:5.0f} y {:5.0f} z {:5.0f}'.format(*imu.mag())) + print('Gyro x {:5.0f} y {:5.0f} z {:5.0f}'.format(*imu.gyro())) + print('Accel x {:5.1f} y {:5.1f} z {:5.1f}'.format(*imu.accel())) + print('Lin acc. x {:5.1f} y {:5.1f} z {:5.1f}'.format(*imu.lin_acc())) + print('Gravity x {:5.1f} y {:5.1f} z {:5.1f}'.format(*imu.gravity())) + print('Heading {:4.0f} roll {:4.0f} pitch {:4.0f}'.format(*imu.euler()))