-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathtest-imu-plot2.py
More file actions
71 lines (60 loc) · 1.73 KB
/
test-imu-plot2.py
File metadata and controls
71 lines (60 loc) · 1.73 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
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
#!/usr/bin/env python
"""test-imu-plot.py: Ask multiwii for raw IMU and plot it using pyqtgraph."""
from pyMultiwii import MultiWii
import pyqtgraph as pg
from pyqtgraph.Qt import QtCore, QtGui
board = MultiWii("/dev/ttyUSB0")
win = pg.GraphicsWindow()
win.setWindowTitle('MultiWii IMU plotting')
p1 = win.addPlot()
win.nextRow()
p2 = win.addPlot()
data1 = [0] * 300
data2 = [0] * 300
data3 = [0] * 300
data4 = [0] * 300
data5 = [0] * 300
data6 = [0] * 300
curve1 = p1.plot(data1, name="ax", pen=(255,0,0))
curve2 = p1.plot(data2, name="ay", pen=(0,255,0))
curve3 = p1.plot(data3, name="az", pen=(0,0,255))
curve4 = p2.plot(data1, name="gx", pen=(255,0,0))
curve5 = p2.plot(data2, name="gy", pen=(0,255,0))
curve6 = p2.plot(data3, name="gz", pen=(0,0,255))
def update1():
global data1, curve1, board
board.getData(MultiWii.RAW_IMU)
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']
data1[:-1] = data1[1:]
data1[-1] = ax
data2[:-1] = data2[1:]
data2[-1] = ay
data3[:-1] = data3[1:]
data3[-1] = az
data4[:-1] = data4[1:]
data4[-1] = gx
data5[:-1] = data5[1:]
data5[-1] = gy
data6[:-1] = data6[1:]
data6[-1] = gz
curve1.setData(data1)
curve2.setData(data2)
curve3.setData(data3)
curve4.setData(data4)
curve5.setData(data5)
curve6.setData(data6)
def update():
update1()
timer = pg.QtCore.QTimer()
timer.timeout.connect(update)
timer.start(10)
if __name__ == '__main__':
import sys
if (sys.flags.interactive != 1) or not hasattr(QtCore, 'PYQT_VERSION'):
QtGui.QApplication.instance().exec_()