if ((abs(error_z) < pos_bound_err) and (abs(error_yaw) < yaw_bound_err) and (abs(err_y) < pos_bound_err) and (abs(err_x) < pos_bound_err)):# Closes Error
seq0_cnt += 1 # just update the sequence if the loop is closed for 3 software loops
if seq0_cnt == 10:
sequence = 1
#Seq. 1: Coarse Search
if sequence == 1:
error_z = controller.rc_alt(alt_com)
error_yaw = controller.rc_yaw(yaw_com)
#error_x_y = controller.rc_xy(x_com, y_com)
err_x = 0#error_x_y[0]
err_y = 0#error_x_y[1]
print('Seq: '+repr(sequence)+' Err Z: '+repr(round(error_z))+' Err Yaw: '+repr(round(error_yaw))+' Err X: '+repr(round(err_x))+' Err y: '+repr(round(err_y)))
if ((abs(error_z) < pos_bound_err) and (abs(error_yaw) < yaw_bound_err) and (abs(err_y) < pos_bound_err) and (abs(err_x) < pos_bound_err)):# Closes Error
# Hold her steady while we img proc
vidro.set_rc_throttle(vidro.base_rc_throttle)
vidro.set_rc_roll(vidro.base_rc_roll)
vidro.set_rc_pitch(vidro.base_rc_pitch)
vidro.set_rc_yaw(vidro.base_rc_yaw)
# Run img proc
yaw_com +=yaw_coarse_step
yaw_pos = vidro.get_yaw_radians() # Grab current yaw val, assuming picture taking could be a while so grab it here son
get_camera_frame()
cx_val,cy_val,area_val,num_objects_val = get_object(frame)
if(num_objects_val>0):
balloon_found = True # If we have something, we'll assume we've found the balloon
if(yaw_pos<0):
yaw_pos+=(2*math.pi) # Keep our bearing estimate between 0 and 2pi
if(area_val>area_max): # If our current imag has a bigger red area, update our estimate
area_max_val = area_val
img_balloon_ber = (cx_val-cx_mid)*cx_fov/640 # Basic camera model, somebody check this
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 cycles_message += 1 previous_time = vidro.rc_msg_time previous_rc = vidro.current_rc_channels[2] while vidro.current_rc_channels[2] > 1300:
