if vidro.vicon_error == True:
curses_print("Vicon Error: " + str(vidro.vicon_error),1,0)
else:
curses_print("Vicon time: " + str(vidro.vicon_time),1,0)
#Print alt data
curses_print("Throttle RC Override: " + str(vidro.current_rc_overrides[2]), 5, 1)
curses_print("Throttle RC Level: " + str(vidro.current_rc_channels[2]), 6, 1)
curses_print("Error: " + str(controller.error_alt), 7, 1)
curses_print("Altitude:" + str(vidro.get_position()[2]), 8, 1)
curses_print("T: "+ str(int(vidro.base_rc_throttle+controller.error_alt*controller.alt_K_P+controller.I_error_alt*controller.alt_K_I)) + " = "+ str(vidro.base_rc_throttle) + " + " + str(controller.error_alt*controller.alt_K_P) + " + " + str(controller.I_error_alt*controller.alt_K_I) + " + " + str(controller.D_error_alt*controller.alt_K_D), 19, 0)
#Print yaw data
curses_print("Yaw RC Level: " + str(vidro.current_rc_channels[3]), 5, 0)
curses_print("Error: " + str(controller.error_yaw), 6, 0)
curses_print("raw vicon : " + str(vidro.get_vicon()[6]), 7, 0)
curses_print("Heading Radians: " + str(vidro.get_yaw_radians()), 8, 0)
curses_print("Heading Degrees: " + str(vidro.get_yaw_degrees()), 9, 0)
curses_print("Y: "+ str(int(vidro.base_rc_yaw+controller.error_yaw*controller.yaw_K_P+controller.I_error_yaw*controller.yaw_K_I)) + " = "+ str(vidro.base_rc_yaw) + " + " + str(controller.error_yaw*controller.yaw_K_P) + " + " + str(controller.I_error_yaw*controller.yaw_K_I) + " + " + str(controller.D_error_yaw*controller.yaw_K_D), 20, 0)
#Print pitch and roll
curses_print("Pitch RC Level: " + str(vidro.current_rc_channels[1]), 11, 0)
curses_print("Roll RC Level: " + str(vidro.current_rc_channels[0]), 11, 1)
curses_print("Pitch: " + str(vidro.get_pitch()), 12, 0)
curses_print("Roll: " + str(vidro.get_roll()), 12, 1)
curses_print("X Error: " + str(round(controller.error_x)), 15, 0)
curses_print("Y Error: " + str(round(controller.error_y)), 15, 1)
curses_print("Roll Error: " + str(round(controller.error_roll)), 13, 1)
curses_print("Pitch Error: " + str(round(controller.error_pitch)), 13, 0)
curses_print("P: " + str(int(vidro.base_rc_pitch+controller.error_pitch*controller.pitch_K_P+controller.I_error_pitch*controller.pitch_K_I+controller.D_error_pitch*controller.pitch_K_D)) + " = " + str(vidro.base_rc_pitch) + " + " + str(controller.error_pitch*controller.pitch_K_P) + " + " + str(controller.I_error_pitch*controller.pitch_K_I) + " + " + str(controller.D_error_pitch*controller.pitch_K_D), 21, 0)
curses_print("R: " + str(int(vidro.base_rc_roll+controller.error_roll*controller.roll_K_P+controller.I_error_roll*controller.roll_K_I+controller.D_error_roll*controller.roll_K_D)) + " = " + str(vidro.base_rc_roll) + " + " + str(controller.error_roll*controller.roll_K_P) + " + " + str(controller.I_error_roll*controller.roll_K_I) + " + " + str(controller.D_error_roll*controller.roll_K_D), 22, 0)
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)