-
Notifications
You must be signed in to change notification settings - Fork 0
/
dataSerial.py
120 lines (85 loc) · 2.9 KB
/
dataSerial.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
#! /usr/bin/python
# coding: utf-8
import serial
import numpy as np
import math as m
import matplotlib.pyplot as plt
import common
import nonblock as nb
from datetime import datetime
import csv
import sys
port_tty = sys.argv[1]
serial_dspic = serial.Serial(port=port_tty, baudrate=460800, timeout=15.0)
serial_dspic.bytesize = serial.EIGHTBITS #number of bits per bytes
serial_dspic.parity = serial.PARITY_NONE #set parity check: no parity
serial_dspic.stopbits = serial.STOPBITS_ONE #number of stop bits
angle_pitch = [0]
angle_roll = [0]
angle_yaw = [0]
rate_pitch = [0]
rate_roll = [0]
rate_yaw = [0]
integr_pitch = [0]
integr_roll = [0]
integr_yaw = [0]
control_pitch = [0]
control_roll = [0]
control_yaw = [0]
motor_power = [0]
battery = [0]
time_array = [0]
sensor_time = 5./1000;
angle_rate = 100.
t = nb.waitOnInput()
def main():
print "Hello!"
serial_dspic.write( '1' )
response = serial_dspic.read( 1 );
if response != '0':
exit()
print "Ok!"
fulltime = 0
t.start()
try:
while nb.program_run:
if t.is_alive():
data = serial_dspic.read( 2 * 14 )
angle_pitch.append( common.bytes_2_int16( data[0:2] ) / angle_rate )
angle_roll.append( common.bytes_2_int16( data[2:4] ) / angle_rate )
angle_yaw.append( common.bytes_2_int16( data[4:6] ) / angle_rate )
rate_pitch.append( common.bytes_2_int16( data[6:8] ) / angle_rate )
rate_roll.append( common.bytes_2_int16( data[8:10] ) / angle_rate )
rate_yaw.append( common.bytes_2_int16( data[10:12] ) / angle_rate )
integr_pitch.append( common.bytes_2_int16( data[12:14] ) )
integr_roll.append( common.bytes_2_int16( data[14:16] ) )
integr_yaw.append( common.bytes_2_int16( data[16:18] ) )
control_pitch.append( common.bytes_2_int16( data[18:20] ) )
control_roll.append( common.bytes_2_int16( data[20:22] ) )
control_yaw.append( common.bytes_2_int16( data[22:24] ) )
motor_power.append( common.bytes_2_int16( data[24:26] ) )
battery.append( common.bytes_2_int16( data[26:28] ) / 10.0 )
fulltime += sensor_time
time_array.append(fulltime)
else:
break
finally:
serial_dspic.write( '1' )
serial_dspic.close()
out = np.array([time_array, angle_pitch, angle_roll, angle_yaw, rate_pitch, rate_roll, rate_yaw, integr_pitch, integr_roll, integr_yaw, control_pitch, control_roll, control_yaw, motor_power, battery])
csvfile = "{:%Y%d%m_%H_%M_%S}".format(datetime.now())
csvfile = "logs/" + csvfile + ".csv"
with open(csvfile, "w") as output:
writer = csv.writer(output, lineterminator='\n')
writer.writerows(out.T)
plt.plot( time_array, rate_yaw, 'y-', label='Rate' )
plt.plot( time_array, angle_yaw, 'g-', label='Angle' )
plt.plot( time_array, integr_yaw, 'k-', label='Integr' )
plt.plot( time_array, control_yaw, 'r-', label='Control' )
plt.ylabel('Angle')
plt.xlabel('Time')
plt.grid()
plt.title('Serial')
plt.legend()
plt.show()
main()