#Setup Logging file 2 (For Vicon Values)
logging.basicConfig(level=logging.INFO)
logger2 = logging.getLogger('2')
handler2 = logging.FileHandler(filename = 'logs/Flight12_2.log', mode = 'w')
handler2.setLevel(logging.INFO)
logger2.addHandler(handler2)
logger2.info('Starting Log 2')
print('Heading to main loop')
#####################
# Main Software Loop#
#####################
while(1):
vidro.update_mavlink() # Grab updated rc channel values. This is the right command for it, but it doesn't always seem to update RC channels
#Auto Loop
#print('Outer Loop')
while vidro.current_rc_channels[4] > 1600:
#Get Timestamp
current_time = time.time() - start_time
#Logging for file 1
logger1.info('Time is %f' %current_time)
logger1.info('Commanded RC Throttle is %f' %vidro.current_rc_channels[2])
logger1.info('Error z is %f' %error_z)
#Logging for file 2
"""
plot_error_pitch_I.append(controller.I_error_pitch)
plot_time_pitch.append(controller.previous_time_xy)
plot_error_pitch_D.append(controller.D_error_pitch)
plot_rc_pitch.append(vidro.current_rc_channels[1])
plot_error_roll.append(controller.error_roll)
plot_error_roll_I.append(controller.I_error_roll)
plot_time_roll.append(controller.previous_time_xy)
plot_rc_roll.append(vidro.current_rc_channels[0])
plot_error_roll_D.append(controller.D_error_roll)
plot_x_current.append(vidro.get_position()[0])
plot_y_current.append(vidro.get_position()[1])
switch = True
vidro.update_mavlink()
vidro.rc_all_reset()
vidro.update_mavlink()
#Erase Plots
if switch == True:
"""
plot.figure(1).clf()
plot.figure(1)
plot.xlabel("Time(sec)")
plot.ylabel("Error(rads)")
plot.title("Yaw")
plot.plot(plot_time_yaw,plot_error_yaw)
"""
"""
