import serial
import time
import struct
from os import system, name
ser = serial.Serial('\\.\COM5', 115200, timeout=0)

SENSORS_PACKET_SIZE = 103

sensors = bytearray([0] * SENSORS_PACKET_SIZE)
magnetic_field = [0 for x in range(12)]

message = struct.pack(">bbb", -0x0F, 1, 0) # Enable magnetometer
ser.write(message)
reply = ser.read()
while len(reply) < 1:
	reply += ser.read()
time.sleep(0.1)

message = struct.pack(">bbb", -0x08, 1, 0) # Get all sensors

while(1):	

	ser.write(message)
	sensors = ser.read()
	while len(sensors) < SENSORS_PACKET_SIZE:
		sensors += ser.read()
	
	# Magnetometer
	magnetic_field[0] = struct.unpack("<f", struct.pack("<BBBB", sensors[24], sensors[25], sensors[26], sensors[27]))[0]
	magnetic_field[1] = struct.unpack("<f", struct.pack("<BBBB", sensors[28], sensors[29], sensors[30], sensors[31]))[0]
	magnetic_field[2] = struct.unpack("<f", struct.pack("<BBBB", sensors[32], sensors[33], sensors[34], sensors[35]))[0]

	print("magnetometer: " + str(magnetic_field[0]) + ", " + str(magnetic_field[1]) + ", " + str(magnetic_field[2]))

			
ser.close()