diff --git a/MAVProxy/modules/mavproxy_link.py b/MAVProxy/modules/mavproxy_link.py index 026ec6a474..21e3aa17cf 100644 --- a/MAVProxy/modules/mavproxy_link.py +++ b/MAVProxy/modules/mavproxy_link.py @@ -1,13 +1,25 @@ #!/usr/bin/env python -'''enable run-time addition and removal of master link, just like --master on the cnd line''' -''' TO USE: - link add 10.11.12.13:14550 - link list - link remove 3 # to remove 3rd output + +'''enable run-time addition and removal of master link, just like --master on the cnd line + +AP_FLAKE8_CLEAN ''' +# TO USE: +# link add 10.11.12.13:14550 +# link list +# link remove 3 # to remove 3rd output + from pymavlink import mavutil -import time, struct, math, sys, fnmatch, traceback, json, os + +import fnmatch +import json +import math +import os +import struct +import sys +import time +import traceback if sys.version_info[0] >= 3: import io as StringIO @@ -18,15 +30,29 @@ from MAVProxy.modules.lib import mp_util if mp_util.has_wxpython: - from MAVProxy.modules.lib.mp_menu import * + from MAVProxy.modules.lib.mp_menu import MPMenuSubMenu + from MAVProxy.modules.lib.mp_menu import MPMenuItem from MAVProxy.modules.lib.wx_addlink import MPMenulinkAddDialog -dataPackets = frozenset(['BAD_DATA','LOG_DATA']) -delayedPackets = frozenset([ 'MISSION_CURRENT', 'SYS_STATUS', 'VFR_HUD', - 'GPS_RAW_INT', 'SCALED_PRESSURE', 'GLOBAL_POSITION_INT', - 'NAV_CONTROLLER_OUTPUT' ]) -activityPackets = frozenset([ 'HEARTBEAT', 'GPS_RAW_INT', 'GPS_RAW', 'GLOBAL_POSITION_INT', 'SYS_STATUS', 'HIGH_LATENCY2' ]) -radioStatusPackets = frozenset([ 'RADIO', 'RADIO_STATUS']) +dataPackets = frozenset(['BAD_DATA', 'LOG_DATA']) +delayedPackets = frozenset([ + 'GLOBAL_POSITION_INT', + 'GPS_RAW_INT', + 'MISSION_CURRENT', + 'NAV_CONTROLLER_OUTPUT', + 'SCALED_PRESSURE', + 'SYS_STATUS', + 'VFR_HUD', +]) +activityPackets = frozenset([ + 'GLOBAL_POSITION_INT', + 'GPS_RAW', + 'GPS_RAW_INT', + 'HEARTBEAT', + 'HIGH_LATENCY2', + 'SYS_STATUS', +]) +radioStatusPackets = frozenset(['RADIO', 'RADIO_STATUS']) preferred_ports = [ '*FTDI*', @@ -45,6 +71,7 @@ '*Qiotek*', ] + class LinkModule(mp_module.MPModule): def __init__(self, mpstate): @@ -86,16 +113,18 @@ def __init__(self, mpstate): def idle_task(self): '''called on idle''' - if mp_util.has_wxpython: + if mp_util.has_wxpython: if self.module('console') is not None: if not self.menu_added_console: self.menu_added_console = True # we don't dynamically update these yet due to a wx bug - self.menu_rm.items = [ MPMenuItem(p, p, '# link remove %s' % p) for p in self.complete_links('') ] + self.menu_rm.items = [ + MPMenuItem(p, p, '# link remove %s' % p) for p in self.complete_links('') + ] self.module('console').add_menu(self.menu) else: self.menu_added_console = False - + for m in self.mpstate.mav_master: m.source_system = self.settings.source_system m.mav.srcSystem = m.source_system @@ -112,8 +141,8 @@ def idle_task(self): with open(self.datarate_logging, 'a') as logfile: for master in self.mpstate.mav_master: highest_msec_key = (self.target_system, self.target_component) - linkdelay = (self.status.highest_msec.get(highest_msec_key, 0) - master.highest_msec.get(highest_msec_key, 0))*1.0e-3 - logfile.write(str(time.strftime("%H:%M:%S")) + "," + + linkdelay = (self.status.highest_msec.get(highest_msec_key, 0) - master.highest_msec.get(highest_msec_key, 0))*1.0e-3 # noqa + logfile.write(str(time.strftime("%H:%M:%S")) + "," + str(self.link_label(master)) + "," + str(master.linknum) + "," + str(self.status.counters['MasterIn'][master.linknum]) + "," + @@ -124,20 +153,20 @@ def idle_task(self): def complete_serial_ports(self, text): '''return list of serial ports''' ports = mavutil.auto_detect_serial(preferred_list=preferred_ports) - return [ p.device for p in ports ] + return [p.device for p in ports] def complete_hl(self, text): '''return list of hl options''' - return [ 'on', 'off' ] + return ['on', 'off'] def complete_dl(self, text): '''return list of datarate_logging options''' - return [ 'on', 'off' ] + return ['on', 'off'] def complete_links(self, text): '''return list of links''' try: - ret = [ m.address for m in self.mpstate.mav_master ] + ret = [m.address for m in self.mpstate.mav_master] for m in self.mpstate.mav_master: ret.append(m.address) if hasattr(m, 'label'): @@ -192,7 +221,7 @@ def cmd_dl(self, args): # Open a new file handle (don't append) for logging with open(self.datarate_logging, 'w') as logfile: logfile.write("time, linkname, linkid, packetsreceived, bytesreceived, delaysec, lostpercent\n") - elif args[0] == "off": + elif args[0] == "off": print("Datarate Logging OFF") self.datarate_logging = None else: @@ -203,7 +232,7 @@ def cmd_hl(self, args): if len(args) < 1: print("High latency mode is " + str(self.high_latency)) return - elif args[0] == "on": + elif args[0] == "on": print("High latency mode ON") self.high_latency = True # Tell ArduPilot to start sending HIGH_LATENCY2 messages @@ -220,7 +249,7 @@ def cmd_hl(self, args): 0, # param6 0) # param7 return - elif args[0] == "off": + elif args[0] == "off": print("High latency mode OFF") self.high_latency = False self.master.mav.command_long_send( @@ -238,12 +267,12 @@ def cmd_hl(self, args): return else: print("usage: hl ") - + def show_link(self): '''show link information''' for master in self.mpstate.mav_master: highest_msec_key = (self.target_system, self.target_component) - linkdelay = (self.status.highest_msec.get(highest_msec_key, 0) - master.highest_msec.get(highest_msec_key, 0))*1.0e-3 + linkdelay = (self.status.highest_msec.get(highest_msec_key, 0) - master.highest_msec.get(highest_msec_key, 0))*1.0e-3 # noqa if master.linkerror: status = "DOWN" else: @@ -256,20 +285,21 @@ def show_link(self): # don't have a signing secret sign_string = ", (no-signing-secret)" else: - sign_string = ", unsigned %u reject %u" % (master.mav.signing.unsigned_count, master.mav.signing.reject_count) - except AttributeError as e: + sign_string = ", unsigned %u reject %u" % (master.mav.signing.unsigned_count, master.mav.signing.reject_count) # noqa + except AttributeError: # some mav objects may not have a "signing" attribute pass - print("link %s %s (%u packets, %u bytes, %.2fs delay, %u lost, %.1f%% loss, rate:%uB/s%s)" % (self.link_label(master), - status, - self.status.counters['MasterIn'][master.linknum], - self.status.bytecounters['MasterIn'][master.linknum].total(), - linkdelay, - master.mav_loss, - master.packet_loss(), - self.status.bytecounters['MasterIn'][master.linknum].rate(), - sign_string)) - + print("link %s %s (%u packets, %u bytes, %.2fs delay, %u lost, %.1f%% loss, rate:%uB/s%s)" % ( + self.link_label(master), + status, + self.status.counters['MasterIn'][master.linknum], + self.status.bytecounters['MasterIn'][master.linknum].total(), + linkdelay, + master.mav_loss, + master.packet_loss(), + self.status.bytecounters['MasterIn'][master.linknum].rate(), + sign_string, + )) def reset_link_stats(self): '''reset link statistics''' @@ -287,7 +317,7 @@ def cmd_alllinks(self, args): self.cmd_vehicle([str(v)]) self.mpstate.functions.process_stdin(' '.join(args), True) self.cmd_vehicle([str(saved_target)]) - + def cmd_link_list(self): '''list links''' print("%u links" % len(self.mpstate.mav_master)) @@ -344,7 +374,7 @@ def link_add(self, descriptor, force_connected=False, retries=3): baud=self.settings.baudrate, force_connected=force_connected, retries=retries) - except Exception as e: + except Exception: # try the same thing but without force-connected for # backwards-compatability conn = mavutil.mavlink_connection(device, autoreconnect=True, @@ -411,8 +441,8 @@ def find_link(self, device): for i in range(len(self.mpstate.mav_master)): conn = self.mpstate.mav_master[i] if (str(i) == device or - conn.address == device or - getattr(conn, 'label', None) == device): + conn.address == device or + getattr(conn, 'label', None) == device): return i return None @@ -497,7 +527,7 @@ def handle_msec_timestamp(self, m, master): return sysid = m.get_srcSystem() compid = m.get_srcComponent() - highest_msec_key = (sysid,compid) + highest_msec_key = (sysid, compid) highest = master.highest_msec.get(highest_msec_key, 0) if msec + 30000 < highest: self.say('Time has wrapped') @@ -512,7 +542,7 @@ def handle_msec_timestamp(self, m, master): master.highest_msec[highest_msec_key] = msec if msec > self.status.highest_msec.get(highest_msec_key, 0): self.status.highest_msec[highest_msec_key] = msec - if msec < self.status.highest_msec.get(highest_msec_key, 0) and len(self.mpstate.mav_master) > 1 and self.mpstate.settings.checkdelay: + if msec < self.status.highest_msec.get(highest_msec_key, 0) and len(self.mpstate.mav_master) > 1 and self.mpstate.settings.checkdelay: # noqa master.link_delayed = True else: master.link_delayed = False @@ -548,13 +578,12 @@ def report_altitude(self, altitude): self.status.altitude = altitude altitude_converted = self.height_convert_units(altitude) if (int(self.mpstate.settings.altreadout) > 0 and - math.fabs(altitude_converted - self.last_altitude_announce) >= - int(self.settings.altreadout)): + math.fabs(altitude_converted - self.last_altitude_announce) >= + int(self.settings.altreadout)): self.last_altitude_announce = altitude_converted - rounded_alt = int(self.settings.altreadout) * ((self.settings.altreadout/2 + int(altitude_converted)) / int(self.settings.altreadout)) + rounded_alt = int(self.settings.altreadout) * ((self.settings.altreadout/2 + int(altitude_converted)) / int(self.settings.altreadout)) # noqa self.say("height %u" % rounded_alt, priority='notification') - def emit_accumulated_statustext(self, key, id, pending): out = pending.accumulated_statustext() if out != self.status.last_apm_msg or time.time() > self.status.last_apm_msg_time+2: @@ -622,7 +651,7 @@ def master_msg_handling(self, m, master): if m.get_type() == "BAD_DATA": if self.mpstate.settings.shownoise and mavutil.all_printable(m.data): out = m.data - if type(m.data) == bytearray: + if isinstance(m.data, bytearray): out = m.data.decode('ascii') self.mpstate.console.write(out, bg='red') return @@ -635,14 +664,14 @@ def master_msg_handling(self, m, master): # keep the pymavlink level target component aligned with the MAVProxy setting print("change target_component %u" % self.settings.target_component) master.target_component = self.settings.target_component - + mtype = m.get_type() if self.heartbeat_is_from_autopilot(m): if self.settings.target_system == 0 and self.settings.target_system != m.get_srcSystem(): self.settings.target_system = m.get_srcSystem() self.settings.target_component = m.get_srcComponent() - self.say("online system %u" % self.settings.target_system,'message') + self.say("online system %u" % self.settings.target_system, 'message') for mav in self.mpstate.mav_master: mav.target_system = self.settings.target_system mav.target_component = self.settings.target_component @@ -670,14 +699,15 @@ def master_msg_handling(self, m, master): self.set_prompt(self.status.flightmode + "> ") if master.flightmode != self.status.last_mode_announced and time.time() > self.status.last_mode_announce + 2: - self.status.last_mode_announce = time.time() - self.status.last_mode_announced = master.flightmode - self.say("Mode " + self.status.flightmode) - - if m.type in [mavutil.mavlink.MAV_TYPE_FIXED_WING, - mavutil.mavlink.MAV_TYPE_VTOL_DUOROTOR, - mavutil.mavlink.MAV_TYPE_VTOL_QUADROTOR, - mavutil.mavlink.MAV_TYPE_VTOL_TILTROTOR]: + self.status.last_mode_announce = time.time() + self.status.last_mode_announced = master.flightmode + self.say("Mode " + self.status.flightmode) + + if m.type in [ + mavutil.mavlink.MAV_TYPE_FIXED_WING, + mavutil.mavlink.MAV_TYPE_VTOL_DUOROTOR, + mavutil.mavlink.MAV_TYPE_VTOL_QUADROTOR, + mavutil.mavlink.MAV_TYPE_VTOL_TILTROTOR]: self.mpstate.vehicle_type = 'plane' self.mpstate.vehicle_name = 'ArduPlane' elif m.type in [mavutil.mavlink.MAV_TYPE_GROUND_ROVER, @@ -730,7 +760,7 @@ def add_chunk(self, m): # m is a statustext message self.chunks[chunk_seq] = m.text if len(m.text) != 50 or mid == 0: - self.expected_count = chunk_seq + 1; + self.expected_count = chunk_seq + 1 def complete(self): return (self.expected_count is not None and @@ -857,10 +887,10 @@ def accumulated_statustext(self): res = mavutil.mavlink.enums["MAV_MISSION_RESULT"][m.type].name res = res[12:] self.mpstate.console.writeln("Got MISSION_ACK: %s: %s" % (t, res)) - except Exception as e: + except Exception: self.mpstate.console.writeln("Got MAVLink msg: %s" % m) else: - #self.mpstate.console.writeln("Got MAVLink msg: %s" % m) + # self.mpstate.console.writeln("Got MAVLink msg: %s" % m) pass self.check_watched_message(m, '<') @@ -962,7 +992,7 @@ def master_callback(self, m, master): target_sysid = self.target_system # pass to modules - for (mod,pm) in self.mpstate.modules: + for (mod, pm) in self.mpstate.modules: if not hasattr(mod, 'mavlink_packet'): continue # Do not send other-system-or-component heartbeat packets to non-multi-vehicle modules @@ -970,9 +1000,9 @@ def master_callback(self, m, master): continue # sysid 51/'3' is used by SiK radio for the injected RADIO/RADIO_STATUS mavlink frames. # In order to be able to pass these to e.g. the graph module, which is not multi-vehicle, - # special handling is needed, so that the module gets both RADIO_STATUS and (single) target + # special handling is needed, so that the module gets both RADIO_STATUS and (single) target # vehicle information. - if not(sysid == 51 and mtype in radioStatusPackets): + if not (sysid == 51 and mtype in radioStatusPackets): if not mod.multi_vehicle and sysid != target_sysid: # only pass packets not from our target to modules that # have marked themselves as being multi-vehicle capable @@ -1013,14 +1043,15 @@ def cmd_vehicle(self, args): m.target_component = self.mpstate.settings.target_component if 'HEARTBEAT' in m.messages: stamp = m.messages['HEARTBEAT']._timestamp - src_system = m.messages['HEARTBEAT'].get_srcSystem() + # src_system = m.messages['HEARTBEAT'].get_srcSystem() if stamp > best_timestamp: best_link = i best_timestamp = stamp - m.link_delayed = False + m.link_delayed = False self.mpstate.settings.link = best_link + 1 print("Set vehicle %s (link %u)" % (args[0], best_link+1)) + def init(mpstate): '''initialise module''' return LinkModule(mpstate)