def __init__(self):

#load config file

sc_config.config.get_file('Smart_Camera')

#get camera specs

self.camera_index = sc_config.config.get_integer('camera','camera_index',0)

self.camera_width = sc_config.config.get_integer('camera', 'camera_width', 640)

self.camera_height = sc_config.config.get_integer('camera', 'camera_height', 480)

self.camera_hfov = sc_config.config.get_float('camera', 'horizontal-fov', 72.42)

self.camera_vfov = sc_config.config.get_float('camera', 'vertical-fov', 43.3)

#use simulator

#self.simulator = sc_config.config.get_boolean('simulator','use_simulator',True)

self.simulator = sc_config.config.get_boolean('simulator','use_simulator',True)

#how many times to attempt a land before giving up

self.search_attempts = sc_config.config.get_integer('general','search_attempts', 5)

#The timeout between losing the target and starting a climb/scan

self.settle_time = sc_config.config.get_integer('general','settle_time', 1.5)

#how high to climb in meters to complete a scan routine

self.climb_altitude = sc_config.config.get_integer('general','climb_altitude', 20)

#the max horizontal speed sent to autopilot

self.vel_speed_max = sc_config.config.get_float('general', 'vel_speed_max', 5)

#P term of the horizontal distance to velocity controller

#Pid P canshu 0.15

self.dist_to_vel = sc_config.config.get_float('general', 'dist_to_vel', 0.15)

#Descent velocity

self.descent_rate = sc_config.config.get_float('general','descent_rate', 0.5)

#roll/pitch value that is considered stable

self.stable_attitude = sc_config.config.get_float('general', 'stable_attitude', 0.18)

#Climb rate when executing a search

self.climb_rate = sc_config.config.get_float('general','climb_rate', -0.5)

#The height at a climb is started if no target is detected

self.abort_height = sc_config.config.get_integer('general', 'abort_height', 10)

#when we have lock on target, only descend if within this radius

self.descent_radius = sc_config.config.get_float('general', 'descent_radius', 2.0)

#The height at which we lock the position on xy axis

self.landing_area_min_alt = sc_config.config.get_integer('general', 'landing_area_min_alt', 0.5)

#The radius of the cylinder surrounding the landing pad

self.landing_area_radius = sc_config.config.get_integer('general', 'landing_area_radius', 20)

#Whether the landing program can be reset after it is disabled

self.allow_reset = sc_config.config.get_boolean('general', 'allow_reset', True)

#Run the program no matter what mode or location; Useful for debug purposes

self.always_run = sc_config.config.get_boolean('general', 'always_run', True)

#whether the companion computer has control of the autopilot or not

self.in_control = False

#how many frames have been captured

self.frame_count = 0

self.last_time = 0

self.cur_time = 0

# horizontal velocity pid controller. maximum effect is 10 degree lean

xy_p = sc_config.config.get_float('general','VEL_XY_P',10.0)

xy_i = sc_config.config.get_float('general','VEL_XY_I',0.0)

xy_d = sc_config.config.get_float('general','VEL_XY_D',0.0)

xy_imax = sc_config.config.get_float('general','VEL_XY_IMAX',10.0)

self.vel_xy_pid = pid.pid(xy_p, xy_i, xy_d, math.radians(xy_imax))

#Reset state machine

self.initialize_landing()

#debugging:

self.kill_camera = False

def name(self):

return "Precision_Land"

def connect(self):

while(veh_control.is_connected() == False):

# connect to droneapi

veh_control.connect(local_connect())

self.vehicle = veh_control.get_vehicle()

def run(self):

sc_logger.text(sc_logger.GENERAL, 'running {0}'.format(self.name()))

#start a video capture

if(self.simulator):

sc_logger.text(sc_logger.GENERAL, 'Using simulator')

sim.set_target_location(veh_control.get_home())

#sim.set_target_location(Location(0,0,0))

else:

sc_video.start_capture(self.camera_index)

#camera = balloon_video.get_camera()

video_writer = balloon_video.open_video_writer()

#create an image processor

detector = CircleDetector()

#create a queue for images

imageQueue = Queue.Queue()

#create a queue for vehicle info

vehicleQueue = Queue.Queue()

while veh_control.is_connected():

'''

#kill camera for testing

if(cv2.waitKey(2) == 1113938):

self.kill_camera = not self.kill_camera

'''

#Reintialize the landing program when entering a landing mode

if veh_control.controlling_vehicle():

if not self.in_control:

if(self.allow_reset):

sc_logger.text(sc_logger.GENERAL, 'Program initialized to start state')

self.initialize_landing()

self.in_control = True

else:

self.in_control = False

#we are in the landing zone or in a landing mode and we are still running the landing program

#just because the program is running does not mean it controls the vehicle

#i.e. in the landing area but not in a landing mode

#FIXME add inside_landing_area() back to conditional

if (self.in_control or self.always_run) and self.pl_enabled:

#update how often we dispatch a command

sc_dispatcher.calculate_dispatch_schedule()

#get info from autopilot

location = veh_control.get_location()

attitude = veh_control.get_attitude()

