diff --git a/scripts/libraries/bno055.py b/scripts/libraries/bno055.py new file mode 100644 index 000000000..5284ef6a2 --- /dev/null +++ b/scripts/libraries/bno055.py @@ -0,0 +1,98 @@ +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("