def MoveTo(latitude, longitude, altitude): # # Vars # MyIP = 'udpin:0.0.0.0:14550' # Only works when on the right network. # # TargetHomeGPS = [39.3031664, -84.4779333, 223] # Hardcoded GPS Coordinate, this should be the input variable # # TargetHomeGPS = [39.302870, -84.477827, 233] # Far downfield, backup coordinates. # # TravelSpeed = 3 # integer, speed in m/s, Matt = [3, 5] # LandingSpeed = 1 # integer, speed in m/s, Matt = 3 # # # Connect # print MyIP # vehicle = connect(MyIP, wait_ready=True) # vehicle.mode = VehicleMode("GUIDED") # vehicle.armed = True # # print "Downloading the vehicle waypoints (commands). Wait until download is complete." # cmds = vehicle.commands # cmds.download() # cmds.wait_ready() # # # Get the home location # home_origin = vehicle.home_location # print "Launch Home = " + str(home_origin) # # # Set Locations # print "New Home" + str(TargetHomeGPS) # home_hover = LocationGlobal(TargetHomeGPS[0], TargetHomeGPS[1], TargetHomeGPS[2]) # home_hover.alt = TargetHomeGPS[2] + 20 # home_ground = LocationGlobal(TargetHomeGPS[0], TargetHomeGPS[1], TargetHomeGPS[2]) # home_ground.alt = TargetHomeGPS[2] - 5 # Make this a number that is slightly below the ground # time.sleep(1) # # # Change Home Location # i = 0 # while str(home_origin) == str(vehicle.home_location): # i = i + 1 # vehicle.home_location = home_ground # print "Updating home location Attempt #: " + str(i) # if i > 10: # print"Could not update new home location. Exiting" # vehicle.close() # time.sleep(1) # print "Successfully updated home location" # # print "Traveling over New Home" # vehicle.airspeed = TravelSpeed # vehicle.groundspeed = TravelSpeed # vehicle.simple_goto(home_hover) # time.sleep(5) # Increases for longer travel distances # # print "Landing" # vehicle.airspeed = LandingSpeed # vehicle.groundspeed = LandingSpeed # vehicle.mode = VehicleMode("RTL") # Go to home # vehicle.armed = False # vehicle.close() # Simple Version MyIP = 'udpin:0.0.0.0:14550' vehicle = connect(MyIP, wait_ready=True) vehicle.mode = VehicleMode("GUIDED") while str(vehicle.mode) == str(VehicleMode("GUIDED")): if args.debug: vehicle.armed = False else: vehicle.armed = True home_origin = vehicle.home_location home_ground = LocationGlobal(latitude, longitude, altitude) home_ground.alt = altitude - 10 time.sleep(1) i = 0 while str(home_origin) == str(vehicle.home_location): i = i + 1 vehicle.home_location = home_ground verbose("Updating home location Attempt #: " + str(i)) if i > 10: verbose("Could not update new home location. Exiting") vehicle.close() time.sleep(1) verbose("Successfully updated home location") vehicle.airspeed = 2 vehicle.groundspeed = 2 verbose("Go home") vehicle.mode = VehicleMode("RTL") while True: if delta(vehicle.altitude) > 0: up = True if level or down: send_ned_velocity(0,0,5,1) elif delta(vehicle.altitude) < 0: down = True pass else: level = True if down: print vehicle.capabilities.flight_termination vehicle.capabilities.flight_termination = True vehicle.armed = False time.sleep(1) if not vehicle.armed: vehicle.close() print "Vehicle Landed successfully" exit() while vehicle.mode != VehicleMode("GUIDED"): verbose("Controller Override, attempting to reconnect...") vehicle.close() MoveTo(args.gps[0], args.gps[1], args.gps[2])
# # print "Landing" # vehicle.airspeed = LandingSpeed # vehicle.groundspeed = LandingSpeed # vehicle.mode = VehicleMode("RTL") # Go to home # vehicle.armed = False # vehicle.close() MyIP = 'udpin:0.0.0.0:14550' # Only works when on the right network. vehicle = connect(MyIP, wait_ready=True) vehicle.mode = VehicleMode("GUIDED") vehicle.armed = True home_origin = vehicle.home_location home_ground = LocationGlobal(TargetHomeGPS[0], TargetHomeGPS[1], TargetHomeGPS[2]) home_ground.alt = TargetHomeGPS[2] - 5 time.sleep(1) i=0 while str(home_origin) == str(vehicle.home_location): i = i + 1 vehicle.home_location = home_ground print "Updating home location Attempt #: " + str(i) if i > 10: print"Could not update new home location. Exiting" vehicle.close() time.sleep(1) print "Successfully updated home location" vehicle.airspeed = 2 vehicle.groundspeed = 2 print "Go home"