'''

#get info from autopilot

location = Location(0.000009,0,location.alt)

attitude = Attitude(0,0,0)

'''

#update simulator

if(self.simulator):

sim.refresh_simulator(location,attitude)

# grab an image

capStart = current_milli_time()

frame = self.get_frame()

capStop = current_milli_time()

'''

if(self.kill_camera):

frame[:] = (0,255,0)

'''

# write the frame

video_writer.write(frame)

#update capture time

sc_dispatcher.update_capture_time(capStop-capStart)

#Process image

#We schedule the process as opposed to waiting for an available core

#This brings consistancy and prevents overwriting a dead process before

#information has been grabbed from the Pipe

if sc_dispatcher.is_ready():

#queue the image for later use: displaying image, overlays, recording

imageQueue.put(frame)

#queue vehicle info for later use: position processing

vehicleQueue.put((location,attitude))

#the function must be run directly from the class

sc_dispatcher.dispatch(balloon_finder.analyse_frame, args=(frame,))

#retreive results

if sc_dispatcher.is_available():

sc_logger.text(sc_logger.GENERAL, 'Frame {0}'.format(self.frame_count))

self.frame_count += 1

#results of image processor

results = sc_dispatcher.retreive()

# get image that was passed with the image processor

img = imageQueue.get()

#get vehicle position that was passed with the image processor

location, attitude = vehicleQueue.get()

#overlay gui

#rend_Image = gui.add_target_highlights(img, results[3])

#show/record images

sc_logger.image(sc_logger.RAW, img)

#sc_logger.image(sc_logger.GUI, rend_Image)

#display/log data

sc_logger.text(sc_logger.ALGORITHM,'found: {0} x: {1} y: {2} radius: {3}'.format(results[0],results[1],results[2],results[3]))

#send commands to autopilot

self.control(results,attitude,location)

else:

if(self.pl_enabled == False):

sc_logger.text(sc_logger.GENERAL, 'Landing disabled')

else:

sc_logger.text(sc_logger.GENERAL, 'Not in landing mode or Landing Area')

#terminate program

sc_logger.text(sc_logger.GENERAL, 'Vehicle disconnected, Program Terminated')

if(self.simulator == False):

sc_video.stop_capture()

#initialize_landing - reset the state machine which controls the flow of the landing routine

def initialize_landing(self):

#how mant times we have attempted landing

self.attempts = 0

#Last time in millis since we had a valid target

self.last_valid_target = 0

#State variable climbing to scan for the target

self.climbing = False

#State variable which determines if this program will continue to run

self.pl_enabled = True

#State variable used to represent if autopilot is active

self.initial_descent = True

#State variable which represents a know target in landing area

self.target_detected = False

#control - how to respond to information captured from camera

def control(self,target_info,attitude,location):

#we have control from autopilot

if self.in_control:

valid_target = False

#now=0;

#print (time.time()-now);

now = time.time()

#print now;

#detected a target

if target_info[0]:

self.target_detected = True

valid_target = True

initial_descent = False

self.last_valid_target = now

#attempt to use precision landing

if(self.inside_landing_area() == 1):

#we have detected a target in landing area

if(self.target_detected):

self.climbing = False

self.initial_descent = False

#we currently see target

if(valid_target):

sc_logger.text(sc_logger.GENERAL, 'Target detected. Moving to target')

##add myself

if(veh_control.get_location().alt < 1):

self.autopilot_land()

sc_logger.text(sc_logger.GENERAL, 'Landing!!!!!!!!')

#move to target

else:

self.move_to_target(target_info,attitude,location)

#lost target

else:

#we have lost the target for more than settle_time

if(now - self.last_valid_target > self.settle_time):

self.target_detected = False

#temporarily lost target,

#top section of cylinder

if(veh_control.get_location().alt > self.abort_height):

sc_logger.text(sc_logger.GENERAL, 'Lost Target: Straight Descent')

#continue descent

self.straight_descent()

else:

sc_logger.text(sc_logger.GENERAL, 'Lost Target: Holding')

#neutralize velocity

veh_control.set_velocity(0,0,0)

#there is no known target in landing area

else:

#currently searching

if(self.climbing):

self.climb()

#not searching, decide next move

else:

#top section of cylinder

if(veh_control.get_location().alt > self.abort_height):

#initial descent entering cylinder

if(self.initial_descent):

sc_logger.text(sc_logger.GENERAL, 'No Target: Initial Descent')

#give autopilot control

self.autopilot_land()

#all other attempts prior to intial target detection

else:

sc_logger.text(sc_logger.GENERAL, 'No target: Straight descent')

#straight descent

self.straight_descent()

#lower section of cylinder

else:

#we can attempt another land

if(self.attempts < self.search_attempts):

self.attempts += 1

sc_logger.text(sc_logger.GENERAL, 'Climbing to attempt {0}'.format(self.attempts))

#start climbing

self.climb()

#give up and

else:

sc_logger.text(sc_logger.GENERAL, 'Out of attempts: Giving up')

#give autopilot control

self.autopilot_land()

#final descent

elif(self.inside_landing_area() == -1):

sc_logger.text(sc_logger.GENERAL, 'In final descent')

