#!/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
""" 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