def main():
if len(sys.argv) < 2:
usage()
print 'ERROR: not enough paramters'
exit(1)
port_name = sys.argv[1]
log_filename = None
if len(sys.argv) >= 3:
log_filename = sys.argv[2]
if os.path.exists(log_filename):
print "ERROR: File",log_filename,"already exists"
return
else:
print 'Starting logging to file "', log_filename, '"'
#
# Initialize serial connection
#
print 'Initializing serial connection on', port_name
ser = serial.Serial(port_name, 115200)
pygame.init()
screen = pygame.display.set_mode((640,480))
vectors_FL = PygameVectors(screen,120,260,100,100)
vectors_FR = PygameVectors(screen,160,120,100,100)
vectors_RR = PygameVectors(screen,520,190,100,100)
plotter = Plotter(50,3, log_filename)
###plotter.add_plot('rd_pitch',0.01)
###plotter.add_plot('rd_roll',0.01)
###plotter.add_plot('rd_forward',0.01)
###plotter.add_plot('rd_yaw',0.01)
###plotter.add_plot('rd_throttle',0.01)
###plotter.add_plot('rd_aux0',0.01)
###plotter.add_plot('rd_aux1',0.01)
###plotter.add_plot('rd_aux2',0.01)
plotter.add_plot('rd_pitch_demand',1)
plotter.add_plot('rd_roll_demand',1)
plotter.add_plot('rd_forward_demand',1)
plotter.add_plot('rd_yaw_demand',1)
plotter.add_plot('rd_throttle_demand',3)
#plotter.add_plot('imu_yaw',0.1)
#plotter.add_plot('imu_pitch',0.1)
#plotter.add_plot('imu_roll',1)
#plotter.add_plot('imu_yaw_rate',1)
#plotter.add_plot('imu_pitch_rate',1)
#plotter.add_plot('imu_roll_rate',1)
#plotter.add_plot('mx_FL_vert',1)
#plotter.add_plot('mx_FR_vert',1)
#plotter.add_plot('mx_RR_vert',1)
#plotter.add_plot('mx_FL_horz',1)
###plotter.add_plot('mx_FR_horz',1)
###plotter.add_plot('mx_RR_horz',1)
#plotter.add_plot('mt_FL_dem',1) # MoTor Front Left demand
#plotter.add_plot('mt_FR_dem',1)
#plotter.add_plot('mt_RR_dem',1)
#plotter.add_plot('an_FL_dem',1) # ANgle Front Left demand
#plotter.add_plot('an_FR_dem',1)
###plotter.add_plot('an_RR_dem',1)
###plotter.add_plot('mt_FL_out',0.001) # MoTor Front Left output
###plotter.add_plot('mt_FR_out',0.001)
###plotter.add_plot('mt_RR_out',0.001)
###plotter.add_plot('an_FL_out',0.001) # ANgle Front Left output
###plotter.add_plot('an_FR_out',0.001)
###plotter.add_plot('an_RR_out',0.001)
#plotter.add_plot('debug_A',1)
#plotter.add_plot('debug_B',1)
#plotter.add_plot('debug_C',1)
#plotter.add_plot('debug_D',1)
###plotter.add_plot('debug_E',1)
###plotter.add_plot('debug_F',1)
plotter.do_legend()
'''
fig = pylab.figure()
ax = Axes(fig, [.1,.1,.8,.8])
fig.add_axes(ax)
l = Line2D([0,1],[0,1])
ax.add_line(l)
pylab.draw()
'''
comms_prot = comms_protocol.CommsProtocol()
i = 0
while True:
raw = ser.read()
messages = comms_prot.decode(raw)
for msg in messages:
i += 1
#pylab.figure(fig.number)
#x = math.sin(i/1000.0)
#y = math.cos(i/1000.0)
#pylab.cla()
#ax.add_line(Line2D([0,x],[0,y]))
#print '-------------'
#print 'len(raw):',len(raw)
try:
# 15*H = 30 bytes
# 29*f = 116 bytes
# 146 bytes total
telemetry = struct.unpack("<HHHHHHHHHfffffffffffffffffffffffHHHHHHffffff",msg);
except:
print "ERROR: Invalid data read. Data len:", len(raw), "Expected: 146 bytes"
continue
rd_pitch = telemetry[1]
rd_roll = telemetry[2]
rd_forward = telemetry[3]
rd_yaw = telemetry[4]
rd_throttle = telemetry[5]
rd_aux0 = telemetry[6]
rd_aux1 = telemetry[7]
rd_aux2 = telemetry[8]
rd_pitch_demand = telemetry[9]
rd_roll_demand = telemetry[10]
rd_forward_demand = telemetry[11]
rd_yaw_demand = telemetry[12]
rd_throttle_demand = telemetry[13]
imu_yaw = telemetry[14]
imu_pitch = telemetry[15]
imu_roll = telemetry[16]
imu_yaw_rate = telemetry[17]
imu_pitch_rate = telemetry[18]
imu_roll_rate = telemetry[19]
mx_FL_vert = telemetry[20]
mx_FR_vert = telemetry[21]
mx_RR_vert = telemetry[22]
mx_FL_horz = telemetry[23]
mx_FR_horz = telemetry[24]
mx_RR_horz = telemetry[25]
mt_FL_dem = telemetry[26]
mt_FR_dem = telemetry[27]
mt_RR_dem = telemetry[28]
an_FL_dem = telemetry[29]
an_FR_dem = telemetry[30]
an_RR_dem = telemetry[31]
mt_FL_out = telemetry[32]
mt_FR_out = telemetry[33]
mt_RR_out = telemetry[34]
an_FL_out = telemetry[35]
an_FR_out = telemetry[36]
an_RR_out = telemetry[37]
debug_A = telemetry[38]
debug_B = telemetry[39]
debug_C = telemetry[40]
debug_D = telemetry[41]
debug_E = telemetry[42]
debug_F = telemetry[43]
'''
print 'A: ', debug_A, ' ',
print 'B: ', debug_B, ' ',
print 'C: ', debug_C, ' ',
print 'D: ', debug_D, ' ',
print 'E: ', debug_E, ' ',
print 'F: ', debug_F, ' '
'''
#
#
# Thrust Visualization Section
#
#
screen.fill( (0,0,0) )
# FL Vector
vectors_FL.draw_grid( -1, 1, 0, 1, 0.1 )
vectors_FL.draw_coord( -1, 1, 0, 1)
vectors_FL.draw_line( (255,0,0),
0,0,
0,mx_FL_vert)
vectors_FL.draw_line( (255,100,100),
0,0,
mx_FL_horz,0)
x = math.sin(an_FL_dem) * mt_FL_dem
y = math.cos(an_FL_dem) * mt_FL_dem
vectors_FL.draw_line( (255,255,0),
0,0,
x,y)
# FR Vector
vectors_FR.draw_grid( -1, 1, 0, 1, 0.1 )
vectors_FR.draw_coord( -1, 1, 0, 1)
vectors_FR.draw_line( (255,0,0),
0,0,
0,mx_FR_vert)
vectors_FR.draw_line( (255,100,100),
0,0,
mx_FR_horz,0)
x = math.sin(an_FR_dem) * mt_FR_dem
y = math.cos(an_FR_dem) * mt_FR_dem
vectors_FR.draw_line( (255,255,0),
0,0,
x,y)
#RR Vector
vectors_RR.draw_grid( -1, 1, 0, 1, 0.1 )
vectors_RR.draw_coord( -1, 1, 0, 1)
vectors_RR.draw_line( (255,0,0),
0,0,
0,mx_RR_vert)
vectors_RR.draw_line( (255,100,100),
0,0,
mx_RR_horz,0)
x = math.sin(an_RR_dem) * mt_RR_dem
y = math.cos(an_RR_dem) * mt_RR_dem
vectors_RR.draw_line( (255,255,0),
0,0,
x,y)
# RR Vector
'''
vectors_RR.draw_grid( -1, 1, 0, 1, 0.1 )
vectors_RR.draw_coord( -1, 1, 0, 1)
vectors_RR.draw_line( (255,0,0),
0,0,
0,mt_RR_dem)
'''
#vectors_RR.draw_line( (255,100,100),
# 0,0,
# debug_D,0)
#x = math.sin(an_RR_dem) * mt_RR_dem
#y = math.cos(an_RR_dem) * mt_RR_dem
#vectors_RR.draw_line( (255,255,0),
# 0,0,
# x,y)
pygame.display.flip()
#
#
# Plotter Section
#
#
plotter.append('rd_pitch',rd_pitch)
plotter.append('rd_roll',rd_roll)
plotter.append('rd_forward',rd_forward)
plotter.append('rd_yaw',rd_yaw)
plotter.append('rd_throttle',rd_throttle)
plotter.append('rd_aux0',rd_aux0)
plotter.append('rd_aux1',rd_aux1)
plotter.append('rd_aux2',rd_aux2)
plotter.append('rd_pitch_demand',rd_pitch_demand)
plotter.append('rd_roll_demand',rd_roll_demand)
plotter.append('rd_forward_demand',rd_forward_demand)
plotter.append('rd_yaw_demand',rd_yaw_demand)
plotter.append('rd_throttle_demand',rd_throttle_demand)
plotter.append('imu_yaw',imu_yaw)
plotter.append('imu_pitch',imu_pitch)
plotter.append('imu_roll',imu_roll)
plotter.append('imu_yaw_rate',imu_yaw_rate)
plotter.append('imu_pitch_rate',imu_pitch_rate)
plotter.append('imu_roll_rate',imu_roll_rate)
plotter.append('mt_FL_dem',mt_FL_dem)
plotter.append('mt_FR_dem',mt_FR_dem)
plotter.append('mt_RR_dem',mt_RR_dem)
plotter.append('an_FL_dem',an_FL_dem)
plotter.append('an_FR_dem',an_FR_dem)
plotter.append('an_RR_dem',an_RR_dem)
plotter.append('mt_FL_out',mt_FL_out)
plotter.append('mt_FR_out',mt_FR_out)
plotter.append('mt_RR_out',mt_RR_out)
plotter.append('an_FL_out',an_FL_out)
plotter.append('an_FR_out',an_FR_out)
plotter.append('an_RR_out',an_RR_out)
plotter.append('debug_A',debug_A)
plotter.append('debug_B',debug_B)
plotter.append('debug_C',debug_C)
plotter.append('debug_D',debug_D)
plotter.append('debug_E',debug_E)
plotter.append('debug_F',debug_F)
plotter.save_log()
if i % 5 == 0:
#print 'mt_FL_out', mt_FL_out
#print 'mt_FR_out', mt_FR_out
#print 'mt_RR_out', mt_RR_out
print 'imu_yaw %0.3f' % (imu_yaw,), ' ',
print 'imu_pitch', imu_pitch, ' ',
print 'imu_roll', imu_roll
plotter.draw_plots()
