| 0,0 → 1,55 |
|---|
| 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() |