Subversion Repositories Code-Repo

import serial

def twos_comp(val, bits):

	'''Convert given value from two's complement to integer'''

	if ((val & (1 << (bits-1))) != 0):

		val = val - (1 << bits)

	return val

if __name__ == '__main__':

	print("Attempting to connect to serial port...")

	# Connect to specified serial port

	ser = serial.Serial()

	ser.baudrate = 115200

	ser.port = "COM3"

	ser.timeout = 3 # Three second timeout

	try:

		ser.open()

		print("Connected to serial port.")

		# Loop reading in data from the sensor

		while(1):

			s = ser.read(21)

			# Ensure that the data properly ends in a newline

			if s[20] == ord('\n'):

				# Read in accelerometer data and convert from two's compliment

				A_X = ((s[1] << 8) | s[0])

				A_Y = ((s[3] << 8) | s[2])

				A_Z = ((s[5] << 8) | s[4])

				A_X_N = twos_comp(A_X, 16) >> 4

				A_Y_N = twos_comp(A_Y, 16) >> 4

				A_Z_N = twos_comp(A_Z, 16) >> 4

				# Read in gyroscope data and convert from two's compliment

				G_Y = (s[7] << 8) | s[6]

				G_X = (s[9] << 8) | s[8]

				G_Z = (s[11] << 8) | s[10]

				G_X_N = twos_comp(G_X, 16)

				G_Y_N = twos_comp(G_Y, 16)

				G_Z_N = twos_comp(G_Z, 16)

				# Read in magnetometer data and convert from two's compliment

				M_X = ((s[13] << 8) | s[12])

				M_Y = ((s[15] << 8) | s[14])

				M_Z = ((s[17] << 8) | s[16])

				M_X_N = twos_comp(M_X, 16)

				M_Y_N = twos_comp(M_Y, 16)

				M_Z_N = twos_comp(M_Z, 16)

				# Read in battery status

				B_H = s[18]

				B_L = s[19]

				# Print out the processed data

				print("A: %-6i %-6i %-6i   G: %-6i %-6i %-6i   M: %-6i %-6i %-6i   B: %u.%u" % \

					(A_X_N, A_Y_N, A_Z_N, G_X_N, G_Y_N, G_Z_N, M_X_N, M_Y_N, M_Z_N, B_H, B_L))

			else:

				break

	except:

		pass

	ser.close()