def test_badimports(self): """test a bad import, ie one that does not exist""" try: mod = getpymavlinkpackage('bad', 1.0) except ValueError as e: assert str(e) == 'Incorrect mavlink dialect' assert 'mod' not in locals() try: mod = getpymavlinkpackage('common', 1.5) except ValueError as e: assert str(e) == 'Incorrect mavlink version (must be 1.0 or 2.0)' assert 'mod' not in locals()
def setUp(self): """Set up some data that is reused in many tests""" self.manager = None self.dialect = 'ardupilotmega' self.version = 2.0 self.mod = getpymavlinkpackage(self.dialect, self.version) self.mavUAS = self.mod.MAVLink(self, srcSystem=4, srcComponent=0, use_native=False) self.VehA = Vehicle(self.loop, "VehA", 255, 0, 4, 0, self.dialect, self.version) self.VehA.onPacketTxAttach(self.vehSendFunc) self.txPackets = {} self.txVehPackets = {} self.manager = moduleManager.moduleManager(self.loop, self.dialect, self.version, False) self.manager.onVehListAttach(self.getVehListCallback) self.manager.onVehGetAttach(self.getVehicleCallback) self.manager.onPktTxAttach(self.txcallback) self.manager.addModule("PaGS.modules.internalPrinterModule")
def setUp(self): """Set up some data that is reused in many tests""" self.manager = None self.dialect = 'ardupilotmega' self.version = 2.0 self.mod = getpymavlinkpackage(self.dialect, self.version) self.mavUAS = self.mod.MAVLink(self, srcSystem=4, srcComponent=0, use_native=False) self.VehA = Vehicle(self.loop, "VehA", 255, 0, 4, 0, self.dialect, self.version) self.VehA.onPacketTxAttach(self.vehSendFunc) self.VehA.hasInitial = True # The PaGS settings dir (just in source dir) self.settingsdir = os.path.join(os.getcwd(), ".PaGS") if not os.path.exists(self.settingsdir): os.makedirs(self.settingsdir) self.manager = moduleManager.moduleManager(self.loop, self.settingsdir, False) self.manager.onVehListAttach(self.getVehListCallback) self.manager.onVehGetAttach(self.getVehicleCallback) self.manager.onPktTxAttach(self.txcallback) self.manager.addModule("internalPrinterModule")
def __init__(self, dialect: str, mavversion: float, name: str, srcsystem: int, srccomp: int, rxcallback, clcallback=None) -> None: self.sourceSystem = srcsystem self.sourceComponent = srccomp self.mod = getpymavlinkpackage(dialect, mavversion) self.packetsRx = collections.deque() self.packetsTx = collections.deque() self.mav = self.mod.MAVLink(self, self.sourceSystem, self.sourceComponent, use_native=False) self.mav.robust_parsing = True # BW measures for RX, per sysid # bytes and time(sec) in measurement period self.bytesmeasure = (0, time.time()) self.bytespersecond = 0 self.callback = rxcallback self.closecallback = clcallback # Loss % per sysid # BW measures for TX, per sysid self.name = name
def setUp(self): """Set up some data that is reused in many tests""" self.manager = None # The PaGS settings dir (just in source dir) self.settingsdir = os.path.join(os.getcwd(), ".PaGS") if not os.path.exists(self.settingsdir): os.makedirs(self.settingsdir) self.dialect = 'ardupilotmega' self.version = 2.0 self.mod = getpymavlinkpackage(self.dialect, self.version) self.mavUAS = self.mod.MAVLink( self, srcSystem=4, srcComponent=0, use_native=False) self.VehA = Vehicle(self.loop, "VehA", 255, 0, 4, 0, self.dialect, self.version) self.VehA.hasInitial = True self.VehB = Vehicle(self.loop, "VehB", 255, 0, 5, 0, self.dialect, self.version) self.VehB.hasInitial = True self.VehC = Vehicle(self.loop, "VehC", 255, 0, 10, 0, self.dialect, self.version) self.VehC.hasInitial = False self.txPackets = {}
def test_modemappings(self): """Test the mode_toString() method""" mavlink = getpymavlinkpackage('ardupilotmega', 2.0) # PX4 pktIn = mavlink.MAVLink_heartbeat_message( mavlink.MAV_TYPE_QUADROTOR, mavlink.MAV_AUTOPILOT_PX4, 0, 0, 0, 2) assert mode_toString(pktIn, mavlink) == "MANUAL" pktIn = mavlink.MAVLink_heartbeat_message( mavlink.MAV_TYPE_QUADROTOR, mavlink.MAV_AUTOPILOT_PX4, 0, 5, 0, 2) assert mode_toString(pktIn, mavlink) == "AUTO_RTL" # APM:Copter pktIn = mavlink.MAVLink_heartbeat_message( mavlink.MAV_TYPE_QUADROTOR, mavlink.MAV_AUTOPILOT_ARDUPILOTMEGA, 0, 0, 0, 2) assert mode_toString(pktIn, mavlink) == "STABILIZE" pktIn = mavlink.MAVLink_heartbeat_message( mavlink.MAV_TYPE_QUADROTOR, mavlink.MAV_AUTOPILOT_ARDUPILOTMEGA, 0, 5, 0, 2) assert mode_toString(pktIn, mavlink) == "LOITER" # APM:Rover pktIn = mavlink.MAVLink_heartbeat_message( mavlink.MAV_TYPE_GROUND_ROVER, mavlink.MAV_AUTOPILOT_ARDUPILOTMEGA, 0, 0, 0, 2) assert mode_toString(pktIn, mavlink) == "MANUAL" pktIn = mavlink.MAVLink_heartbeat_message( mavlink.MAV_TYPE_GROUND_ROVER, mavlink.MAV_AUTOPILOT_ARDUPILOTMEGA, 0, 10, 0, 2) assert mode_toString(pktIn, mavlink) == "AUTO" # APM:Sub pktIn = mavlink.MAVLink_heartbeat_message( mavlink.MAV_TYPE_SUBMARINE, mavlink.MAV_AUTOPILOT_ARDUPILOTMEGA, 0, 0, 0, 2) assert mode_toString(pktIn, mavlink) == "STABILIZE" pktIn = mavlink.MAVLink_heartbeat_message( mavlink.MAV_TYPE_SUBMARINE, mavlink.MAV_AUTOPILOT_ARDUPILOTMEGA, 0, 4, 0, 2) assert mode_toString(pktIn, mavlink) == "GUIDED" # APM:Plane pktIn = mavlink.MAVLink_heartbeat_message( mavlink.MAV_TYPE_FIXED_WING, mavlink.MAV_AUTOPILOT_ARDUPILOTMEGA, 0, 0, 0, 2) assert mode_toString(pktIn, mavlink) == "MANUAL" pktIn = mavlink.MAVLink_heartbeat_message( mavlink.MAV_TYPE_FIXED_WING, mavlink.MAV_AUTOPILOT_ARDUPILOTMEGA, 0, 20, 0, 2) assert mode_toString(pktIn, mavlink) == "QLAND" # APM:Antenna Tracker pktIn = mavlink.MAVLink_heartbeat_message( mavlink.MAV_TYPE_ANTENNA_TRACKER, mavlink.MAV_AUTOPILOT_ARDUPILOTMEGA, 0, 0, 0, 2) assert mode_toString(pktIn, mavlink) == "MANUAL" pktIn = mavlink.MAVLink_heartbeat_message( mavlink.MAV_TYPE_ANTENNA_TRACKER, mavlink.MAV_AUTOPILOT_ARDUPILOTMEGA, 0, 2, 0, 2) assert mode_toString(pktIn, mavlink) == "SCAN"
def setUp(self): """Set up some data that is reused in many tests""" self.manager = None self.dialect = 'ardupilotmega' self.version = 2.0 self.mod = getpymavlinkpackage(self.dialect, self.version) self.mavUAS = self.mod.MAVLink(self, srcSystem=4, srcComponent=0, use_native=False) self.callbacks = {}
def setUp(self): """Set up some data that is reused in many tests""" self.manager = None self.dialect = 'ardupilotmega' self.version = 2.0 self.mod = getpymavlinkpackage(self.dialect, self.version) self.mavUAS = self.mod.MAVLink( self, srcSystem=4, srcComponent=0, use_native=False) self.VehA = Vehicle(self.loop, "VehA", 255, 0, 4, 0, self.dialect, self.version) self.txPackets = {}
def setUp(self): """Set up some data that is reused in many tests""" self.dialect = 'ardupilotmega' self.version = 2.0 self.ip = '127.0.0.1' self.port = 15000 self.cname = 'udpclient:127.0.0.1:15000' self.sname = 'udpserver:127.0.0.1:15000' self.mod = getpymavlinkpackage(self.dialect, self.version) self.mav = self.mod.MAVLink(self, srcSystem=0, srcComponent=0, use_native=False) self.cnum = 0 self.snum = 0
def setUp(self): """Set up some data that is reused in many tests""" self.dialect = 'ardupilotmega' self.version = 2.0 self.ip = '127.0.0.1' self.port = 15001 self.baud = 115200 self.cname = 'tcpclient:127.0.0.1:15001' self.sname = None # the serial <-> tcp server process. Will vary depending on OS # Based on # https://github.com/Apollon77/SupportingFiles/blob/master/README_SERIAL_TESTING.md self.serialServer = None if platform.system() == "Windows": self.serialServer = subprocess.Popen( [ r'.\\tests\\\support\\com2tcp.exe', '--ignore-dsr', '--baud', '115200', '--parity', 'e', '\\\\.\\CNCA0', '127.0.0.1', '15001'], stdout=subprocess.PIPE) self.sname = 'serial:\\\\.\\CNCB0:115200' self.sPort = '\\\\.\\CNCB0' elif platform.system() == "Linux": self.serialServer = subprocess.Popen( [ 'socat', '-D', '-x', '-s', 'pty,link=/tmp/virtualcom0,ispeed=115200,ospeed=115200,raw,waitslave', 'tcp:127.0.0.1:15001'], stdout=subprocess.PIPE) self.sname = 'serial:/tmp/virtualcom0:115200' self.sPort = '/tmp/virtualcom0' self.mod = getpymavlinkpackage(self.dialect, self.version) self.mav = self.mod.MAVLink( self, srcSystem=0, srcComponent=0, use_native=False) self.cnum = 0 self.snum = 0
def setUp(self): """Set up some data that is reused in many tests""" self.dialect = 'ardupilotmega' self.version = 2.0 self.port = 15000 self.ip = "127.0.0.1" self.cname = 'tcpclient:127.0.0.1:15000' self.mod = getpymavlinkpackage(self.dialect, self.version) self.mav = self.mod.MAVLink( self, srcSystem=4, srcComponent=0, use_native=False) self.connmtrx = None self.allvehicles = None self.allModules = None self.cnum = 0
def setUp(self): """Set up some data that is reused in many tests""" self.dialect = 'ardupilotmega' self.mavversion = 2.0 self.source_system = 255 self.source_component = 0 self.target_system = 1 self.target_component = 0 # This is the "vehicle" to respond to messages self.mod = getpymavlinkpackage(self.dialect, self.mavversion) self.mavVehicle = self.mod.MAVLink(self, srcSystem=self.target_system, srcComponent=self.target_component, use_native=False) self.mavVehicle.robust_parsing = True self.veh = None self.txpackets = [] self.onNewPackets = []
def setUp(self): """Set up some data that is reused in many tests""" self.dialect = 'ardupilotmega' self.version = 2.0 self.ip = "127.0.0.1" # The links self.linkA = 'tcpclient:127.0.0.1:15001' self.linkB = 'tcpserver:127.0.0.1:15020' self.linkC = 'udpclient:127.0.0.1:15002' self.linkD = 'udpserver:127.0.0.1:15021' # The vehicles. Note the vehicles A and C have the same sysid # Source s/c then target s/c self.VehA = Vehicle(self.loop, "VehA", 255, 0, 4, 0, self.dialect, self.version) self.VehB = Vehicle(self.loop, "VehB", 254, 0, 3, 0, self.dialect, self.version) self.VehC = Vehicle(self.loop, "VehC", 255, 0, 4, 0, self.dialect, self.version) # Dict of data rx'd by each link self.rxdata = {} # Dict of data rx'd by each vehicle self.vehpkts = {} self.mod = getpymavlinkpackage(self.dialect, self.version) # From the vehicle self.mavUAS = self.mod.MAVLink( self, srcSystem=4, srcComponent=0, use_native=False) self.mavoneUAS = self.mod.MAVLink( self, srcSystem=3, srcComponent=0, use_native=False) self.mavGCS = self.mod.MAVLink( self, srcSystem=255, srcComponent=0, use_native=False) self.mavoneGCS = self.mod.MAVLink( self, srcSystem=254, srcComponent=0, use_native=False)
def test_goodimports(self): """Test importing known good modules""" for dialect in self.dialects: for version in self.versions: mod = getpymavlinkpackage(dialect, version) assert mod is not None
def __init__(self, loop, name: str, source_system: int, source_component: int, target_system: int, target_component: int, dialect: str, mavversion: float): self.loop = loop # The latest of each packet type self.latestPacketDict = dict() # The vehicle self.source_system = source_system self.source_component = source_component self.target_system = target_system self.target_component = target_component self.dialect = dialect self.mavversion = mavversion # working folder for this vehicle self.folderdir = "" # Vehicle name self.name = name # Mavlink encoder for sending messages self.mod = getpymavlinkpackage(self.dialect, self.mavversion) self.mav = self.mod.MAVLink(self, srcSystem=source_system, srcComponent=source_component, use_native=False) self.mav.robust_parsing = True # Tx callback to connectionManager self.txcallback = None # parameters dict. Note all keys are byte arrays self.params = dict() self.params_type = dict() # Status of getting params: [n, total, [got_ids]] self.paramstatus = None # Waypoints, fence, rally points arrays self.waypoints = [] self.fence = [] self.rally = [] # Vehicle strings self.fcName = "" # MAV_AUTOPILOT_ string self.fcVersion = "" self.OSVersion = "" self.vehType = None # MAV_TYPE_ string # Vehicle state self.isArmed = None # True if armed, False if disarmed, None if unknown self.flightMode = None # None is unknown, string MAV_MODE_ otherwise self.isConnected = False # True if getting hb packets # Heartbeats (tx and rx) self.hbTimeout = 1 # Seconds with no hb packet = no connection self.TimeoutTask = asyncio.ensure_future(self.waitrxtimeout()) self.timeoflasthb = 0 # time of last rx'd hb self.hbInterval = 1 # Seconds between hb sending self.hbTxTask = asyncio.ensure_future(self.sendHeartbeat())
def __init__(self, loop, txClbk, vehListClk, vehObjClk, cmdProcessClk, cmdPrint, dialect, mavversion, isGUI): self.tabs = [] # all the vehicles, one in each tab self.loop = loop # Event actions self.txCallback = txClbk self.vehListCallback = vehListClk self.vehObjCallback = vehObjClk self.commandProcessor = cmdProcessClk self.printer = cmdPrint # Mavlink self.dialect = dialect self.mavversion = mavversion self.mod = getpymavlinkpackage(self.dialect, self.mavversion) # commands self.shortName = "terminal" self.commandDict = {'watch': self.watch} # Tell prompt_toolkit to use asyncio. terminal_use_async() self.tabbar = [] self.style_extensions = { # Tabs 'tabbar': 'noinherit', 'tabbar.tab': '', 'tabbar.tab.active': 'bold noinherit reverse', } self.current_style = Style.from_dict(self.style_extensions) # make the screen self.hscreen = [] self.hscreen.append( Window(height=1, content=FormattedTextControl(self.tabbar, style='class:tabbar'), align=WindowAlign.LEFT)) self.hscreen.append(Window(height=1, char='-', style='class:line')) self.hscreen.append(Window(content=None)) self.hscreen.append(Window(height=1, char='-', style='class:line')) self.hscreen.append(Window(height=1, content=None)) self.root_container = HSplit(self.hscreen) self.layout = Layout(self.root_container) self.application = Application(layout=self.layout, key_bindings=KB, full_screen=True, style=self.current_style) # event linkages self.application.nextTab = self.nextTab # initial layout self.tabbar.append(('class:tabbar.tab', ' {0} '.format("tmp"))) self.tabbar.append(('class:tabbar', ' ')) self.hscreen[0].content = FormattedTextControl(self.tabbar, style='class:tabbar') self.hscreen[2].content = BufferControl(focusable=False) self.hscreen[4].content = BufferControl(focusable=True) self.runUI()