def field_to_string(self, name, keep_literal=False, fallback_format=None): val = getattr(self.struct, name) # Trying prefixed constants first for name, const in self.type_inspector.list_enum_constants(name): if const.value == val: return ('%s (%r)' % (name, val)) if keep_literal else name # If the struct contains only one field if len(uavcan.get_fields(self.struct)) == 1: for name, cvalue in uavcan.get_constants(self.struct).items(): if cvalue == val: return name return (fallback_format or '%r') % val
def field_to_string(self, name, keep_literal=False, fallback_format=None): val = getattr(self.struct, name) # Trying prefixed constants first for name, const in self.type_inspector.list_enum_constants(name): if const.value == val: return ('%s (%r)' % (name, val)) if keep_literal else name # If the struct contains only one field if len(uavcan.get_fields(self.struct)) == 1: for name, cvalue in uavcan.get_constants(self.struct).items(): if cvalue == val: return name return (fallback_format or '%r') % val
def _read(self, e): logger.debug("[#{0:03d}:uavcan.protocol.file.Read] {1!r} @ offset {2:d}" .format(e.transfer.source_node_id, e.request.path.path.decode(), e.request.offset)) try: with open(self._resolve_path(e.request.path), "rb") as f: f.seek(e.request.offset) resp = uavcan.protocol.file.Read.Response() read_size = uavcan.get_uavcan_data_type(uavcan.get_fields(resp)['data']).max_size resp.data = bytearray(f.read(read_size)) resp.error.value = resp.error.OK except Exception: logger.exception("[#{0:03d}:uavcan.protocol.file.Read] error") resp = uavcan.protocol.file.Read.Response() resp.error.value = resp.error.UNKNOWN_ERROR return resp
def _extract_struct_fields(m): if isinstance(m, uavcan.transport.CompoundValue): out = CompactMessage(uavcan.get_uavcan_data_type(m).full_name) for field_name, field in uavcan.get_fields(m).items(): if uavcan.is_union(m) and uavcan.get_active_union_field(m) != field_name: continue val = _extract_struct_fields(field) if val is not None: out._add_field(field_name, val) return out elif isinstance(m, uavcan.transport.ArrayValue): # cannot say I'm breaking the rules container = bytes if uavcan.get_uavcan_data_type(m).is_string_like else list # if I can glue them back together return container(filter(lambda x: x is not None, (_extract_struct_fields(item) for item in m))) elif isinstance(m, uavcan.transport.PrimitiveValue): return m.value elif isinstance(m, (int, float, bool)): return m elif isinstance(m, uavcan.transport.VoidValue): pass else: raise ValueError(':(')
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 _to_yaml_impl(obj, indent_level=0, parent=None, name=None, uavcan_type=None): buf = StringIO() def write(fmt, *args): buf.write((fmt % args) if len(args) else fmt) def indent_newline(): buf.write(os.linesep + ' ' * 2 * indent_level) # Decomposing PrimitiveValue to value and type. This is ugly but it's by design... if isinstance(obj, PrimitiveValue): uavcan_type = uavcan.get_uavcan_data_type(obj) obj = obj.value # CompoundValue if isinstance(obj, CompoundValue): first_field = True # Rendering all fields than can be rendered for field_name, field in uavcan.get_fields(obj).items(): if uavcan.is_union(obj) and uavcan.get_active_union_field(obj) != field_name: continue if isinstance(field, VoidValue): continue if (first_field and indent_level > 0) or not first_field: indent_newline() first_field = False rendered_field = _to_yaml_impl(field, indent_level=indent_level + 1, parent=obj, name=field_name) write('%s: %s', field_name, rendered_field) # Special case - empty non-union struct is rendered as empty map if first_field and not uavcan.is_union(obj): if indent_level > 0: indent_newline() write('{}') # ArrayValue elif isinstance(obj, ArrayValue): t = uavcan.get_uavcan_data_type(obj) if t.value_type.category == t.value_type.CATEGORY_PRIMITIVE: def is_nice_character(ch): if 32 <= ch <= 126: return True if ch in b'\n\r\t': return True return False as_bytes = '[%s]' % ', '.join([_to_yaml_impl(x, indent_level=indent_level + 1, uavcan_type=t.value_type) for x in obj]) if t.is_string_like and all(map(is_nice_character, obj)): write('%r # ', obj.decode()) write(as_bytes) else: if len(obj) == 0: write('[]') else: for x in obj: indent_newline() write('- %s', _to_yaml_impl(x, indent_level=indent_level + 1, uavcan_type=t.value_type)) # Primitive types elif isinstance(obj, float): assert uavcan_type is not None float_fmt = { 16: '%.4f', 32: '%.6f', 64: '%.9f', }[uavcan_type.bitlen] write(float_fmt, obj) elif isinstance(obj, bool): write('%s', 'true' if obj else 'false') elif isinstance(obj, int): write('%s', obj) if parent is not None and name is not None: resolved_name = value_to_constant_name(parent, name) if isinstance(resolved_name, str): write(' # %s', resolved_name) # Non-printable types elif isinstance(obj, VoidValue): pass # Unknown types else: raise ValueError('Cannot generate YAML representation for %r' % type(obj)) return buf.getvalue()