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
#Update of gains before going into control loop
if vidro.current_rc_channels[5] > 1600:
controller.update_gains()
#control loop
while vidro.current_rc_channels[5] > 1600:
#Get the position of the wand
try:
target_x = vidro.get_vicon()[4]
target_y = vidro.get_vicon()[5]
target_z = vidro.get_vicon()[6]
except:
logging.error('Unable to get position data from the vicon for wand')
pass
#filter position of wand between values
target_z = filter_value(1000,5000,target_z)
target_x = filter_value(-2000,2000,target_x)
target_y = filter_value(-2000,2000,target_y)
#Send control values
try:
controller.rc_alt(target_z)
controller.rc_yaw(0)
