def set_up():
parser = argparse.ArgumentParser(description='Commands vehicle using vehicle.simple_goto.')
parser.add_argument('--connect',
help="Vehicle connection target string. If not specified, SITL automatically started and used.")
args = parser.parse_args()
connection_string = args.connect
sitl = None
################################################################################################
#Start SITL if no connection string specified
################################################################################################
if not connection_string:
import dronekit_sitl
#sitl = dronekit_sitl.start_default()
#connection_string = sitl.connection_string()
ardupath ="/home/uav/git/ardupilot"
home = "41.714801,-86.241871,221,0"
sitl_defaults = os.path.join(ardupath, 'Tools', 'autotest', 'default_params', 'copter.parm')
sitl_args = ['-I{}'.format(0), '--home', home, '--model', '+', '--defaults', sitl_defaults]
sitl = dronekit_sitl.SITL(path=os.path.join(ardupath, 'build', 'sitl', 'bin', 'arducopter'))
sitl.launch(sitl_args, await_ready=True)
tcp, ip, port = sitl.connection_string().split(':')
port = str(int(port) + 0 * 10)
connection_string = ':'.join([tcp, ip, port])
def initializeVehicle(self, ardupath, connection_string, home, name):
self.logActive = False
home = home + ",221,0"
if not connection_string:
sitl_defaults = os.path.join(ardupath, 'Tools', 'autotest',
'default_params', 'copter.parm')
sitl_args = [
'-I{}'.format(len(Vehicle.vehicles)), '--home', home,
'--model', '+', '--defaults', sitl_defaults
]
sitl = dronekit_sitl.SITL(path=os.path.join(
ardupath, 'build', 'sitl', 'bin', 'arducopter'))
sitl.launch(sitl_args, await_ready=True)
tcp, ip, port = sitl.connection_string().split(':')
port = str(int(port) + len(Vehicle.vehicles) * 10)
connection_string = ':'.join([tcp, ip, port])
print('Connecting to vehicle on: %s' % connection_string)
self.vehicle = connect(connection_string, wait_ready=True)
Vehicle.vehicles[name] = self.vehicle
self.vehicle.wait_ready(timeout=500)
return self.vehicle
def bringup_drone(self, connection_string=None):
"""Connect to a dronekit vehicle or instantiate an sitl simulator.
Call this once everything is set up and you're ready to fly.
This is some deep magic related to running multiple drones at once. We
don't technically need it, since the network architecture I've set up
has the drones all be independant (if you want to simulate multiple
drones with the current setup you run multiple instances of the SITL
simulator). Chris got this all working by talking with the dronekit
devs, so he knows a lot more about it than I do.
connection_string -- Connect to an existing mavlink (SITL or the actual
ArduPilot). Provide None and it'll start its own
simulator.
"""
if not connection_string:
# Start SITL if no connection string specified
print "Starting SITL"
self.sitl = dronekit_sitl.SITL()
self.sitl.download('copter', '3.3', verbose=True)
sitl_args = ['--model', 'quad',
'--home=32.990756,-117.128362,243,0',
'--speedup', str(Pilot.sim_speedup),
'--instance', str(self.instance)]
working_dir = tempfile.mkdtemp()
self.sitl.launch(sitl_args,
verbose=True,
await_ready=True,
restart=True,
wd=working_dir)
time.sleep(6) # Allow time for the parameter to go back to EEPROM
connection_string = "tcp:127.0.0.1:{0}".format(5760 + 10 * self.instance)
#connection_string = "tcp:127.0.0.1:14550")
new_sysid = self.instance + 1
vehicle = dronekit.connect(connection_string, wait_ready=True)
while vehicle.parameters["SYSID_THISMAV"] != new_sysid:
vehicle.parameters["SYSID_THISMAV"] = new_sysid
time.sleep(0.1)
time.sleep(5) # allow eeprom write
vehicle.close()
self.sitl.stop()
# Do it again, and this time SYSID_THISMAV will have changed
self.sitl.launch(sitl_args,
verbose=True,
await_ready=True,
restart=True,
use_saved_data=True,
wd=working_dir)
self.vehicle = dronekit.connect(connection_string, wait_ready=True)
print vehicle
else:
# Connect to existing vehicle
print 'Connecting to vehicle on: %s' % connection_string
print "Connect to {0}, instance {1}".format(connection_string, self.instance)
self.vehicle = dronekit.connect(connection_string, wait_ready=True)
print "Success {0}".format(connection_string)
def bringup_drone(self, connection_string=None):
"""
Call this once everything is set up and you're ready to fly
:param connection_string: Connect to an existing mavlink (SITL or the actual ArduPilot)
Provide None and it'll start its own simulator
:return:
"""
if not connection_string:
# Start SITL if no connection string specified
print "Starting SITL"
self.sitl = dronekit_sitl.SITL()
self.sitl.download('copter', '3.3', verbose=True)
sitl_args = [
'--model', 'quad', '--home=32.990756,-117.128362,243,0',
'--speedup',
str(Pilot.sim_speedup), '--instance',
str(self.instance)
]
working_dir = tempfile.mkdtemp()
self.sitl.launch(sitl_args,
verbose=True,
await_ready=True,
restart=True,
wd=working_dir)
time.sleep(6) # Allow time for the parameter to go back to EEPROM
connection_string = "tcp:127.0.0.1:{0}".format(5760 +
10 * self.instance)
#connection_string = "tcp:127.0.0.1:14550")
new_sysid = self.instance + 1
vehicle = dronekit.connect(connection_string, wait_ready=True)
while vehicle.parameters["SYSID_THISMAV"] != new_sysid:
vehicle.parameters["SYSID_THISMAV"] = new_sysid
time.sleep(0.1)
time.sleep(5) # allow eeprom write
vehicle.close()
self.sitl.stop()
# Do it again, and this time SYSID_THISMAV will have changed
self.sitl.launch(sitl_args,
verbose=True,
await_ready=True,
restart=True,
use_saved_data=True,
wd=working_dir)
self.vehicle = dronekit.connect(connection_string, wait_ready=True)
print vehicle
else:
# Connect to existing vehicle
print 'Connecting to vehicle on: %s' % connection_string
print "Connect to {0}, instance {1}".format(
connection_string, self.instance)
self.vehicle = dronekit.connect(connection_string, wait_ready=True)
print "Success {0}".format(connection_string)
def initialize_drone(
self,
vehicle_num,
home="41.7144367,-86.2417136,221,0",
connection_string="",
):
self.vehicle_id = "UAV-" + str(vehicle_num)
print("\nInitializing drone: " + self.vehicle_id)
parser = argparse.ArgumentParser(
description='Print out vehicle state information')
parser.add_argument(
'--connect',
help=
"vehicle connection target string. If not specified, SITL automatically started and used"
)
args = parser.parse_args()
connection_string = args.connect
sitl = None
#Start SITL if no connection string specified
if not connection_string:
import dronekit_sitl
# connection_string = sitl.connection_string()
ardupath = "/home/uav/git/ardupilot"
self.home = home # In this example, all UAVs start on top of each other!
sitl_defaults = os.path.join(ardupath, 'Tools', 'autotest',
'default_params', 'copter.parm')
sitl_args = [
'-I{}'.format(vehicle_num), '--home', home, '--model', '+',
'--defaults', sitl_defaults
]
sitl = dronekit_sitl.SITL(path=os.path.join(
ardupath, 'build', 'sitl', 'bin', 'arducopter'))
sitl.launch(sitl_args, await_ready=True)
tcp, ip, port = sitl.connection_string().split(':')
print(port + " " + str(vehicle_num))
port = str(int(port) + vehicle_num * 10)
connection_string = ':'.join([tcp, ip, port])
################################################################################################
# Connect to the Vehicle
################################################################################################
print 'Connecting to vehicle on: %s' % connection_string
self.vehicle = connect(connection_string, wait_ready=True)
self.vehicle.wait_ready(timeout=120)
return self.vehicle, sitl
time.sleep(10)
def connect_vehicle(instance, home):
home_ = tuple(home) + (0,)
home_ = ','.join(map(str, home_))
sitl_defaults = os.path.join(ARDUPATH, 'Tools', 'autotest', 'default_params', 'copter.parm')
sitl_args = ['-I{}'.format(instance), '--home', home_, '--model', '+', '--defaults', sitl_defaults]
sitl = dronekit_sitl.SITL(path=os.path.join(ARDUPATH, 'build', 'sitl', 'bin', 'arducopter'))
sitl.launch(sitl_args, await_ready=True)
tcp, ip, port = sitl.connection_string().split(':')
port = str(int(port) + instance * 10)
conn_string = ':'.join([tcp, ip, port])
vehicle = dronekit.connect(conn_string)
vehicle.wait_ready(timeout=120)
return vehicle, sitl
def start_sitl(sitl_instance, ardupath, home=(41.71544, -86.24284, 0)):
home_str = ','.join(map(str, tuple(home) + (0, )))
sitl_defaults = os.path.join(ardupath, 'Tools', 'autotest',
'default_params', 'copter.parm')
sitl_args = [
'-I{}'.format(sitl_instance), '--home', home_str, '--model', '+',
'--defaults', sitl_defaults
]
sitl = dronekit_sitl.SITL(
path=os.path.join(ardupath, 'build', 'sitl', 'bin', 'arducopter'))
sitl.launch(sitl_args, await_ready=True)
tcp, ip, port = sitl.connection_string().split(':')
port = str(int(port) + sitl_instance * 10)
conn_string = ':'.join([tcp, ip, port])
return sitl, conn_string
def connect_vehicle(self, vehicle_type, vehicle_id, home, ardupath):
wait_till_armable = True
defaults = os.path.join(ardupath, 'Tools', 'autotest', 'default_params', 'copter.parm')
if len(home) == 2:
home = tuple(home) + (0,0)
else:
home = tuple(home)
if vehicle_type == 'VRTL':
sitl_args = ['--home', ','.join(map(str, home))]
print "Trying to launch SIT instance"
sitl = dronekit_sitl.SITL(path=os.path.join(ardupath, 'build', 'sitl', 'bin', 'arducopter'))
sitl.launch(sitl_args, await_ready=True)
tcp, ip, port = sitl.connection_string().split(':')
conn_string = ':'.join([tcp, ip, port])
vehicle = dronekit.connect(conn_string)
vehicle.wait_ready(timeout=120)
print "Vehicle connected"
if wait_till_armable:
while not vehicle.is_armable:
time.sleep(3)
print "Vehicle armable"
time.sleep(3)
self.vehicle = vehicle
self.vid = vehicle_id
handshake_message = DroneHandshakeMessage(self.vid, self.vehicle)
self.handshake_to_dronology_msgs.put_message(handshake_message.from_vehicle(self.vehicle, self.vid))
print "sent handshake"
self.state_msg_timer = Timer(self.state_interval, self.send_state_message)
################################################################################################
#Start SITL if no connection string specified
################################################################################################
if not connection_string:
import dronekit_sitl
#sitl = dronekit_sitl.start_default()
#connection_string = sitl.connection_string()
ardupath = "/home/uav/git/ardupilot"
home = "41.7144367,-86.2417136,221,0"
sitl_defaults = os.path.join(ardupath, 'Tools', 'autotest',
'default_params', 'copter.parm')
sitl_args = [
'-I{}'.format(0), '--home', home, '--model', '+', '--defaults',
sitl_defaults
]
sitl = dronekit_sitl.SITL(
path=os.path.join(ardupath, 'build', 'sitl', 'bin', 'arducopter'))
sitl.launch(sitl_args, await_ready=True)
tcp, ip, port = sitl.connection_string().split(':')
port = str(int(port) + 0 * 10)
connection_string = ':'.join([tcp, ip, port])
#vehicle = dronekit.connect(conn_string)
#vehicle.wait_ready(timeout=120)
################################################################################################
# Connect to the Vehicle
################################################################################################
print 'Connecting to vehicle on: %s' % connection_string
vehicle = connect(connection_string, wait_ready=True)
print vehicle.location.global_relative_frame.lon
'Print out vehicle state information. Connects to SITL on local PC by default.'
)
parser.add_argument(
'--connect',
help=
"vehicle connection target string. If not specified, SITL automatically started and used."
)
args = parser.parse_args()
connection_string = args.connect
sitl = None
if not connection_string:
import dronekit_sitl
sitl = dronekit_sitl.SITL()
sitl.download("copter", "3.3")
sitl.launch(["--home=51.449,5.493,1,0 --rate 30"], await_ready=True)
connection_string = sitl.connection_string()
drone = Cyclone(connection_string, configs)
# drone.awake_script()
drone.arm_and_takeoff(5)
drone.set_airspeed(5)
for i in range(len(listWPs)):
print('Start Mission %d' % (i + 1))
drone.goto_wp_global(LocationGlobalRelative(*listWPs[i]))
# drone.mode_rtl()
del drone
if sitl:
def _connect_vehicle(self,
vehicle_type,
vehicle_id,
ip,
instance,
ardupath,
rate,
home,
baud,
speedup,
defaults=None,
wait_till_armable=True):
"""
Connect to a vehicle
:param vehicle_type:
:param vehicle_id:
:param ip:
:param instance:
:param ardupath:
:param rate:
:param home:
:param baud:
:param speedup:
:param wait_till_armable:
:return:
"""
self._connection_initiated = True
status = 0
vehicle = None
if defaults is None:
defaults = os.path.join(ardupath, 'Tools', 'autotest',
'default_params', 'copter.parm')
if home is not None:
if len(home) == 2:
home = tuple(home) + (0, 0)
else:
home = tuple(home)
try:
if vehicle_type == DRONE_TYPE_PHYS:
vehicle = dronekit.connect(ip, wait_ready=True, baud=baud)
self._v_type = DRONE_TYPE_PHYS
if vehicle_id is None:
vehicle_id = ip
elif vehicle_type == DRONE_TYPE_SITL_VRTL:
if vehicle_id is None:
vehicle_id = vehicle_type + str(instance)
sitl_args = [
'-I{}'.format(str(instance)), '--model', '+', '--home',
','.join(map(str, home)), '--rate',
str(rate), '--speedup',
str(speedup), '--defaults', defaults
]
_LOG.debug(
'Trying to launch SITL instance {} on port {}...'.format(
instance, 5760 + instance * 10))
sitl = dronekit_sitl.SITL(path=os.path.join(
ardupath, 'build', 'sitl', 'bin', 'arducopter'))
sitl.launch(sitl_args, await_ready=True)
tcp, ip, port = sitl.connection_string().split(':')
port = str(int(port) + instance * 10)
conn_string = ':'.join([tcp, ip, port])
_LOG.debug('SITL instance {} launched on: {}'.format(
instance, conn_string))
vehicle = dronekit.connect(conn_string, baud=baud)
vehicle.wait_ready(timeout=120)
_LOG.info('Vehicle {} connected on {}'.format(
vehicle_id, conn_string))
self._v_type = DRONE_TYPE_SITL_VRTL
self._sitl = sitl
else:
_LOG.warn(
'vehicle type {} not supported!'.format(vehicle_type))
status = -1
if wait_till_armable:
# Force wait until vehicle is armable. (ensure all pre-flight checks are completed)
while not vehicle.is_armable:
time.sleep(3.0)
# Allow some extra time for things to initialize
time.sleep(3.0)
with self._drone_lock:
self._vehicle = vehicle
self._vid = vehicle_id
self._register_message_handlers(vehicle)
except dronekit.APIException:
status = -1
self._connection_complete = True
if status >= 0:
_LOG.info('Vehicle {} successfully initialized.'.format(self._vid))
self._handshake_out_msgs.put_message(
message.DroneHandshakeMessage.from_vehicle(
self._vehicle, self._vid))
self._state_msg_timer = util.etc.RepeatedTimer(
self._state_t, self.send_state_message)
else:
_LOG.error('Vehicle {} failed to initialize.'.format(self._vid))
def connect_vehicle(self):
#threading.Thread(target=self.connect_vehicle_thread).start()
#def connect_vehicle_thread(self):
print("Entering connect thread =======================")
drone = self.drone
ip = self.drone['ip']
vehicle = None
baud = 57600
status = 1
ardupath = drone["ardupath"]
defaults = os.path.join(ardupath, 'Tools', 'autotest',
'default_params', 'copter.parm')
home = drone['home']
if len(home) == 2:
home = tuple(home) + (0, 0)
else:
home = tuple(home)
try:
if self.drone['vehicle_type'] == 'PHYS':
vehicle = dronekit.connect(ip, wait_ready=True, baud=baud)
elif self.drone['vehicle_type'] == 'VRTL':
sitl_args = [
'--model', '+', '--home', ','.join(map(str, home)),
'--rate',
str(10), '--speedup',
str(1.0), '--defaults', defaults
]
sitl = dronekit_sitl.SITL(path=os.path.join(
ardupath, 'build', 'sitl', 'bin', 'arducopter'))
sitl.launch(sitl_args, await_ready=True)
tcp, ip, port = sitl.connection_string().split(':')
conn_string = ':'.join([tcp, ip, port])
vehicle = dronekit.connect(conn_string, baud=baud)
vehicle.wait_ready(timeout=120)
self.sitl = sitl
else:
print("Error: connect_vehicle")
while not vehicle.is_armable:
time.sleep(3.0)
time.sleep(3.0)
self.vehicle = vehicle
except dronekit.APIException:
status = -1
if status >= 0:
message = {}
message['type'] = "handshake"
message['uavid'] = drone['vehicle_id']
message['sendtimestamp'] = long(round(time.time() * 1000))
message['data'] = {}
message['data']['home'] = {}
message['data']['home']['x'] = home[0]
message['data']['home']['y'] = home[1]
message['data']['home']['z'] = home[2]
self.to_dronology.put_message(json.dumps(message))
else:
print("Error: connect_vehicle1")
print("exiting connect thread +++++++++++++++++++++++++++++")