#straight descent

#change LAND mode

#self.straight_descent()

self.vehicle.mode = VehicleMode("LAND")

self.vehicle.flush()

self.target_detected = False

#outside cylinder

else:

sc_logger.text(sc_logger.GENERAL, 'Outside landing zone')

#give autopilot control

self.autopilot_land()

self.target_detected = False

self.initial_descent = True

#the program is running but the autopilot is in an invalid mode

else:

sc_logger.text(sc_logger.GENERAL, 'Not in control of vehicle')

#release_control - give the autopilot full control and leave it in a stable state

def release_control(self):

sc_logger.text(sc_logger.GENERAL, 'Releasing control')

#put vehicle in stable state

veh_control.set_velocity(0,0,0)

#autopilot_land()

#dont let us take control ever again

self.pl_enabled = False

'''

# if in GUIDED mode switch back to LOITER

if self.vehicle.mode.name == "GUIDED":

self.vehicle.mode = VehicleMode("LOITER")

self.vehicle.flush()

'''

#move_to_target - fly aircraft to landing pad

def move_to_target(self,target_info,attitude,location):

x,y = target_info[1],target_info[2]

#shift origin to center of image

x,y = shift_to_origin((x,y),self.camera_width,self.camera_height)

#this is necessary because the simulator is 100% accurate

if(self.simulator):

hfov = 48.7

vfov = 49.7

else:

hfov = self.camera_hfov

vfov = self.camera_vfov

#stabilize image with vehicle attitude

x -= (self.camera_width / hfov) * math.degrees(attitude.roll)

y += (self.camera_height / vfov) * math.degrees(attitude.pitch)

#convert to distance

X, Y, Xp, Yp = self.pixel_point_to_position_xy((x,y),location.alt)

#convert to world coordinates

target_heading = math.atan2(Yp,Xp) % (2*math.pi)

target_heading = (attitude.yaw - target_heading)

target_distance = math.sqrt(X**2 + Y**2)

sc_logger.text(sc_logger.GENERAL, "Distance to target: {0}".format(round(target_distance,2)))

#calculate speed toward target

speed = target_distance * self.dist_to_vel

#apply max speed limit

speed = min(speed,self.vel_speed_max)

#calculate cartisian speed

vx = speed * math.sin(target_heading) * -1.0

vy = speed * math.cos(target_heading)

#only descend when on top of target

if(target_distance > self.descent_radius):

vz = 0

else:

vz = self.descent_rate

#send velocity commands toward target heading

veh_control.set_velocity(vx,vy,vz)

#autopilot_land - Let the autopilot execute its normal landing procedure

def autopilot_land(self):

#descend velocity

veh_control.set_velocity(0,0,self.descent_rate)

#veh_control.set_velocity(9999,9999,9999)

#straight_descent - send the vehicle straight down

def straight_descent(self):

veh_control.set_velocity(0,0,self.descent_rate)

#climb - climb to a certain alitude then stop.

def climb(self):

if(veh_control.get_location().alt < self.climb_altitude):

sc_logger.text(sc_logger.GENERAL, 'climbing')

veh_control.set_velocity(0,0,self.climb_rate)

self.climbing = True

else:

sc_logger.text(sc_logger.GENERAL, 'Reached top of search zone')

veh_control.set_velocity(0,0,0)

self.climbing = False

#inside_landing_area - determine is we are in a landing zone 0 = False, 1 = True, -1 = below the zone

def inside_landing_area(self):

vehPos = PositionVector.get_from_location(veh_control.get_location())

landPos = PositionVector.get_from_location(veh_control.get_landing())

'''

vehPos = PositionVector.get_from_location(Location(0,0,10))

landPos = PositionVector.get_from_location(Location(0,0,0))

'''

if(PositionVector.get_distance_xy(vehPos,landPos) < self.landing_area_radius):

#below area

if(vehPos.z < self.landing_area_min_alt):

return -1

#in area

else:

return 1

#outside area

else:

return 0

#get_frame - pull an image from camera or simulator

def get_frame(self):

if(self.simulator):

return sim.get_frame()

else:

return sc_video.get_image()

#pixel_point_to_position_xy - convert position in pixels to position in meters

#pixel_position - distance in pixel from CENTER of image

#distance- distance from the camera to the object in meters

def pixel_point_to_position_xy(self,pixel_position,distance):

thetaX = pixel_position[0] * self.camera_hfov / self.camera_width

thetaY = pixel_position[1] * self.camera_vfov / self.camera_height

x = distance * math.tan(math.radians(thetaX))

y = distance * math.tan(math.radians(thetaY))

print x,y

print 1

# pid

# get time since last time velocity pid controller was run

dt = self.vel_xy_pid.get_dt(200.0)

# calculate thetaX correction and final X movement

thetaX_correction = self.vel_xy_pid.get_pid(thetaX, dt)

# calculate thetaX correction and final X movement

thetaY_correction = self.vel_xy_pid.get_pid(thetaY, dt)

xp = distance * math.tan(math.radians(thetaX_correction))

yp = distance * math.tan(math.radians(thetaY_correction))

print xp,yp