Python dronekit_sitl.SITL類代碼示例

	def connection(self,args):
		args=str(args)
		if args is 'sitl' or args is None:
			self._log("Starting copter simulator (SITL)")
			from dronekit_sitl import SITL
			sitl = SITL()
			self.sitl=sitl
			sitl.download('copter', '3.3', verbose=True)
			sitl_args = ['-I0', '--model', 'quad', '--home=-35.363261,149.165230,584,0']
			sitl.launch(sitl_args, await_ready=True, restart=True)
			connection_string='tcp:127.0.0.1:5760'
		elif args=='0':
			connection_string='/dev/ttyACM0'
		elif args=='1': 
			connection_string='/dev/ttyACM1'
		else:
			connection_string=str(args)
		self._log('Connecting to vehicle on: %s' % connection_string)
		vehicle = connect(connection_string, wait_ready=True)

		# Register observers
		vehicle.add_attribute_listener('location',self.location_callback)
		#vehicle.add_attribute_listener('battery',self.battery_callback)
		#vehicle.add_attribute_listener('heading',self.heading_callback)
		return vehicle

def test_can_arm():
    sitl = SITL()
    sitl.download('copter', '3.3')
    sitl.launch(copter_args, verbose=True, await_ready=True)
    vehicle = dronekit.connect("tcp:127.0.0.1:5760", wait_ready=True)
    # Wait for vehicle to be armable
    timeout = time.time() + 10
    while not vehicle.is_armable and time.time() < timeout:
        time.sleep(0.35)
    timed_out = time.time() >= timeout
    if timed_out:
        sitl.stop()
    assert_false(timed_out, "Vehicle init timed out")

    # Arm it
    vehicle.mode = dronekit.VehicleMode("GUIDED")
    vehicle.armed = True

    # Confirm that it armed
    timeout = time.time() + 10
    while not vehicle.armed and time.time() < timeout:
        time.sleep(0.35)
    timed_out = time.time() >= timeout
    vehicle.close()
    sitl.stop()
    assert_false(timed_out, "Vehicle arming timed out")

def initializeDrone(connectionString, isSimulator):
    if isSimulator:
        sitl = SITL()
        sitl.download('copter', '3.3', verbose=True)
        sitl_args = [
            '-I0', '--model', 'quad', '--home=-35.363261,149.165230,584,353'
        ]
        sitl.launch(sitl_args, await_ready=True, restart=True)
        print "Connecting to vehicle on: 'tcp:127.0.0.1:5760'"
        vehicle = connect('tcp:127.0.0.1:5760', wait_ready=True)
        # print information about vehicle
        print "Global Location: %s" % vehicle.location.global_frame
        print "Global Location (relative altitude): %s" % vehicle.location.global_relative_frame
        print "Local Location: %s" % vehicle.location.local_frame  #NED
        print "Attitude: %s" % vehicle.attitude
        print "Velocity: %s" % vehicle.velocity
        print "GPS: %s" % vehicle.gps_0
        print "Groundspeed: %s" % vehicle.groundspeed
        print "Airspeed: %s" % vehicle.airspeed
        print "Battery: %s" % vehicle.battery
        print "EKF OK?: %s" % vehicle.ekf_ok
        print "Last Heartbeat: %s" % vehicle.last_heartbeat
        print "Rangefinder: %s" % vehicle.rangefinder
        print "Rangefinder distance: %s" % vehicle.rangefinder.distance
        print "Rangefinder voltage: %s" % vehicle.rangefinder.voltage
        print "Heading: %s" % vehicle.heading
        print "Is Armable?: %s" % vehicle.is_armable
        print "System status: %s" % vehicle.system_status.state
        print "Mode: %s" % vehicle.mode.name  # settable
        print "Armed: %s" % vehicle.armed
    else:
        # Connect to the Vehicle
        print 'Connecting to vehicle;'
        vehicle = connect(connectionString, wait_ready=True)
        # print information about vehicle
        print "Global Location: %s" % vehicle.location.global_frame
        print "Global Location (relative altitude): %s" % vehicle.location.global_relative_frame
        print "Local Location: %s" % vehicle.location.local_frame  #NED
        print "Attitude: %s" % vehicle.attitude
        print "Velocity: %s" % vehicle.velocity
        print "GPS: %s" % vehicle.gps_0
        print "Groundspeed: %s" % vehicle.groundspeed
        print "Airspeed: %s" % vehicle.airspeed
        print "Battery: %s" % vehicle.battery
        print "EKF OK?: %s" % vehicle.ekf_ok
        print "Last Heartbeat: %s" % vehicle.last_heartbeat
        print "Rangefinder: %s" % vehicle.rangefinder
        print "Rangefinder distance: %s" % vehicle.rangefinder.distance
        print "Rangefinder voltage: %s" % vehicle.rangefinder.voltage
        print "Heading: %s" % vehicle.heading
        print "Is Armable?: %s" % vehicle.is_armable
        print "System status: %s" % vehicle.system_status.state
        print "Mode: %s" % vehicle.mode.name  # settable
        print "Armed: %s" % vehicle.armed

