#!/usr/bin/env python # -*- coding: utf-8 -*- from vidro import Vidro, ViconStreamer from position_controller import PositionController import sys, math, time import socket, struct, threading import numpy as np import cv2 #Setup of vidro and controller vidro = Vidro(False, 1) flight_ready = vidro.connect() controller = PositionController(vidro) start_time = time.time() previous_time = time.time() cycle = 0 controller.update_gains() sequence = 0 seq0_cnt = 0 seq1_cnt = 0 seq2_cnt = 0 seq3_cnt = 0 pos_bound_err = 300 yaw_bound_err = 0.5 yaw = 0
#!/usr/bin/env python # -*- coding: utf-8 -*- from vidro import Vidro, ViconStreamer from position_controller import PositionController import sys, math, time import socket, struct, threading import numpy as np import Adafruit_BBIO.ADC as ADC import logging ############################### #Setup of vidro and controller# ############################### vidro = Vidro(False, 1) flight_ready = vidro.connect_vicon() vidro.connect_mavlink() controller = PositionController(vidro) start_time = time.time() #Load gains controller.update_gains() #################### # Controller Values# #################### take_off_alt = 1000 # Pos Commands
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) #Setup of vidro and controller vidro = Vidro(False, 1) flight_ready = vidro.connect() controller = PositionController(vidro) #Initalization of curses screen = curses.initscr() screen.clear() screen.refresh() plot.ion() switch = False timer = time.time() while (vidro.current_rc_channels[4] > 1600) and (flight_ready == True):
#image_data = [cx ,cy, area, min_con_x, max_con_x, min_con_y, max_con_y, out_of_bounds, num_objects] image_data = [cx,cy,area_max,num_objects] return image_data def get_camera_frame(): global frame global new_frame _, frame = cap.read() frame = cv2.flip(frame,-1,-1) new_frame = True ############################### #Setup of vidro and controller# ############################### vidro = Vidro(False, 1) flight_ready = vidro.connect() controller = PositionController(vidro) start_time = time.time() # Load gains controller.update_gains() #################### # Controller Values# #################### take_off_alt = 1000 # Pos Commands x_com = 0 y_com = 0 alt_com = 0
# -*- coding: utf-8 -*- from vidro import Vidro, ViconStreamer from position_controller import PositionController import sys, math, time import socket, struct, threading import numpy as np import cv2 import logging #Start of a log logging.basicConfig(filename='bft_log.log', level=logging.INFO) logging.info('Starting') #Setup of vidro and controller vidro = Vidro(False, 1) flight_ready = vidro.connect() controller = PositionController(vidro) # Start Time for Logging global start_time start_time = time.time() cycle = 0 controller.update_gains() sequence = 0 seq0_cnt = 0 seq1_cnt = 0 seq2_cnt = 0 seq3_cnt = 0
cy = 0 cv2.circle(frame,(cx,cy),5,(255,0,0),-1) #print 'Num Objs: ',repr(num_objects),' Cx: ',repr(cx),' Cy: ',repr(cy),' Area: ',repr(area_max) #image_data = [cx ,cy, area, min_con_x, max_con_x, min_con_y, max_con_y, out_of_bounds, num_objects] image_data = [cx,cy,area_max,num_objects] return image_data def get_camera_frame(): global frame global new_frame _, frame = cap.read() frame = cv2.flip(frame,-1,-1) new_frame = True #Setup of vidro and controller vidro = Vidro(False, 1) flight_ready = vidro.connect() controller = PositionController(vidro) start_time = time.time() previous_time = time.time() cycle = 0 controller.update_gains() sequence = 0 seq0_cnt = 0 seq1_cnt = 0 seq2_cnt = 0 seq3_cnt = 0
""" Simple test for connecting to the vidro class. Some things like time may not be working """ from vidro 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(False, 1) vidro.connect() controller = PositionController(vidro) screen = curses.initscr() screen.clear() screen.refresh() while_check = 1 while True: while_check=1 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) curses_print("Quad Arm: " + str(vidro.current_rc_channels[4]),4,0)
#!/usr/bin/env python # -*- coding: utf-8 -*- from vidro import Vidro, ViconStreamer from position_controller import PositionController import sys, math, time import socket, struct, threading import numpy as np import cv2 #Setup of vidro and controller vidro = Vidro(False, 1) flight_ready = vidro.connect() controller = PositionController(vidro) start_time = time.time() previous_time = time.time() cycle = 0 controller.update_gains() sequence = 0 seq0_cnt = 0 seq1_cnt = 0 seq2_cnt = 0 seq3_cnt = 0 yaw = 0
#!/usr/bin/env python # -*- coding: utf-8 -*- from vidro import Vidro, ViconStreamer from position_controller import PositionController import sys, math, time import socket, struct, threading import numpy as np import cv2 #Setup of vidro and controller vidro = Vidro(False, 1) flight_ready = vidro.connect() controller = PositionController(vidro) start_time = time.time() previous_time = time.time() cycle = 0 controller.update_gains() sequence = 0 seq0_cnt = 0 seq1_cnt = 0 seq2_cnt = 0 seq3_cnt = 0 pos_bound_err = 100 yaw_bound_err = 0.2 yaw = 0
#image_data = [cx ,cy, area, min_con_x, max_con_x, min_con_y, max_con_y, out_of_bounds, num_objects] image_data = [cx,cy,area_max,num_objects] return image_data def get_camera_frame(): global frame global new_frame _, frame = cap.read() frame = cv2.flip(frame,-1,-1) new_frame = True ############################### #Setup of vidro and controller# ############################### vidro = Vidro(False, 1) #flight_ready = vidro.connect() controller = PositionController(vidro) start_time = time.time() # Load gains controller.update_gains() #################### # Controller Values# #################### take_off_alt = 1000 # Pos Commands x_com = 0 y_com = 0 alt_com = 0