-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathimu_usb.py
150 lines (135 loc) · 4.68 KB
/
imu_usb.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
# coding:UTF-8
# Version: V1.0.1
import serial
ACCData = [0.0]*8
GYROData = [0.0]*8
AngleData = [0.0]*8
FrameState = 0 # What is the state of the judgment
Bytenum = 0 # Read the number of digits in this paragraph
CheckSum = 0 # Sum check bit
a = [0.0]*3
w = [0.0]*3
Angle = [0.0]*3
def DueData(inputdata): # New core procedures, read the data partition, each read to the corresponding array
global FrameState # Declare global variables
global Bytenum
global CheckSum
global acc
global gyro
global Angle
for data in inputdata: # Traversal the input data
if FrameState == 0: # When the state is not determined, enter the following judgment
if data == 0x55 and Bytenum == 0: # When 0x55 is the first digit, start reading data and increment bytenum
CheckSum = data
Bytenum = 1
continue
elif data == 0x51 and Bytenum == 1: # Change the frame if byte is not 0 and 0x51 is identified
CheckSum += data
FrameState = 1
Bytenum = 2
elif data == 0x52 and Bytenum == 1:
CheckSum += data
FrameState = 2
Bytenum = 2
elif data == 0x53 and Bytenum == 1:
CheckSum += data
FrameState = 3
Bytenum = 2
elif FrameState == 1: # acc
if Bytenum < 10: # Read 8 data
ACCData[Bytenum-2] = data # Starting from 0
CheckSum += data
Bytenum += 1
else:
if data == (CheckSum & 0xff): # verify check bit
acc = get_acc(ACCData)
CheckSum = 0 # Each data is zeroed and a new circular judgment is made
Bytenum = 0
FrameState = 0
elif FrameState == 2: # gyro
if Bytenum < 10:
GYROData[Bytenum-2] = data
CheckSum += data
Bytenum += 1
else:
if data == (CheckSum & 0xff):
gyro = get_gyro(GYROData)
CheckSum = 0
Bytenum = 0
FrameState = 0
elif FrameState == 3: # angle
if Bytenum < 10:
AngleData[Bytenum-2] = data
CheckSum += data
Bytenum += 1
else:
if data == (CheckSum & 0xff):
Angle = get_angle(AngleData)
result = acc+gyro+Angle
print(
"acc:%10.3f %10.3f %10.3f \ngyro:%10.3f %10.3f %10.3f \nangle:%10.3f %10.3f %10.3f" % result)
CheckSum = 0
Bytenum = 0
FrameState = 0
def get_acc(datahex):
axl = datahex[0]
axh = datahex[1]
ayl = datahex[2]
ayh = datahex[3]
azl = datahex[4]
azh = datahex[5]
k_acc = 16.0
acc_x = (axh << 8 | axl) / 32768.0 * k_acc
acc_y = (ayh << 8 | ayl) / 32768.0 * k_acc
acc_z = (azh << 8 | azl) / 32768.0 * k_acc
if acc_x >= k_acc:
acc_x -= 2 * k_acc
if acc_y >= k_acc:
acc_y -= 2 * k_acc
if acc_z >= k_acc:
acc_z -= 2 * k_acc
return acc_x, acc_y, acc_z
def get_gyro(datahex):
wxl = datahex[0]
wxh = datahex[1]
wyl = datahex[2]
wyh = datahex[3]
wzl = datahex[4]
wzh = datahex[5]
k_gyro = 2000.0
gyro_x = (wxh << 8 | wxl) / 32768.0 * k_gyro
gyro_y = (wyh << 8 | wyl) / 32768.0 * k_gyro
gyro_z = (wzh << 8 | wzl) / 32768.0 * k_gyro
if gyro_x >= k_gyro:
gyro_x -= 2 * k_gyro
if gyro_y >= k_gyro:
gyro_y -= 2 * k_gyro
if gyro_z >= k_gyro:
gyro_z -= 2 * k_gyro
return gyro_x, gyro_y, gyro_z
def get_angle(datahex):
rxl = datahex[0]
rxh = datahex[1]
ryl = datahex[2]
ryh = datahex[3]
rzl = datahex[4]
rzh = datahex[5]
k_angle = 180.0
angle_x = (rxh << 8 | rxl) / 32768.0 * k_angle
angle_y = (ryh << 8 | ryl) / 32768.0 * k_angle
angle_z = (rzh << 8 | rzl) / 32768.0 * k_angle
if angle_x >= k_angle:
angle_x -= 2 * k_angle
if angle_y >= k_angle:
angle_y -= 2 * k_angle
if angle_z >= k_angle:
angle_z -= 2 * k_angle
return angle_x, angle_y, angle_z
if __name__ == '__main__':
port = '/dev/ttyUSB0' # USB serial port
baud = 9600 # Same baud rate as the INERTIAL navigation module
ser = serial.Serial(port, baud, timeout=0.5)
print("Serial is Opened:", ser.is_open)
while(1):
datahex = ser.read(33)
DueData(datahex)