def start_default_sitl(lat=None, lon=None):
    print "Starting copter simulator (SITL)"
    from dronekit_sitl import SITL
    sitl = SITL()
    sitl.download('copter', '3.3', verbose=True)
    if ((lat is not None and lon is None) or
        (lat is None and lon is not None)):
        print("Supply both lat and lon, or neither")
        exit(1)
    sitl_args = ['-I0', '--model', 'quad', ]
    if lat is not None:
        sitl_args.append('--home=%f,%f,584,353' % (lat,lon,))
    sitl.launch(sitl_args, await_ready=True, restart=True)
    connection_string='tcp:127.0.0.1:5760'
    return (sitl, connection_string)

 def test_set_current(self):
     sitl = SITL()
     sitl.download('copter', '3.3', verbose=True)
     sitl_args = ['-I0', '--model', 'quad', '--home=-35.363261,149.165230,584,353']
     sitl.launch(sitl_args, await_ready=True, restart=True)
     print "Connecting to vehicle on: 'tcp:127.0.0.1:5760'"
     vehicle = connect('tcp:127.0.0.1:5760', wait_ready=True)
     print " Battery: %s" % vehicle.battery
     battery_test = Battery(vehicle.battery)
     # check number negative the limit
     battery_test.set_current(NEGATIVE_NUMBER)
     self.assertEqual(battery_test.current, 0)
     # check number positive the limit
     battery_test.set_current(POSITIVE_NUMBER)
     self.assertEqual(battery_test.get_current(), POSITIVE_NUMBER)
     # check ZERO
     battery_test.set_current(0)
     self.assertEqual(battery_test.get_current(), 0)
     #check char that non number
     battery_test.set_current(CHARACTER)
     self.assertEqual(battery_test.get_current(), 0)
     #check char that  number
     battery_test.set_current(CHAR_NUMBER)
     self.assertEqual(battery_test.get_current(), int(CHAR_NUMBER))
     sitl.stop()

 def test_land(self):
     sitl = SITL()
     sitl.download('copter', '3.3', verbose=True)
     sitl_args = ['-I0', '--model', 'quad', '--home=31.768923,35.193595,0,0']
     sitl.launch(sitl_args, await_ready=True, restart=True)
     drone = Drone('tcp:127.0.0.1:5760')
     headBefor =drone.vehicle.heading
     locBefor = drone.vehicle.location.global_relative_frame
     drone.take_off(25)
     print drone.vehicle.location.global_relative_frame
     drone.land()
     print drone.vehicle.location.global_relative_frame
     drone.take_off(4)
     print drone.vehicle.location.global_relative_frame
     drone.land()
     print drone.vehicle.location.global_relative_frame
     drone.take_off(2)
     print drone.vehicle.location.global_relative_frame
     drone.land()
     print drone.vehicle.location.global_relative_frame
     drone.take_off(10)
     print drone.vehicle.location.global_relative_frame
     drone.land()
     print drone.vehicle.location.global_relative_frame
     drone.vehicle.close()
     sitl.stop()

	def __init__(self , connectionString,mode):

         
            if mode =='r': #run on vihcle
                print 'Connecting to vehicle on: %s' % connectionString
                self.vehicle = connect(connectionString, baud=57600)
        
            if mode =='s': #run on simultor
                self.sitl = SITL()
                self.sitl.download('copter', '3.3', verbose=True)
                self.sitl_args = ['-I0','--gimbal' ,'--model', 'quad', '--home=31.7396,35.1956,3,-1']
                self.sitl.launch(self.sitl_args, await_ready=True, restart=True)
                self.vehicle = connect('tcp:127.0.0.1:5760', wait_ready=True)

 def test_take_off(self):
     sitl = SITL()
     sitl.download('copter', '3.3', verbose=True)
     sitl_args = ['-I0', '--model', 'quad', '--home=31.768923,35.193595,0,0']
     sitl.launch(sitl_args, await_ready=True, restart=True)
     drone = Drone('tcp:127.0.0.1:5760')
     headBefor =drone.vehicle.heading
     locBefor = drone.vehicle.location.global_relative_frame
     drone.take_off(3)
     headAfter = drone.vehicle.heading
     locAfter = drone.vehicle.location.global_relative_frame
     self.assertTrue( round(locAfter.alt-locBefor.alt)==3 and ((headBefor+5)%360<headAfter or(headBefor-5)%360 >headAfter))
     drone.vehicle.close()
     sitl.stop()

