def image_capture_background(self, imgcap_connection):
# exit immediately if imgcap_connection is invalid
if imgcap_connection is None:
sc_logger.text(sc_logger.GENERAL, "image_capture failed because pipe is uninitialised")
return
# clear latest image
latest_image = None
while True:
# constantly get the image from the webcam
success_flag, image=self.camera.read()
# if successful overwrite our latest image
if success_flag:
latest_image = image
# check if the parent wants the image
if imgcap_connection.poll():
recv_obj = imgcap_connection.recv()
# if -1 is received we exit
if recv_obj == -1:
break
# otherwise we return the latest image
imgcap_connection.send(latest_image)
# release camera when exiting
self.camera.release()
def image_capture_background(self, imgcap_connection):
# exit immediately if imgcap_connection is invalid
if imgcap_connection is None:
sc_logger.text(
sc_logger.GENERAL,
"image_capture failed because pipe is uninitialised")
return
# clear latest image
latest_image = None
while True:
# constantly get the image from the webcam
success_flag, image = self.camera.read()
# if successful overwrite our latest image
if success_flag:
latest_image = image
# check if the parent wants the image
if imgcap_connection.poll():
recv_obj = imgcap_connection.recv()
# if -1 is received we exit
if recv_obj == -1:
break
# otherwise we return the latest image
imgcap_connection.send(latest_image)
# release camera when exiting
self.camera.release()
def set_velocity(self, velocity_x, velocity_y, velocity_z):
#only let commands through at 10hz
if (time.time() - self.last_set_velocity) > self.vel_update_rate:
self.last_set_velocity = time.time()
# create the SET_POSITION_TARGET_LOCAL_NED command
msg = self.vehicle.message_factory.set_position_target_local_ned_encode(
0, # time_boot_ms (not used)
0,
0, # target system, target component
mavutil.mavlink.MAV_FRAME_LOCAL_NED, # frame
0x01C7, # type_mask (ignore pos | ignore acc)
0,
0,
0, # x, y, z positions (not used)
velocity_x,
velocity_y,
velocity_z, # x, y, z velocity in m/s
0,
0,
0, # x, y, z acceleration (not used)
0,
0) # yaw, yaw_rate (not used)
# send command to vehicle
self.vehicle.send_mavlink(msg)
self.vehicle.flush()
sc_logger.text(
sc_logger.AIRCRAFT, 'Sent Vx: {0}, Vy: {1}, Vz: {2}'.format(
velocity_x, velocity_y, velocity_z))
def retreive(self):
if self.is_available():
#grab results
results = self.parent_conn.recv()
#update processTime. All processes must return a runtime through the pipe
self.processTime, self.processTimeSet = self.rolling_average(
self.processTimeSet, results[0])
#Calculate real runtime. Diagnostic purposes only
#In an ideal system the dispatch rate would equal the retreival rate. That is not the case here
actualRunTime = current_milli_time() - self.lastRetreival
self.lastRetreival = current_milli_time()
sc_logger.text(
sc_logger.PERFORMANCE,
"DispatchHz: {0} runHz: {1} ProcessHz: {2} CaptureHz: {3} Processes: {4} "
.format(round(1000.0 / (self.runTime)),
round(1000.0 / actualRunTime),
round((1000.0 / results[0])),
round(1000.0 / self.captureTime),
len(multiprocessing.active_children())))
return results
return None
def move_to_target_cam_forward(self,target_info,attitude,location):
x,y = target_info[1]
pitch_dir,yaw_dir = balloon_finder.pixels_to_direction(x, y, attitude.roll, attitude.pitch, attitude.yaw)
yaw_dir = yaw_dir % 360
print "Target Yaw:%f" %(yaw_dir)
target_distance = target_info[2]*10
print "Target Distance:%f" %(target_distance)
#shift origin to center of image
print "Found Target at yaw:%f heading:%f Alt:%f Dist:%f" % (attitude.yaw,yaw_dir,location.z,target_distance)
sc_logger.text(sc_logger.GENERAL, "Distance to target: {0}".format(round(target_distance,2)))
print "mode detection"
#self.condition_yaw(math.fabs(math.degrees(target_heading)))
if(target_distance > 4):
vx = 0
vy = 0
vz = 0
else:
vx = 0
vy = 0
vz = 0
#send velocity commands toward target heading
self.send_nav_velocity(vx,vy,vz)
self.condition_yaw(yaw_dir)
def get_home(self, wait_for_arm=False):
#wait unitl we are armed to grab the INITIAL home position. This will lock up the program.
if (wait_for_arm and self.last_home is None):
sc_logger.text(
sc_logger.GENERAL,
'Waiting for intial home lock: Requires armed status....')
while (self.vehicle.armed == False):
time.sleep(0.3)
sc_logger.text(sc_logger.GENERAL, 'Got valid intial home location')
if (time.time() - self.last_home_call > self.home_update_rate):
self.last_home_call = time.time()
# download the vehicle waypoints
mission_cmds = self.vehicle.commands
mission_cmds.download()
mission_cmds.wait_valid()
# get the home lat and lon
home_lat = mission_cmds[0].x
home_lon = mission_cmds[0].y
self.last_home = Location(home_lat, home_lon, 0)
return self.last_home
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
'''
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 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 '''
def get_camera(self,index):
# setup video capture
self.camera = cv2.VideoCapture(index)
self.camera.set(cv2.cv.CV_CAP_PROP_FRAME_WIDTH,self.img_width)
self.camera.set(cv2.cv.CV_CAP_PROP_FRAME_HEIGHT,self.img_height)
# check we can connect to camera
if not self.camera.isOpened():
sc_logger.text(sc_logger.GENERAL,"failed to open camera, exiting!")
sys.exit(0)
return self.camera
def run(self):
sc_logger.text(sc_logger.GENERAL, 'running {0}'.format(self.name()))
#start a video capture
sc_video.start_capture(self.camera_index)
#create an image processor
detector = CircleDetector()
#create a queue for images
imageQueue = Queue.Queue()
while True:
#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()
#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)
#the function must be run directly from the class
sc_dispatcher.dispatch(target=detector.analyze_frame, args=(frame,None,))
#retreive results
if sc_dispatcher.is_available():
#results of image processor
results = sc_dispatcher.retreive()
# get image that was passed with the image processor
img = imageQueue.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)
def get_camera(self, index):
# setup video capture
self.camera = cv2.VideoCapture(index)
self.camera.set(cv2.cv.CV_CAP_PROP_FRAME_WIDTH, self.img_width)
self.camera.set(cv2.cv.CV_CAP_PROP_FRAME_HEIGHT, self.img_height)
# check we can connect to camera
if not self.camera.isOpened():
sc_logger.text(sc_logger.GENERAL,
"failed to open camera, exiting!")
sys.exit(0)
return self.camera
def move_to_target(self,target_info,attitude,location):
x,y = target_info[1]
#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 = self.pixel_point_to_position_xy((x,y),location.alt)
#convert to world coordinates
target_heading = math.atan2(Y,X) % (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)
def move_to_target(self, target_info, attitude, location):
x, y = target_info[1]
#pitch_dir,yaw_dir = balloon_finder.pixels_to_direction(x, y, attitude.roll, attitude.pitch, attitude.yaw)
#yaw_dir = yaw_dir % 360
#print "Target Yaw:%f" %(yaw_dir)
#target_distance = target_info[2]
#print "Target Distance:%f" %(target_distance*100)
#shift origin to center of image
x, y = shift_to_origin((x, y), self.camera_width, self.camera_height)
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 = self.pixel_point_to_position_xy((x, y), location.z)
#convert to world coordinates
target_headings = math.atan2(Y, X) #% (2*math.pi)
target_heading = (attitude.yaw - target_headings)
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)
print "Found Target go to vx:%f vy:%f Alt:%f target_distance:%f" % (
vx, vy, location.z, target_distance)
#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
self.send_nav_velocity(vx, vy, vz)
def check_video_out(self):
# return immediately if video has already been started
if not self.writer is None:
return
#self.writer = balloon_video.open_video_writer()
# start video once vehicle is armed
if self.vehicle.armed and self.arm:
sc_logger.text(sc_logger.GENERAL, "armed")
self.arm = False
#self.writer = balloon_video.open_video_writer()
if not self.vehicle.armed and not self.arm:
sc_logger.text(sc_logger.GENERAL, "disarm")
self.arm = True
def move_to_target(self, target_info, attitude, location):
x, y = target_info[1]
#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 = self.pixel_point_to_position_xy((x, y), location.alt)
#convert to world coordinates
target_heading = math.atan2(Y, X) % (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)
def retreive(self):
if self.is_available():
#grab results
results = self.parent_conn.recv()
#update processTime. All processes must return a runtime through the pipe
self.processTime, self.processTimeSet = self.rolling_average(self.processTimeSet, results[0])
#Calculate real runtime. Diagnostic purposes only
#In an ideal system the dispatch rate would equal the retreival rate. That is not the case here
actualRunTime = current_milli_time() - self.lastRetreival
self.lastRetreival = current_milli_time()
sc_logger.text(sc_logger.PERFORMANCE, "DispatchHz: {0} runHz: {1} ProcessHz: {2} CaptureHz: {3} Processes: {4} ".format(round(1000.0/(self.runTime)), round(1000.0/actualRunTime), round((1000.0/results[0])),round(1000.0/self.captureTime), len(multiprocessing.active_children())))
return results
return None
def set_velocity(self, velocity_x, velocity_y, velocity_z):
#only let commands through at 10hz
if(time.time() - self.last_set_velocity) > self.vel_update_rate:
self.last_set_velocity = time.time()
# create the SET_POSITION_TARGET_LOCAL_NED command
msg = self.vehicle.message_factory.set_position_target_local_ned_encode(
0, # time_boot_ms (not used)
0, 0, # target system, target component
mavutil.mavlink.MAV_FRAME_LOCAL_NED, # frame
0x01C7, # type_mask (ignore pos | ignore acc)
0, 0, 0, # x, y, z positions (not used)
velocity_x, velocity_y, velocity_z, # x, y, z velocity in m/s
0, 0, 0, # x, y, z acceleration (not used)
0, 0) # yaw, yaw_rate (not used)
# send command to vehicle
self.vehicle.send_mavlink(msg)
self.vehicle.flush()
sc_logger.text(sc_logger.AIRCRAFT, 'Sent Vx: {0}, Vy: {1}, Vz: {2}'.format(velocity_x,velocity_y,velocity_z))
def analisis_image(self):
# record time
now = time.time()
#PositionVectors = PositionVector()
# capture vehicle position
self.vehicle_pos = PositionVector.get_from_location(
self.vehicle.location.global_relative_frame)
# capture vehicle attitude in buffer
self.att_hist.update()
# get delayed attitude from buffer
veh_att_delayed = self.att_hist.get_attitude(now - self.attitude_delay)
# get new image from camera
f = self.get_frame()
#create an image processor
if self.mission_on_guide_mode == 4:
self.fire_found, xpos, ypos, size = fire_finder.filter(f)
#result = detector.analyze_frame(f)
self.attitude = self.get_attitude()
if self.fire_found == True:
self.b = 0
sc_logger.text(sc_logger.GENERAL, "Target terdeteksi!")
print xpos, ypos, size
if self.vehicle.mode.name == "GUIDED":
sc_logger.text(sc_logger.GENERAL,
"Target terdeteksi, mendekati target!")
self.move_to_target_fire(xpos, ypos, size, self.attitude,
self.vehicle_pos)
self.relay1(1) #lampu indikator on
#self.servo(0)#test untuk servo kamera
elif self.vehicle.mode.name == "GUIDED":
self.relay1(0) #lampu indikator off
#self.servo(1)#test untuk servo kamera
self.b += 1
if self.b > 25: #15
self.b = 0
#print "Api tidak terdeteksi, ubah mode ke AUTO"
#self.lanjut_cmd += 1
sc_logger.text(
sc_logger.GENERAL,
"Target tidak terdeteksi, ubah mode ke AUTO")
self.a = False
self.waktu1 = 0
#self.c = True
self.controlling_vehicle = False
self.vehicle.mode = VehicleMode("AUTO")
self.vehicle.flush()
#rend_Image = detector.add_target_highlights(f, result[3])
sc_logger.image(sc_logger.GUI, f)
if not self.writer is None:
# save image for debugging later
self.writer.write(f)
# increment frames analysed for stats
self.num_frames_analysed += 1
if self.stats_start_time == 0:
self.stats_start_time = time.time()
def get_home(self, wait_for_arm = False):
#wait unitl we are armed to grab the INITIAL home position. This will lock up the program.
if(wait_for_arm and self.last_home is None):
sc_logger.text(sc_logger.GENERAL, 'Waiting for intial home lock: Requires armed status....')
while(self.vehicle.armed == False):
time.sleep(0.3)
sc_logger.text(sc_logger.GENERAL, 'Got valid intial home location')
if(time.time() - self.last_home_call > self.home_update_rate):
self.last_home_call = time.time()
# download the vehicle waypoints
mission_cmds = self.vehicle.commands
mission_cmds.download()
mission_cmds.wait_valid()
# get the home lat and lon
home_lat = mission_cmds[0].x
home_lon = mission_cmds[0].y
self.last_home = Location(home_lat,home_lon,0)
return self.last_home
def run(self):
sc_logger.text(sc_logger.GENERAL, 'running {0}'.format(self.name()))
def check_status(self):
# Reset lanjut_cmd
if (self.vehicle.mode.name == "LOITER" or self.vehicle.mode.name == "STABILIZE") and self.lanjut_cmd > 1:
sc_logger.text(sc_logger.GENERAL, "Resetting Mission")
self.c = False
self.drop = 0
self.relay(0)
self.lanjut_cmd = 1
# download the vehicle waypoints if we don't have them already
# To-Do: do not load waypoints if vehicle is armed
if self.mission_cmds is None:
self.fetch_mission()
return
# Check for Auto mode and executing Nav-Guided command
if self.vehicle.mode.name == "AUTO":
self.relay1(0) #lampu indikator off
# get active command number
active_command = self.vehicle.commands.next
#print "Active Command dan Lanjut Command adalah %s, %s" % (active_command,self.lanjut_cmd)
sc_logger.text(sc_logger.GENERAL, "Active dan Lanjut Command: %s, %s" % (active_command, self.lanjut_cmd))
# Buat ubah ke GUIDED
if active_command == 1 and self.lanjut_cmd == 1:
sc_logger.text(sc_logger.GENERAL, "Taking Off!!!")
self.lanjut_cmd = 3
if (active_command < self.vehicle.commands.count) and (active_command == self.lanjut_cmd): # WP cek api
self.lanjut_cmd += 1
sc_logger.text(sc_logger.GENERAL, "ON TARGET, ubah Mode ke GUIDED")
self.c = True
self.a = True
self.waktu1 = 0
self.vehicle.mode = VehicleMode("GUIDED")
self.vehicle.flush()
if active_command == self.vehicle.commands.count:
sc_logger.text(sc_logger.GENERAL, "Misi selesai, Mode RTL dan LAND")
self.vehicle.mode = VehicleMode("RTL")
self.vehicle.flush()
# we are active in guided mode
if self.vehicle.mode.name == "GUIDED":
if not self.controlling_vehicle:
self.controlling_vehicle = True
print "taking control of vehicle in GUIDED"
print "Mulai cari Target"
sc_logger.text(sc_logger.GENERAL, "taking control of vehicle in GUIDED dan mulai cari Target")
return
# if we got here then we are not in control
if self.controlling_vehicle:
self.controlling_vehicle = False
print "giving up control of vehicle in %s" % self.vehicle.mode.name
def move_to_target_fire(self,x,y,size,attitude,location):
# exit immediately if we are not controlling the vehicle
if not self.controlling_vehicle:
return
# get active command
active_command = self.vehicle.commands.next
# get current time
now = time.time()
# exit immediately if it's been too soon since the last update
if (now - self.guided_last_update) < self.vel_update_rate:
return;
# if we have a new balloon position recalculate velocity vector
if (self.fire_found):
#x,y = target_info[1]
#pitch_dir,yaw_dir = balloon_finder.pixels_to_direction(x, y, attitude.roll, attitude.pitch, attitude.yaw)
#yaw_dir = yaw_dir % 360
#print "Target Yaw:%f" %(yaw_dir)
#target_distance = target_info[2]
#print "Target Distance:%f" %(target_distance*100)
#shift origin to center of image
x,y = shift_to_origin((x,y),self.camera_width,self.camera_height)
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 = self.pixel_point_to_position_xy((x,y),location.z)
#convert to world coordinates
target_headings = math.atan2(Y,X) #% (2*math.pi)
target_heading = (attitude.yaw - target_headings)
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) #*-1.0
print "Found Target go to vx:%f vy:%f Alt:%f target_distance:%f headings:%f heading:%f " % (vx,vy,location.z,target_distance,target_headings,target_heading)
#only descend when on top of target
if(location.z > 3.5):
vz = 0.25
else:
vz = 0
if active_command == 3: #PICK MP1
#Jika ketinggian sudah dibawah 4 meter, maka MAGNET ON
#self.servo(0)#kamera siap untuk pick
if (location.z < 4.0):
if(location.z > 1.6):
vz = 0.2
else:
vz = 0
# if (location.z > 1.60): #Menurunkan Kecepatan Descent
# vz = 0.2
# else:
# vz = 0
if (self.c == True):
self.relay2(1) #magnet ON
self.waktu = 0
self.c = False
if (location.z < 1.7): #if (GPIO.20 == HIGH):
print ("Payload sudah diambil, lanjut misi")
sc_logger.text(sc_logger.GENERAL, "Payload sudah diambil, lanjut misi")
self.controlling_vehicle = False
self.vehicle.mode = VehicleMode("AUTO")
self.vehicle.flush()
# if active_command = 3:
# if
if active_command == 4: #DROP MP1
#self.servo(1)#kamera siap untuk drop
if(location.z > 2.0):
vz = 0.25
else:
if (location.z > 2.5): #Menurunkan Kecepatan Descent
vz = 0.2
else:
vz = 0
if (self.c == True):
self.relay2(0) #magnet OFF
self.waktu = 0
self.c = False
self.drop = 1
print ("Payload sudah DROP, lanjut misi")
sc_logger.text(sc_logger.GENERAL, "Payload sudah DROP, lanjut misi")
self.vehicle.commands.next = 5
self.controlling_vehicle = False
self.vehicle.mode = VehicleMode("AUTO")
self.vehicle.flush()
if active_command == 5: #DROP MP1 atau MP2
#self.servo(1)#kamera siap untuk drop
if(location.z > 2.0):
vz = 0.25
else:
if (location.z > 2.5): #Menurunkan Kecepatan Descent
vz = 0.2
else:
vz = 0
if (self.c == True):
self.relay2(0) #magnet OFF
self.waktu = 0
self.c = False
self.drop = 2
print ("Payload sudah DROP, lanjut misi")
sc_logger.text(sc_logger.GENERAL, "Payload sudah DROP, lanjut misi")
self.controlling_vehicle = False
self.vehicle.mode = VehicleMode("AUTO")
self.vehicle.flush()
if active_command == 6: #DROP LOG
#self.servo(1)#kamera siap untuk drop
if(location.z > 2.5):
vz = 0.25
else:
if (location.z > 3.5): #Menurunkan Kecepatan Descent
vz = 0.2
else:
vz = 0
if (self.c == True):
self.relay(1) #buka payload
self.waktu = 0
self.c = False
print ("Payload sudah DROP, lanjut misi")
sc_logger.text(sc_logger.GENERAL, "Payload sudah DROP, lanjut misi")
self.controlling_vehicle = False
self.vehicle.mode = VehicleMode("AUTO")
self.vehicle.flush()
if active_command == 7: #PICK MP1
#Jika ketinggian sudah dibawah 4 meter, maka MAGNET ON
#self.servo(0)#kamera siap untuk pick
if (location.z < 4.0):
if(location.z > 1.6):
vz = 0.2
else:
vz = 0
# if (location.z > 1.60): #Menurunkan Kecepatan Descent
# vz = 0.2
# else:
# vz = 0
if (self.c == True):
self.relay2(1) #magnet ON
self.waktu = 0
self.c = False
if (location.z < 1.7): #if (GPIO.20 == HIGH):
print ("Payload sudah diambil, lanjut misi")
sc_logger.text(sc_logger.GENERAL, "Payload sudah diambil, lanjut misi")
self.controlling_vehicle = False
if (self.drop == 2):
self.vehicle.commands.next = 8
self.lanjut_cmd += 1
self.vehicle.mode = VehicleMode("AUTO")
self.vehicle.flush()
if active_command == 9: #DROP MP2
#self.servo(1)#kamera siap untuk drop
if(location.z > 2.0):
vz = 0.25
else:
if (location.z > 2.5): #Menurunkan Kecepatan Descent
vz = 0.2
else:
vz = 0
if (self.c == True):
self.relay2(0) #magnet OFF
self.waktu = 0
self.c = False
print ("Payload sudah DROP, lanjut misi")
sc_logger.text(sc_logger.GENERAL, "Payload sudah DROP, lanjut misi")
self.controlling_vehicle = False
self.vehicle.mode = VehicleMode("AUTO")
self.vehicle.flush()
#send velocity commands toward target heading
self.send_nav_velocity(vx,vy,vz)
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()
parser.add_argument('-c', '--camera', default=0, help="Select the camera index for opencv")
parser.add_argument('-i', '--input', default=False, help='use a video filename as an input instead of a webcam')
parser.add_argument('-f', '--file', default='Smart_Camera.cnf', help='load a config file other than the default')
args, unknown = parser.parse_known_args()
'''
#run a strategy
Strategy = None
if args.Strategy == 'land':
Strategy = PrecisionLand(args)
if args.Strategy == 'redballoon':
Strategy = RedBalloon(args)
if args.Strategy == 'opticalflow':
Strategy = OpticalFlow(args)
#configure a logger
sc_logger.set_name(Strategy.name())
sc_logger.text(sc_logger.GENERAL, 'Starting {0}'.format(Strategy.name()))
Strategy.run()
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()
'''
#parse arguments
parser = argparse.ArgumentParser(description="Use image processing to peform visual navigation")
#optional arguments
parser.add_argument('-S', '--Strategy', action="store", default='land', type=str,
choices=['land','redballoon','opticalflow'],
help='Land: vision assisted landing \n redballoon: redballoon finder for AVC 2014 \n opticalflow: Optical flow using webcam')
parser.add_argument('-c', '--camera', default=0, help="Select the camera index for opencv")
parser.add_argument('-i', '--input', default=False, help='use a video filename as an input instead of a webcam')
parser.add_argument('-f', '--file', default='Smart_Camera.cnf', help='load a config file other than the default')
args, unknown = parser.parse_known_args()
'''
#run a strategy
Strategy = None
if args.Strategy == 'land':
Strategy = PrecisionLand(args)
if args.Strategy == 'redballoon':
Strategy = RedBalloon(args)
if args.Strategy == 'opticalflow':
Strategy = OpticalFlow(args)
#configure a logger
sc_logger.set_name(Strategy.name())
sc_logger.text(sc_logger.GENERAL, 'Starting {0}'.format(Strategy.name()))
Strategy.run()
def run(self):
sc_logger.text(sc_logger.GENERAL, 'running {0}'.format(self.name()))
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)
#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]))
#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]))
#print Location(-35.363554, 149.165139,0)
# if starting from mavproxy
if __name__ == "__builtin__":
# start precision landing
strat = GetPara()
# connect to droneapi
sc_logger.text(sc_logger.GENERAL, 'Connecting to vehicle...')
strat.connect()
sc_logger.text(sc_logger.GENERAL, 'Vehicle connected!')
# run strategy
strat.run()
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))
return (x, y)
# if starting from mavproxy
if __name__ == "__builtin__":
# start precision landing
strat = PrecisionLand()
# connect to droneapi
sc_logger.text(sc_logger.GENERAL, 'Connecting to vehicle...')
strat.connect()
sc_logger.text(sc_logger.GENERAL, 'Vehicle connected!')
# run strategy
strat.run()
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 = 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')
