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 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)