from dronekit_sitl import SITL
import dronekit
import time
from dronekit_sitl import version_list
from dronekit_sitl import download
import sys

copter_args = ['-S', '--model', 'quad', '--home=-35.363261,149.165230,584,353']

sitl = SITL()
sitl.launch(copter_args, await_ready=True, use_saved_data=True)
vehicle = dronekit.connect(connection_string, wait_ready=True)
print vehicle.parameters["COMPASS_USE"]
vehicle.close()
sitl.stop()

sys.exit(0)

sitl = SITL()
sitl.stop()
sitl.download('copter', '3.3')
sitl.launch(copter_args, verbose=True, await_ready=True, use_saved_data=False)
sitl.stop()

sys.exit(0)

versions = version_list()
models = list(versions.keys())
print models.sort()

download('copter','3.3', None)

parser = argparse.ArgumentParser(description="Demonstrates how to create attributes from MAVLink messages. ")
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 args.connect:
    print "Starting copter simulator (SITL)"
    from dronekit_sitl import SITL

    sitl = SITL()
    sitl.download("copter", "3.3", verbose=True)
    sitl_args = ["-I0", "--model", "quad", "--home=-35.363261,149.165230,584,353"]
    sitl.launch(sitl_args, await_ready=True, restart=True)
    connection_string = "tcp:127.0.0.1:5760"


# Connect to the Vehicle
print "Connecting to vehicle on: %s" % connection_string
vehicle = connect(connection_string, wait_ready=True, vehicle_class=MyVehicle)

# Add observer for the custom attribute


def raw_imu_callback(self, attr_name, value):
    # attr_name == 'raw_imu'

