示例#1
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 = 300
yaw_bound_err = 0.5
yaw = 0
示例#2
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