return if line > 22 or line < 0: return #Print to screen using curses if col == 0: screen.addstr(line, 0, string) if col == 1: screen.addstr(line, 40, string) screen.refresh() logging.basicConfig(filename='flight_test.log', level=logging.DEBUG) vidro = Vidro(False, 1) vidro.connect() controller = PositionController(vidro) screen = curses.initscr() screen.clear() screen.refresh() switch = False plot.ion() timer = time.time() while vidro.current_rc_channels[4] > 1600:
#Check for bad inputs if col > 1 or col < 0: return if line > 22 or line < 0: return #Print to screen using curses if col == 0: screen.addstr(line, 0, string) if col == 1: screen.addstr(line, 40, string) screen.refresh() vidro = Vidro(False) vidro.connect() controller = PositionController(vidro) screen = curses.initscr() screen.clear() screen.refresh() RC_yaw_min = controller.base_rc_yaw RC_yaw_max = controller.base_rc_yaw RC_throttle_min = controller.base_rc_throttle RC_throttle_max = controller.base_rc_throttle RC_pitch_min = controller.base_rc_pitch RC_pitch_max = controller.base_rc_pitch RC_roll_min = controller.base_rc_roll RC_roll_max = controller.base_rc_roll
#Check for bad inputs if col > 1 or col < 0: return if line > 22 or line < 0: return #Print to screen using curses if col == 0: screen.addstr(line, 0, string) if col == 1: screen.addstr(line, 40, string) screen.refresh() vidro = Vidro(True) vidro.connect() controller = PositionController(vidro) screen = curses.initscr() screen.clear() screen.refresh() switch = False plot.ion() while vidro.current_rc_channels[4] > 1600: controller.update_gains()
#screen.refresh() #Filter for fencing values given by wand def filter_value(low,high,value): if value < low: value = low if value > high: value = high return value #Initialization of log logging.basicConfig(filename='demo.log', level=logging.DEBUG) #Creation of vidro and controller objects vidro = Vidro(False, 2) vidro.connect() controller = PositionController(vidro) #Setup the screen for curses #screen = curses.initscr() #screen.clear() #screen.refresh() #switch for knowing if first time out of control loop switch = False #Possibly needed for live plotting (Need to test) plot.ion() #Setup of timer
#Check for bad inputs
if col > 1 or col < 0:
return
if line > 22 or line < 0:
return
#Print to screen using curses
if col == 0:
screen.addstr(line, 0, string)
if col == 1:
screen.addstr(line, 40, string)
screen.refresh()
vidro = Vidro(False, 1)
vidro.connect()
controller = PositionController(vidro)
screen = curses.initscr()
screen.clear()
screen.refresh()
while True:
curses_print("Roll: " + str(vidro.current_rc_channels[0]),0,0)
curses_print("Pitch: " + str(vidro.current_rc_channels[1]),1,0)
curses_print("Throttle: " + str(vidro.current_rc_channels[2]),2,0)
curses_print("Yaw: " + str(vidro.current_rc_channels[3]),3,0)
time.sleep(.005)
screen.refresh()
""" Simple test for connecting to the vidro class. Some things like time may not be working """ from vidro_class import Vidro, ViconStreamer import sys, math, time import socket, struct, threading import curses import utm import matplotlib.pyplot as plot vidro = Vidro(True, 115200,"127.0.0.1:14551") vidro.connect() cycles_message = 0 cycles_change = 0 previous_time = 0 previous_rc = 0 while vidro.current_rc_channels[4] > 1500: while vidro.current_rc_channels[2] < 1800: print str(vidro.current_rc_channels[2]) + " " + str(vidro.rc_msg_time) + " " + str(cycles_message) + " " + str(cycles_change) vidro.set_rc_throttle(vidro.current_rc_channels[2]+10) vidro.get_mavlink() time.sleep(.01) if vidro.rc_msg_time < previous_time: cycles_message = 0 if vidro.current_rc_channels[2] != previous_rc: cycles_change = 0 cycles_change += 1
#Check for bad inputs if col > 1 or col < 0: return if line > 22 or line < 0: return #Print to screen using curses if col == 0: screen.addstr(line, 0, string) if col == 1: screen.addstr(line, 40, string) screen.refresh() vidro = Vidro(True) vidro.connect() controller = PositionController(vidro) screen = curses.initscr() screen.clear() screen.refresh() while vidro.current_rc_channels[4] > 1600: controller.update_gains() try: controller.rc_alt(500) #controller.rc_yaw(0) #controller.rc_xy(500,500)