def setup_sitl():
    global sitl
    sitl = SITL('copter', '3.3-rc5')
    sitl.launch(sitl_args, await_ready=True, restart=True)

	def connect2FCU(self):
		connection_string = self.args.connect
		sitl=None
		if not self.args.connect:
	    		print "The connect string was not specified. Running SITL!"
			from dronekit_sitl import SITL
			sitl = SITL()
			sitl.download('copter', '3.3', verbose=True)
			sitl_args = ['-I0', '--model', 'quad', '--home=-35.363261,149.165230,584,353']
			sitl.launch(sitl_args, await_ready=True, restart=True)
			connection_string = 'tcp:127.0.0.1:5760'

		# Connect to the Vehicle. 
		#   Set `wait_ready=True` to ensure default attributes are populated before `connect()` returns.
		print "\nConnecting to vehicle on: %s" % connection_string
		vehicle = connect(connection_string, wait_ready=True)
		vehicle.wait_ready('autopilot_version')
		# Get all vehicle attributes (state)

		print "======================================================="
		print " Autopilot Firmware version: %s" % vehicle.version
		print "   Major version number: %s" % vehicle.version.major
		print "   Minor version number: %s" % vehicle.version.minor
		print "   Patch version number: %s" % vehicle.version.patch
		print "   Release type: %s" % vehicle.version.release_type()
		print "   Release version: %s" % vehicle.version.release_version()
		print "   Stable release?: %s" % vehicle.version.is_stable()
		'''
		print " Autopilot capabilities"
		print "   Supports MISSION_FLOAT message type: %s" % vehicle.capabilities.mission_float
		print "   Supports PARAM_FLOAT message type: %s" % vehicle.capabilities.param_float
		print "   Supports MISSION_INT message type: %s" % vehicle.capabilities.mission_int
		print "   Supports COMMAND_INT message type: %s" % vehicle.capabilities.command_int
		print "   Supports PARAM_UNION message type: %s" % vehicle.capabilities.param_union
		print "   Supports ftp for file transfers: %s" % vehicle.capabilities.ftp
		print "   Supports commanding attitude offboard: %s" % vehicle.capabilities.set_attitude_target
		print "   Supports commanding position and velocity targets in local NED frame: %s" % vehicle.capabilities.set_attitude_target_local_ned
		print "   Supports set position + velocity targets in global scaled integers: %s" % vehicle.capabilities.set_altitude_target_global_int
		print "   Supports terrain protocol / data handling: %s" % vehicle.capabilities.terrain
		print "   Supports direct actuator control: %s" % vehicle.capabilities.set_actuator_target
		print "   Supports the flight termination command: %s" % vehicle.capabilities.flight_termination
		print "   Supports mission_float message type: %s" % vehicle.capabilities.mission_float
		print "   Supports onboard compass calibration: %s" % vehicle.capabilities.compass_calibration
		print " Global Location: %s" % vehicle.location.global_frame
		print " Global Location (relative altitude): %s" % vehicle.location.global_relative_frame
		print " Local Location: %s" % vehicle.location.local_frame
		print " Attitude: %s" % vehicle.attitude
		print " Velocity: %s" % vehicle.velocity
		print " GPS: %s" % vehicle.gps_0
		print " Gimbal status: %s" % vehicle.gimbal
		print " Battery: %s" % vehicle.battery
		print " EKF OK?: %s" % vehicle.ekf_ok
		print " Last Heartbeat: %s" % vehicle.last_heartbeat
		print " Rangefinder: %s" % vehicle.rangefinder
		print " Rangefinder distance: %s" % vehicle.rangefinder.distance
		print " Rangefinder voltage: %s" % vehicle.rangefinder.voltage
		print " Heading: %s" % vehicle.heading
		print " Is Armable?: %s" % vehicle.is_armable
		print " System status: %s" % vehicle.system_status.state
		print " Groundspeed: %s" % vehicle.groundspeed    # settable
		print " Airspeed: %s" % vehicle.airspeed    # settable
		print " Mode: %s" % vehicle.mode.name    # settable
		print " Armed: %s" % vehicle.armed    # settable
		'''
		print "======================================================="
		return vehicle, sitl

def test_preserve_eeprom():
    # Start an SITL instance and change COMPASS_USE
    sitl = SITL()
    sitl.download('copter', '3.3')
    sitl.launch(copter_args, verbose=True, await_ready=True, use_saved_data=False)
    connection_string = sitl.connection_string()
    vehicle = dronekit.connect(connection_string, wait_ready=True)
    new_compass_use = 10
    print("Changing COMPASS_USE to {0}".format(new_compass_use))
    while vehicle.parameters["COMPASS_USE"] != new_compass_use:
        vehicle.parameters["COMPASS_USE"] = new_compass_use
        time.sleep(0.1)
    print("Changed COMPASS_USE to {0}".format(new_compass_use))
    time.sleep(5) # give parameters time to write
    sitl.stop()
    vehicle.close()

    # Now see if it persisted
    sitl.launch(copter_args, await_ready=True, use_saved_data=True)
    vehicle = dronekit.connect(connection_string, wait_ready=True)
    assert_equals(new_compass_use, vehicle.parameters["COMPASS_USE"])

    vehicle.close()
    sitl.stop()

 def test_download_404(self):
     sitl = SITL()
     sitl.download('rocket', '1.123.098.123')

def test_sitl():
    sitl = SITL()
    sitl.download('copter', '3.3')
    sitl.launch(copter_args)
    sitl.block_until_ready()
    
    assert_equals(sitl.poll(), None, 'SITL should still be running.')
    assert_equals(sitl.using_sim, False, 'SITL for copter-3.3 should not be using pysim')

    sitl.stop()
    assert sitl.poll() != None, 'SITL should stop running after kill.'

    # Test "relaunch"
    sitl.launch(copter_args)
    try:
    	sitl.launch(copter_args)
    	assert False, 'SITL should fail to launch() again when running'
    except:
    	pass
    try:
    	sitl.launch(copter_args, restart=True)
    except:
    	assert False, 'SITL should succeed in launch() when restart=True'

    sitl.stop()