-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathtest-imu-plot.py
More file actions
52 lines (44 loc) · 1.63 KB
/
test-imu-plot.py
File metadata and controls
52 lines (44 loc) · 1.63 KB
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
#!/usr/bin/env python
"""test-imu-plot.py: Ask multiwii for raw IMU and plot it using matplotlib."""
from pyMultiwii import MultiWii
import time
import matplotlib.pyplot as plt
class Chart(object):
def __init__(self):
self.senses = 0
self.sb, self.axbuf, self.aybuf, self.azbuf = 0, 0, 0, 0
self.gxbuf, self.gybuf, self.gzbuf = 0, 0, 0
plt.show(block=False)
plt.ion()
self.fig = plt.figure(1, figsize=(19, 6))
plt.ylim([-1000, 1000])
plt.xlim([0, 300])
def plot(self, ax, ay, az, gx, gy, gz):
self.senses += 1
plt.plot([self.sb, self.senses], [self.axbuf, ax], color='r', label='AX')
plt.plot([self.sb, self.senses], [self.aybuf, ay], color='g', label='AY')
plt.plot([self.sb, self.senses], [self.azbuf, az], color='b', label='AZ')
plt.plot([self.sb, self.senses], [self.gxbuf, gx], color='y', label='GX')
plt.plot([self.sb, self.senses], [self.gybuf, gy], color='black', label='GY')
plt.plot([self.sb, self.senses], [self.gzbuf, gz], color='pink', label='GZ')
self.fig.canvas.draw()
self.sb, self.axbuf, self.aybuf, self.azbuf, self.gxbuf, self.gybuf, self.gzbuf = self.senses, ax, ay, az, gx, gy, gz
if __name__ == "__main__":
chart = Chart()
serialPort = "/dev/ttyUSB0"
#serialPort = "/dev/tty.SLAB_USBtoUART"
board = MultiWii(115200)
try:
while True:
board.getData(MultiWii.RAW_IMU)
#print board.rawIMU
t = float(board.rawIMU['timestamp'])
ax = board.rawIMU['ax']
ay = board.rawIMU['ay']
az = board.rawIMU['az']
gx = board.rawIMU['gx']
gy = board.rawIMU['gy']
gz = board.rawIMU['gz']
chart.plot(ax, ay, az, gx, gy, gz)
except Exception,error:
print "Error on Main: "+str(error)