import struct import pyb 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 AXIS_P0 = bytes([0x21, 0x04]) AXIS_P1 = bytes([0x24, 0x00]) AXIS_P2 = bytes([0x24, 0x06]) AXIS_P3 = bytes([0x21, 0x02]) AXIS_P4 = bytes([0x24, 0x03]) AXIS_P5 = bytes([0x21, 0x01]) AXIS_P6 = bytes([0x21, 0x07]) AXIS_P7 = bytes([0x24, 0x05]) _MODE_REGISTER = 0x3d _POWER_REGISTER = 0x3e _AXIS_MAP_CONFIG = 0x41 class BNO055: def __init__(self, i2c, address=0x28, mode = NDOF_MODE, axis = AXIS_P4): self.i2c = i2c self.address = address if self.read_id() != bytes([0xA0, 0xFB, 0x32, 0x0F]): raise RuntimeError('Failed to find expected ID register values. Check wiring!') self.operation_mode(CONFIG_MODE) self.system_trigger(0x20)# reset pyb.delay(700) self.power_mode(0x00)#POWER_NORMAL self.axis(axis) self.page(0) pyb.delay(10) self.operation_mode(mode) self.system_trigger(0x80) # external oscillator pyb.delay(200) def read_registers(self, register, size=1): return(self.i2c.readfrom_mem(self.address, register, size)) def write_registers(self, register, data): self.i2c.writeto_mem(self.address, register, data) def operation_mode(self, mode=None): if mode: self.write_registers(_MODE_REGISTER, bytes([mode])) else: return(self.read_registers(_MODE_REGISTER, 1)[0]) def system_trigger(self, data): self.write_registers(0x3f, bytes([data])) def power_mode(self, mode=None): if mode: self.write_registers(_POWER_REGISTER, bytes([mode])) else: return(self.read_registers(_POWER_REGISTER, 1)) def page(self, num=None): if num: self.write_registers(0x3f, bytes([num])) else: self.read_registers(0x3f) def temperature(self): return(self.read_registers(0x34, 1)[0]) def read_id(self): return(self.read_registers(0x00, 4)) def axis(self, placement=None): if placement: self.write_registers(_AXIS_MAP_CONFIG, placement) else: return(self.read_registers(_AXIS_MAP_CONFIG, 2)) def quaternion(self): data = struct.unpack("