def main(): args = parse_args() level = logging.INFO if args.verbose: level = logging.DEBUG logging.basicConfig(level=level) if os.getuid() != 0: logging.error("must run as root.") sys.exit(1) uavcan.load_dsdl(args.dsdl) tun_fd = open_tun_interface(args.ip_address) node = uavcan.make_node(args.interface, node_id=42) tap_to_can = Queue() can_to_tap = Queue() logging.info("waiting for packets, press 3x Ctrl-C to stop...") rx_thd = threading.Thread(target=rx_thread, args=(tun_fd, tap_to_can, args.packets_per_second)) tx_thd = threading.Thread(target=tx_thread, args=(tun_fd, can_to_tap)) node_thd = threading.Thread(target=node_thread, args=(tun_fd, node, can_to_tap, tap_to_can)) rx_thd.start() tx_thd.start() node_thd.start() node_thd.join() rx_thd.join() tx_thd.join()
def __init__(self) -> None: super().__init__() self.last_message = "" self.speed = 0 self.node = node = uavcan.make_node( '/dev/serial/by-id/usb-Zubax_Robotics_Zubax_Babel_32002E0018514D563935392000000000-if00', node_id=10, bitrate=1000000, ) # setup node_monitor = uavcan.app.node_monitor.NodeMonitor(node) dynamic_node_id_allocator = uavcan.app.dynamic_node_id.CentralizedServer( node, node_monitor) # Waiting for at least one other node to appear online (our local node is already online). while len(dynamic_node_id_allocator.get_allocation_table()) <= 1: print('Waiting for other nodes to become online...') node.spin(timeout=1) node.periodic(0.05, self.update) node.add_handler(uavcan.equipment.esc.Status, self.listen) self.thread = threading.Thread(target=node.spin, daemon=True).start()
def __init__(self, can_iface_name: str, bitrate: int, can_dump_directory: str, file_server_lookup_paths: list, self_node_id: int): os.makedirs(can_dump_directory, exist_ok=True) can_dump_base_name = 'can_%s.dump' % time.strftime('%Y-%m-%d-%H-%M-%S') can_dump_file = os.path.join(can_dump_directory, can_dump_base_name) self._can_dump_file = open(can_dump_file, 'w', encoding='utf8') self._node = uavcan.make_node(can_iface_name, bitrate=bitrate, mode=NODE_STATUS_CONST.MODE_OPERATIONAL) self._node.node_info.name = 'com.zubax.bootloader_tester' self._node.node_id = self_node_id self._node.can_driver.add_io_hook(self._can_io_hook) self.file_server = uavcan.app.file_server.FileServer( self._node, file_server_lookup_paths) self.monitor = uavcan.app.node_monitor.NodeMonitor(self._node) self.node_id_allocator = uavcan.app.dynamic_node_id.CentralizedServer( self._node, self.monitor) def logging_callback(event): logger.info('uavcan.protocol.debug.LogMessage\n%s', uavcan.to_yaml(event)) self._node.add_handler(uavcan.protocol.debug.LogMessage, logging_callback)
def __init__(self, can_interface, node_id): uavcan.load_dsdl(DSDL_PATH) node_info = ( uavcan.protocol.GetNodeInfo.Response() ) node_info.name = "com.wibotic.ros_connector" node_info.software_version.major = 1 try: self.uavcan_node = uavcan.make_node( can_interface, node_id=node_id, node_info=node_info, mode=uavcan.protocol.NodeStatus().MODE_OPERATIONAL, ) except OSError: rospy.logerr( "ERROR: Device not found. " "Please confirm the device name is correctly set!" ) else: self.monitor = uavcan.app.node_monitor.NodeMonitor( self.uavcan_node ) self.uavcan_node.add_handler( uavcan.thirdparty.wibotic.WiBoticInfo, self.wibotic_info_callback, )
def main(): args = parse_args() if os.getuid() != 0: print("must run as root.") sys.exit(1) uavcan.load_dsdl(args.dsdl) tun_fd = open_tun_interface(args.ip_address) node = uavcan.make_node(args.interface, node_id=42) tap_to_can = Queue() can_to_tap = Queue() print("waiting for packets, press 3x Ctrl-C to stop...") rx_thd = threading.Thread(target=rx_thread, args=(tun_fd, tap_to_can)) tx_thd = threading.Thread(target=tx_thread, args=(tun_fd, can_to_tap)) node_thd = threading.Thread( target=node_thread, args=(tun_fd, node, can_to_tap, tap_to_can) ) rx_thd.start() tx_thd.start() node_thd.start() node_thd.join() rx_thd.join() tx_thd.join()
def main(): args = parse_args() with open(args.config) as file: config = yaml.load(file) config = config['actuator'][args.board] config = config_fill_missing(config) # start UAVCAN node uavcan.load_dsdl(args.dsdl) node = uavcan.make_node(args.port, node_id=127) # set up board discovery disc = NodeDiscovery(node, args.board) while disc.get_id() is None: node.spin(0.1) print('board "{}" has ID {}'.format(args.board, disc.get_id())) print('Load config...') config_send(node, config, disc.get_id()) node.spin(0.1)
def main(): args = parse_args() if args.output: output_file = csv.DictWriter(args.output, ['ts', 'X', 'Y', 'Z']) output_file.writeheader() def orientation_cb(event): msg = event.message print(msg.timestamp.usec) x, y, z, w = tuple(msg.orientation_xyzw) x, y, z = quaternion_to_euler_angle(w, x, y, z) if args.output: output_file.writerow({ 'ts': msg.timestamp.usec, 'X': x, 'Y': y, 'Z': z }) else: x = math.degrees(x) y = math.degrees(y) z = math.degrees(z) print("{:.1f}° {:.1f}° {:.1f}°".format(x, y, z)) node = uavcan.make_node(args.port, node_id=127) node.add_handler(uavcan.equipment.ahrs.Solution, orientation_cb) node.spin()
def __init__(self, interface): self.node_lock = threading.RLock() self.rpc_answered = threading.Semaphore(value=0) self.node = uavcan.make_node(interface, node_id=127) threading.Thread(target=self._uavcan_thread).start() self.node.add_handler(uavcan.protocol.NodeStatus, self._node_status_callback)
def __init__(self, can_iface_name: str, bitrate: int, can_dump_directory: str, file_server_lookup_paths: list, self_node_id: int): os.makedirs(can_dump_directory, exist_ok=True) can_dump_base_name = 'can_%s.dump' % time.strftime('%Y-%m-%d-%H-%M-%S') can_dump_file = os.path.join(can_dump_directory, can_dump_base_name) self._can_dump_file = open(can_dump_file, 'w', encoding='utf8') self._node = uavcan.make_node(can_iface_name, bitrate=bitrate, mode=NODE_STATUS_CONST.MODE_OPERATIONAL) self._node.node_info.name = 'com.zubax.bootloader_tester' self._node.node_id = self_node_id self._node.can_driver.add_io_hook(self._can_io_hook) self.file_server = uavcan.app.file_server.FileServer(self._node, file_server_lookup_paths) self.monitor = uavcan.app.node_monitor.NodeMonitor(self._node) self.node_id_allocator = uavcan.app.dynamic_node_id.CentralizedServer(self._node, self.monitor) def logging_callback(event): logger.info('uavcan.protocol.debug.LogMessage\n%s', uavcan.to_yaml(event)) self._node.add_handler(uavcan.protocol.debug.LogMessage, logging_callback)
def main(): args = parse_args() if args.output: output_file = csv.DictWriter(args.output, ['ts', 'anchor_addr', 'range']) output_file.writeheader() def range_cb(event): msg = event.message if args.anchor and msg.anchor_addr != args.anchor: return print("Received a range from {}: {:.3f}".format( msg.anchor_addr, msg.range)) if args.output: output_file.writerow({ 'ts': msg.timestamp.usec, 'range': msg.range, 'anchor_addr': msg.anchor_addr }) args.output.flush() node = uavcan.make_node(args.port, node_id=127) # TODO This path is a bit too hardcoded dsdl_path = os.path.join(os.path.dirname(__file__), '..', '..', 'uavcan_data_types', 'cvra', 'uwb_beacon') uavcan.load_dsdl(dsdl_path) node.add_handler(uavcan.thirdparty.uwb_beacon.RadioRange, range_cb) node.spin()
def createNode(com): node = uavcan.make_node(com, node_id=126, bitrate=1000000, baudrate=1000000) node_monitor = uavcan.app.node_monitor.NodeMonitor(node) # dynamic_node_id_allocator = uavcan.app.dynamic_node_id.CentralizedServer(node, node_monitor) # while len(dynamic_node_id_allocator.get_allocation_table()) <= 1: # print('Waiting for other nodes to become online...') # node.spin(timeout=1) while len(node_monitor.get_all_node_id()) < 1: print('Waiting for other nodes to become online...') node.spin(timeout=1) all_node_ids = list(node_monitor.get_all_node_id()) print('\nDetected Node IDs', all_node_ids) print( 'Node ID in use', all_node_ids[0], ) node_dict = node_monitor.get( all_node_ids[0] ) #momentarily, always use the first, one shouldk use an ESC detection scheme as in the examples return node
def main(): args = parse_args() node = uavcan.make_node(args.port) uavcan.load_dsdl(DSDL_DIR) handle = node.add_handler(uavcan.thirdparty.cvra.sensor.ColorRGBC, color_cb) node.spin()
def createNodeDynamicId(com): node = uavcan.make_node(com, node_id=126, bitrate=1000000, baudrate=1000000) node_monitor = uavcan.app.node_monitor.NodeMonitor(node) dynamic_node_id_allocator = uavcan.app.dynamic_node_id.CentralizedServer(node, node_monitor) waitForAllNodesDynamicId(dynamic_node_id_allocator,node) return node_monitor, node, dynamic_node_id_allocator
def main(config_filename, *args): node_ids = set() # we keep a cache of them so we can request node info config = read_config(config_filename) influxdb_client = influxdb.InfluxDBClient( config.get('influxdb', 'host'), config.getint('influxdb', 'port'), config.get('influxdb', 'username'), config.get('influxdb', 'password'), config.get('influxdb', 'database'), ) influxdb_client.create_database(config.get('influxdb', 'database')) node_info = uavcan.protocol.GetNodeInfo.Response() node_info.name = config.get('node', 'name') node = uavcan.make_node( config.get('canbus', 'ifname'), node_id=config.getint('node', 'id'), node_info=node_info, ) uavcan_types = [ uavcan.protocol.NodeStatus, uavcan.thirdparty.homeautomation.EventCount, uavcan.thirdparty.homeautomation.Environment, uavcan.thirdparty.homeautomation.ConductionSensor, uavcan.thirdparty.homeautomation.PumpStatus, ] for uavcan_type in uavcan_types: node.add_handler( uavcan_type, record_event_data, influxdb_client=influxdb_client, ) node.add_handler(uavcan.protocol.NodeStatus, lambda event: node_ids.add(event.transfer.source_node_id)) node.add_handler(uavcan.protocol.RestartNode, restart_request) node.mode = uavcan.protocol.NodeStatus().MODE_OPERATIONAL node.health = uavcan.protocol.NodeStatus().HEALTH_OK print('started') while True: try: node.spin(1) get_node_infos(node, node_ids, influxdb_client) receive_node_info(config.getint('node', 'id'), node_info, influxdb_client) except uavcan.UAVCANException as ex: print('Node error:', ex)
def main(): logger.info('Starting the application') app = QApplication(sys.argv) while True: # Asking the user to specify which interface to work with try: iface, iface_kwargs = run_iface_config_window(get_app_icon()) if not iface: sys.exit(0) except Exception as ex: show_error('Fatal error', 'Could not list available interfaces', ex, blocking=True) sys.exit(1) # Trying to start the node on the specified interface try: node_info = uavcan.protocol.GetNodeInfo.Response() node_info.name = NODE_NAME node_info.software_version.major = __version__[0] node_info.software_version.minor = __version__[1] node = uavcan.make_node( iface, node_info=node_info, mode=uavcan.protocol.NodeStatus().MODE_OPERATIONAL, **iface_kwargs) # Making sure the interface is alright node.spin(0.1) except Exception as ex: logger.error('UAVCAN node init failed', exc_info=True) show_error('Fatal error', 'Could not initialize UAVCAN node', ex, blocking=True) else: break logger.info('Creating main window; iface %r', iface) window = MainWindow(node, iface) window.show() try: update_checker.begin_async_check(window) except Exception: logger.error('Could not start update checker', exc_info=True) logger.info('Init complete, invoking the Qt event loop') exit_code = app.exec_() node.close() sys.exit(exit_code)
def main(): args = parse_args() node = uavcan.make_node(args.port) uavcan.load_dsdl(DSDL_DIR) handle = node.add_handler(uavcan.thirdparty.cvra.io.DigitalInput, digital_input_cb) node.spin()
def main(): args = parse_args() node = uavcan.make_node(args.port) uavcan.load_dsdl(DSDL_DIR) handle = node.add_handler(uavcan.thirdparty.cvra.sensor.DistanceVL6180X, distance_cb) node.spin()
def main(): args = parse_args() node = uavcan.make_node(args.port, node_id=42) uavcan.load_dsdl(DSDL_DIR) send_pwm(node, args.id, args.value) # Spin node for 1 second node.spin(1) node.close()
def main(): args = parse_args() node = uavcan.make_node(args.port, node_id=42) uavcan.load_dsdl(DSDL_DIR) set_servo(node, args.id, servo_setpoint(args.servo, args.pos, args.vel, args.acc)) # Spin node for 1 second node.spin(1) node.close()
def main(): logger.info("Starting the application") app = QApplication(sys.argv) while True: # Asking the user to specify which interface to work with try: iface, iface_kwargs = run_iface_config_window(get_app_icon()) if not iface: sys.exit(0) except Exception as ex: show_error("Fatal error", "Could not list available interfaces", ex, blocking=True) sys.exit(1) # Trying to start the node on the specified interface try: node_info = uavcan.protocol.GetNodeInfo.Response() node_info.name = NODE_NAME node_info.software_version.major = __version__[0] node_info.software_version.minor = __version__[1] node = uavcan.make_node( iface, node_info=node_info, mode=uavcan.protocol.NodeStatus().MODE_OPERATIONAL, **iface_kwargs ) # Making sure the interface is alright node.spin(0.1) except uavcan.transport.TransferError: # allow unrecognized messages on startup: logger.warn("UAVCAN Transfer Error occured on startup", exc_info=True) break except Exception as ex: logger.error("UAVCAN node init failed", exc_info=True) show_error("Fatal error", "Could not initialize UAVCAN node", ex, blocking=True) else: break logger.info("Creating main window; iface %r", iface) window = MainWindow(node, iface) window.show() try: update_checker.begin_async_check(window) except Exception: logger.error("Could not start update checker", exc_info=True) logger.info("Init complete, invoking the Qt event loop") exit_code = app.exec_() node.close() sys.exit(exit_code)
def createNode(com): node = uavcan.make_node(com, node_id=126, bitrate=1000000, baudrate=1000000) node_monitor = uavcan.app.node_monitor.NodeMonitor(node) # dynamic_node_id_allocator = uavcan.app.dynamic_node_id.CentralizedServer(node, node_monitor) # while len(dynamic_node_id_allocator.get_allocation_table()) <= 1: # print('Waiting for other nodes to become online...') # node.spin(timeout=1) waitForAllNodes(node_monitor,node) return node_monitor, node
def main(): args = parse_args() uavcan.load_dsdl(args.dsdl) node = uavcan.make_node(args.interface, node_id=42) def do_publish(): msg = uavcan.thirdparty.cvra.uwb_beacon.DataPacket() msg.dst_addr = 42 node.broadcast(msg, priority=uavcan.TRANSFER_PRIORITY_LOWEST) handle = node.periodic(1, do_publish) node.spin()
def main(): args = parse_args() node = uavcan.make_node(args.port) uavcan.load_dsdl(DSDL_DIR) handle = node.add_handler(uavcan.thirdparty.cvra.proximity_beacon.Signal, beacon_cb) print("Listening for messages...") # Spin node for 1 second try: node.spin() except KeyboardInterrupt: node.close()
def main(args): if args.dsdl is not None: uavcan.load_dsdl(args.dsdl) node_info = uavcan.protocol.GetNodeInfo.Response(name=args.name) node = uavcan.make_node(args.interface, node_id=args.id, node_info=node_info) def publish(msg): node.broadcast(msg, priority=uavcan.TRANSFER_PRIORITY_LOWEST) def publish_current(): publish( uavcan.thirdparty.cvra.motor.feedback.CurrentPID( current_setpoint=step(scale=1, divider=0.5), current=random.uniform(0, 1), ) ) def publish_velocity(): publish( uavcan.thirdparty.cvra.motor.feedback.VelocityPID( velocity_setpoint=step(scale=1, divider=1), velocity=random.uniform(0, 1), ) ) def publish_position(): publish( uavcan.thirdparty.cvra.motor.feedback.PositionPID( position_setpoint=step(scale=1, divider=2), position=random.uniform(0, 1), ) ) if args.dsdl is not None: handle_current = node.periodic(0.01, publish_current) handle_velocity = node.periodic(0.03, publish_velocity) handle_position = node.periodic(0.1, publish_position) while True: try: node.spin(1) except uavcan.UAVCANException as ex: print("Node error:", ex)
def node_process(iface, iface_kwargs, on_transfer_callback): logging.info('Starting the spinner process') # Initializing the node node = uavcan.make_node(iface, **iface_kwargs) # Passive mode # Patching the node object to receive all messages from the bus # TODO: Add required logic to the library def call_handlers_replacement(transfer): if not transfer.service_not_message: on_transfer_callback(transfer) node._handler_dispatcher.call_handlers = call_handlers_replacement # Spinning the node while True: try: node.spin() except Exception: logging.error('Node spin exception', exc_info=True)
def main(): app = QApplication(sys.argv) # noinspection PyBroadException try: app_icon = QIcon(os.path.join(os.path.dirname(__file__), 'icon.png')) except Exception: logger.error('Could not load icon', exc_info=True) app_icon = QIcon() while True: # Asking the user to specify which interface to work with try: iface, iface_kwargs = run_iface_config_window(app_icon) if not iface: exit(0) except Exception as ex: show_error('Fatal error', 'Could not list available interfaces', ex) exit(1) # Trying to start the node on the specified interface try: node_info = uavcan.protocol.GetNodeInfo.Response() node_info.name = NODE_NAME node_info.software_version.major = 1 # TODO: share with setup.py node_info.software_version.minor = 0 node = uavcan.make_node( iface, node_info=node_info, mode=uavcan.protocol.NodeStatus().MODE_OPERATIONAL, **iface_kwargs) except Exception as ex: show_error('Fatal error', 'Could not initialize UAVCAN node', ex) else: break window = MainWindow(app_icon, node, iface) window.show() exit(app.exec_())
def main(): args = parse_args() node = uavcan.make_node(args.port, node_id=123) node.lock = threading.RLock() # TODO This path is a bit too hardcoded dsdl_path = os.path.join(os.path.dirname(__file__), '..', 'beacon_messages') uavcan.load_dsdl(dsdl_path) samples = [] def range_cb(event): samples.append(event.message.range) print(event.message.range) if len(samples) >= 100: bias = mean(samples) - args.distance print("Estimated bias: {:.3f}".format(bias)) print("Delta to add in decawave units: {}".format(int((bias / 2) / SPEED_OF_LIGHT))) node.add_handler(uavcan.thirdparty.beacon_messages.equipment.RadioRange, range_cb) node.spin()
def main(): args = parse_args() node = uavcan.make_node(args.port, node_id=123) node.lock = threading.RLock() uavcan.load_dsdl(args.dsdl) samples = [] def range_cb(event): samples.append(event.message.range) print(event.message.range) if len(samples) >= 100: bias = mean(samples) - args.distance print("Estimated bias: {:.3f}".format(bias)) print("Delta to add in decawave units: {}".format( int((bias / 2) / SPEED_OF_LIGHT))) node.add_handler(uavcan.thirdparty.cvra.uwb_beacon.RadioRange, range_cb) node.spin()
def main(): app = QApplication(sys.argv) # noinspection PyBroadException try: app_icon = QIcon(os.path.join(os.path.dirname(__file__), 'icon.png')) except Exception: logger.error('Could not load icon', exc_info=True) app_icon = QIcon() while True: # Asking the user to specify which interface to work with try: iface, iface_kwargs = run_iface_config_window(app_icon) if not iface: exit(0) except Exception as ex: show_error('Fatal error', 'Could not list available interfaces', ex) exit(1) # Trying to start the node on the specified interface try: node_info = uavcan.protocol.GetNodeInfo.Response() node_info.name = NODE_NAME node_info.software_version.major = 1 # TODO: share with setup.py node_info.software_version.minor = 0 node = uavcan.make_node(iface, node_info=node_info, mode=uavcan.protocol.NodeStatus().MODE_OPERATIONAL, **iface_kwargs) except Exception as ex: show_error('Fatal error', 'Could not initialize UAVCAN node', ex) else: break window = MainWindow(app_icon, node, iface) window.show() exit(app.exec_())
def run_uavcan(node_infos, request_queue): if getattr(uavcan.thirdparty, 'homeautomation', None) is None: uavcan.load_dsdl( os.path.join(os.path.dirname(__file__), 'dsdl_files', 'homeautomation')) node_info = uavcan.protocol.GetNodeInfo.Response() node_info.name = os.environ.get('NODE__NAME', config.get('node', 'name')) node = uavcan.make_node( os.environ.get('CANBUS__IFNAME', config.get('canbus', 'ifname')), node_id=int(os.environ.get('NODE__ID', config.getint('node', 'id'))), node_info=node_info, ) node.mode = uavcan.protocol.NodeStatus().MODE_OPERATIONAL node.health = uavcan.protocol.NodeStatus().HEALTH_OK def node_status_cb(event): node_infos[event.transfer.source_node_id]['last_seen'] = datetime.now() node.add_handler(uavcan.protocol.NodeStatus, node_status_cb) while running: try: node.spin(0.2) while not request_queue.empty(): request = request_queue.get_nowait() response_queue = request['response_queue'] node.request( request['request'], request['node_id'], lambda event: response_queue.put(event), ) request_queue.task_done() except uavcan.UAVCANException as ex: print('Node error:', ex)
def reconnect(self): while True: zubax = set(find_serial('zubax').keys()) if len(zubax) != 1: logger.error(f"Zubax controller not determined, {zubax}") sleep(1) continue serial_device = next(iter(zubax), None) print(serial_device) logger.info(f"Zubax controller determined, {zubax}") try: self.node: Optional[uavcan.node.Node] = uavcan.make_node( serial_device, node_id=10, bitrate=1000000, ) # setup node_monitor = uavcan.app.node_monitor.NodeMonitor(self.node) dynamic_node_id_allocator = uavcan.app.dynamic_node_id.CentralizedServer( self.node, node_monitor) # Waiting for at least one other node to appear online (our local node is already online). while len( dynamic_node_id_allocator.get_allocation_table()) <= 1: logger.error('Waiting for other nodes to become online...') self.node.spin(timeout=1) # how fast can we blast this? self.node.periodic(0.05, self.update) self.node.add_handler(uavcan.equipment.esc.Status, self.listen) logger.info('New zubax node: %s', serial_device) return except Exception as e: logger.error("Opening zubax failed %s", e) sleep(1) continue
def run(self): self.node = uavcan.make_node(self.port, node_id=127) self.node.add_handler(uavcan.protocol.NodeStatus, self._node_status_callback) self.node.add_handler(uavcan.thirdparty.cvra.motor.feedback.CurrentPID, self._current_pid_callback) self.node.add_handler( uavcan.thirdparty.cvra.motor.feedback.VelocityPID, self._velocity_pid_callback) self.node.add_handler( uavcan.thirdparty.cvra.motor.feedback.PositionPID, self._position_pid_callback) self.node.periodic(0.1, self._publish_setpoint) while True: with self.lock: try: self.node.spin(0.2) except uavcan.transport.TransferError: pass
def main(): # Init ROS node rospy.init_node(canros.ros_node_name) # Get can_interface parameter try: can_interface = rospy.get_param('~can_interface') except KeyError: print("'can_interface' ROS parameter must be set") return # Get uavcan_node_id parameter try: uavcan_node_id = int(rospy.get_param('~uavcan_id')) if uavcan_node_id < 0 or uavcan_node_id > 127: raise ValueError() except KeyError: print("'uavcan_id' ROS parameter must be set") return except ValueError: print("'uavcan_id' must be an integer from 0-127") return try: blacklist = list(rospy.get_param('~blacklist')) except KeyError: print("'blacklist' ROS parameter must be set") except ValueError: print("'blacklist' must be a list or strings") try: ignore_uavcan_msgs = rospy.get_param('~ignore_uavcan_msgs') except KeyError: print("'ignore_uavcan_msgs' ROS parameter must be set") # Init UAVCAN logging uavcan.driver.slcan.logger.addHandler(logging.StreamHandler()) uavcan.driver.slcan.logger.setLevel('DEBUG') # Set UAVCAN node information uavcan_node_info = uavcan.protocol.GetNodeInfo.Response() uavcan_node_info.name = canros.uavcan_name uavcan_node_info.software_version.major = 0 uavcan_node_info.software_version.minor = 1 uavcan_node_info.hardware_version.unique_id = hardware_id() # Start UAVCAN node global uavcan_node #pylint: disable=W0603 uavcan_node = uavcan.make_node(can_interface, node_id=uavcan_node_id, node_info=uavcan_node_info) # Load types for uavcan_name, typ in uavcan.TYPENAMES.iteritems(): if typ.default_dtid is None: continue if uavcan_name in blacklist: continue if ignore_uavcan_msgs and uavcan_name.startswith('uavcan'): continue _ = Message(typ) if typ.kind == typ.KIND_MESSAGE else Service(typ) # GetInfo def GetInfoHandler(_): rosmsg = canros.srv.GetNodeInfoResponse() rosmsg.node_info = canros.copy_uavcan_ros(rosmsg.node_info, uavcan_node.node_info, request=False) setattr(rosmsg.node_info.status, canros.uavcan_id_field_name, uavcan_node.node_id) return rosmsg rospy.Service(canros.get_info_topic, canros.srv.GetNodeInfo, GetInfoHandler) # Spin uavcan_errors = 0 while not rospy.is_shutdown(): try: uavcan_node.spin(0) if uavcan_errors > 0: uavcan_errors = 0 except uavcan.transport.TransferError: uavcan_errors += 1 if uavcan_errors >= 1000: print("Too many UAVCAN transport errors") break uavcan_node.close() print("canros server exited successfully")
def test_uavcan(): node_info = uavcan.protocol.GetNodeInfo.Response() node_info.name = 'com.zubax.drwatson.sapog' iface = init_can_iface() with closing(uavcan.make_node(iface, bitrate=CAN_BITRATE, node_id=127, mode=uavcan.protocol.NodeStatus().MODE_OPERATIONAL, node_info=node_info)) as n: def safe_spin(timeout): try: n.spin(timeout) except uavcan.UAVCANException: logger.error('Node spin failure', exc_info=True) @contextmanager def time_limit(timeout, error_fmt, *args): aborter = n.defer(timeout, partial(abort, error_fmt, *args)) yield aborter.remove() try: # Dynamic node ID allocation nsmon = uavcan.app.node_monitor.NodeMonitor(n) alloc = uavcan.app.dynamic_node_id.CentralizedServer(n, nsmon) info('Waiting for the node to show up on the CAN bus...') with time_limit(10, 'The node did not show up in time. Check CAN interface and crystal oscillator.'): while True: safe_spin(1) target_nodes = list(nsmon.find_all(lambda e: e.info and e.info.name.decode() == PRODUCT_NAME)) if len(target_nodes) == 1: break if len(target_nodes) > 1: abort('Expected to find exactly one target node, found more: %r', target_nodes) node_id = target_nodes[0].node_id info('Node %r initialized', node_id) for nd in target_nodes: logger.info('Discovered node %r', nd) def request(what): response_event = None def cb(e): nonlocal response_event if not e: abort('Request has timed out: %r', what) response_event = e n.request(what, node_id, cb) while response_event is None: safe_spin(0.1) return response_event.response # Starting the node and checking its self-reported diag outputs def wait_for_init(): with time_limit(10, 'The node did not complete initialization in time'): while True: safe_spin(1) if nsmon.exists(node_id) and nsmon.get(node_id).status.mode == \ uavcan.protocol.NodeStatus().MODE_OPERATIONAL: break def check_status(): status = nsmon.get(node_id).status enforce(status.mode == uavcan.protocol.NodeStatus().MODE_OPERATIONAL, 'Unexpected operating mode') enforce(status.health == uavcan.protocol.NodeStatus().HEALTH_OK, 'Bad node health') info('Waiting for the node to complete initialization...') wait_for_init() check_status() info('Resetting the configuration to factory defaults...') enforce(request(uavcan.protocol.param.ExecuteOpcode.Request( opcode=uavcan.protocol.param.ExecuteOpcode.Request().OPCODE_ERASE)).ok, 'The node refused to reset configuration to factory defaults') col_esc_status = uavcan.app.message_collector.MessageCollector(n, uavcan.equipment.esc.Status, timeout=10) def check_everything(check_rotation=False): check_status() try: m = col_esc_status[node_id].message except KeyError: abort('Rock is dead.') else: if check_rotation: enforce(m.rpm > 100 and m.power_rating_pct > 0, 'Could not start the motor') enforce(m.current > 0, 'Current is not positive') enforce(m.error_count < ESC_ERROR_LIMIT, 'High error count: %r', m.error_count) temp_degc = convert_units_from_to(m.temperature, 'Kelvin', 'Celsius') enforce(TEMPERATURE_RANGE_DEGC[0] <= temp_degc <= TEMPERATURE_RANGE_DEGC[1], 'Invalid temperature: %r degC', temp_degc) # Testing before the motor is started imperative('CAUTION: THE MOTOR WILL START IN 2 SECONDS, KEEP CLEAR') safe_spin(2) check_everything() # Starting the motor esc_raw_command_bitlen = \ uavcan.get_uavcan_data_type(uavcan.get_fields(uavcan.equipment.esc.RawCommand())['cmd'])\ .value_type.bitlen # SO EASY TO USE def do_publish(duty_cycle, check_rotation): command_value = int(duty_cycle * (2 ** (esc_raw_command_bitlen - 1))) n.broadcast(uavcan.equipment.esc.RawCommand(cmd=[command_value])) check_everything(check_rotation) info('Starting the motor') publisher = n.periodic(0.01, partial(do_publish, STARTUP_DUTY_CYCLE, False)) safe_spin(5) publisher.remove() info('Checking stability...') for dc in STABILITY_TEST_DUTY_CYCLES: info('Setting duty cycle %d%%...', int(dc * 100)) publisher = n.periodic(0.01, partial(do_publish, dc, True)) safe_spin(5) publisher.remove() info('Stopping...') latest_status = col_esc_status[node_id].message safe_spin(1) check_everything() # Final results info('Validate the latest ESC status variables (units are SI):\n%s', uavcan.to_yaml(latest_status)) # Testing CAN2 with BackgroundSpinner(safe_spin, 0.1): input('1. Disconnect CAN1 and connect to CAN2\n' '2. Terminate CAN2\n' '3. Press ENTER') safe_spin(1) try: check_status() except Exception as ex: logger.info('CAN2 test failed', exc_info=True) abort('CAN2 test failed [%r]', ex) # Testing LED info('Testing LED') def set_led(): rgb = uavcan.equipment.indication.RGB565(red=0b11111, green=0b111111, blue=0b11111) slc = uavcan.equipment.indication.SingleLightCommand(light_id=0, color=rgb) n.broadcast(uavcan.equipment.indication.LightsCommand(commands=[slc])) check_everything() publisher = n.periodic(0.1, set_led) with BackgroundSpinner(safe_spin, 0.1): if not input('Is the LED glowing bright white?', yes_no=True, default_answer=True): abort('LED is not working properly') publisher.remove() except Exception: for nid in nsmon.get_all_node_id(): logger.info('UAVCAN test failed; last known state of the device node: %r' % nsmon.get(nid)) raise
def test_uavcan(): node_info = uavcan.protocol.GetNodeInfo.Response() # @UndefinedVariable node_info.name.encode("com.zubax.drwatson.zubax_gnss") iface = init_can_iface() with closing( uavcan.make_node(iface, bitrate=CAN_BITRATE, node_id=127, mode=uavcan.protocol.NodeStatus().MODE_OPERATIONAL) ) as n: # @UndefinedVariable def safe_spin(timeout): try: n.spin(timeout) except uavcan.transport.TransferError: logger.debug( "Transfer error while spinning the node. " "Reporting at the DEBUG level because of https://github.com/UAVCAN/pyuavcan/issues/14", exc_info=True, ) except uavcan.UAVCANException: logger.error("Node spin failure", exc_info=True) @contextmanager def time_limit(timeout, error_fmt, *args): aborter = n.defer(timeout, partial(abort, error_fmt, *args)) yield aborter.remove() # Dynamic node ID allocation try: nsmon = uavcan.monitors.NodeStatusMonitor(n) alloc = uavcan.monitors.DynamicNodeIDServer(n, nsmon) # @UnusedVariable with time_limit(10, "The node did not show up in time. Check CAN interface and crystal oscillator."): while True: safe_spin(1) target_nodes = list(nsmon.find_all(lambda e: e.info and e.info.name.decode() == PRODUCT_NAME)) if len(target_nodes) == 1: break if len(target_nodes) > 1: abort("Expected to find exactly one target node, found more: %r", target_nodes) node_id = target_nodes[0].node_id info("Node %r initialized", node_id) for nd in target_nodes: logger.info("Discovered node %r", nd) def request(what, fire_and_forget=False): response_event = None def cb(e): nonlocal response_event if not e: abort("Request has timed out: %r", what) response_event = e # @UnusedVariable if fire_and_forget: n.request(what, node_id, lambda _: None) safe_spin(0.1) else: n.request(what, node_id, cb) while response_event is None: safe_spin(0.1) return response_event.response # Starting the node and checking its self-reported diag outputs def wait_for_init(): with time_limit(10, "The node did not complete initialization in time"): while True: safe_spin(1) if ( nsmon.exists(node_id) and nsmon.get(node_id).status.mode == uavcan.protocol.NodeStatus().MODE_OPERATIONAL ): # @UndefinedVariable break def check_status(): status = nsmon.get(node_id).status enforce( status.mode == uavcan.protocol.NodeStatus().MODE_OPERATIONAL, # @UndefinedVariable "Unexpected operating mode", ) enforce( status.health == uavcan.protocol.NodeStatus().HEALTH_OK, "Bad node health" # @UndefinedVariable ) info("Waiting for the node to complete initialization...") wait_for_init() check_status() info("Reconfiguring the node...") def log_all_params(): for index in range(10000): r = request(uavcan.protocol.param.GetSet.Request(index=index)) # @UndefinedVariable if not r.name: break logger.info("Param %-30r %r" % (r.name.decode(), getattr(r.value, r.value.union_field))) def set_param(name, value, union_field=None): union_field = ( union_field or {int: "integer_value", float: "real_value", bool: "boolean_value", str: "string_value"}[ type(value) ] ) logger.info("Setting parameter %r field %r value %r", name, union_field, value) req = uavcan.protocol.param.GetSet.Request() # @UndefinedVariable req.name.encode(name) setattr(req.value, union_field, value) r = request(req) enforce(r.value.union_field == union_field, "Union field mismatch in param set response for %r", name) enforce(getattr(r.value, union_field) == value, "The node refused to set parameter %r", name) set_param("uavcan.pubp-time", 10000) set_param("uavcan.pubp-stat", 2000) set_param("uavcan.pubp-pres", 10000) set_param("uavcan.pubp-mag", 20000) set_param("uavcan.pubp-fix", 66666) set_param("uavcan.pubp-aux", 100000) enforce( request( uavcan.protocol.param.ExecuteOpcode.Request( # @UndefinedVariable opcode=uavcan.protocol.param.ExecuteOpcode.Request().OPCODE_SAVE ) ).ok, # @UndefinedVariable "Could not save configuration", ) request( uavcan.protocol.RestartNode.Request( # @UndefinedVariable magic_number=uavcan.protocol.RestartNode.Request().MAGIC_NUMBER ), # @UndefinedVariable fire_and_forget=True, ) # Don't tell anybody I wrote this. I'm ashamed of this shit and too tired to redesign it. :( with BackgroundSpinner(safe_spin, 0.1): wait_for_boot() wait_for_init() check_status() log_all_params() def make_collector(data_type, timeout=0.1): return uavcan.monitors.MessageCollector(n, data_type, timeout=timeout) col_fix = make_collector(uavcan.equipment.gnss.Fix, 0.2) # @UndefinedVariable col_aux = make_collector(uavcan.equipment.gnss.Auxiliary, 0.2) # @UndefinedVariable col_mag = make_collector(uavcan.equipment.ahrs.MagneticFieldStrength) # @UndefinedVariable col_pressure = make_collector(uavcan.equipment.air_data.StaticPressure) # @UndefinedVariable col_temp = make_collector(uavcan.equipment.air_data.StaticTemperature) # @UndefinedVariable def check_everything(): check_status() try: m = col_pressure[node_id].message except KeyError: abort("Pressure measurements are not available. Check the sensor.") else: if not 50000 < m.static_pressure < 150000: abort("Invalid pressure reading: %d Pascal. Check the sensor.", m.static_pressure) try: m = col_temp[node_id].message except KeyError: abort("Temperature measurements are not available. Check the sensor.") else: if not 10 < (m.static_temperature - 273.15) < 50: abort("Invalid temperature reading: %d Kelvin. Check the sensor.", m.static_temperature) try: m = col_mag[node_id].message except KeyError: abort("Magnetic field measurements are not available. Check the sensor.") else: magnetic_field_scalar = numpy.linalg.norm(m.magnetic_field_ga) # @UndefinedVariable if not 0.01 < magnetic_field_scalar < 2: abort( "Invalid magnetic field strength reading: %d Gauss. Check the sensor.", magnetic_field_scalar, ) imperative( "Testing GNSS performance. Place the device close to a window to ensure decent GNSS reception. " "Please note that this test is very crude, it can only detect whether GNSS circuit is working " "at all or not. If GNSS performance is degraded due to improper manufacturing process, " "this test may fail to detect it, so please double check that your manufacturing process " "adheres to the documentation." ) info("Waiting for GNSS fix...") with time_limit(GNSS_FIX_TIMEOUT, "GNSS fix timeout. Check the RF circuit, AFE, antenna, and receiver"): while True: safe_spin(1) check_everything() sats_visible = col_aux[node_id].message.sats_visible sats_used = col_aux[node_id].message.sats_used sys.stdout.write("\rsat stats: visible %d, used %d \r" % (sats_visible, sats_used)) sys.stdout.flush() if col_fix[node_id].message.status >= 3: break info("Waiting for %d satellites...", GNSS_MIN_SAT_NUM) with time_limit( GNSS_MIN_SAT_TIMEOUT, "GNSS performance is degraded. Could be caused by incorrectly assembled RF circuit.", ): while True: safe_spin(0.5) check_everything() num = col_fix[node_id].message.sats_used pos_cov = list(col_fix[node_id].message.position_covariance) sys.stdout.write("\r%d sats, pos covariance: %r \r" % (num, pos_cov)) sys.stdout.flush() if num >= GNSS_MIN_SAT_NUM: break check_everything() info("Last sampled sensor measurements are provided below. They appear to be correct.") info("GNSS fix: %r", col_fix[node_id].message) info("GNSS aux: %r", col_aux[node_id].message) info("Magnetic field [Ga]: %r", col_mag[node_id].message) info("Pressure [Pa]: %r", col_pressure[node_id].message) info("Temperature [K]: %r", col_temp[node_id].message) # Finalizing the test info("Resetting the configuration to factory default...") enforce( request( uavcan.protocol.param.ExecuteOpcode.Request( # @UndefinedVariable opcode=uavcan.protocol.param.ExecuteOpcode.Request().OPCODE_ERASE ) ).ok, # @UndefinedVariable "Could not erase configuration", ) request( uavcan.protocol.RestartNode.Request( # @UndefinedVariable magic_number=uavcan.protocol.RestartNode.Request().MAGIC_NUMBER ), # @UndefinedVariable fire_and_forget=True, ) with BackgroundSpinner(safe_spin, 0.1): wait_for_boot() wait_for_init() check_status() log_all_params() except Exception: for nid in nsmon.get_all_node_id(): print("Node state: %r" % nsmon.get(nid)) raise # Blocking questions are moved out of the node scope because blocking breaks CAN communications if not input("Is the PPS LED blinking once per second?", yes_no=True, default_answer=True): abort("PPS LED is not working") if not input("Is the CAN1 LED blinking or glowing solid?", yes_no=True, default_answer=True): abort("CAN1 LED is not working (however the interface is fine)") if not input("Is the STATUS LED blinking once per second?", yes_no=True, default_answer=True): abort("STATUS LED is not working") # Testing CAN2 input("1. Disconnect CAN1 and connect to CAN2\n" "2. Terminate CAN2\n" "3. Press ENTER") if not input("Is the CAN2 LED blinking or glowing solid?", yes_no=True, default_answer=True): abort("Either CAN2 or its LED are not working")
)) print(value_to_constant_name( uavcan.protocol.file.Error(value=uavcan.protocol.file.Error().ACCESS_DENIED), 'value' )) print(value_to_constant_name( uavcan.equipment.power.BatteryInfo(status_flags= uavcan.equipment.power.BatteryInfo().STATUS_FLAG_NEED_SERVICE), 'status_flags' )) print(value_to_constant_name( uavcan.equipment.power.BatteryInfo(status_flags= uavcan.equipment.power.BatteryInfo().STATUS_FLAG_NEED_SERVICE | uavcan.equipment.power.BatteryInfo().STATUS_FLAG_TEMP_HOT | uavcan.equipment.power.BatteryInfo().STATUS_FLAG_CHARGED), 'status_flags' )) print(value_to_constant_name( uavcan.protocol.AccessCommandShell.Response(flags= uavcan.protocol.AccessCommandShell.Response().FLAG_SHELL_ERROR | uavcan.protocol.AccessCommandShell.Response(). FLAG_HAS_PENDING_STDOUT), 'flags' )) # Printing transfers node = uavcan.make_node('vcan0', node_id=42) node.request(uavcan.protocol.GetNodeInfo.Request(), 100, lambda e: print(to_yaml(e))) node.add_handler(uavcan.protocol.NodeStatus, lambda e: print(to_yaml(e))) node.spin()
def test_uavcan(): node_info = uavcan.protocol.GetNodeInfo.Response() node_info.name = 'com.zubax.drwatson.zubax_gnss' iface = init_can_iface() with closing(uavcan.make_node(iface, bitrate=CAN_BITRATE, node_id=127, mode=uavcan.protocol.NodeStatus().MODE_OPERATIONAL, node_info=node_info)) as n: def safe_spin(timeout): try: n.spin(timeout) except uavcan.transport.TransferError: logger.debug('Transfer error while spinning the node. ' 'Reporting at the DEBUG level because of https://github.com/UAVCAN/pyuavcan/issues/14', exc_info=True) except uavcan.UAVCANException: logger.error('Node spin failure', exc_info=True) @contextmanager def time_limit(timeout, error_fmt, *args): aborter = n.defer(timeout, partial(abort, error_fmt, *args)) yield aborter.remove() # Dynamic node ID allocation try: nsmon = uavcan.app.node_monitor.NodeMonitor(n) alloc = uavcan.app.dynamic_node_id.CentralizedServer(n, nsmon) with time_limit(10, 'The node did not show up in time. Check the CAN interface and the crystal oscillator.'): while True: safe_spin(1) target_nodes = list(nsmon.find_all(lambda e: e.info and e.info.name.decode() == PRODUCT_NAME)) if len(target_nodes) == 1: break if len(target_nodes) > 1: abort('Expected to find exactly one target node, found more: %r', target_nodes) node_id = target_nodes[0].node_id info('Node %r initialized', node_id) for nd in target_nodes: logger.info('Discovered node %r', nd) def request(what, fire_and_forget=False, timeout=2): response_event = None def cb(e): nonlocal response_event if not e: abort('Request has timed out: %r', what) response_event = e if fire_and_forget: n.request(what, node_id, lambda _: None, timeout=timeout) safe_spin(0.1) else: n.request(what, node_id, cb, timeout=timeout) while response_event is None: safe_spin(0.1) return response_event.response # Starting the node and checking its self-reported diag outputs def wait_for_init(): with time_limit(12, 'The node did not complete initialization in time'): while True: safe_spin(1) if nsmon.exists(node_id) and nsmon.get(node_id).status.mode == \ uavcan.protocol.NodeStatus().MODE_OPERATIONAL: break def check_status(): status = nsmon.get(node_id).status enforce(status.mode == uavcan.protocol.NodeStatus().MODE_OPERATIONAL, 'Unexpected operating mode') enforce(status.health == uavcan.protocol.NodeStatus().HEALTH_OK, 'Bad node health') info('Waiting for the node to complete initialization...') wait_for_init() check_status() info('Reconfiguring the node...') def log_all_params(): for index in range(10000): r = request(uavcan.protocol.param.GetSet.Request(index=index)) if not r.name: break logger.info('Param %-30r %r' % (r.name.decode(), getattr(r.value, uavcan.get_active_union_field(r.value)))) def set_param(name, value, union_field=None): union_field = union_field or { int: 'integer_value', float: 'real_value', bool: 'boolean_value', str: 'string_value' }[type(value)] logger.info('Setting parameter %r field %r value %r', name, union_field, value) req = uavcan.protocol.param.GetSet.Request() req.name.encode(name) setattr(req.value, union_field, value) r = request(req) enforce(uavcan.get_active_union_field(r.value) == union_field, 'Union field mismatch in param set response for %r', name) enforce(getattr(r.value, union_field) == value, 'The node refused to set parameter %r', name) set_param('uavcan.pubp-time', 10000) set_param('uavcan.pubp-stat', 2000) set_param('uavcan.pubp-pres', 10000) set_param('uavcan.pubp-mag', 20000) set_param('uavcan.pubp-fix', 66666) set_param('uavcan.pubp-aux', 100000) enforce(request(uavcan.protocol.param.ExecuteOpcode.Request( opcode=uavcan.protocol.param.ExecuteOpcode.Request().OPCODE_SAVE)).ok, 'Could not save configuration') request(uavcan.protocol.RestartNode.Request( magic_number=uavcan.protocol.RestartNode.Request().MAGIC_NUMBER), fire_and_forget=True) # Don't tell anybody I wrote this. I'm ashamed of this shit and too tired to redesign it. :( with BackgroundSpinner(safe_spin, 0.1): wait_for_boot() wait_for_init() check_status() log_all_params() def make_collector(data_type, timeout=0.5): return uavcan.app.message_collector.MessageCollector(n, data_type, timeout=timeout) col_fix = make_collector(uavcan.equipment.gnss.Fix2) col_aux = make_collector(uavcan.equipment.gnss.Auxiliary) col_mag = make_collector(uavcan.equipment.ahrs.MagneticFieldStrength) col_pressure = make_collector(uavcan.equipment.air_data.StaticPressure) col_temp = make_collector(uavcan.equipment.air_data.StaticTemperature) def check_everything(): check_status() try: m = col_pressure[node_id].message except KeyError: abort('Pressure measurements are not available. Check the sensor.') else: if not 50000 < m.static_pressure < 150000: abort('Invalid pressure reading: %d Pascal. Check the sensor.', m.static_pressure) try: m = col_temp[node_id].message except KeyError: abort('Temperature measurements are not available. Check the sensor.') else: if not 10 < (m.static_temperature - 273.15) < 50: abort('Invalid temperature reading: %d Kelvin. Check the sensor.', m.static_temperature) try: m = col_mag[node_id].message except KeyError: abort('Magnetic field measurements are not available. Check the sensor.') else: magnetic_field_scalar = numpy.linalg.norm(m.magnetic_field_ga) if not 0.01 < magnetic_field_scalar < 2: abort('Invalid magnetic field strength reading: %d Gauss. Check the sensor.', magnetic_field_scalar) imperative('Testing GNSS performance.') info('Waiting for GNSS fix...') with time_limit(GNSS_FIX_TIMEOUT, 'GNSS fix timeout. Check the RF circuit, AFE, antenna, and receiver'): while True: safe_spin(1) check_everything() sats_visible = col_aux[node_id].message.sats_visible sats_used = col_aux[node_id].message.sats_used sys.stdout.write('\rsat stats: visible %d, used %d \r' % (sats_visible, sats_used)) sys.stdout.flush() if col_fix[node_id].message.status >= 3: break info('Waiting for %d satellites...', GNSS_MIN_SAT_NUM) with time_limit(GNSS_MIN_SAT_TIMEOUT, 'GNSS performance is degraded. Could be caused by incorrectly assembled RF circuit.'): while True: safe_spin(0.5) check_everything() num = col_fix[node_id].message.sats_used cov = list(col_fix[node_id].message.covariance) sys.stdout.write('\r%d sats, covariance: %r \r' % (num, cov)) sys.stdout.flush() if num >= GNSS_MIN_SAT_NUM: break check_everything() info('The last sampled sensor measurements are provided below. They appear to be correct.') info('GNSS fix: %r', col_fix[node_id].message) info('GNSS aux: %r', col_aux[node_id].message) info('Magnetic field [Ga]: %r', col_mag[node_id].message) info('Pressure [Pa]: %r', col_pressure[node_id].message) info('Temperature [K]: %r', col_temp[node_id].message) # Finalizing the test info('Resetting the configuration to the factory defaults...') enforce(request(uavcan.protocol.param.ExecuteOpcode.Request( opcode=uavcan.protocol.param.ExecuteOpcode.Request().OPCODE_ERASE)).ok, 'Could not erase configuration') request(uavcan.protocol.RestartNode.Request( magic_number=uavcan.protocol.RestartNode.Request().MAGIC_NUMBER), fire_and_forget=True) with BackgroundSpinner(safe_spin, 0.1): wait_for_boot() wait_for_init() check_status() log_all_params() except Exception: for nid in nsmon.get_all_node_id(): print('Node state: %r' % nsmon.get(nid)) raise # Blocking questions are moved out of the node scope because blocking breaks CAN communications # Note that we must instantiate the driver in order to ensure proper traffic LED behavior # Periodic calls to receive() are needed to avoid RX queue overflow with closing(uavcan.make_driver(iface, bitrate=CAN_BITRATE)) as d: with BackgroundSpinner(lambda: d.receive(0.01)): if not input('Is the PPS LED blinking once per second?', yes_no=True, default_answer=True): abort('PPS LED is not working') if not input('Is the CAN1 LED blinking or glowing solid?', yes_no=True, default_answer=True): abort('CAN1 LED is not working (however the interface is fine)') if not input('Is the STATUS LED blinking once per second?', yes_no=True, default_answer=True): abort('STATUS LED is not working') # Testing CAN2 input('1. Disconnect the cable from CAN1 and connect it to CAN2\n' '2. Terminate CAN2\n' '3. Press ENTER') if not input('Is the CAN2 LED blinking or glowing solid?', yes_no=True, default_answer=True): abort('Either CAN2 or its LED are not working')