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(19)
                        # Ensure that the data properly ends in a newline
                        if s[18] == 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)

                                # Print out the processed data
                                print("A: %-6i %-6i %-6i    G: %-6i %-6i %-6i    M: %-6i %-6i %-6i" % \
                                        (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))
                        else:
                                break
        except:
                pass
        ser.close()