def test_get_coordinates_other(self): relative_location = LocationGlobalRelative(3.0, 2.0, 1.0) self.relative_mock.configure_mock(return_value=relative_location) self.assertEqual(self.geometry.get_coordinates(self.locations_mock), (3.0, 2.0, 1.0)) self.relative_mock.assert_called_once_with()
while not drone.armed: print("waiting for arming...") time.sleep(1) print("Ready for takeoff, taking off...") drone.simple_takeoff(TargetAltitude) while True: Altitude = drone.location.global_relative_frame.alt print("Altitude", Altitude) time.sleep(1) if Altitude >= TargetAltitude * 0.95: print("Altitude reached") break #Vehicle Connection drone = connect("127.0.0.1:14551",wait_ready=True) arm_and_takeoff(20) drone.airspeed = 10 a_location = LocationGlobalRelative(-35.362144, 149.164409, 20) drone.simple_goto(a_location) time.sleep(50)
print (" System status: %s" % vehicle.system_status.state) print (" GPS: %s" % vehicle.gps_0) print (" Alt: %s" % vehicle.location.global_relative_frame.alt) # Continuous loop reading messages and handling commands while True: received_data = ser.read() time.sleep(0.03) data_left = ser.inWaiting() received_data += ser.read(data_left) received_str = received_data.decode() if received_str.startswith("DRONE_CMD_TAKEOFF"): arm_and_takeoff(10) elif received_str.startswith("DRONE_CMD_GOTO"): lat = float(re.search("(?<=;).*(?=:)", received_str).group()) lon = float(re.search("(?<=:)(.*)(?=DRONE_END)", received_str).group()) if not vehicle.armed: arm_and_takeoff(10) print("Set default/target airspeed to 3") vehicle.airspeed = 3 print("Going towards GPS point " + str(lat) + ", " + str(lon)) point1 = LocationGlobalRelative(lat, lon, 20) vehicle.simple_goto(point1) elif received_str.startswith( "DRONE_CMD_RTH"): vehicle.mode = VehicleMode("RTL") print("returning home") elif received_str.startswith("DRONE_CMD_LAND"): vehicle.mode = VehicleMode("LAND") else : ser.write(received_data)
#another drone is broadcasting its position, check if escape is needed e = escape((lat, lon, alt), (d[1], d[3], d[5])) if e is not None: #If escape is needed, return the escape point return LocationGlobalRelative(e[0], e[1], 5) #else: nothing to return return None #Hardcoded for testing purposes mission = [ #LocationGlobalRelative(50.862077, 4.684963, 5),#In front of the castle #LocationGlobalRelative(50.862003, 4.684558, 5),#In front of the castle #LocationGlobalRelative(50.862428, 4.684319, 5),#In front of the castle LocationGlobalRelative(50.862262, 4.684424, 5) #, #LocationGlobalRelative(50.861970, 4.682687, 5) #birds place ] esc_point = None arm_and_takeoff(vehicle, 4) vehicle.airspeed = 1.5 time.sleep(10) lat, lon, alt = getGPS(vehicle) ind = 0 eind = [] while running: if ind == len(mission):
arm_and_takeoff(vehicles[1], 10) print("Set default/target airspeed to 3") vehicles[0].airspeed = 10 vehicles[1].airspeed = 10 time.sleep(2) print("v#1 Going towards v#2 point for 30 seconds ...") target_act=[ vehicles[1]._location._lat, vehicles[1]._location._lon] rssi_obs=[] while True: v2_lat= vehicles[1]._location._lat v2_lon= vehicles[1]._location._lon v1_lat= vehicles[0]._location._lat v1_lon= vehicles[0]._location._lon point1 = LocationGlobalRelative(v2_lat, v2_lon, 20) #vehicles.simple_goto(point1) rssival, v1_xy = rssi([v1_lat,v1_lon],[v2_lat, v2_lon]) rssi_obs.append([v1_xy[0], v1_xy[1], rssival]) next_target_list=generate_next4(v1_xy[0], v1_xy[1]) print (next_target_list) #generate four moving candidate, return a list rssi_targetlist = predict_rssi (next_target_list) #return the list with corresponding rssi value maxrssi=max(rssi_targetlist) maxrssiind = rssi_targetlist.index(maxrssi) target=next_target_list[maxrssiind]
def go_to_point(lat, long, alt): point = LocationGlobalRelative(lat, long, alt) vehicle.simple_goto(point)
def handleRCs(self, channels): # check socket for a new ROI from the app self.checkSocket() # if we have never received an ROI if not self.rawROI: return # smooth ROI and calculate translateVel for follow self.filterROI() # if we are not warmed up, dont do anything if self.followState == FOLLOW_WAIT: return ''' Position Control Note: All follow controllers return position and velocity of the drone in "absolute" space (as opposed to relative to the ROI) Pos: Lat, lon, alt. Alt is relative to home location. NEU frame.. Vel: Speed in the x,y, and z directions. NEU frame. vel.z needs to be inverted before passing to the autopilot. ''' # Look At Me Mode (Vehicle stays put) if self.followState == FOLLOW_LOOKAT: pos, vel = self.pathController.move(channels) # Free Look Follow Mode (Vehicle controls are similar to FLY) elif self.followState == FOLLOW_FREELOOK: pos, vel = self.pathController.move(channels, newHeading=self.camYaw, newOrigin=self.filteredROI, roiVel=self.translateVel) # Follow Me Mode (Vehicle controls are similar to ORBIT) elif self.followState == FOLLOW_ORBIT: pos, vel = self.pathController.move( channels, cruiseSpeed=self.pathHandler.cruiseSpeed, newroi=self.filteredROI, roiVel=self.translateVel) elif self.followState == FOLLOW_LEASH: pos, vel = self.pathController.move(channels, newroi=self.filteredROI, roiVel=self.translateVel) # learn any changes to controller alt offset to apply it to other controllers when instantiated (to avoid jerks) self.followControllerAltOffset = pos.alt - self.filteredROI.alt # mavlink expects 10^7 integer for accuracy latInt = int(pos.lat * 10000000) lonInt = int(pos.lon * 10000000) # Convert from NEU to NED to send to autopilot vel.z *= -1 # create the SET_POSITION_TARGET_GLOBAL_INT command msg = self.vehicle.message_factory.set_position_target_global_int_encode( 0, # time_boot_ms (not used) 0, 1, # target system, target component mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, # frame 0b0000110111000000, # Pos Vel type_mask latInt, lonInt, pos.alt, # x, y, z positions vel.x, vel.y, vel.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) ''' Pointing Control''' # If followState mandates that the user controls the pointing if self.followState == FOLLOW_FREELOOK: self.manualPitch(channels[THROTTLE]) self.manualYaw(channels) self.handleFreeLookPointing() # If followState mandates that the copter should point at the ROI else: # Adjust the height of the ROI using the paddle # we pass in both the filtered gimbal paddle value as well as the raw one self.updateROIAltOffset(channels[RAW_PADDLE]) # we make a copy of the ROI to allow us to add in an altitude offset # this means the ROI doesn't get polluted with the alt nudging tempROI = LocationGlobalRelative(self.filteredROI.lat, self.filteredROI.lon, self.filteredROI.alt) self.handleLookAtPointing(tempROI)
#Arm and take off to altitude of 5 meters arm_and_takeoff(5) while True: if vehicle.mode.name != "GUIDED": print("User has changed flight modes - aborting follow-me") break # Read the GPS state from the laptop next(gpsd) # Once we have a valid location (see gpsd documentation) we can start moving our vehicle around if (gpsd.valid & gps.LATLON_SET) != 0: altitude = 30 # in meters dest = LocationGlobalRelative(gpsd.fix.latitude, gpsd.fix.longitude, altitude) print("Going to: %s" % dest) # A better implementation would only send new waypoints if the position had changed significantly vehicle.simple_goto(dest) # Send a new target every two seconds # For a complete implementation of follow me you'd want adjust this delay time.sleep(2) except socket.error: print( "Error: gpsd service does not seem to be running, plug in USB GPS or run run-fake-gps.sh" ) sys.exit(1)
time.sleep(1) print("Target altitude reached.") return None ##### MAIN EXECUTABLE ##### vehicle = connectMyCopter() arm_and_takeoff(10) # set the default speed vehicle.airspeed = 7 # go to wp1 print("go to wp1") wp1 = LocationGlobalRelative(47.193077,-1.465763,10) vehicle.simple_goto(wp1) # Wait 30sec time.sleep(30) # Coming back print("Coming Back") vehicle.mode = VehicleMode("RTL") time.sleep(20) #Close connection vehicle.close()
vehicle = connect(connection_string, wait_ready=True, baud=57600) if simulation: vehicle.mode = VehicleMode("GUIDED") while True: if (vehicle.mode == "GUIDED"): break time.sleep(0.1) if (simulation): print("Running simulation...") sim.load_target((os.path.dirname(os.path.realpath(__file__))) + '/Resources/target.PNG') print("Target loaded.") target = LocationGlobalRelative( vehicle.location.global_relative_frame.lat + 0.00002, vehicle.location.global_relative_frame.lon - 0.00002, vehicle.location.global_relative_frame.alt) sim.set_target_location(target) print("Target set.") arm_and_takeoff(vehicle, 10) else: video.startCamera() if ((re.compile("3*")).match(cv2.__version__)): fourcc = cv2.VideoWriter_fourcc(*'XVID') else: fourcc = cv2.cv.CV_FOURCC(*'XVID') i = len([ name for name in os.listdir((os.path.dirname(os.path.realpath(__file__))) +
break time.sleep(1) def my_goto_function(target): print('Target position is: %s' % target) print('Current position of vehicle is: %s' % vehicle.location.global_frame) print('Traveling') vehicle.simple_goto(target, groundspeed=10) time.sleep(30) print('Current position of vehicle is: %s' % vehicle.location.global_frame) arm_and_takeoff(10) initialLoc = LocationGlobalRelative(vehicle.location.global_frame.lat, vehicle.location.global_frame.lon, vehicle.location.global_frame.alt) point1 = LocationGlobalRelative(vehicle.location.global_frame.lat - 0.00008, vehicle.location.global_frame.lon + 0.00008, vehicle.location.global_frame.alt) my_goto_function(point1) point2 = LocationGlobalRelative(vehicle.location.global_frame.lat + 0.0001, vehicle.location.global_frame.lon, vehicle.location.global_frame.alt) my_goto_function(point2) my_goto_function(initialLoc) ####################################
# determine angle to target relative to drone and world phi1 = math.atan2(vcoord, hcoord) phi2 = yaw - phi1 # determine distance to target hyp = math.hypot(hcoord, vcoord) # determine location relative to drone, with global orientation y = hyp * math.sin(phi2) x = hyp * math.cos(phi2) print "Coordinates acquired: ", x, ", ", y, ", ", z # convert coordinates from feet relative to drone to global latitude and longitude xfactor = 1 / 298171.5253016029 yfactor = 1 / 363999.33433628065 x2 = (x * xfactor) + currentlocation.lon y2 = (y * yfactor) + currentlocation.lat print "Coordinates refactored: ", x2, ", ", y2, ", ", z, "; deploying" coords = LocationGlobalRelative(y2, x2, z) print "Going to: ", (coords.lon - launchloc.lon) / xfactor, ", ", ( coords.lat - launchloc.lat) / yfactor, ", ", coords.alt - launchloc.alt currentlocation = vehicle.location.global_relative_frame # print location, first in global lat and lon, then in feet from drone print "Location: ", currentlocation.lon, " ", currentlocation.lat, " ", currentlocation.alt, " Rloc: ", ( currentlocation.lon - launchloc.lon) / xfactor, ", ", ( currentlocation.lat - launchloc.lat) / yfactor, ", ", currentlocation.alt - launchloc.alt # begin travelling to target vehicle.simple_goto(coords) # loop to output location to track progress i = 0 while i < 18: time.sleep(0.5) currentlocation = vehicle.location.global_relative_frame
# Wait until the vehicle reaches a safe height before processing the goto # (otherwise the command after Vehicle.simple_takeoff will execute # immediately). while True: print(" Altitude: ", vehicle.location.global_relative_frame.alt) # Break and return from function just below target altitude. if vehicle.location.global_relative_frame.alt >= aTargetAltitude * 0.95: print("Reached target altitude") break time.sleep(1) lat = vehicle.location.global_relative_frame.lat lon = vehicle.location.global_relative_frame.lon alt = 5 start_point = LocationGlobalRelative(lat, lon, alt) arm_and_takeoff(5) T1 = time.time() def fly_to_GPS(aLocation, groundspeed): """ Fly to a specific GPS point """ currentLocation = vehicle.location.global_relative_frame #targetLocation = LocationGlobalRelative(34.25654,146.549946,3) targetDistance = get_distance_metres(currentLocation, aLocation) vehicle.simple_goto(aLocation, groundspeed)
def orbit_poi(vehicle, poi, configs): waypoints = [] # waypoints in LLA altitude = configs["altitude"] poi_scan_altitude = configs["detailed_search_specific"][ "poi_scan_altitude"] waypoint_tolerance = configs["waypoint_tolerance"] radius = configs["radius"] # radius of circle travelled orbit_number = configs["orbit_number"] # how many times we repeat orbit x, y, z = geodetic2ecef(poi.lat, poi.lon, poi_scan_altitude) # LLA -> ECEF n, e, d = ecef2ned(x, y, z, poi.lat, poi.lon, poi_scan_altitude) # ECEF -> NED cmds = vehicle.commands cmds.clear() # add circle of waypoints around the poi c = math.sqrt(2) / 2 point_list = [[1, 0], [c, c], [0, 1], [-c, c], [-1, 0], [-c, -c], [0, -1], [c, -c], [1, 0]] for orbit in range(orbit_number): for point in point_list: a = (radius * point[0]) + n b = (radius * point[1]) + e lat, lon, alt = ned2geodetic(a, b, d, poi.lat, poi.lon, poi_scan_altitude) # NED -> LLA waypoints.append(LocationGlobalRelative(lat, lon, alt)) # Go to center of POI if (configs["vehicle_type"] == "VTOL"): cmds.add( Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, waypoint_tolerance, 0, 0, poi.lat, poi.lon, poi_scan_altitude)) elif (configs["vehicle_type"] == "Quadcopter"): cmds.add( Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, poi.lat, poi.lon, poi_scan_altitude)) # Transition to quadcopter if applicable if (configs["vehicle_type"] == "VTOL"): cmds.add( Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_DO_VTOL_TRANSITION, 0, 0, mavutil.mavlink.MAV_VTOL_STATE_MC, 0, 0, 0, 0, 0, 0)) # Circular waypoints if (configs["vehicle_type"] == "VTOL"): for point in waypoints: cmds.add( Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, waypoint_tolerance, 0, 0, 0, point.lat, point.lon, point.alt)) elif (configs["vehicle_type"] == "Quadcopter"): for point in waypoints: cmds.add( Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, point.lat, point.lon, point.alt)) # Transition to forward flight if applicable if (configs["vehicle_type"] == "VTOL"): cmds.add( Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_DO_VTOL_TRANSITION, 0, 0, mavutil.mavlink.MAV_VTOL_STATE_MC, 0, 0, 0, 0, 0, 0)) # Add dummy endpoint cmds.add( Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, poi.lat, poi.lon, poi_scan_altitude)) print("Upload new commands to vehicle") cmds.upload()
def search(coordinates): global batteryPercent, personFound, personLocation, numOfRescued, emergencyLand, velocity,\ testCoordinates, stopDrone, Retreat, multi_rescue altitude = 5 # Get mission coordinates. print("BEFORE - coordinates: {}" .format(coordinates)) coordinates = search_algorithm(coordinates, altitude) print("AFTER - coordinates: {}" .format(coordinates)) global vehicle, DroneAction, bodyLocation arm_n_takeoff(altitude, vehicle) # 3.05m == 10ft vehicle.airspeed = 20 # Set drone speed in m/s. index = 1 # Used for printing current waypoint # source = 0 # 0 for first available webcam cap = cv2.VideoCapture(source) # Loading image from the camera if cap is None or not cap.isOpened(): # If unable to open the camera print('Warning: unable to open video source:', source) else: print('Opening video source:', source) # Execute until no more coordinates. This loop controls what the drone does if special case arises. for wp in coordinates: # Create LocationGlobalRelative variable of current waypoint to then pass to simple_goto() destination = LocationGlobalRelative(wp[0], wp[1], altitude) # Get distance between current location and destination. Used to detect arrival to waypoint. distance = get_distance_meters(vehicle.location.global_relative_frame, destination) # These 2 variables are used for comparison to detect if drone is stuck at waypoint. init_distance = distance count = 1 # Update telemtry telemetry() print("\nHeading to waypoint {}: {}\n" .format(index, wp)) DroneAction = "\nHeading to waypoint {}: {}\n" .format(index, wp) # Tells drone to move to destination. vehicle.simple_goto(destination) # Keep moving to destination as long as battery ok and no UI interaction occurs. while distance >= 1.5 and not Retreat and not emergencyLand and not stopDrone and batteryPercent > 20: # Detects if drone gets stuck while heading to waypoint by monitoring change in distance. If 10 iterations have passed # and the current distance is 95% or greater of the initial distance; then drone is stuck. # simple_goto cmd might have gotten lost. if count == 15 and distance/init_distance >= .95: print("Getting un-stuck.") vehicle.simple_goto(destination) # Resend goto cmd to finish heading to wp. count = 1 # Reset count variable to detect if stuck again. init_distance = distance # Update the ini_distance var so that it does not contain starting distance else: init_distance = distance # Update the ini_distance var so that it does not contain starting distance # If drone randomly changes to RTL go back to GUIDED. if vehicle.mode.name == 'RTL': vehicle.mode = VehicleMode("GUIDED") while not vehicle.mode.name == "GUIDED": print("FIXING RANDOM RTL...") print("FIXED: IN GUIDED AGAIN...") vehicle.simple_goto(destination) # Set the 'personFound' variable to be equal to the value outputted from the CV model personFound = CV_Model(cap, multi_rescue) if personFound: if multi_rescue: # If rescueing multiple do not RTL at first target find. # Store person location personLocation.append( (vehicle.location.global_frame.lat, vehicle.location.global_frame.lon) ) print("\nFOUND PERSON AT: ({0:.4f}, {1:.4f})" .format(personLocation[-1][-2], \ personLocation[-1][-1])) # Last appended lat & lon bodyLocation = "\nFound person at: ({0:.4f}, {1:.4f})" .format(personLocation[-1][-2], personLocation[-1][-1]) # Lower flag to not trigger again, count person, and sleep for 2 seconds to avoid detecting same person. personFound = False numOfRescued += 1 sleep(2) else: # Finds 1 target and RTL. print("\nFOUND PERSON AT: ({0:.4f}, {1:.4f})" .format(vehicle.location.global_frame.lat, \ vehicle.location.global_frame.lon)) # Last appended lat & lon print("Mission Complete!") bodyLocation = "\nFOUND PERSON AT: ({0:.4f}, {1:.4f})" .format(vehicle.location.global_frame.lat, vehicle.location.global_frame.lon) land() # Stop the camera cap.release() cv2.destroyAllWindows() print("Remaining distance: {0:.2f}m | Speed: {1:.2f}mph" .format(distance, velocity)) #DroneAction = "\nHeading to waypoint {}: {}\n" .format(index, wp) DroneAction = "Waypoint:" + str(index) + " | Remaining distance: {0:.2f}m".format(distance) telemetry() distance = get_distance_meters(vehicle.location.global_frame, destination) # Increment count. At count = 10 the code will detect if the distance has changed or not. count += 1 # Used to slow down the amount output printed. sleep(.5) #vehicle.simple_goto(destination) '''NOTE: NOT WORKING - Uncomment next two lines to test Retreat.''' #sleep(4) #stopDrone = True '''Uncomment next two lines to test Retreat.''' #sleep(4) #Retreat = True '''Uncomment next two lines to test emergencyLand.''' #sleep(4) #emergencyLand = True '''Uncomment next two lines to test finding people.''' #if index == 4: #sleep(3) #personFound = True # If user presses Retreat button exit and RTL if Retreat: print("\n-------------------------------------" "\nRETREAAAAT!" "\n-------------------------------------") DroneAction = "Retreating..." land() Retreat = False # If user presses emergency land button, drone lands. # Vehicle object closed and script exits. if emergencyLand: print("\n-------------------------------------" "\nEMERGENCY LAND!" "\n-------------------------------------") DroneAction = "Emergency Landing..." land() emergencyLand = False # If battery low, RTL. if batteryPercent <= 20: print("\n-------------------------------------" "\nBATTERY LOW! END OF MISSION." "\n-------------------------------------") DroneAction = "Battery Low... Ending Mission" sleep(.5) #emergencyLand = True land() index += 1 land()
from math import sin, cos, sqrt, atan2, radians import RPi.GPIO as GPIO import time import subprocess GPIO.setwarnings(False) #---------USER INPUTS------------- target_lat = 52.2296756 # Where the dispenser lands target_lng = 21.0122287 # Where the dispenser lands impact_lat = 41.390997 # Where the payload lands impact_lng = -73.953330 # Where the payload lands impact_alt = 5 # The altitude where the payload lands impact_point = LocationGlobalRelative(impact_lat, impact_lng, impact_alt) # Sets location for impact point dist_thresh = 9000000 # Distance Threshold alt_thresh = 5000 # Altitude Threshold #-----------SETUP----------------- loop_flag = 1 alt_flag = 0 low_alt_flag = 0 dist_flag = 0 connection_string = ('/dev/serial/by-id/usb-3D_Robotics_PX4_FMU_v2.x_0-if00') # Path that RPI looks to connect to Pixhawk vehicle = connect(connection_string, wait_ready = True) # Connects to vehicle print("Vehicle Connected, waiting 20 seconds") time.sleep(1)
while True: print "Altitude: ", vehicle.location.global_relative_frame.alt if vehicle.location.global_relative_frame.alt >= targetAltitude * 0.95: print "Reached target altitude" break time.sleep(1) # 座標指定で移動1 北 # 目標ロケーションの設定 target1_location_lat = vehicle.location.global_relative_frame.lat + 0.002 target1_location_lon = vehicle.location.global_relative_frame.lon target1_location_alt = 30 aLocation = LocationGlobalRelative(target1_location_lat, target1_location_lon, target1_location_alt) vehicle.simple_goto(aLocation) # 目標のロケーションに達するまで待つ # 場所検知がうまくいかないため、時間指定(60秒)待つ #while True: for i in range(60): # ロケーション情報の表示 ### このように常にモニタリングしておきたい属性がある場合はリスナー関数を作って登録しておくとすっきりします。 ### https://dronekit-python.readthedocs.io/en/latest/automodule.html#dronekit.Vehicle.add_attribute_listener print "\n Current location: %s " % vehicle.location.global_relative_frame print "\n Traget location: %s " % aLocation # if vehicle.location.global_relative_frame.lat >= target1_location_lat * 0.9: # print "Reached target lat"
def move_to_position(self, lat, lon, alt): location = LocationGlobalRelative(lat, lon, alt) self.vehicle.simple_goto(location)
print " " print "Altitude = %.0fcm" % z_cm print "Marker found x = %5.0f cm y = %5.0f cm -> angle_x = %5f angle_y = %5f" % ( x_cm, y_cm, angle_x * rad_2_deg, angle_y * rad_2_deg) north, east = uav_to_ne(x_cm, y_cm, vehicle.attitude.yaw) print "Marker N = %5.0f cm E = %5.0f cm Yaw = %.0f deg" % ( north, east, vehicle.attitude.yaw * rad_2_deg) marker_lat, marker_lon = get_location_metres( uav_location, north * 0.01, east * 0.01) #-- If angle is good, descend if check_angle_descend(angle_x, angle_y, angle_descend): print "Low error: descending" location_marker = LocationGlobalRelative( marker_lat, marker_lon, uav_location.alt - (land_speed_cms * 0.01 / freq_send)) else: location_marker = LocationGlobalRelative( marker_lat, marker_lon, uav_location.alt) vehicle.simple_goto(location_marker) print "UAV Location Lat = %.7f Lon = %.7f" % (uav_location.lat, uav_location.lon) print "Commanding to Lat = %.7f Lon = %.7f" % ( location_marker.lat, location_marker.lon) #--- COmmand to land if z_cm <= land_alt_cm: if vehicle.mode == "GUIDED": print(" -->>COMMANDING TO LAND<<")
def carrymission(target): actualdistance = calculatedistance(target, vehicle.location.global_relative_frame) vehicle.simple_goto(target) while vehicle.mode == "GUIDED": currentdist = calculatedistance(target, vehicle.location.global_relative_frame) if currentdist < 0.01 * actualdistance: time.sleep(2) break time.sleep(2) time.sleep(1) print("target reached") return None wp1 = LocationGlobalRelative(10.12057624, 76.35245919, 10) vehicle = connectmycopter() armandtakeoff(10) carrymission(wp1) vehicle.mode = VehicleMode("LAND") while vehicle.mode != "LAND": print("waiting to change to land") print("changedtoland") while true: sleep(1) vehicle.close()
def filterROI(self): '''Filters app ROI using a 5th order linear filter and calculates an associated roi velocity''' # store last 5 raw roi values self.rawROIQueue.append(self.rawROI) # only run filter if we have enough data in the queues. if len(self.rawROIQueue) == MIN_RAW_ROI_QUEUE_LENGTH and len( self.filteredROIQueue) == MIN_FILT_ROI_QUEUE_LENGTH: num = [ 0, 0.000334672774973874, 0.00111965413719632, -0.000469533537393159, -0.000199779184127412 ] den = [ 1, -3.48113699710809, 4.56705782792063, -2.67504447769757, 0.589908661075676 ] filteredLat = ( (num[4] * self.rawROIQueue[0].lat + num[3] * self.rawROIQueue[1].lat + num[2] * self.rawROIQueue[2].lat + num[1] * self.rawROIQueue[3].lat + num[0] * self.rawROIQueue[4].lat) - (den[4] * self.filteredROIQueue[0].lat + den[3] * self.filteredROIQueue[1].lat + den[2] * self.filteredROIQueue[2].lat + den[1] * self.filteredROIQueue[3].lat)) / den[0] filteredLon = ( (num[4] * self.rawROIQueue[0].lon + num[3] * self.rawROIQueue[1].lon + num[2] * self.rawROIQueue[2].lon + num[1] * self.rawROIQueue[3].lon + num[0] * self.rawROIQueue[4].lon) - (den[4] * self.filteredROIQueue[0].lon + den[3] * self.filteredROIQueue[1].lon + den[2] * self.filteredROIQueue[2].lon + den[1] * self.filteredROIQueue[3].lon)) / den[0] filteredAlt = ROI_ALT_FILTER_GAIN * self.filteredROIQueue[-1].alt + ( 1 - ROI_ALT_FILTER_GAIN ) * self.rawROI.alt # index -1 should refer to most recent value in the queue. self.filteredROI = LocationGlobalRelative(filteredLat, filteredLon, filteredAlt) else: self.filteredROI = self.rawROI # store last 4 filtered roi values self.filteredROIQueue.append(self.filteredROI) # only called once - when we have an ROI from phone if len(self.filteredROIQueue) == 1: # initialize ROI velocity for Guided controller self.roiVelocity = Vector3(0, 0, 0) # initialize the altitude maintained for each controller # the first ROI sent from the app is supposed to have 0 altitude, but in case it doesn't subtract it out. self.followControllerAltOffset = self.vehicle.location.global_relative_frame.alt - self.rawROI.alt logger.log( '[Follow Me]: First ROI received from app has altitude: %f m' % self.rawROI.alt) # go into Look At Me as default state. iOS app changes state to Orbit after 3 seconds. self.initState(FOLLOW_LOOKAT) elif len(self.filteredROIQueue) > 1 and self.roiDeltaTime: #calculate vector from previous to new roi in North,East,Up vec = location_helpers.getVectorFromPoints( self.filteredROIQueue[-2], self.filteredROIQueue[-1]) # calculate velocity based on time difference # roiVelocity in NEU frame self.roiVelocity = vec / self.roiDeltaTime # calculate desiredYaw and desiredPitch from new ROI self.desiredYaw, self.desiredPitch = location_helpers.calcYawPitchFromLocations( self.vehicle.location.global_relative_frame, self.filteredROI) #x component accel limit if self.roiVelocity.x > self.translateVel.x: self.translateVel.x += ACCEL_PER_TICK self.translateVel.x = min(self.translateVel.x, self.roiVelocity.x) elif self.roiVelocity.x < self.translateVel.x: self.translateVel.x -= ACCEL_PER_TICK self.translateVel.x = max(self.translateVel.x, self.roiVelocity.x) #y component accel limit if self.roiVelocity.y > self.translateVel.y: self.translateVel.y += ACCEL_PER_TICK self.translateVel.y = min(self.translateVel.y, self.roiVelocity.y) elif self.roiVelocity.y < self.translateVel.y: self.translateVel.y -= ACCEL_PER_TICK self.translateVel.y = max(self.translateVel.y, self.roiVelocity.y) #z component accel limit if self.roiVelocity.z > self.translateVel.z: self.translateVel.z += ACCEL_PER_TICK self.translateVel.z = min(self.translateVel.z, self.roiVelocity.z) elif self.roiVelocity.z < self.translateVel.z: self.translateVel.z -= ACCEL_PER_TICK self.translateVel.z = max(self.translateVel.z, self.roiVelocity.z)
def test_goto(connpath): vehicle = connect(connpath, wait_ready=True) # NOTE these are *very inappropriate settings* # to make on a real vehicle. They are leveraged # exclusively for simulation. Take heed!!! vehicle.parameters['FS_GCS_ENABLE'] = 0 vehicle.parameters['FS_EKF_THRESH'] = 100 def arm_and_takeoff(aTargetAltitude): """ Arms vehicle and fly to aTargetAltitude. """ # Don't let the user try to fly autopilot is booting i = 60 while not vehicle.is_armable and i > 0: time.sleep(1) i = i - 1 assert_equals(vehicle.is_armable, True) # Copter should arm in GUIDED mode vehicle.mode = VehicleMode("GUIDED") i = 60 while vehicle.mode.name != 'GUIDED' and i > 0: # print " Waiting for guided %s seconds..." % (i,) time.sleep(1) i = i - 1 assert_equals(vehicle.mode.name, 'GUIDED') # Arm copter. vehicle.armed = True i = 60 while not vehicle.armed and vehicle.mode.name == 'GUIDED' and i > 0: # print " Waiting for arming %s seconds..." % (i,) time.sleep(1) i = i - 1 assert_equals(vehicle.armed, True) # Take off to target altitude vehicle.simple_takeoff(aTargetAltitude) # Wait until the vehicle reaches a safe height before # processing the goto (otherwise the command after # Vehicle.simple_takeoff will execute immediately). while True: # print " Altitude: ", vehicle.location.global_relative_frame.alt # Test for altitude just below target, in case of undershoot. if vehicle.location.global_relative_frame.alt >= aTargetAltitude * 0.95: # print "Reached target altitude" break assert_equals(vehicle.mode.name, 'GUIDED') time.sleep(1) arm_and_takeoff(10) # print "Going to first point..." point1 = LocationGlobalRelative(-35.361354, 149.165218, 20) vehicle.simple_goto(point1) # sleep so we can see the change in map time.sleep(3) # print "Going to second point..." point2 = LocationGlobalRelative(-35.363244, 149.168801, 20) vehicle.simple_goto(point2) # sleep so we can see the change in map time.sleep(3) # print "Returning to Launch" vehicle.mode = VehicleMode("RTL") vehicle.close()
break time.sleep(1) arm_and_takeoff(ALT) dist = 0 while 1: a = vehicle.location.global_frame.lon #get Longitude and Latitude b = vehicle.location.global_frame.lat print "longitude=",a print "latitude=",b lat = 0 lon = 0.000011503*STEP_SIZE # move in this direction with the distance of STEP_SIZE. (in degrees) new_lat = b + lat new_lon = a + lon print "Going towards east point" point1 = LocationGlobalRelative(new_lat, new_lon, ALT) #set destination accordinag to the step size of the drone. vehicle.simple_goto(point1) #give command to the drone so it will go in that direction time.sleep(5) #give it 5 seconds to reach there yaw = vehicle.attitude.yaw #get yaw yaw = math.degrees(yaw) #convert it in degrees alpha = math.degrees(np.arctan2(lat,lon)) # It is the direction of the drone in Polar Coordinate System if alpha < 0: alpha = alpha + 360 alpha = alpha + 90 # The camera attached with the drone is 90 degrees rotated respect to the drone direction = yaw + alpha # actual direction of the picture captured by the raspberry pi dist = dist + STEP_SIZE data = struct.pack('ff',direction,STEP_SIZE) #pack direction and step size in one packet s.send(data) #send that packet to the raspberry pi data = s.recv(1024) # wait for the acknowledgement from the raspberry pi if data == 'pic_taken': print "confirmation : pic has taken"
if vehicle.location.global_relative_frame.alt >= aTargetAltitude * 0.95: print("Reached target altitude") break time.sleep(1) #get home/landing point GPS start_point=vehicle.location.global_relative_frame arm_and_takeoff(3) T1=time.time() print("Set default/target airspeed to 3") vehicle.airspeed = 3 point1 = LocationGlobalRelative(-35.361354, 149.165218,3) point2 = LocationGlobalRelative(-35.363244, 149.168801,3) point3 = LocationGlobalRelative(-35.365489, 149.164801,3) point4 = LocationGlobalRelative(-35.361452, 149.161801,3) i=0 N=8 while i<N print("Going towards P1") fly_to_GPS(point1) print("Going towards P2") fly_to_GPS(point2) print("Going towards P3") fly_to_GPS(point3)
while True: print(" Altitude: ", vehicle.location.global_relative_frame.alt) # Break and return from function just below target altitude. if vehicle.location.global_relative_frame.alt >= aTargetAltitude * 0.95: print("Reached target altitude") break time.sleep(1) arm_and_takeoff(3) print("Set default/target airspeed to 3") vehicle.airspeed = 3 print("Going towards first point for 30 seconds ...") point1 = LocationGlobalRelative(-35.361354, 149.165218,2) vehicle.simple_goto(point1) # sleep so we can see the change in map time.sleep(10) print("Returning to Launch") vehicle.mode = VehicleMode("RTL") # Close vehicle object before exiting script print("Close vehicle object") vehicle.close()
if Altitude >= TargetAltitude * 0.95: print("Altitude reached") break #Vehicle Connection #Esto es basicamente la direccion que tiene el dron para que lo podamos conectar en las terminales de Ubuntu. #Y la velocidad a la que esta yendo al igual que la altura que queremos. drone = connect('127.0.0.1:14551', wait_ready=True) arm_and_takeoff(20) drone.airspeed = 10 #Estas cuatro lineas de codigo son para darles las cordenadas a nuestro dron de a donde tiene que ir en la mision. a_location = LocationGlobalRelative(20.7377260, -103.4570155, 20) b_location = LocationGlobalRelative(20.7376890, -103.4565404, 20) c_location = LocationGlobalRelative(20.7371951, -103.4565840, 20) d_location = LocationGlobalRelative(20.7372334, -103.4570624, 20) print("Despegue Exitoso") print("Going to point a") #Esto que queda sirve para que el dron vaya a las cordenadas y regrese a casa. drone.simple_goto(a_location) time.sleep(40) print("a location reached going to point b") drone.simple_goto(b_location) time.sleep(40) print("b location reached going to point c") drone.simple_goto(c_location)
'd', 0 ) # initializing direction value 0-> no val 1->right 2->centre 3->left p1 = multiprocessing.Process( target=cv ) # assigning parallel processes to detect faces and collision simultaneously p2 = multiprocessing.Process(target=collision) p1.start() p2.start() loc = [28.46951466793185, 77.53698735091803] # vehicle home location vehicle = connectMyCopter() arm_and_takeoff(5) pt1 = LocationGlobalRelative(loc[0], loc[1], 10) vehicle.simple_goto(pt1) while True: print("the direction i'm receiving is ", direc.value) d = collision() if d < 20: print('collsion') set_velocity_body(vehicle, 0, gnd_speed, 0) vehicle.simple_goto(pt1) print vehicle.location.global_relative_frame.lat, vehicle.location.global_relative_frame.lon
def excecuteMission(vehicle): print("Excecuting Mission") waypoint = LocationGlobalRelative(-35.362270,149.165091,20) vehicle.simple_goto(waypoint) time.sleep(30)
#vehicle.mode = VehicleMode("LAND") vehicle.armed = False time.sleep(1) currstate = 'idle' #currstate='land' elif currstate == 'arm': print("main thread enter arm state") arm_and_takeoff(2) print("Set default/target airspeed to 3") vehicle.airspeed = 3 currstate = 'armed' elif currstate == 'wp': print("main thread enter wp state") print("Going towards first point for 30 seconds ...") point1 = LocationGlobalRelative(-35.361354, 149.165218, 20) vehicle.simple_goto(point1) # sleep so we can see the change in map time.sleep(30) print( "Going towards second point for 30 seconds (groundspeed set to 10 m/s) ..." ) point2 = LocationGlobalRelative(-35.363244, 149.168801, 20) vehicle.simple_goto(point2, groundspeed=10) # sleep so we can see the change in map time.sleep(30) currstate = 'rtl' elif currstate == 'localwp': print(
def _make_relative_location(self, x, y, z=0.0): return LocationGlobalRelative(x, y, z)