reset_val = 0
#print('RC 5 '+repr(vidro.current_rc_channels[4])+' RC 6 '+repr(vidro.current_rc_channels[5]))
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
#Reset of errors after each time control loop finishes
controller.I_error_alt = 0
controller.I_error_pitch = 0
controller.I_error_roll = 0
controller.I_error_yaw = 0
controller.previous_time_alt = (time.time() - controller.timer) * 10
controller.previous_time_yaw = (time.time() - controller.timer) * 10
controller.previous_time_xy = (time.time() - controller.timer) * 10
vidro.previous_error_alt = 0
vidro.previous_error_yaw = 0
vidro.previous_error_roll = 0
vidro.previous_error_pitch = 0
print('Armed Loop')
#Update of gains before going into control loop
if vidro.current_rc_channels[5] > 1600:
controller.update_gains() # Potentially move this into the main loop
# Auto Loop
while vidro.current_rc_channels[5] > 1600:
vidro.update_mavlink() # Grab updated rc channel values
print('Auto Loop')
# Seq. 0: Takeoff to 1 m and origin ###############=> Separate these commands maybe....maybe in all of them, just make a positioning thread maybe
if sequence == 0:
# Assign Commands
