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