def arm_and_takeoff(self):
"""Dangerous: Arm and takeoff vehicle - use only in simulation"""
# NEVER DO THIS WITH A REAL VEHICLE - it is turning off all flight safety checks
# but fine for experimenting in the simulator.
#self.connect(self)
print "Arming and taking off"
self.vehicle.mode = VehicleMode("STABILIZE")
self.vehicle.parameters["ARMING_CHECK"] = 0
self.vehicle.armed = True
self.vehicle.flush()
while not self.vehicle.armed:
print "Waiting for arming..."
time.sleep(1)
print "Taking off!"
self.vehicle.commands.takeoff(10) # Take off to 20m height
# Pretend we have a RC transmitter connected
rc_channels = self.vehicle.channel_override
rc_channels[3] = 1500 # throttle
self.vehicle.channel_override = rc_channels
print veh_control.get_location().alt
self.vehicle.flush()
time.sleep(1)
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
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
def main(self):
veh_control.connect(local_connect())
self.set_target_location(veh_control.get_location())
while(veh_control.is_connected()):
location = veh_control.get_location()
attitude = veh_control.get_attitude()
self.refresh_simulator(location,attitude)
frame = self.get_frame()
cv2.imshow('frame',frame)
key = cv2.waitKey(1)
print key
if key ==1113938:
veh_control.set_velocity(2,0,0) #forward
elif key == 1113940:
veh_control.set_velocity(-2,0,0) #backward
elif key == 1113937:
veh_control.set_velocity(0,-2,0) #left
elif key ==1113939:
veh_control.set_velocity(0,2,0) #right
elif(key == 1048690):
yaw = math.degrees(attitude.yaw) #yaw left
veh_control.set_yaw(yaw - 5)
elif(key == 1048692):
yaw = math.degrees(attitude.yaw) #yaw right
veh_control.set_yaw(yaw + 5)
elif(key == 1048677):
veh_control.set_velocity(0,0,-2) #down
elif(key == 1048689):
veh_control.set_velocity(0,0,2) #up
else:
veh_control.set_velocity(0,0,0) #still
def main(self):
veh_control.connect(local_connect())
self.set_target_location(veh_control.get_location())
while (veh_control.is_connected()):
location = veh_control.get_location()
attitude = veh_control.get_attitude()
self.refresh_simulator(location, attitude)
frame = self.get_frame()
cv2.imshow('frame', frame)
key = cv2.waitKey(1)
print key
if key == 1113938:
veh_control.set_velocity(2, 0, 0) #forward
elif key == 1113940:
veh_control.set_velocity(-2, 0, 0) #backward
elif key == 1113937:
veh_control.set_velocity(0, -2, 0) #left
elif key == 1113939:
veh_control.set_velocity(0, 2, 0) #right
elif (key == 1048690):
yaw = math.degrees(attitude.yaw) #yaw left
veh_control.set_yaw(yaw - 5)
elif (key == 1048692):
yaw = math.degrees(attitude.yaw) #yaw right
veh_control.set_yaw(yaw + 5)
elif (key == 1048677):
veh_control.set_velocity(0, 0, -2) #down
elif (key == 1048689):
veh_control.set_velocity(0, 0, 2) #up
else:
veh_control.set_velocity(0, 0, 0) #still
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
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
if alt>1:
self.changemode("LAND")
'''
def changemode(self,str):
self.vehicle.mode = VehicleMode(str)
self.vehicle.flush()
# if starting from mavproxy
if __name__ == "__builtin__":
# start precision landing
start = takeland()
start.connect()
# run strategy
start.arm_and_takeoff()
print veh_control.get_location().alt
while veh_control.get_location().alt<1:
print veh_control.get_location().alt
#print veh_control.get_location().alt
#while veh_control.get_location().alt>2:
start.changemode("GUIDED")
'''
while 1:
veh_control.set_velocity(10,0,0)
time.sleep(0.15);
'''
#veh_control.set_velocity(-0.1,-0.1,0);
#time.sleep(0.15);
#start.changemode("LOITER")
#start.straight_raise()
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():
#get info from autopilot
location = veh_control.get_location()
attitude = veh_control.get_attitude()
print location
print attitude
# Take each frame
#_, frame = camera.read()
#update how often we dispatch a command
sc_dispatcher.calculate_dispatch_schedule()
# grab an image
capStart = current_milli_time()
frame = sc_video.get_image()
capStop = current_milli_time()
#frame = sc_video.undisort_image(frame)
#cv2.imshow('frame',frame)
# 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(target=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]))
def control(self, target_info, attitude, location):
#we have control from autopilot
if self.in_control:
valid_target = False
now = time.time()
#detected a target
if target_info[1] is not None:
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')
#move to target
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
self.straight_descent()
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')
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)
#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)
'''
#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(target=detector.analyze_frame,
args=(
frame,
attitude,
))
#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,
'RunTime: {0} Center: {1} Distance: {2} Raw Target: {3}'
.format(results[0], results[1], results[2],
results[3]))
sc_logger.text(sc_logger.AIRCRAFT, attitude)
sc_logger.text(sc_logger.AIRCRAFT, location)
#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()
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[1] is not None:
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')
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)
#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)
'''
#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(target=detector.analyze_frame, args=(frame,attitude,))
#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,'RunTime: {0} Center: {1} Distance: {2} Raw Target: {3}'.format(results[0],results[1],results[2],results[3]))
sc_logger.text(sc_logger.AIRCRAFT,attitude)
sc_logger.text(sc_logger.AIRCRAFT,location)
#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()
if alt>1:
self.changemode("LAND")
'''
def changemode(self,str):
self.vehicle.mode = VehicleMode(str)
self.vehicle.flush()
# if starting from mavproxy
if __name__ == "__builtin__":
# start precision landing
start = takeland()
start.connect()
# run strategy
start.arm_and_takeoff()
print veh_control.get_location().alt
#while veh_control.get_location().alt<1:
# print veh_control.get_locaguition().alt
#print veh_control.get_location().alt
#while veh_control.get_location().alt>2:
start.changemode("GUIDED")
#while veh_control.get_location().alt<20:
# veh_control.set_velocity(0,0,-2);
#start.changemode("LOITER")
#start.straight_raise()
#start.straight_descent()
#time.sleep(5)
#start.changemode("LAND")
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')
#-35.362664, 149.166803 -35.362902 149.166249
sim.set_target_location(Location(-35.363134 ,149.165429,0))
#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
# seach
if(self.flag==False):
# has issues
#search in area
if(self.point==1):
point1 = Location(self.point1_lat, self.point1_lon, 5, is_relative=True)
#point1 = Location(30.264233, 120.118813, 3, is_relative=True)
self.vehicle.commands.goto(point1)
self.vehicle.flush()
if(veh_control.get_location().lon>=self.point1_lon):
self.point=2
if(self.point==2):
point2 = Location(self.point2_lat, self.point2_lon, 5, is_relative=True)
#point1 = Location(30.264233, 120.118813, 3, is_relative=True)
self.vehicle.commands.goto(point2)
self.vehicle.flush()
if(veh_control.get_location().lat>=self.point1_lat):
self.point=3
if(self.point==3):
point3 = Location(self.point3_lat, self.point3_lon, 5, is_relative=True)
#point1 = Location(30.264233, 120.118813, 3, is_relative=True)
self.vehicle.commands.goto(point3)
self.vehicle.flush()
if(veh_control.get_location().lon>=self.point3_lon):
self.point=4
if(self.point==4):
point4 = Location(self.point4_lat, self.point4_lon, 5, is_relative=True)
#point1 = Location(30.264233, 120.118813, 3, is_relative=True)
self.vehicle.commands.goto(point4)
self.vehicle.flush()
if(veh_control.get_location().lat>=self.point4_lat):
self.point=1
'''
print "Going to first point..."
#30.264233, 120.118813 -35.362664, 149.166803
point1 = Location(30.264233, 120.118813, 4, is_relative=True)
self.vehicle.commands.goto(point1)
self.vehicle.flush()
'''
#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()
#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()
# 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(target=detector.analyze_frame, args=(frame,attitude,))
#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,'RunTime: {0} Center: {1} Distance: {2} Raw Target: {3}'.format(results[0],results[1],results[2],results[3]))
sc_logger.text(sc_logger.AIRCRAFT,attitude)
sc_logger.text(sc_logger.AIRCRAFT,location)
#send commands to autopilot
if(results[2]!=-1):
self.flag = True
if(self.flag==True):
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()
