#import libraries
import u3
import time

#set these to however you have your u3 connected to the bno055
dio_sda = 0
dio_sdl = 1

bno055_slave_address = 0x28
ndof_mode_addresses = [0x3D, 0X0C]

#data field names
field_names = ( \
    "accel_x" \
  , "accel_y" \
  , "accel_z" \
  , "mag_x" \
  , "mag_y" \
  , "mag_z" \
  , "gyro_x" \
  , "gyro_y" \
  , "gyro_z" \
  , "euler_h" \
  , "euler_r" \
  , "euler_p" \
  , "quaternion_w" \
  , "quaternion_x" \
  , "quaternion_y" \
  , "quaternion_z" \
  , "linear_accel_x" \
  , "linear_accel_y" \
  , "linear_accel_z" \
  , "gravity_x" \
  , "gravity_y" \
  , "gravity_z" \
)


#define a function to convert MSB/LSB to int
def Convert_U8toF32(a, b):
	value_u16 = int(a) + int(b) * 256
	if value_u16 >= 32768:
		value = value_u16 - 65536
	else:
		value = value_u16
	return(value)

#connect to u3
d = u3.U3()

#initial configuration
#	resets the connection & enters ndof mode
d.i2c(
		Address = bno055_slave_address \
		, I2CBytes = ndof_mode_addresses \
		, EnableClockStretching = True \
		, NoStopWhenRestarting = False \
		, ResetAtStart = True \
		, SpeedAdjust = 0 \
		, SDAPinNum = dio_sda \
		, SCLPinNum = dio_sdl \
		, NumI2CBytesToReceive = 0 \
		, AddressByte = None \
)

#loop for 2s, reading all data (except temperature)
#  starts at 0x08 and reads 2 bytes per field
#  for a subset of readings, see addresses at: [https://github.com/adafruit/Adafruit_Python_BNO055/blob/master/Adafruit_BNO055/BNO055.py]
start = time.time()
while time.time() < (start+2):
	rx = d.i2c(
		Address = bno055_slave_address \
		, I2CBytes = [0x08] \
		, EnableClockStretching = True \
		, NoStopWhenRestarting = False \
		, ResetAtStart = False \
		, SpeedAdjust = 0 \
		, SDAPinNum = dio_sda \
		, SCLPinNum = dio_sdl \
		, NumI2CBytesToReceive = len(field_names)*2 \
		, AddressByte = None \
	)
	rx = rx['I2CBytes']
	#data received as LSB/MSB, convert to int
	values = dict()
	for i in range(len(field_names)):
		values[field_names[i]] = Convert_U8toF32(rx[(i+1)*2-2], rx[(i+1)*2-1])
	#
	print(values)