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)
"""
"""
plot.figure(2).clf()
plot.figure(2)
