-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathmw-logdata.py
More file actions
91 lines (80 loc) · 3.37 KB
/
mw-logdata.py
File metadata and controls
91 lines (80 loc) · 3.37 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
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
#!/usr/bin/env python
""" Drone Pilot - Control of MRUAV """
""" mw-logdata.py: Script that logs data from a vehicle with MultiWii flight controller and a MoCap system."""
__author__ = "Aldo Vargas"
__copyright__ = "Copyright 2015 Aldux.net"
__license__ = "GPL"
__version__ = "1.5"
__maintainer__ = "Aldo Vargas"
__status__ = "Development"
import time, datetime, csv, threading
from modules.pyMultiwii import MultiWii
import modules.UDPserver as udp
import modules.utils as utils
# MRUAV initialization
#vehicle = MultiWii("/dev/tty.usbserial-A801WZA1")
vehicle = MultiWii("/dev/ttyUSB0")
# RC commnads to be sent to the MultiWii
rcCMD = [1500,1500,1500,1000,1000,1000,1000,1000]
# Function to update commands and attitude to be called by a thread
def logit():
global vehicle, rcCMD
"""
Function to manage data, print it and save it in a csv file, to be run in a thread
"""
while True:
if udp.active:
print "UDP server is active..."
break
else:
print "Waiting for UDP server to be active..."
time.sleep(0.5)
try:
st = datetime.datetime.fromtimestamp(time.time()).strftime('%m_%d_%H-%M-%S')+".csv"
f = open("logs/mw-"+st, "w")
logger = csv.writer(f)
logger.writerow(('timestamp','roll','pitch','yaw','proll','ppitch','throttle','pyaw','x','y','z','attx','atty','attz'))
while True:
current = time.time()
elapsed = 0
rcCMD[0] = udp.message[0]
rcCMD[1] = udp.message[1]
rcCMD[2] = udp.message[2]
rcCMD[3] = udp.message[3]
#vehicle.sendCMD(16,MultiWii.SET_RAW_RC,rcCMD)
#time.sleep(0.005) # Time to allow the Naze32 respond the last attitude command
#vehicle.getData(MultiWii.ATTITUDE)
#vehicle.getData(MultiWii.RC)
vehicle.getData(MultiWii.RAW_IMU)
#print "Time to ask two commands -> %0.3f" % (time.time()-elapsed)
#print "%s %s" % (vehicle.attitude,rcCMD)
print "%s %s" % (vehicle.rawIMU,udp.message)
# Save log
row = (time.time(), \
#vehicle.attitude['angx'], vehicle.attitude['angy'], vehicle.attitude['heading'], \
vehicle.rawIMU['ax'], vehicle.rawIMU['ay'], vehicle.rawIMU['az'], vehicle.rawIMU['gx'], vehicle.rawIMU['gy'], vehicle.rawIMU['gz'], \
#vehicle.rcChannels['roll'], vehicle.rcChannels['pitch'], vehicle.rcChannels['throttle'], vehicle.rcChannels['yaw'], \
udp.message[0], udp.message[1], udp.message[3], udp.message[2], \
udp.message[8], udp.message[9], udp.message[10], \
udp.message[5], udp.message[4], udp.message[6] )
logger.writerow(row)
# 100hz loop
while elapsed < 0.01:
elapsed = time.time() - current
# End of the main loop
except Exception,error:
print "Error in logit thread: "+str(error)
f.close()
if __name__ == "__main__":
try:
logThread = threading.Thread(target=logit)
logThread.daemon=True
logThread.start()
udp.startTwisted()
except Exception,error:
print "Error on main: "+str(error)
vehicle.ser.close()
except KeyboardInterrupt:
print "Keyboard Interrupt, exiting."
exit()