diff --git a/hw_device_mgr/cia_301/command.py b/hw_device_mgr/cia_301/command.py index 8dfddaa1..3c702e08 100644 --- a/hw_device_mgr/cia_301/command.py +++ b/hw_device_mgr/cia_301/command.py @@ -25,7 +25,9 @@ def scan_bus(self, bus=0): """Scan bus, returning list of addresses and IDs for each device.""" @abc.abstractmethod - def upload(self, address=None, index=None, subindex=0, datatype=None): + def upload( + self, address=None, index=None, subindex=0, datatype=None, **kwargs + ): """Upload a value from a device SDO.""" @abc.abstractmethod @@ -36,6 +38,7 @@ def download( subindex=0, value=None, datatype=None, + **kwargs, ): """Download a value to a device SDO.""" diff --git a/hw_device_mgr/cia_301/config.py b/hw_device_mgr/cia_301/config.py index 7033b030..3f2193d8 100644 --- a/hw_device_mgr/cia_301/config.py +++ b/hw_device_mgr/cia_301/config.py @@ -1,6 +1,7 @@ from .data_types import CiA301DataType -from .command import CiA301Command, CiA301SimCommand +from .command import CiA301Command, CiA301SimCommand, CiA301CommandException from .sdo import CiA301SDO +from functools import cached_property class CiA301Config: @@ -28,11 +29,12 @@ class CiA301Config: # Mapping of model_id to a dict of (index, subindex) to SDO object _model_sdos = dict() + # Mapping of model_id to a dict of (index, subindex) to DC object + _model_dcs = dict() def __init__(self, address=None, model_id=None): self.address = address self.model_id = self.format_model_id(model_id) - self._config = None @classmethod def format_model_id(cls, model_id): @@ -98,17 +100,45 @@ def sdo_ix(cls, ix): ix = (dtc.uint16(ix[0]), dtc.uint8(ix[1])) return ix + @cached_property + def sdos(self): + assert self.model_id in self._model_sdos + return self._model_sdos[self.model_id].values() + def sdo(self, ix): if isinstance(ix, self.sdo_class): return ix ix = self.sdo_ix(ix) return self._model_sdos[self.model_id][ix] + @classmethod + def add_device_dcs(cls, dcs_data): + """Add device model distributed clock descriptions.""" + for model_id, dcs in dcs_data.items(): + assert isinstance(dcs, list) + cls._model_dcs[model_id] = dcs + assert None not in cls._model_dcs + + def dcs(self): + """Get list of distributed clocks for this device.""" + return self._model_dcs[self.model_id] + + def dump_param_values(self): + res = dict() + for sdo in self.sdos: + try: + res[sdo] = self.upload(sdo, stderr_to_devnull=True) + except CiA301CommandException as e: + # Objects may not exist, like variable length PDO mappings + self.logger.debug(f"Upload {sdo} failed: {e}") + pass + return res + # # Param read/write # - def upload(self, sdo): + def upload(self, sdo, **kwargs): # Get SDO object sdo = self.sdo(sdo) res_raw = self.command().upload( @@ -116,10 +146,11 @@ def upload(self, sdo): index=sdo.index, subindex=sdo.subindex, datatype=sdo.data_type, + **kwargs, ) return sdo.data_type(res_raw) - def download(self, sdo, val, dry_run=False): + def download(self, sdo, val, dry_run=False, **kwargs): # Get SDO object sdo = self.sdo(sdo) # Check before setting value to avoid unnecessary NVRAM writes @@ -128,6 +159,7 @@ def download(self, sdo, val, dry_run=False): index=sdo.index, subindex=sdo.subindex, datatype=sdo.data_type, + **kwargs, ) if sdo.data_type(res_raw) == val: return # SDO value already correct @@ -140,6 +172,7 @@ def download(self, sdo, val, dry_run=False): subindex=sdo.subindex, value=val, datatype=sdo.data_type, + **kwargs, ) # @@ -172,8 +205,7 @@ def set_device_config(cls, config): - `pdo_mapping`: PDO mapping SM types only; `dict`: - `index`: Index of PDO mapping object - `entries`: Dictionary objects to be mapped; `dict`: - - `index`: Index of dictionary object - - `subindex`: Subindex of dictionary object (default 0) + - `index`: Index of dictionary object, e.g. "6041h" or "1A00-03h" - `name`: Name, a handle for the data object - `bits`: Instead of `name`, break out individual bits, names specified by a `list` @@ -187,48 +219,46 @@ def set_device_config(cls, config): cls._device_config.clear() cls._device_config.extend(config) - def munge_config(self, config_raw): + @classmethod + def munge_config(cls, config_raw, position): + config_cooked = config_raw.copy() + # Convert model ID ints + model_id = (config_raw["vendor_id"], config_raw["product_code"]) + model_id = cls.format_model_id(model_id) + config_cooked["vendor_id"], config_cooked["product_code"] = model_id # Flatten out param_values key - pv = dict() + config_cooked["param_values"] = dict() for ix, val in config_raw.get("param_values", dict()).items(): - ix = self.sdo_class.parse_idx_str(ix) + ix = cls.sdo_class.parse_idx_str(ix) if isinstance(val, list): - pos_ix = config_raw["positions"].index(self.position) + pos_ix = config_raw["positions"].index(position) val = val[pos_ix] - pv[ix] = val - dtc = self.data_type_class - config_raw["vendor_id"] = dtc.uint32(config_raw["vendor_id"]) - config_raw["product_code"] = dtc.uint32(config_raw["product_code"]) - config_cooked = dict( - vendor_id=config_raw["vendor_id"], - product_code=config_raw["product_code"], - param_values=pv, - sync_manager=config_raw.get("sync_manager", dict()), - ) + config_cooked["param_values"][ix] = val # Return pruned config dict return config_cooked - @property + @classmethod + def gen_config(cls, model_id, address): + bus, position = address + # Find matching config + for conf in cls._device_config: + if "vendor_id" not in conf: + continue # In tests only + if model_id != (conf["vendor_id"], conf["product_code"]): + continue + if bus != conf["bus"]: + continue + if position not in conf["positions"]: + continue + break + else: + raise KeyError(f"No config for device at {address}") + # Prune & return config + return cls.munge_config(conf, position) + + @cached_property def config(self): - if self._config is None: - # Find matching config - for conf in self._device_config: - if "vendor_id" not in conf: - continue # In tests only - if self.model_id != (conf["vendor_id"], conf["product_code"]): - continue - if self.bus != conf["bus"]: - continue - if self.position not in conf["positions"]: - continue - break - else: - raise KeyError(f"No config for device at {self.address}") - # Prune & cache config - self._config = self.munge_config(conf) - - # Return cached config - return self._config + return self.gen_config(self.model_id, self.address) def write_config_param_values(self): for sdo, value in self.config["param_values"].items(): diff --git a/hw_device_mgr/cia_301/device.py b/hw_device_mgr/cia_301/device.py index e76132e4..2f76238e 100644 --- a/hw_device_mgr/cia_301/device.py +++ b/hw_device_mgr/cia_301/device.py @@ -80,8 +80,8 @@ def log_operational_changes(self): def munge_sdo_data(cls, sdo_data): # Turn per-model name SDO data from YAML into per-model_id SDO data res = dict() - for model_name, sd in sdo_data.items(): - device_cls = cls.get_model_by_name(model_name) + for model_id, sd in sdo_data.items(): + device_cls = cls.get_model(model_id) model_id = device_cls.device_model_id() res[model_id] = sd assert res @@ -98,6 +98,16 @@ def add_device_sdos(cls, sdo_data): """ cls.config_class.add_device_sdos(cls.munge_sdo_data(sdo_data)) + @classmethod + def add_device_dcs(cls, dcs_data): + """ + Configure device distributed clocks. + + Pass to the `Config` class the information needed to configure + DCs for this `model_id`. + """ + cls.config_class.add_device_dcs(dcs_data) + @classmethod def get_device(cls, address=None, **kwargs): registry = cls._address_registry.setdefault(cls.name, dict()) @@ -178,10 +188,11 @@ def sim_device_data_address(cls, sim_device_data): return model_id @classmethod - def init_sim(cls, *, sim_device_data, sdo_data): + def init_sim(cls, *, sim_device_data, sdo_data, dcs_data): super().init_sim(sim_device_data=sim_device_data) sim_device_data = cls._sim_device_data[cls.category] cls.add_device_sdos(sdo_data) + cls.add_device_dcs(dcs_data) cls.config_class.init_sim(sim_device_data=sim_device_data) def set_sim_feedback(self, **kwargs): diff --git a/hw_device_mgr/cia_301/tests/base_test_class.py b/hw_device_mgr/cia_301/tests/base_test_class.py index fec64cdc..8918f4b3 100644 --- a/hw_device_mgr/cia_301/tests/base_test_class.py +++ b/hw_device_mgr/cia_301/tests/base_test_class.py @@ -20,10 +20,16 @@ class BaseCiA301TestClass(BaseTestClass): # # The device configuration, as in a real system - device_config_yaml = "cia_301/tests/device_config.yaml" + device_config_package = "hw_device_mgr.cia_301.tests" + device_config_yaml = "device_config.yaml" # Device model SDOs; for test fixture - device_sdos_yaml = "cia_301/tests/bogus_devices/sim_sdo_data.yaml" + device_sdos_package = "hw_device_mgr.cia_301.tests" + device_sdos_yaml = "sim_sdo_data.yaml" + + # Device model DCs; for test fixture + device_dcs_package = "hw_device_mgr.cia_301.tests" + device_dcs_yaml = "dcs_data.yaml" # Classes under test in this module data_type_class = CiA301DataType @@ -37,25 +43,41 @@ class BaseCiA301TestClass(BaseTestClass): BogusCiA301V1IO, ) - # Whether to pass SDO data to device_class.init_sim() - pass_init_sim_device_sdos = True + # Whether to pass SDO/DC data to device_class.init_sim() + pass_init_sim_device_description = True @classmethod def init_sim(cls, **kwargs): - if cls.pass_init_sim_device_sdos: + """Create sim device objects with configured SDOs.""" + if cls.pass_init_sim_device_description: # Init sim SDO data - path, sdo_data = cls.load_yaml(cls.device_sdos_yaml, True) - print(f" Raw sdo_data from {path}") + sdo_data = cls.load_sdo_data() + print(f" init_sim() sdo_data from {cls.sdo_data_resource()}") kwargs["sdo_data"] = cls.munge_sdo_data(sdo_data) + # Init DC data + dcs_data = cls.load_dcs_data() + print(f" init_sim() dcs_data from {cls.dcs_data_resource()}") + kwargs["dcs_data"] = cls.munge_dcs_data(dcs_data) # Init sim device data super().init_sim(**kwargs) @classmethod def munge_device_config(cls, device_config): - # Make device_config.yaml reusable by monkey-patching device vendor_id - # and product_code keys based on test_category key + """ + Munge raw device config. + + Return a copy of `device_config` with minor processing. + + Optionally, to make the YAML file reusable, each configuration's + `vendor_id` and `product_code` keys may be replaced with a `category` + key matching a parent of classes listed; this fixture will re-add those + keys. + """ new_device_config = list() for conf in device_config: + if "test_category" not in conf: # No monkey-patching needed + new_device_config.append(conf) + continue device_cls = cls.test_category_class(conf["test_category"]) assert device_cls new_device_config.append(conf) @@ -84,30 +106,34 @@ def config_cls(self, device_cls, device_config): def command_cls(self, device_cls): yield self.command_class + @classmethod + def load_device_config(cls): + """ + Load device configuration from package resource. + + The `importlib.resources` resource is named by + `device_config_package` and `device_config_yaml` attrs. + """ + rsrc = (cls.device_config_package, cls.device_config_yaml) + dev_conf = cls.load_yaml_resource(*rsrc) + assert dev_conf, f"Empty device config in package resource {rsrc}" + print(f" Raw device_config from {rsrc}") + return dev_conf + @pytest.fixture def device_config(self): """ Device configuration data fixture. - Load device configuration from file named in - `device_config_yaml` attr. + Load device configuration with `load_device_config()` and munge with + `mung_device_config()`. Device configuration in the same format as non-test configuration, described in `Config` classes. - - The absolute path is stored in the test object - `device_config_path` attribute. - - Optionally, to make the YAML file reusable, each - configuration's `vendor_id` and `product_code` keys may be - replaced with a `category` key matching a parent of classes - listed; this fixture will re-add those keys. """ - path, dev_conf = self.load_yaml(self.device_config_yaml, True) - print(f" Raw device_config from {path}") - dev_conf = self.munge_device_config(dev_conf) + conf_raw = self.load_device_config() + dev_conf = self.munge_device_config(conf_raw) self.device_config = dev_conf - self.device_config_path = path yield dev_conf @pytest.fixture @@ -196,7 +222,7 @@ def munge_sdo_data(cls, sdo_data, conv_sdos=False): for test_category, old_sdos in sdo_data.items(): device_cls = cls.test_category_class(test_category) assert device_cls - sdos = new_sdo_data[device_cls.name] = dict() + sdos = new_sdo_data[device_cls.device_model_id()] = dict() for ix, sdo in old_sdos.items(): if conv_sdos: ix = cls.config_class.sdo_ix(ix) @@ -217,6 +243,53 @@ def munge_sim_device_data(cls, sim_device_data): dev["test_address"] = (dev["bus"], dev["position"]) return sim_device_data + @classmethod + def sdo_data_resource(cls): + return (cls.device_sdos_package, cls.device_sdos_yaml) + + @classmethod + def load_sdo_data(cls): + rsrc = cls.sdo_data_resource() + sdo_data = cls.load_yaml_resource(*rsrc) + assert sdo_data, f"Empty SDO data in package resource {rsrc}" + return sdo_data + + @pytest.fixture + def dcs_data(self, _dcs_data, device_cls): + """ + Parametrize test with values from `device_dcs_yaml` resource. + + When combined with the `sim_device_data` fixture, `dcs_data` + values will match that fixture's device model. + + The `dcs_data` is also available in the test instance's + `dcs_data` attribute. + """ + self.dcs_data = _dcs_data + yield _dcs_data + + @classmethod + def dcs_data_resource(cls): + return (cls.device_dcs_package, cls.device_dcs_yaml) + + @classmethod + def load_dcs_data(cls): + rsrc = cls.dcs_data_resource() + dcs_data = cls.load_yaml_resource(*rsrc) + assert dcs_data, f"Empty DC data in package resource {rsrc}" + return dcs_data + + @classmethod + def munge_dcs_data(cls, dcs_data): + new_dcs_data = dict() + for test_category, dcs in dcs_data.items(): + device_cls = cls.test_category_class(test_category) + assert device_cls + new_dcs_data[device_cls.device_model_id()] = dcs + assert new_dcs_data + assert None not in new_dcs_data + return new_dcs_data + def pytest_generate_tests(self, metafunc): # Dynamic parametrization from sim_device_data_yaml: # - _sim_device_data: iterate through `sim_device_data` list @@ -224,10 +297,9 @@ def pytest_generate_tests(self, metafunc): # - _sdo_data: iterate through `sdo_data` values # - bus: iterate through `sim_device_data` unique `bus` values # *Note all three cases are mutually exclusive - path, dev_data = self.load_yaml(self.sim_device_data_yaml, True) - dev_data = self.munge_sim_device_data(dev_data) - path, sdo_data = self.load_yaml(self.device_sdos_yaml, True) - sdo_data = self.munge_sdo_data(sdo_data, conv_sdos=True) + dev_data = self.munge_sim_device_data(self.load_sim_device_data()) + sdo_data = self.munge_sdo_data(self.load_sdo_data(), conv_sdos=True) + dcs_data = self.munge_dcs_data(self.load_dcs_data()) names = list() vals, ids = (list(), list()) if "_sim_device_data" in metafunc.fixturenames: @@ -235,19 +307,33 @@ def pytest_generate_tests(self, metafunc): assert "bus" not in metafunc.fixturenames # sim_device_data["bus"] if "_sdo_data" in metafunc.fixturenames: names.append("_sdo_data") + if "_dcs_data" in metafunc.fixturenames: + names.append("_dcs_data") for dev in dev_data: ids.append(f"{dev['test_name']}@{dev['test_address']}") + dev_vals = [dev] + device_cls = self.test_category_class(dev["test_category"]) + assert device_cls is not None if "_sdo_data" in metafunc.fixturenames: - device_cls = self.test_category_class(dev["test_category"]) - assert device_cls is not None - vals.append([dev, sdo_data[device_cls.name]]) + dev_vals.append(sdo_data[device_cls.device_model_id()]) + if "_dcs_data" in metafunc.fixturenames: + dev_vals.append(dcs_data[device_cls.device_model_id()]) + if len(dev_vals) == 1: + vals.append(dev_vals[0]) else: - vals.append(dev) + vals.append(dev_vals) elif "_sdo_data" in metafunc.fixturenames: names.append("_sdo_data") - for category, device_sdos in sdo_data.items(): + for model_id, device_sdos in sdo_data.items(): vals.append(device_sdos) - ids.append(category) + dev_cls = self.device_class.get_model(model_id) + ids.append(dev_cls.test_category) + elif "_dcs_data" in metafunc.fixturenames: + names.append("_dcs_data") + for model_id, device_dcs in dcs_data.items(): + vals.append(device_dcs) + dev_cls = self.device_class.get_model(model_id) + ids.append(dev_cls.test_category) elif "bus" in metafunc.fixturenames: names.append("bus") vals = list(d for d in {d["bus"] for d in dev_data}) diff --git a/hw_device_mgr/cia_301/tests/bogus_devices/sim_sdo_data.yaml b/hw_device_mgr/cia_301/tests/bogus_devices/sim_sdo_data.yaml deleted file mode 100644 index a84b3911..00000000 --- a/hw_device_mgr/cia_301/tests/bogus_devices/sim_sdo_data.yaml +++ /dev/null @@ -1,132 +0,0 @@ -# Simplified view of SDOs for each category -bogus_v1_servo: - 6040h: - index: 0x6040 - data_type: uint16 - name: Control word - pdo_mapping: R - 6041h: - index: 0x6041 - data_type: uint16 - name: Status word - pdo_mapping: T - ro: True - 605Ah: - index: 0x605A - data_type: int16 - name: Quick stop mode - min_value: 0 - max_value: 7 - default_value: 2 - 6060h: - index: 0x6060 - data_type: uint8 - name: Control mode - pdo_mapping: R - 6061h: - index: 0x6061 - data_type: uint8 - name: Control mode feedback - pdo_mapping: T - ro: True - 6064h: - index: 0x6064 - data_type: int32 - name: Position feedback - pdo_mapping: T - ro: True - 607Ah: - index: 0x607A - data_type: int32 - name: Position command - pdo_mapping: R - 607D-00h: - index: 0x607D - subindex: 0x00 - data_type: uint8 - name: SubIndex 000 - index_name: Position limit - default_value: 2 - ro: True - 607D-01h: - index: 0x607D - subindex: 0x01 - data_type: int32 - name: Position limit min. - index_name: Position limit - default_value: -500000 - pdo_mapping: R - 607D-02h: - index: 0x607D - subindex: 0x02 - data_type: int32 - name: Position limit max. - index_name: Position limit - default_value: 500000 - pdo_mapping: R -bogus_v2_servo: - 6040h: - index: 0x6040 - data_type: uint16 - name: Control word - pdo_mapping: R - 6041h: - index: 0x6041 - data_type: uint16 - name: Status word - pdo_mapping: T - ro: True - 605Ah: - index: 0x605A - data_type: int16 - name: Quick stop mode - min_value: 0 - max_value: 7 - default_value: 2 - 6060h: - index: 0x6060 - data_type: uint8 - name: Control mode - pdo_mapping: R - 6061h: - index: 0x6061 - data_type: uint8 - name: Control mode feedback - pdo_mapping: T - ro: True - 6064h: - index: 0x6064 - data_type: int32 - name: Position feedback - pdo_mapping: T - ro: True - 607Ah: - index: 0x607A - data_type: int32 - name: Position command - pdo_mapping: R - 607D-00h: - index: 0x607D - subindex: 0x00 - data_type: uint8 - name: SubIndex 000 - index_name: Position limit - default_value: 2 - ro: True - 607D-01h: - index: 0x607D - subindex: 0x01 - data_type: int32 - name: Position limit min. - index_name: Position limit - default_value: -500000 - pdo_mapping: R - 607D-02h: - index: 0x607D - subindex: 0x02 - data_type: int32 - name: Position limit max. - index_name: Position limit - default_value: 500000 - pdo_mapping: R -bogus_v1_io: {} # Only PDOs diff --git a/hw_device_mgr/cia_301/tests/dcs_data.yaml b/hw_device_mgr/cia_301/tests/dcs_data.yaml new file mode 100644 index 00000000..1c15672d --- /dev/null +++ b/hw_device_mgr/cia_301/tests/dcs_data.yaml @@ -0,0 +1,21 @@ +bogus_v1_servo: + - Name: DC Sync + Desc: DC for synchronization + AssignActivate: 0x300 + CycleTimeSync0: 0 + ShiftTimeSync0: 0 + CycleTimeSync1: 0 + - Name: DC Off + Desc: DC unused + AssignActivate: 0x0 +bogus_v2_servo: + - Name: DC Sync + Desc: DC for synchronization + AssignActivate: 0x300 + CycleTimeSync0: 0 + ShiftTimeSync0: 0 + CycleTimeSync1: 0 + - Name: DC Off + Desc: DC unused + AssignActivate: 0x0 +bogus_v1_io: [] # No DCs diff --git a/hw_device_mgr/cia_301/tests/device_config.yaml b/hw_device_mgr/cia_301/tests/device_config.yaml index 6d755d00..fa8bc140 100644 --- a/hw_device_mgr/cia_301/tests/device_config.yaml +++ b/hw_device_mgr/cia_301/tests/device_config.yaml @@ -13,6 +13,9 @@ bus: 0 positions: [10, 11, 12, 13, 14, 15] + dc_conf: + sync0Shift: 500000 + # Sync manager configuration sync_manager: '0': @@ -39,38 +42,6 @@ entries: - index: 6041h name: status_word - - index: 6041h - bits: # Break bits out into separate items - # Bit0: Ready to switch on - - ready_to_switch_on - # Bit1: Switched on - - switched_on - # Bit2: Operation enabled - - operation_enabled - # Bit3: Fault - - fault - # Bit4: Voltage enabled - - voltage_enabled - # Bit5: Quick stop - - quick_stop_fb - # Bit6: Switch on disabled - - switch_on_disabled - # Bit7: Warning bit - - warning - # Bit8: Manufacturer specific - - bit_len: 1 - # Bit9: Remote - - remote - # Bit10: Goes high when target position is reached (HM, CSP modes) - - target_reached - # Bit11: Internal limit active (HM, CSP modes) - - internal_limit_active - # Bit12~Bit13 Operation mode specific - - status_bit12 - - status_bit13 - # Bit14~Bit15: Manufacturer specific - - status_bit14 - - homing_done_fb - index: 6064h name: position_fb @@ -110,7 +81,7 @@ # RPDOs: Receive PDOs ("out" to drive) dir: out pdo_mapping: - index: 1700h + index: 1601h entries: - index: 6040h name: control_word @@ -120,10 +91,8 @@ # TPDOs: Transmit PDOs ("in" from drive) dir: in pdo_mapping: - index: 1B00h + index: 1A01h entries: - - index: 6041h - name: status_word - index: 6041h bits: # Break bits out into separate items # Bit0: Ready to switch on @@ -143,7 +112,7 @@ # Bit7: Warning bit - warning # Bit8: Manufacturer specific - - bit_len: 1 + - null # Bit9: Remote - remote # Bit10: Goes high when target position is reached (HM, CSP modes) @@ -185,6 +154,8 @@ bus: 0 positions: [16, 17] + config_pdos: False + # Sync manager configuration sync_manager: '0': @@ -199,21 +170,25 @@ pdo_mapping: index: 1600h entries: - - index: 7000h + - index: 7000-01h bits: - out0 - out1 - out2 + # Unused bits + - bitLen: 5 '3': # TPDOs: Transmit PDOs dir: in # from drive pdo_mapping: index: 1A00h entries: - - index: 6000h + - index: 6000-01h bits: - in0 - in1 - in2 + # Unused bits + - bitLen: 5 # (No parameters) diff --git a/hw_device_mgr/cia_301/tests/sim_sdo_data.yaml b/hw_device_mgr/cia_301/tests/sim_sdo_data.yaml new file mode 100644 index 00000000..dd74545f --- /dev/null +++ b/hw_device_mgr/cia_301/tests/sim_sdo_data.yaml @@ -0,0 +1,654 @@ +# Simplified view of SDOs for each category +bogus_v1_servo: + # Any changes here must match ../../devices/device_xml/BogusServo.xml + 1600-00h: + index: 0x1600 + subindex: 0x00 + index_name: RxPDO Map 1 + name: Subindex 000 + data_type: uint8 + # default_data: 0x04 + 1600-01h: + index: 0x1600 + subindex: 0x01 + index_name: RxPDO Map 1 + name: RxPDO Map 1 Element 1 + data_type: uint16 + # default_data: 0x10004060 + 1600-02h: + index: 0x1600 + subindex: 0x02 + index_name: RxPDO Map 1 + name: RxPDO Map 1 Element 2 + data_type: uint16 + # default_data: 0x20007a60 + 1600-03h: + index: 0x1600 + subindex: 0x03 + index_name: RxPDO Map 1 + name: RxPDO Map 1 Element 3 + data_type: uint16 + # default_data: 0x2000ff60 + 1600-04h: + index: 0x1600 + subindex: 0x04 + index_name: RxPDO Map 1 + name: RxPDO Map 1 Element 4 + data_type: uint16 + # default_data: 0x08006060 + 1600-05h: + index: 0x1600 + subindex: 0x05 + index_name: RxPDO Map 1 + name: RxPDO Map 1 Element 5 + data_type: uint16 + # default_data: 0x00000000 + 1600-06h: + index: 0x1600 + subindex: 0x06 + index_name: RxPDO Map 1 + name: RxPDO Map 1 Element 6 + data_type: uint16 + # default_data: 0x00000000 + 1600-07h: + index: 0x1600 + subindex: 0x07 + index_name: RxPDO Map 1 + name: RxPDO Map 1 Element 7 + data_type: uint16 + # default_data: 0x00000000 + 1600-08h: + index: 0x1600 + subindex: 0x08 + index_name: RxPDO Map 1 + name: RxPDO Map 1 Element 8 + data_type: uint16 + # default_data: 0x00000000 + 1600-09h: + index: 0x1600 + subindex: 0x09 + index_name: RxPDO Map 1 + name: RxPDO Map 1 Element 9 + data_type: uint16 + # default_data: 0x00000000 + 1600-0Ah: + index: 0x1600 + subindex: 0x0A + index_name: RxPDO Map 1 + name: RxPDO Map 1 Element 10 + data_type: uint16 + # default_data: 0x00000000 + 1600-0Bh: + index: 0x1600 + subindex: 0x0B + index_name: RxPDO Map 1 + name: RxPDO Map 1 Element 11 + data_type: uint16 + # default_data: 0x00000000 + 1600-0Ch: + index: 0x1600 + subindex: 0x0C + index_name: RxPDO Map 1 + name: RxPDO Map 1 Element 12 + data_type: uint16 + # default_data: 0x00000000 + 1600-0Dh: + index: 0x1600 + subindex: 0x0D + index_name: RxPDO Map 1 + name: RxPDO Map 1 Element 13 + data_type: uint16 + # default_data: 0x00000000 + 1600-0Eh: + index: 0x1600 + subindex: 0x0E + index_name: RxPDO Map 1 + name: RxPDO Map 1 Element 14 + data_type: uint16 + # default_data: 0x00000000 + 1600-0Fh: + index: 0x1600 + subindex: 0x0F + index_name: RxPDO Map 1 + name: RxPDO Map 1 Element 15 + data_type: uint16 + # default_data: 0x00000000 + 1601-00h: + index: 0x1601 + subindex: 0x00 + index_name: 2nd receive PDO-Mapping + name: largest sub-index supported + data_type: uint8 + 1601-01h: + index: 0x1601 + subindex: 0x01 + index_name: 2nd receive PDO-Mapping + name: Target Velocity + data_type: uint16 + 1601-02h: + index: 0x1601 + subindex: 0x02 + index_name: 2nd receive PDO-Mapping + name: Controlword + data_type: uint16 + 1A00-00h: + index: 0x1A00 + subindex: 0x00 + index_name: TxPDO Map 1 + name: Subindex 000 + data_type: uint8 + # default_data: 0x04 + 1A00-01h: + index: 0x1A00 + subindex: 0x01 + index_name: TxPDO Map 1 + name: TxPDO Map 1 Element 1 + data_type: uint16 + # default_data: 0x10004160 + 1A00-02h: + index: 0x1A00 + subindex: 0x02 + index_name: TxPDO Map 1 + name: TxPDO Map 1 Element 2 + data_type: uint16 + # default_data: 0x20006460 + 1A00-03h: + index: 0x1A00 + subindex: 0x03 + index_name: TxPDO Map 1 + name: TxPDO Map 1 Element 3 + data_type: uint16 + # default_data: 0x20006c60 + 1A00-04h: + index: 0x1A00 + subindex: 0x04 + index_name: TxPDO Map 1 + name: TxPDO Map 1 Element 4 + data_type: uint16 + # default_data: 0x08006160 + 1A00-05h: + index: 0x1A00 + subindex: 0x05 + index_name: TxPDO Map 1 + name: TxPDO Map 1 Element 5 + data_type: uint16 + # default_data: 0x00000000 + 1A00-06h: + index: 0x1A00 + subindex: 0x06 + index_name: TxPDO Map 1 + name: TxPDO Map 1 Element 6 + data_type: uint16 + # default_data: 0x00000000 + 1A00-07h: + index: 0x1A00 + subindex: 0x07 + index_name: TxPDO Map 1 + name: TxPDO Map 1 Element 7 + data_type: uint16 + # default_data: 0x00000000 + 1A00-08h: + index: 0x1A00 + subindex: 0x08 + index_name: TxPDO Map 1 + name: TxPDO Map 1 Element 8 + data_type: uint16 + # default_data: 0x00000000 + 1A00-09h: + index: 0x1A00 + subindex: 0x09 + index_name: TxPDO Map 1 + name: TxPDO Map 1 Element 9 + data_type: uint16 + # default_data: 0x00000000 + 1A00-0Ah: + index: 0x1A00 + subindex: 0x0A + index_name: TxPDO Map 1 + name: TxPDO Map 1 Element 10 + data_type: uint16 + # default_data: 0x00000000 + 1A00-0Bh: + index: 0x1A00 + subindex: 0x0B + index_name: TxPDO Map 1 + name: TxPDO Map 1 Element 11 + data_type: uint16 + # default_data: 0x00000000 + 1A00-0Ch: + index: 0x1A00 + subindex: 0x0C + index_name: TxPDO Map 1 + name: TxPDO Map 1 Element 12 + data_type: uint16 + # default_data: 0x00000000 + 1A00-0Dh: + index: 0x1A00 + subindex: 0x0D + index_name: TxPDO Map 1 + name: TxPDO Map 1 Element 13 + data_type: uint16 + # default_data: 0x00000000 + 1A00-0Eh: + index: 0x1A00 + subindex: 0x0E + index_name: TxPDO Map 1 + name: TxPDO Map 1 Element 14 + data_type: uint16 + # default_data: 0x00000000 + 1A00-0Fh: + index: 0x1A00 + subindex: 0x0F + index_name: TxPDO Map 1 + name: TxPDO Map 1 Element 15 + # default_data: 0x00000000 + data_type: uint16 + 1A01-00h: + index: 0x1A01 + subindex: 0x00 + index_name: 2nd transmit PDO-Mapping + name: largest sub-index supported + data_type: uint8 + 1A01-01h: + index: 0x1A01 + subindex: 0x01 + index_name: 2nd transmit PDO-Mapping + name: Position Actual Value + data_type: uint16 + 1A01-02h: + index: 0x1A01 + subindex: 0x02 + index_name: 2nd transmit PDO-Mapping + name: Torque Actual Value + data_type: uint16 + 1A01-03h: + index: 0x1A01 + subindex: 0x03 + index_name: 2nd transmit PDO-Mapping + name: Statusword + data_type: uint16 + 6040h: + index: 0x6040 + data_type: uint16 + name: Control word + pdo_mapping: R + 6041h: + index: 0x6041 + data_type: uint16 + name: Status word + pdo_mapping: T + ro: True + 605Ah: + index: 0x605A + data_type: int16 + name: Quick stop mode + min_value: 0 + max_value: 7 + default_value: 2 + 6060h: + index: 0x6060 + data_type: uint8 + name: Control mode + pdo_mapping: R + 6061h: + index: 0x6061 + data_type: uint8 + name: Control mode feedback + pdo_mapping: T + ro: True + 6064h: + index: 0x6064 + data_type: int32 + name: Position feedback + pdo_mapping: T + ro: True + 607Ah: + index: 0x607A + data_type: int32 + name: Position command + pdo_mapping: R + 607D-00h: + index: 0x607D + subindex: 0x00 + data_type: uint8 + name: SubIndex 000 + index_name: Position limit + default_value: 2 + ro: True + 607D-01h: + index: 0x607D + subindex: 0x01 + data_type: int32 + name: Position limit min. + index_name: Position limit + default_value: -500000 + pdo_mapping: R + 607D-02h: + index: 0x607D + subindex: 0x02 + data_type: int32 + name: Position limit max. + index_name: Position limit + default_value: 500000 + pdo_mapping: R +bogus_v2_servo: + # Any changes here must match ../../devices/device_xml/BogusServo.xml + 1600-00h: + index: 0x1600 + subindex: 0x00 + name: Subindex 000 + data_type: uint8 + # default_data: 0x04 + 1600-01h: + index: 0x1600 + subindex: 0x01 + name: RxPDO Map 1 Element 1 + data_type: uint16 + # default_data: 0x10004060 + 1600-02h: + index: 0x1600 + subindex: 0x02 + name: RxPDO Map 1 Element 2 + data_type: uint16 + # default_data: 0x20007a60 + 1600-03h: + index: 0x1600 + subindex: 0x03 + name: RxPDO Map 1 Element 3 + data_type: uint16 + # default_data: 0x2000ff60 + 1600-04h: + index: 0x1600 + subindex: 0x04 + name: RxPDO Map 1 Element 4 + data_type: uint16 + # default_data: 0x08006060 + 1600-05h: + index: 0x1600 + subindex: 0x05 + name: RxPDO Map 1 Element 5 + data_type: uint16 + # default_data: 0x00000000 + 1600-06h: + index: 0x1600 + subindex: 0x06 + name: RxPDO Map 1 Element 6 + data_type: uint16 + # default_data: 0x00000000 + 1600-07h: + index: 0x1600 + subindex: 0x07 + name: RxPDO Map 1 Element 7 + data_type: uint16 + # default_data: 0x00000000 + 1600-08h: + index: 0x1600 + subindex: 0x08 + name: RxPDO Map 1 Element 8 + data_type: uint16 + # default_data: 0x00000000 + 1600-09h: + index: 0x1600 + subindex: 0x09 + name: RxPDO Map 1 Element 9 + data_type: uint16 + # default_data: 0x00000000 + 1600-0Ah: + index: 0x1600 + subindex: 0x0A + name: RxPDO Map 1 Element 10 + data_type: uint16 + # default_data: 0x00000000 + 1600-0Bh: + index: 0x1600 + subindex: 0x0B + name: RxPDO Map 1 Element 11 + data_type: uint16 + # default_data: 0x00000000 + 1600-0Ch: + index: 0x1600 + subindex: 0x0C + name: RxPDO Map 1 Element 12 + data_type: uint16 + # default_data: 0x00000000 + 1600-0Dh: + index: 0x1600 + subindex: 0x0D + name: RxPDO Map 1 Element 13 + data_type: uint16 + # default_data: 0x00000000 + 1600-0Eh: + index: 0x1600 + subindex: 0x0E + name: RxPDO Map 1 Element 14 + data_type: uint16 + # default_data: 0x00000000 + 1600-0Fh: + index: 0x1600 + subindex: 0x0F + name: RxPDO Map 1 Element 15 + data_type: uint16 + # default_data: 0x00000000 + 1601-00h: + index: 0x1601 + subindex: 0x00 + name: largest sub-index supported + data_type: uint8 + 1601-01h: + index: 0x1601 + subindex: 0x01 + name: Target Velocity + data_type: uint16 + 1601-02h: + index: 0x1601 + subindex: 0x02 + name: Controlword + data_type: uint16 + 1A00-00h: + index: 0x1A00 + subindex: 0x00 + name: Subindex 000 + data_type: uint8 + # default_data: 0x04 + 1A00-01h: + index: 0x1A00 + subindex: 0x01 + name: TxPDO Map 1 Element 1 + data_type: uint16 + # default_data: 0x10004160 + 1A00-02h: + index: 0x1A00 + subindex: 0x02 + name: TxPDO Map 1 Element 2 + data_type: uint16 + # default_data: 0x20006460 + 1A00-03h: + index: 0x1A00 + subindex: 0x03 + name: TxPDO Map 1 Element 3 + data_type: uint16 + # default_data: 0x20006c60 + 1A00-04h: + index: 0x1A00 + subindex: 0x04 + name: TxPDO Map 1 Element 4 + data_type: uint16 + # default_data: 0x08006160 + 1A00-05h: + index: 0x1A00 + subindex: 0x05 + name: TxPDO Map 1 Element 5 + data_type: uint16 + # default_data: 0x00000000 + 1A00-06h: + index: 0x1A00 + subindex: 0x06 + name: TxPDO Map 1 Element 6 + data_type: uint16 + # default_data: 0x00000000 + 1A00-07h: + index: 0x1A00 + subindex: 0x07 + name: TxPDO Map 1 Element 7 + data_type: uint16 + # default_data: 0x00000000 + 1A00-08h: + index: 0x1A00 + subindex: 0x08 + name: TxPDO Map 1 Element 8 + data_type: uint16 + # default_data: 0x00000000 + 1A00-09h: + index: 0x1A00 + subindex: 0x09 + name: TxPDO Map 1 Element 9 + data_type: uint16 + # default_data: 0x00000000 + 1A00-0Ah: + index: 0x1A00 + subindex: 0x0A + name: TxPDO Map 1 Element 10 + data_type: uint16 + # default_data: 0x00000000 + 1A00-0Bh: + index: 0x1A00 + subindex: 0x0B + name: TxPDO Map 1 Element 11 + data_type: uint16 + # default_data: 0x00000000 + 1A00-0Ch: + index: 0x1A00 + subindex: 0x0C + name: TxPDO Map 1 Element 12 + data_type: uint16 + # default_data: 0x00000000 + 1A00-0Dh: + index: 0x1A00 + subindex: 0x0D + name: TxPDO Map 1 Element 13 + data_type: uint16 + # default_data: 0x00000000 + 1A00-0Eh: + index: 0x1A00 + subindex: 0x0E + name: TxPDO Map 1 Element 14 + data_type: uint16 + # default_data: 0x00000000 + 1A00-0Fh: + index: 0x1A00 + subindex: 0x0F + name: TxPDO Map 1 Element 15 + # default_data: 0x00000000 + data_type: uint16 + 1A01-00h: + index: 0x1A01 + subindex: 0x00 + name: largest sub-index supported + data_type: uint8 + 1A01-01h: + index: 0x1A01 + subindex: 0x01 + name: Position Actual Value + data_type: uint16 + 1A01-02h: + index: 0x1A01 + subindex: 0x02 + name: Torque Actual Value + data_type: uint16 + 1A01-03h: + index: 0x1A01 + subindex: 0x03 + name: Statusword + data_type: uint16 + 6040h: + index: 0x6040 + data_type: uint16 + name: Control word + pdo_mapping: R + 6041h: + index: 0x6041 + data_type: uint16 + name: Status word + pdo_mapping: T + ro: True + 605Ah: + index: 0x605A + data_type: int16 + name: Quick stop mode + min_value: 0 + max_value: 7 + default_value: 2 + 6060h: + index: 0x6060 + data_type: uint8 + name: Control mode + pdo_mapping: R + 6061h: + index: 0x6061 + data_type: uint8 + name: Control mode feedback + pdo_mapping: T + ro: True + 6064h: + index: 0x6064 + data_type: int32 + name: Position feedback + pdo_mapping: T + ro: True + 607Ah: + index: 0x607A + data_type: int32 + name: Position command + pdo_mapping: R + 607D-00h: + index: 0x607D + subindex: 0x00 + data_type: uint8 + name: SubIndex 000 + index_name: Position limit + default_value: 2 + ro: True + 607D-01h: + index: 0x607D + subindex: 0x01 + data_type: int32 + name: Position limit min. + index_name: Position limit + default_value: -500000 + pdo_mapping: R + 607D-02h: + index: 0x607D + subindex: 0x02 + data_type: int32 + name: Position limit max. + index_name: Position limit + default_value: 500000 + pdo_mapping: R +bogus_v1_io: + # Any changes here must match ../../devices/device_xml/BogusIO.xml + 1600-00h: + # From RxPdo element + index: 0x1600 + subindex: 0x00 + data_type: int8 + name: Subindex 000 + index_name: DOut(0-2) + 7000-01h: + # From RxPdo Entry element + index: 0x7000 + subindex: 0x01 + data_type: uint8 + name: Outputs + 1A00-00h: + # From TxPdo element + index: 0x1A00 + subindex: 0x00 + data_type: int8 + name: Subindex 000 + index_name: Din(0-2) + 6000-01h: + # From TxPdo element + index: 0x6000 + subindex: 0x01 + data_type: uint8 + name: Inputs diff --git a/hw_device_mgr/cia_301/tests/test_config.py b/hw_device_mgr/cia_301/tests/test_config.py index df1e4afc..866c6614 100644 --- a/hw_device_mgr/cia_301/tests/test_config.py +++ b/hw_device_mgr/cia_301/tests/test_config.py @@ -20,12 +20,12 @@ def obj(self, sim_device_data, config_cls): yield self.obj def test_add_device_sdos(self, obj, config_cls, sdo_data): - print("model_id:", obj.model_id) - print("registered models:", list(config_cls._model_sdos)) - print("config_cls._model_sdos:", config_cls._model_sdos) + print("registered models w/SDOs:", list(config_cls._model_sdos)) + print("test obj model_id:", obj.model_id) assert obj.model_id in config_cls._model_sdos obj_sdos = obj._model_sdos[obj.model_id] - assert len(obj_sdos) == len(sdo_data) + print("test obj SDOs:", obj_sdos) + assert list(sorted(obj_sdos)) == list(sorted(sdo_data)) for ix, expected_sdo in sdo_data.items(): assert ix in obj_sdos obj_sdo = obj_sdos[ix] @@ -71,3 +71,13 @@ def test_write_config_param_values(self, obj, sdo_data): obj.write_config_param_values() for sdo_ix, val in obj.config["param_values"].items(): assert obj.upload(sdo_ix) == val + + def test_add_device_dcs(self, obj, config_cls, dcs_data): + print("model_id:", obj.model_id) + print("config_cls._model_dcs:", config_cls._model_dcs) + dcs_data = [dict(dc) for dc in dcs_data] + print("expected dcs:", dcs_data) + print("object dcs:", obj.dcs()) + assert len(obj.dcs()) == len(dcs_data) + for expected_dc in dcs_data: + assert dict(expected_dc) in obj.dcs() diff --git a/hw_device_mgr/cia_301/tests/test_device.py b/hw_device_mgr/cia_301/tests/test_device.py index 09c6c692..3bcd583d 100644 --- a/hw_device_mgr/cia_301/tests/test_device.py +++ b/hw_device_mgr/cia_301/tests/test_device.py @@ -10,8 +10,8 @@ class TestCiA301Device(BaseCiA301TestClass, _TestDevice): *_TestDevice.expected_mro, ] - # Test CiA NMT init: online & operational status - read_update_write_yaml = "cia_301/tests/read_update_write.cases.yaml" + # CiA NMT init online & operational status test cases + read_update_write_package = "hw_device_mgr.cia_301.tests" @pytest.fixture def obj(self, device_cls, sim_device_data): diff --git a/hw_device_mgr/cia_402/device.py b/hw_device_mgr/cia_402/device.py index bbdd2c6f..8ccf35f4 100644 --- a/hw_device_mgr/cia_402/device.py +++ b/hw_device_mgr/cia_402/device.py @@ -173,10 +173,10 @@ def get_feedback(self): goal_reasons.append(f"state flag {flag_name} != {not flag_val}") if not goal_reached: - fb_out.update( - goal_reached=False, goal_reason="; ".join(goal_reasons) - ) - self.logger.debug(f"Device {self.address}: Goal not reached:") + goal_reason = "; ".join(goal_reasons) + fb_out.update(goal_reached=False, goal_reason=goal_reason) + if fb_out.changed("goal_reason"): + self.logger.debug(f"{self}: Goal not reached: {goal_reason}") return fb_out state_bits = { @@ -492,13 +492,13 @@ def set_sim_feedback(self): # Log changes if self.sim_feedback.changed("control_mode_fb"): cm = self.sim_feedback.get("control_mode_fb") - self.logger.info(f"{self} next control_mode_fb: 0x{cm:04X}") + self.logger.info(f"{self} sim control_mode_fb: 0x{cm:04X}") if self.sim_feedback.changed("status_word"): sw = self.sim_feedback.get("status_word") flags = ",".join(k for k, v in sw_flags.items() if v) flags = f" flags: {flags}" if flags else "" self.logger.info( - f"{self} sim next status_word: 0x{sw:04X} {state} {flags}" + f"{self} sim status_word: 0x{sw:04X} {state} {flags}" ) return sfb diff --git a/hw_device_mgr/cia_402/tests/base_test_class.py b/hw_device_mgr/cia_402/tests/base_test_class.py index e547c5df..d3b049e7 100644 --- a/hw_device_mgr/cia_402/tests/base_test_class.py +++ b/hw_device_mgr/cia_402/tests/base_test_class.py @@ -10,7 +10,8 @@ class BaseCiA402TestClass(BaseCiA301TestClass): # test_read_update_write_402() configuration - read_update_write_402_yaml = "cia_402/tests/read_update_write.cases.yaml" + read_update_write_402_package = "hw_device_mgr.cia_402.tests" + read_update_write_402_yaml = "read_update_write.cases.yaml" # Classes under test in this module device_class = BogusCiA402Device diff --git a/hw_device_mgr/cia_402/tests/test_device.py b/hw_device_mgr/cia_402/tests/test_device.py index 698627cb..d69d5cb2 100644 --- a/hw_device_mgr/cia_402/tests/test_device.py +++ b/hw_device_mgr/cia_402/tests/test_device.py @@ -29,11 +29,12 @@ def read_update_write_conv_test_data(self): if key in intf_data: intf_data[key] = uint16(intf_data[key]) - def test_read_update_write(self, obj, fpath): + def test_read_update_write(self, obj): if hasattr(obj, "MODE_CSP"): # CiA 402 device + self.read_update_write_package = self.read_update_write_402_package self.read_update_write_yaml = self.read_update_write_402_yaml self.is_402_device = True else: self.is_402_device = False - super().test_read_update_write(obj, fpath) + super().test_read_update_write(obj) diff --git a/hw_device_mgr/config_io.py b/hw_device_mgr/config_io.py new file mode 100644 index 00000000..03c20b13 --- /dev/null +++ b/hw_device_mgr/config_io.py @@ -0,0 +1,39 @@ +import ruamel.yaml +from pathlib import Path +from importlib.resources import open_binary + + +class ConfigIO: + @classmethod + def open_path(cls, path, *args, **kwargs): + """Return open file object for `path`.""" + path_obj = Path(path) + return path_obj.open(*args, **kwargs) + + @classmethod + def open_resource(cls, package, resource): + """Return open file object for importlib package resource.""" + return open_binary(package, resource) + + @classmethod + def load_yaml_path(cls, path): + """Read and return `data` from YAML formatted file `path`.""" + yaml = ruamel.yaml.YAML() + with cls.open_path(path) as f: + data = yaml.load(f) + return data + + @classmethod + def dump_yaml_path(cls, path, data): + """Dump `data` in YAML format to `path`.""" + yaml = ruamel.yaml.YAML() + with cls.open_path(path, "w") as f: + yaml.dump(data, f) + + @classmethod + def load_yaml_resource(cls, package, resource): + """Load YAML from importlib package resource.""" + yaml = ruamel.yaml.YAML() + with cls.open_resource(package, resource) as f: + data = yaml.load(f) + return data diff --git a/hw_device_mgr/device.py b/hw_device_mgr/device.py index e1a2a98f..2f9ed336 100644 --- a/hw_device_mgr/device.py +++ b/hw_device_mgr/device.py @@ -1,8 +1,9 @@ import abc -from importlib.resources import path as imp_path from .logging import Logging from .interface import Interface from .data_types import DataType +from functools import cached_property +import re class Device(abc.ABC): @@ -36,7 +37,7 @@ def __init__(self, address=None): self.address = address self.init_interfaces() - def init(self, index=None): + def init(self): """ Initialize device. @@ -44,7 +45,7 @@ def init(self, index=None): outside the constructor. Implementations should always call `super().init()`. """ - self.index = index + pass @classmethod def merge_dict_attrs(cls, attr): @@ -62,6 +63,21 @@ def merge_dict_attrs(cls, attr): res.update(c_attr) return res + slug_separator = "." + + @cached_property + def addr_slug(self): + """ + Return a slug generated from the device address. + + The slug is computed by separating numeric components of the + device `address` string with the `slug_separator` character, + default `.`, e.g. `(0,5)` -> `0.5`. This is intended to be + useful for inclusion into identifiers. + """ + addr_prefix = re.sub(r"[^0-9]+", self.slug_separator, str(self.address)) + return addr_prefix.strip(self.slug_separator) + def init_interfaces(self): intfs = self._interfaces = dict() dt_name2cls = self.data_type_class.by_shared_name @@ -95,6 +111,7 @@ def interface_changed(self, what, key, return_vals=False): def read(self): """Read `feedback_in` from hardware interface.""" + self._interfaces["feedback_in"].set() def get_feedback(self): """Process `feedback_in` and return `feedback_out` interface.""" @@ -114,14 +131,6 @@ def write(self): def log_status(self): pass - @classmethod - def pkg_path(cls, path): - """Return `pathlib.Path` object for this package's directory.""" - # Find cls's module & package - pkg = ".".join(cls.__module__.split(".")[:-1]) - with imp_path(pkg, path) as p: - return p - def __str__(self): return f"<{self.name}@{self.address}>" @@ -338,7 +347,12 @@ def sim_device_data_address(cls, sim_device_data): @classmethod def init_sim(cls, *, sim_device_data): - """Massage device test data for usability.""" + """ + Create sim device objects for tests. + + Construct sim device objects with device class, address, etc. + from `sim_device_data`. + """ cls_sim_data = cls._sim_device_data[cls.category] = dict() for dev in sim_device_data: @@ -369,7 +383,7 @@ def read(self): """Read `feedback_in` from hardware interface.""" super().read() sfb = self._interfaces["sim_feedback"].get() - self._interfaces["feedback_in"].set(**sfb) + self._interfaces["feedback_in"].update(**sfb) def set_sim_feedback(self): """Simulate feedback from command and feedback.""" diff --git a/hw_device_mgr/devices/bogus.py b/hw_device_mgr/devices/bogus.py index fe25f20b..e88738a4 100644 --- a/hw_device_mgr/devices/bogus.py +++ b/hw_device_mgr/devices/bogus.py @@ -7,6 +7,7 @@ class BogusDevice(EtherCATSimDevice, CiA402SimDevice): category = "bogus_servo" vendor_id = 0xB090C0 + xml_description_package = "hw_device_mgr.devices.device_xml" xml_description_fname = "BogusServo.xml" diff --git a/hw_device_mgr/devices/device_xml/BogusIO.xml b/hw_device_mgr/devices/device_xml/BogusIO.xml index d6a564b3..d88e0626 100644 --- a/hw_device_mgr/devices/device_xml/BogusIO.xml +++ b/hw_device_mgr/devices/device_xml/BogusIO.xml @@ -1,5 +1,6 @@ + + USINT 8 + + DT1600 + 496 + + 0 + Subindex 000 + USINT + 8 + 0 + + rw + o + 0 + 0 + + + + 1 + RxPDO Map 1 Element 1 + UDINT + 32 + 16 + + rw + o + 0 + 0 + + + + 2 + RxPDO Map 1 Element 2 + UDINT + 32 + 48 + + rw + o + 0 + 0 + + + + 3 + RxPDO Map 1 Element 3 + UDINT + 32 + 80 + + rw + o + 0 + 0 + + + + 4 + RxPDO Map 1 Element 4 + UDINT + 32 + 112 + + rw + o + 0 + 0 + + + + 5 + RxPDO Map 1 Element 5 + UDINT + 32 + 144 + + rw + o + 0 + 0 + + + + 6 + RxPDO Map 1 Element 6 + UDINT + 32 + 176 + + rw + o + 0 + 0 + + + + 7 + RxPDO Map 1 Element 7 + UDINT + 32 + 208 + + rw + o + 0 + 0 + + + + 8 + RxPDO Map 1 Element 8 + UDINT + 32 + 240 + + rw + o + 0 + 0 + + + + 9 + RxPDO Map 1 Element 9 + UDINT + 32 + 272 + + rw + o + 0 + 0 + + + + 10 + RxPDO Map 1 Element 10 + UDINT + 32 + 304 + + rw + o + 0 + 0 + + + + 11 + RxPDO Map 1 Element 11 + UDINT + 32 + 336 + + rw + o + 0 + 0 + + + + 12 + RxPDO Map 1 Element 12 + UDINT + 32 + 368 + + rw + o + 0 + 0 + + + + 13 + RxPDO Map 1 Element 13 + UDINT + 32 + 400 + + rw + o + 0 + 0 + + + + 14 + RxPDO Map 1 Element 14 + UDINT + 32 + 432 + + rw + o + 0 + 0 + + + + 15 + RxPDO Map 1 Element 15 + UDINT + 32 + 464 + + rw + o + 0 + 0 + + + + + DT1601 + 80 + + 0 + largest sub-index supported + USINT + 8 + 0 + + ro + o + + + + 1 + Target Velocity + UDINT + 32 + 16 + + ro + o + + + + 2 + Controlword + UDINT + 32 + 48 + + ro + o + + + + + DT1A00 + 496 + + 0 + Subindex 000 + USINT + 8 + 0 + + rw + o + 0 + 0 + + + + 1 + TxPDO Map 1 Element 1 + UDINT + 32 + 16 + + rw + o + 0 + 0 + + + + 2 + TxPDO Map 1 Element 2 + UDINT + 32 + 48 + + rw + o + 0 + 0 + + + + 3 + TxPDO Map 1 Element 3 + UDINT + 32 + 80 + + rw + o + 0 + 0 + + + + 4 + TxPDO Map 1 Element 4 + UDINT + 32 + 112 + + rw + o + 0 + 0 + + + + 5 + TxPDO Map 1 Element 5 + UDINT + 32 + 144 + + rw + o + 0 + 0 + + + + 6 + TxPDO Map 1 Element 6 + UDINT + 32 + 176 + + rw + o + 0 + 0 + + + + 7 + TxPDO Map 1 Element 7 + UDINT + 32 + 208 + + rw + o + 0 + 0 + + + + 8 + TxPDO Map 1 Element 8 + UDINT + 32 + 240 + + rw + o + 0 + 0 + + + + 9 + TxPDO Map 1 Element 9 + UDINT + 32 + 272 + + rw + o + 0 + 0 + + + + 10 + TxPDO Map 1 Element 10 + UDINT + 32 + 304 + + rw + o + 0 + 0 + + + + 11 + TxPDO Map 1 Element 11 + UDINT + 32 + 336 + + rw + o + 0 + 0 + + + + 12 + TxPDO Map 1 Element 12 + UDINT + 32 + 368 + + rw + o + 0 + 0 + + + + 13 + TxPDO Map 1 Element 13 + UDINT + 32 + 400 + + rw + o + 0 + 0 + + + + 14 + TxPDO Map 1 Element 14 + UDINT + 32 + 432 + + rw + o + 0 + 0 + + + + 15 + TxPDO Map 1 Element 15 + UDINT + 32 + 464 + + rw + o + 0 + 0 + + + + + DT1A01 + 112 + + 0 + largest sub-index supported + USINT + 8 + 0 + + ro + o + + + + 1 + Position Actual Value + UDINT + 32 + 16 + + ro + o + + + + 2 + Torque Actual Value + UDINT + 32 + 48 + + ro + o + + + + 3 + Statusword + UDINT + 32 + 80 + + ro + o + + + DT607D 80 @@ -103,6 +613,226 @@ + + #x1600 + RxPDO Map 1 + DT1600 + 496 + + + Subindex 000 + + 04 + + + + RxPDO Map 1 Element 1 + + 10004060 + + + + RxPDO Map 1 Element 2 + + 20007a60 + + + + RxPDO Map 1 Element 3 + + 2000ff60 + + + + RxPDO Map 1 Element 4 + + 08006060 + + + + RxPDO Map 1 Element 5 + + 00000000 + + + + RxPDO Map 1 Element 6 + + 00000000 + + + + RxPDO Map 1 Element 7 + + 00000000 + + + + RxPDO Map 1 Element 8 + + 00000000 + + + + RxPDO Map 1 Element 9 + + 00000000 + + + + RxPDO Map 1 Element 10 + + 00000000 + + + + RxPDO Map 1 Element 11 + + 00000000 + + + + RxPDO Map 1 Element 12 + + 00000000 + + + + RxPDO Map 1 Element 13 + + 00000000 + + + + RxPDO Map 1 Element 14 + + 00000000 + + + + RxPDO Map 1 Element 15 + + 00000000 + + + + + + #x1601 + 2nd receive PDO-Mapping + DT1601 + 80 + + + #x1a00 + TxPDO Map 1 + DT1A00 + 496 + + + Subindex 000 + + 04 + + + + TxPDO Map 1 Element 1 + + 10004160 + + + + TxPDO Map 1 Element 2 + + 20006460 + + + + TxPDO Map 1 Element 3 + + 20006c60 + + + + TxPDO Map 1 Element 4 + + 08006160 + + + + TxPDO Map 1 Element 5 + + 00000000 + + + + TxPDO Map 1 Element 6 + + 00000000 + + + + TxPDO Map 1 Element 7 + + 00000000 + + + + TxPDO Map 1 Element 8 + + 00000000 + + + + TxPDO Map 1 Element 9 + + 00000000 + + + + TxPDO Map 1 Element 10 + + 00000000 + + + + TxPDO Map 1 Element 11 + + 00000000 + + + + TxPDO Map 1 Element 12 + + 00000000 + + + + TxPDO Map 1 Element 13 + + 00000000 + + + + TxPDO Map 1 Element 14 + + 00000000 + + + + TxPDO Map 1 Element 15 + + 00000000 + + + + + + #x1A01 + 2nd transmit PDO-Mapping + DT1A01 + 112 + #x6040 Control word @@ -287,13 +1017,18 @@ - DC - Distributed Clock + DC Sync + DC for synchronization #x300 0 0 0 + + DC Off + DC unused + 0 + @@ -339,10 +1074,519 @@ 16 - - USINT - 8 + + USINT + 8 + + + DT1600 + 496 + + 0 + Subindex 000 + USINT + 8 + 0 + + rw + o + 0 + 0 + + + + 1 + RxPDO Map 1 Element 1 + UDINT + 32 + 16 + + rw + o + 0 + 0 + + + + 2 + RxPDO Map 1 Element 2 + UDINT + 32 + 48 + + rw + o + 0 + 0 + + + + 3 + RxPDO Map 1 Element 3 + UDINT + 32 + 80 + + rw + o + 0 + 0 + + + + 4 + RxPDO Map 1 Element 4 + UDINT + 32 + 112 + + rw + o + 0 + 0 + + + + 5 + RxPDO Map 1 Element 5 + UDINT + 32 + 144 + + rw + o + 0 + 0 + + + + 6 + RxPDO Map 1 Element 6 + UDINT + 32 + 176 + + rw + o + 0 + 0 + + + + 7 + RxPDO Map 1 Element 7 + UDINT + 32 + 208 + + rw + o + 0 + 0 + + + + 8 + RxPDO Map 1 Element 8 + UDINT + 32 + 240 + + rw + o + 0 + 0 + + + + 9 + RxPDO Map 1 Element 9 + UDINT + 32 + 272 + + rw + o + 0 + 0 + + + + 10 + RxPDO Map 1 Element 10 + UDINT + 32 + 304 + + rw + o + 0 + 0 + + + + 11 + RxPDO Map 1 Element 11 + UDINT + 32 + 336 + + rw + o + 0 + 0 + + + + 12 + RxPDO Map 1 Element 12 + UDINT + 32 + 368 + + rw + o + 0 + 0 + + + + 13 + RxPDO Map 1 Element 13 + UDINT + 32 + 400 + + rw + o + 0 + 0 + + + + 14 + RxPDO Map 1 Element 14 + UDINT + 32 + 432 + + rw + o + 0 + 0 + + + + 15 + RxPDO Map 1 Element 15 + UDINT + 32 + 464 + + rw + o + 0 + 0 + + + + + DT1601 + 80 + + 0 + largest sub-index supported + USINT + 8 + 0 + + ro + o + + + + 1 + Target Velocity + UDINT + 32 + 16 + + ro + o + + + + 2 + Controlword + UDINT + 32 + 48 + + ro + o + + + + + DT1A00 + 496 + + 0 + Subindex 000 + USINT + 8 + 0 + + rw + o + 0 + 0 + + + + 1 + TxPDO Map 1 Element 1 + UDINT + 32 + 16 + + rw + o + 0 + 0 + + + + 2 + TxPDO Map 1 Element 2 + UDINT + 32 + 48 + + rw + o + 0 + 0 + + + + 3 + TxPDO Map 1 Element 3 + UDINT + 32 + 80 + + rw + o + 0 + 0 + + + + 4 + TxPDO Map 1 Element 4 + UDINT + 32 + 112 + + rw + o + 0 + 0 + + + + 5 + TxPDO Map 1 Element 5 + UDINT + 32 + 144 + + rw + o + 0 + 0 + + + + 6 + TxPDO Map 1 Element 6 + UDINT + 32 + 176 + + rw + o + 0 + 0 + + + + 7 + TxPDO Map 1 Element 7 + UDINT + 32 + 208 + + rw + o + 0 + 0 + + + + 8 + TxPDO Map 1 Element 8 + UDINT + 32 + 240 + + rw + o + 0 + 0 + + + + 9 + TxPDO Map 1 Element 9 + UDINT + 32 + 272 + + rw + o + 0 + 0 + + + + 10 + TxPDO Map 1 Element 10 + UDINT + 32 + 304 + + rw + o + 0 + 0 + + + + 11 + TxPDO Map 1 Element 11 + UDINT + 32 + 336 + + rw + o + 0 + 0 + + + + 12 + TxPDO Map 1 Element 12 + UDINT + 32 + 368 + + rw + o + 0 + 0 + + + + 13 + TxPDO Map 1 Element 13 + UDINT + 32 + 400 + + rw + o + 0 + 0 + + + + 14 + TxPDO Map 1 Element 14 + UDINT + 32 + 432 + + rw + o + 0 + 0 + + + + 15 + TxPDO Map 1 Element 15 + UDINT + 32 + 464 + + rw + o + 0 + 0 + + + + DT1A01 + 112 + + 0 + largest sub-index supported + USINT + 8 + 0 + + ro + o + + + + 1 + Position Actual Value + UDINT + 32 + 16 + + ro + o + + + + 2 + Torque Actual Value + UDINT + 32 + 48 + + ro + o + + + + 3 + Statusword + UDINT + 32 + 80 + + ro + o + + + DT607D 80 @@ -381,6 +1625,226 @@ + + #x1600 + RxPDO Map 1 + DT1600 + 496 + + + Subindex 000 + + 04 + + + + RxPDO Map 1 Element 1 + + 10004060 + + + + RxPDO Map 1 Element 2 + + 20007a60 + + + + RxPDO Map 1 Element 3 + + 2000ff60 + + + + RxPDO Map 1 Element 4 + + 08006060 + + + + RxPDO Map 1 Element 5 + + 00000000 + + + + RxPDO Map 1 Element 6 + + 00000000 + + + + RxPDO Map 1 Element 7 + + 00000000 + + + + RxPDO Map 1 Element 8 + + 00000000 + + + + RxPDO Map 1 Element 9 + + 00000000 + + + + RxPDO Map 1 Element 10 + + 00000000 + + + + RxPDO Map 1 Element 11 + + 00000000 + + + + RxPDO Map 1 Element 12 + + 00000000 + + + + RxPDO Map 1 Element 13 + + 00000000 + + + + RxPDO Map 1 Element 14 + + 00000000 + + + + RxPDO Map 1 Element 15 + + 00000000 + + + + + + #x1601 + 2nd receive PDO-Mapping + DT1601 + 80 + + + #x1a00 + TxPDO Map 1 + DT1A00 + 496 + + + Subindex 000 + + 04 + + + + TxPDO Map 1 Element 1 + + 10004160 + + + + TxPDO Map 1 Element 2 + + 20006460 + + + + TxPDO Map 1 Element 3 + + 20006c60 + + + + TxPDO Map 1 Element 4 + + 08006160 + + + + TxPDO Map 1 Element 5 + + 00000000 + + + + TxPDO Map 1 Element 6 + + 00000000 + + + + TxPDO Map 1 Element 7 + + 00000000 + + + + TxPDO Map 1 Element 8 + + 00000000 + + + + TxPDO Map 1 Element 9 + + 00000000 + + + + TxPDO Map 1 Element 10 + + 00000000 + + + + TxPDO Map 1 Element 11 + + 00000000 + + + + TxPDO Map 1 Element 12 + + 00000000 + + + + TxPDO Map 1 Element 13 + + 00000000 + + + + TxPDO Map 1 Element 14 + + 00000000 + + + + TxPDO Map 1 Element 15 + + 00000000 + + + + + + #x1A01 + 2nd transmit PDO-Mapping + DT1A01 + 112 + #x6040 Control word @@ -565,13 +2029,18 @@ - DC - Distributed Clock + DC Sync + DC for synchronization #x300 0 0 0 + + DC Off + DC unused + 0 + diff --git a/hw_device_mgr/devices/device_xml/SV660_EOE_1Axis_V9.12.xml b/hw_device_mgr/devices/device_xml/SV660_EOE_1Axis_V9.12.xml index 631a7da5..648b9f3f 100644 --- a/hw_device_mgr/devices/device_xml/SV660_EOE_1Axis_V9.12.xml +++ b/hw_device_mgr/devices/device_xml/SV660_EOE_1Axis_V9.12.xml @@ -2093,6 +2093,28 @@ rw + + + 6 + Stop mode at S-ON OFF + UINT + 16 + 96 + + rw + + + + + 7 + Stop mode at No. 2 fault + INT + 16 + 112 + + rw + + 8 Stop mode at limit switch signal @@ -2541,6 +2563,17 @@ rw + + + 24 + EtherCAT forced DO output logic in non-OP status + UINT + 16 + 112 + + rw + + DT2005 @@ -2585,6 +2618,28 @@ rw + + + 8 + Numerator of electronic gear ratio + UDINT + 32 + 128 + + rw + + + + + 10 + Denominator of electronic gear ratio + UDINT + 32 + 160 + + rw + + 20 Speed feedforward control selection @@ -4841,6 +4896,17 @@ rw + + + 3 + Offline inertia autotuning selection + UINT + 16 + 48 + + rw + + 5 Encoder ROM reading/writing @@ -6923,6 +6989,24 @@ 0 + + + Stop mode at S-ON OFF + + -3 + 1 + 0 + + + + + Stop mode at No. 2 fault + + -5 + 3 + 2 + + Stop mode at limit switch signal @@ -7295,6 +7379,15 @@ 0 + + + EtherCAT forced DO output logic in non-OP status + + 0 + 7 + 1 + + ro @@ -7337,6 +7430,24 @@ 0 + + + Numerator of electronic gear ratio + + 0 + 4294967295 + 1 + + + + + Denominator of electronic gear ratio + + 0 + 4294967295 + 1 + + Speed feedforward control selection @@ -9059,6 +9170,15 @@ 0 + + + Offline inertia autotuning selection + + 0 + 1 + 0 + + Encoder ROM reading/writing diff --git a/hw_device_mgr/devices/elmo_gold.py b/hw_device_mgr/devices/elmo_gold.py index c70a4bc8..98097442 100644 --- a/hw_device_mgr/devices/elmo_gold.py +++ b/hw_device_mgr/devices/elmo_gold.py @@ -6,6 +6,7 @@ class ElmoGold(EtherCATDevice, CiA402Device): """Base class for Elmo Gold EtherCAT Family Devices.""" vendor_id = 0x0000009A + xml_description_package = "hw_device_mgr.devices.device_xml" # FIXME The original ESI has models that differ only by revision, # but the ESI parser doesn't support this yet # xml_description_fname = "Elmo_ECAT_00010420_V11.xml" diff --git a/hw_device_mgr/devices/inovance_is620n.py b/hw_device_mgr/devices/inovance_is620n.py index 70191481..6e310e05 100644 --- a/hw_device_mgr/devices/inovance_is620n.py +++ b/hw_device_mgr/devices/inovance_is620n.py @@ -7,6 +7,7 @@ class InovanceIS620N(EtherCATDevice, CiA402Device): vendor_id = 0x00100000 product_code = 0x000C0108 + xml_description_package = "hw_device_mgr.devices.device_xml" xml_description_fname = "IS620N_v2.6.7.xml" def set_params_volatile(self, nv=False): diff --git a/hw_device_mgr/devices/inovance_sv660.py b/hw_device_mgr/devices/inovance_sv660.py index 08b06ff5..95089a74 100644 --- a/hw_device_mgr/devices/inovance_sv660.py +++ b/hw_device_mgr/devices/inovance_sv660.py @@ -7,6 +7,7 @@ class InovanceSV660(EtherCATDevice, CiA402Device): vendor_id = 0x00100000 product_code = 0x000C010D + xml_description_package = "hw_device_mgr.devices.device_xml" xml_description_fname = "SV660_EOE_1Axis_V9.12.xml" def set_params_volatile(self, nv=False): diff --git a/hw_device_mgr/devices/itegva_e7x.py b/hw_device_mgr/devices/itegva_e7x.py index d7e5bb2a..f93823d7 100644 --- a/hw_device_mgr/devices/itegva_e7x.py +++ b/hw_device_mgr/devices/itegva_e7x.py @@ -5,6 +5,7 @@ class ITegvaE7xDevice(EtherCATDevice): """Base class for iTegva E7x series IO modules.""" vendor_id = 0x00000A09 + xml_description_package = "hw_device_mgr.devices.device_xml" xml_description_fname = "iTegva_E7x_Series.xml" diff --git a/hw_device_mgr/devices/tests/base_test_class.py b/hw_device_mgr/devices/tests/base_test_class.py index eeba5ddc..e44367ec 100644 --- a/hw_device_mgr/devices/tests/base_test_class.py +++ b/hw_device_mgr/devices/tests/base_test_class.py @@ -21,6 +21,7 @@ class BaseDevicesTestClass(BaseLCECTestClass): EVEXCREForTest, ) - device_config_yaml = "devices/tests/device_config.yaml" - sim_device_data_yaml = "devices/tests/sim_devices.yaml" - device_sdos_yaml = "devices/tests/sim_sdo_data.yaml" + device_config_package = "hw_device_mgr.devices.tests" + sim_device_data_package = "hw_device_mgr.devices.tests" + device_sdos_package = "hw_device_mgr.devices.tests" + device_dcs_package = "hw_device_mgr.devices.tests" diff --git a/hw_device_mgr/devices/tests/dcs_data.yaml b/hw_device_mgr/devices/tests/dcs_data.yaml new file mode 100644 index 00000000..5f90f2c7 --- /dev/null +++ b/hw_device_mgr/devices/tests/dcs_data.yaml @@ -0,0 +1,47 @@ +elmo_gold_420_test: + - Name: DC Sync + Desc: DC for synchronization + AssignActivate: 0x300 + CycleTimeSync0: 0 + ShiftTimeSync0: 0 + CycleTimeSync1: 0 + - Name: DC Off + Desc: DC unused + AssignActivate: 0x0 +elmo_gold_520_test: + - Name: DC Sync + Desc: DC for synchronization + AssignActivate: 0x300 + CycleTimeSync0: 0 + ShiftTimeSync0: 0 + CycleTimeSync1: 0 + - Name: DC Off + Desc: DC unused + AssignActivate: 0x0 +inovance_is620n_test: + - Name: DC + Desc: DC-Synchron + AssignActivate: 0x300 + CycleTimeSync0: 0 + ShiftTimeSync0: 0 + CycleTimeSync1: 0 +inovance_sv660n_test: + - Name: DC + Desc: DC-Synchron + AssignActivate: 0x300 + CycleTimeSync0: 0 + ShiftTimeSync0: 0 + CycleTimeSync1: 0 +everest_xcr_e_test: + - Name: Synchron + Desc: SM-Synchron + AssignActivate: 0x0 + CycleTimeSync0: 0 + ShiftTimeSync0: 0 + CycleTimeSync1: 0 + - Name: DCSync + Desc: DC-Synchron + AssignActivate: 0x300 + CycleTimeSync0: 0 + ShiftTimeSync0: 0 + CycleTimeSync1: 0 diff --git a/hw_device_mgr/devices/tests/device_xml b/hw_device_mgr/devices/tests/device_xml deleted file mode 120000 index c58533e3..00000000 --- a/hw_device_mgr/devices/tests/device_xml +++ /dev/null @@ -1 +0,0 @@ -../device_xml \ No newline at end of file diff --git a/hw_device_mgr/errors/device.py b/hw_device_mgr/errors/device.py index 0dcfc544..69918069 100644 --- a/hw_device_mgr/errors/device.py +++ b/hw_device_mgr/errors/device.py @@ -1,9 +1,9 @@ from ..device import Device, SimDevice from ..data_types import DataType -import ruamel.yaml +from ..config_io import ConfigIO -class ErrorDevice(Device): +class ErrorDevice(Device, ConfigIO): """ Abstract class representing a device error code handling. @@ -14,57 +14,63 @@ class ErrorDevice(Device): strings to feedback. """ - device_error_dir = "device_err" + device_error_package = None + device_error_yaml = None - feedback_data_types = dict(error_code="uint32") - feedback_defaults = dict( + feedback_in_data_types = dict(error_code="uint32") + feedback_in_defaults = dict(error_code=0) + + feedback_out_defaults = dict( error_code=0, description="No error", advice="No error" ) - no_error = feedback_defaults + + no_error = feedback_out_defaults data_type_class = DataType _error_descriptions = dict() - @classmethod - def error_descriptions_yaml(cls): - return cls.pkg_path(cls.device_error_dir) / f"{cls.name}.yaml" - @classmethod def error_descriptions(cls): """ Return dictionary of error code data. - Data is read from YAML file `{device_error_dir}/{name}.yaml` and - cached. + Data is read from YAML resource from package + `device_error_package`, name `device_error_yaml`. """ if cls.name not in cls._error_descriptions: errs = cls._error_descriptions[cls.name] = dict() - path = cls.error_descriptions_yaml() - if path.exists(): - yaml = ruamel.yaml.YAML() - with path.open() as f: - err_yaml = yaml.load(f) + if cls.device_error_yaml: + err_yaml = cls.load_yaml_resource( + cls.device_error_package, cls.device_error_yaml + ) for err_code_str, err_data in err_yaml.items(): errs[int(err_code_str, 0)] = err_data return cls._error_descriptions[cls.name] - def set_feedback(self, error_code=0, **kwargs): - super().set_feedback(**kwargs) + def get_feedback(self): + fb_out = super().get_feedback() + error_code = self.feedback_in.get("error_code") if not error_code: - self.feedback.update(**self.no_error) - return + self.feedback_out.update(**self.no_error) + return fb_out error_info = self.error_descriptions().get(error_code, None) if error_info is None: - self.feedback.update( + fb_out.update( description=f"Unknown error code {error_code}", advice="Please consult with hardware vendor", error_code=error_code, ) - return - - self.feedback.update(error_code=error_code, **error_info) + return fb_out + else: + fb_out.update(error_code=error_code, **error_info) + if fb_out.changed("error_code"): + msg = "error code {}: {}".format( + error_code, fb_out.get("description") + ) + self.logger.error(msg) + return fb_out class ErrorSimDevice(ErrorDevice, SimDevice): diff --git a/hw_device_mgr/errors/tests/bogus_devices/device.py b/hw_device_mgr/errors/tests/bogus_devices/device.py index 59432342..1380e477 100644 --- a/hw_device_mgr/errors/tests/bogus_devices/device.py +++ b/hw_device_mgr/errors/tests/bogus_devices/device.py @@ -3,6 +3,7 @@ class BogusErrorDevice(ErrorSimDevice): category = "bogus_error_devices" + device_error_package = "hw_device_mgr.errors.tests.bogus_devices.device_err" @classmethod def scan_devices(cls, **kwargs): @@ -13,15 +14,18 @@ class BogusErrorV1Servo(BogusErrorDevice): name = "bogus_v1_error_servo" test_category = "bogus_v1_servo" model_id = 0xB0905041 + device_error_yaml = "bogus_v1_v2_error_servo.yaml" class BogusErrorV2Servo(BogusErrorDevice): name = "bogus_v2_error_servo" test_category = "bogus_v2_servo" model_id = 0xB0905042 + device_error_yaml = "bogus_v1_v2_error_servo.yaml" class BogusErrorV1IO(BogusErrorDevice): name = "bogus_v1_error_io" test_category = "bogus_v1_io" model_id = 0xB0901041 + device_error_yaml = "bogus_v1_error_io.yaml" diff --git a/hw_device_mgr/errors/tests/bogus_devices/device_err/__init__.py b/hw_device_mgr/errors/tests/bogus_devices/device_err/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/hw_device_mgr/errors/tests/bogus_devices/device_err/bogus_v1_error_servo.yaml b/hw_device_mgr/errors/tests/bogus_devices/device_err/bogus_v1_v2_error_servo.yaml similarity index 100% rename from hw_device_mgr/errors/tests/bogus_devices/device_err/bogus_v1_error_servo.yaml rename to hw_device_mgr/errors/tests/bogus_devices/device_err/bogus_v1_v2_error_servo.yaml diff --git a/hw_device_mgr/errors/tests/bogus_devices/device_err/bogus_v2_error_servo.yaml b/hw_device_mgr/errors/tests/bogus_devices/device_err/bogus_v2_error_servo.yaml deleted file mode 120000 index e9045116..00000000 --- a/hw_device_mgr/errors/tests/bogus_devices/device_err/bogus_v2_error_servo.yaml +++ /dev/null @@ -1 +0,0 @@ -bogus_v1_error_servo.yaml \ No newline at end of file diff --git a/hw_device_mgr/errors/tests/test_device.py b/hw_device_mgr/errors/tests/test_device.py index 98e106da..d710e7ce 100644 --- a/hw_device_mgr/errors/tests/test_device.py +++ b/hw_device_mgr/errors/tests/test_device.py @@ -8,6 +8,7 @@ class TestErrorDevice(ErrorBaseTestClass, _TestDevice): "ErrorSimDevice", "ErrorDevice", *_TestDevice.expected_mro, + "ConfigIO", ] @pytest.fixture @@ -19,7 +20,7 @@ def obj(self): def test_error_descriptions(self): for cls in self.device_model_classes: print("cls:", cls) - print("yaml:", cls.error_descriptions_yaml()) + print("yaml:", cls.device_error_package, cls.device_error_yaml) errs = cls.error_descriptions() assert isinstance(errs, dict) assert len(errs) > 0 diff --git a/hw_device_mgr/ethercat/config.py b/hw_device_mgr/ethercat/config.py index df5d1898..e32e4ffd 100644 --- a/hw_device_mgr/ethercat/config.py +++ b/hw_device_mgr/ethercat/config.py @@ -3,6 +3,7 @@ from .data_types import EtherCATDataType from .xml_reader import EtherCATXMLReader from .command import EtherCATCommand, EtherCATSimCommand +from functools import lru_cache class EtherCATConfig(CiA301Config): @@ -23,14 +24,50 @@ class EtherCATConfig(CiA301Config): command_class = EtherCATCommand # - # Object dictionary + # Device ESI # @classmethod - def get_device_sdos_from_esi(cls, esi_path): - """Read in device configuration from ESI file at `esi_path`.""" - esi_reader = cls.esi_reader_class() - return esi_reader.add_device_descriptions(esi_path) + @lru_cache + def read_esi(cls, package, fname, LcId="1033"): + """ + Read ESI XML and return `EtherCATXMLReader` object. + + ESI may be a package resource from `package` and `fname`; + otherwise, if `package` is `None`, a file from path `fname`. + """ + esi_reader_class = cls.esi_reader_class + if package: + esi_reader = esi_reader_class.read_from_resource( + package, fname, LcId=LcId + ) + else: + esi_reader = esi_reader_class.read_from_path(fname, LcId=LcId) + return esi_reader + + @classmethod + @lru_cache + def get_device_sdos_from_esi(cls, package, fname, LcId="1033"): + """ + Read in device SDOs from ESI. + + The `package` and `fname` args are supplied to the `read_esi` + method. + """ + esi_reader = cls.read_esi(package, fname, LcId=LcId) + return esi_reader.parse_sdos() + + @classmethod + @lru_cache + def get_device_dcs_from_esi(cls, package, fname, LcId="1033"): + """ + Read in device distributed clocks from ESI. + + The `package` and `fname` args are supplied to the `read_esi` + method. + """ + esi_reader = cls.read_esi(package, fname, LcId=LcId) + return esi_reader.parse_dc_opmodes() class EtherCATSimConfig(EtherCATConfig, CiA301SimConfig): diff --git a/hw_device_mgr/ethercat/data_types.py b/hw_device_mgr/ethercat/data_types.py index 6ba0fd56..cf2afac4 100644 --- a/hw_device_mgr/ethercat/data_types.py +++ b/hw_device_mgr/ethercat/data_types.py @@ -14,7 +14,7 @@ class EtherCATDataType(DataType): int16=dict(name="INT"), int32=dict(name="DINT"), int64=dict(name="LINT"), - uint8=dict(name="USINT"), + uint8=dict(name="USINT", name_re=r"USINT|BITARR8"), uint16=dict(name="UINT"), uint32=dict(name="UDINT"), uint64=dict(name="ULINT"), diff --git a/hw_device_mgr/ethercat/device.py b/hw_device_mgr/ethercat/device.py index c9f26fe0..7bc70246 100644 --- a/hw_device_mgr/ethercat/device.py +++ b/hw_device_mgr/ethercat/device.py @@ -18,16 +18,18 @@ class EtherCATDevice(CiA301Device, abc.ABC): # Resource names for locating device description XML and error files device_xml_dir = "device_xml" - # Filename of XML description + # Package and filename of XML description resource + xml_description_package = None xml_description_fname = None # Swappable utility classes data_type_class = EtherCATDataType config_class = EtherCATConfig - def __init__(self, **kwargs): + def __init__(self, LcId="1033", **kwargs): super().__init__(**kwargs) - self.add_device_sdos_from_esi() + self.add_device_sdos_from_esi(LcId=LcId) + self.add_device_dcs_from_esi(LcId=LcId) @property def master(self): @@ -46,34 +48,22 @@ def set_params_volatile(self, nv=False): """ @classmethod - def xml_description_path(cls): - """ - Return path to device ESI file. - - Path is under the module directory, - `{device_xml_dir}/{xml_description_fname}`. - """ - path = cls.pkg_path(cls.device_xml_dir) / cls.xml_description_fname - return path.resolve() - - @classmethod - def read_device_sdos_from_esi(cls): + def read_device_sdos_from_esi(cls, LcId="1033"): sdo_data = dict() - dev_esi_paths = set() for dev in cls.get_model(): - esi_path = dev.xml_description_path() - if esi_path in dev_esi_paths: - assert dev.device_model_id() in sdo_data - continue - dev_esi_paths.add(esi_path) - dev_sdo_data = dev.config_class.get_device_sdos_from_esi(esi_path) + conf = dev.config_class + dev_sdo_data = conf.get_device_sdos_from_esi( + dev.xml_description_package, + dev.xml_description_fname, + LcId=LcId, + ) sdo_data.update(dev_sdo_data) return sdo_data @classmethod - def add_device_sdos_from_esi(cls): + def add_device_sdos_from_esi(cls, LcId="1033"): """Read device SDOs from ESI file and add to configuration.""" - sdo_data = cls.read_device_sdos_from_esi() + sdo_data = cls.read_device_sdos_from_esi(LcId=LcId) cls.add_device_sdos(sdo_data) @classmethod @@ -81,6 +71,25 @@ def munge_sdo_data(cls, sdo_data): # SDO data from ESI parser already in correct format return sdo_data + @classmethod + def read_device_dcs_from_esi(cls, LcId="1033"): + dcs_data = dict() + for dev in cls.get_model(): + conf = dev.config_class + dev_dcs_data = conf.get_device_dcs_from_esi( + dev.xml_description_package, + dev.xml_description_fname, + LcId=LcId, + ) + dcs_data.update(dev_dcs_data) + return dcs_data + + @classmethod + def add_device_dcs_from_esi(cls, LcId="1033"): + """Read device DCs from ESI file and add to configuration.""" + dcs_data = cls.read_device_dcs_from_esi(LcId=LcId) + cls.add_device_dcs(dcs_data) + class EtherCATSimDevice(EtherCATDevice, CiA301SimDevice): config_class = EtherCATSimConfig @@ -93,7 +102,7 @@ def set_params_volatile(self, nv=False): self.params_volatile = not nv @classmethod - def init_sim(cls, **kwargs): + def init_sim(cls, LcId="1033", **kwargs): """ Configure device, config, command for sim EtherCAT devices. @@ -101,5 +110,6 @@ def init_sim(cls, **kwargs): from EtherCAT ESI description file and pass with sim device data to parent class's method. """ - sdo_data = cls.read_device_sdos_from_esi() - super().init_sim(sdo_data=sdo_data, **kwargs) + sdo_data = cls.read_device_sdos_from_esi(LcId=LcId) + dcs_data = cls.read_device_dcs_from_esi(LcId=LcId) + super().init_sim(sdo_data=sdo_data, dcs_data=dcs_data, **kwargs) diff --git a/hw_device_mgr/ethercat/esi_base_types.xml b/hw_device_mgr/ethercat/esi_base_types.xml new file mode 100644 index 00000000..fb6dfc82 --- /dev/null +++ b/hw_device_mgr/ethercat/esi_base_types.xml @@ -0,0 +1,46 @@ + + + BOOL + 1 + + + SINT + 8 + + + BYTE + 8 + + + USINT + 8 + + + INT + 16 + + + UINT + 16 + + + DINT + 32 + + + UDINT + 32 + + + LINT + 64 + + + ULINT + 64 + + + REAL + 32 + + diff --git a/hw_device_mgr/ethercat/tests/base_test_class.py b/hw_device_mgr/ethercat/tests/base_test_class.py index 2e18ff84..c7a07dbd 100644 --- a/hw_device_mgr/ethercat/tests/base_test_class.py +++ b/hw_device_mgr/ethercat/tests/base_test_class.py @@ -8,7 +8,6 @@ BogusEtherCATServo, BogusOtherCATServo, BogusEtherCATIO, - RelocatableESIDevice, ) import re import pytest @@ -37,7 +36,7 @@ class BaseEtherCATTestClass(BaseCiA402TestClass): ) ] ) - pass_init_sim_device_sdos = False # SDO data from ESI file + pass_init_sim_device_description = False # Data from ESI file @property def model_id_clone_map(self): @@ -45,38 +44,38 @@ def model_id_clone_map(self): @pytest.fixture def device_xml(self, tmp_path): - if not issubclass(self.device_class, RelocatableESIDevice): - # Don't rewrite ESI files - yield - else: - # Subclasses will have different product_code, so customize ESI file - self.device_class.set_device_xml_dir(tmp_path) - finished_paths = set() - re_str = "|".join(rf"{pc[1]:08X}" for pc in self.sdo_model_id_clone) - re_str = r"#x(" + re_str + r")" - pat = re.compile(re_str) - # Map of orig ESI file product code to new ESI file product code - cm = { - k[1]: v.device_model_id()[1] - for k, v in self.model_id_clone_map.items() - } - for id_orig, cls in self.model_id_clone_map.items(): - esi_orig = cls.orig_xml_description_path() - esi_new = cls.xml_description_path() - if esi_orig in finished_paths: - continue - finished_paths.add(esi_orig) - esi_new.parent.mkdir(exist_ok=True) - with open(esi_orig) as f_orig: - with open(esi_new, "w") as f_new: - for line in f_orig: - line = pat.sub( - lambda m: f"#x{cm[int(m.group(1), 16)]}", line - ) - f_new.write(line) - print(f"Wrote ESI file to {esi_new}") - print(f" Original in {esi_orig}") - yield + # Subclasses will have different product_code, so customize ESI file + finished = set() + re_str = "|".join(rf"{pc[1]:08X}" for pc in self.sdo_model_id_clone) + re_str = r"#x(" + re_str + r")" + pat = re.compile(re_str) + # Map of orig ESI file product code to new ESI file product code + cm = { + k[1]: f"{v.device_model_id()[1]:08X}" + for k, v in self.model_id_clone_map.items() + } + for id_orig, cls in self.model_id_clone_map.items(): + if not hasattr(cls, "alt_xml_description"): + print(f"Using original ESI file for device {cls.name}") + continue + esi_orig = (cls.xml_description_package, cls.xml_description_fname) + print(f"Model {cls.name} ESI resource: {esi_orig}") + esi_new = tmp_path / cls.xml_description_fname + cls.alt_xml_description = esi_new + if esi_new in finished: + print(f" Already written to {esi_new}") + continue # Only process each ESI file once + finished.add(esi_new) + print(f" Writing to {esi_new}") + with self.open_resource(*esi_orig) as f_orig: + with self.open_path(esi_new, "w") as f_new: + for line in f_orig: + line = line.decode() + line = pat.sub( + lambda m: f"#x{cm[int(m.group(1), 16)]}", line + ) + f_new.write(line) + yield @pytest.fixture def extra_fixtures(self, device_xml): diff --git a/hw_device_mgr/ethercat/tests/bogus_devices/device.py b/hw_device_mgr/ethercat/tests/bogus_devices/device.py index 3ba32537..c8bfbfa7 100644 --- a/hw_device_mgr/ethercat/tests/bogus_devices/device.py +++ b/hw_device_mgr/ethercat/tests/bogus_devices/device.py @@ -5,6 +5,7 @@ class BogusEtherCATDevice(RelocatableESIDevice): category = "bogus_ethercat_devices" vendor_id = 0xB090C0 + xml_description_package = "hw_device_mgr.devices.device_xml" class BogusEtherCATServo(BogusEtherCATDevice, CiA402SimDevice): diff --git a/hw_device_mgr/ethercat/tests/bogus_devices/device_xml/BogusIO.xml b/hw_device_mgr/ethercat/tests/bogus_devices/device_xml/BogusIO.xml deleted file mode 120000 index f20a1a22..00000000 --- a/hw_device_mgr/ethercat/tests/bogus_devices/device_xml/BogusIO.xml +++ /dev/null @@ -1 +0,0 @@ -../../../../devices/device_xml/BogusIO.xml \ No newline at end of file diff --git a/hw_device_mgr/ethercat/tests/bogus_devices/device_xml/BogusServo.xml b/hw_device_mgr/ethercat/tests/bogus_devices/device_xml/BogusServo.xml deleted file mode 120000 index 3c31ec11..00000000 --- a/hw_device_mgr/ethercat/tests/bogus_devices/device_xml/BogusServo.xml +++ /dev/null @@ -1 +0,0 @@ -../../../../devices/device_xml/BogusServo.xml \ No newline at end of file diff --git a/hw_device_mgr/ethercat/tests/relocatable_esi_device.py b/hw_device_mgr/ethercat/tests/relocatable_esi_device.py index 3f2ca2ab..816ce635 100644 --- a/hw_device_mgr/ethercat/tests/relocatable_esi_device.py +++ b/hw_device_mgr/ethercat/tests/relocatable_esi_device.py @@ -4,18 +4,26 @@ class RelocatableESIDevice(EtherCATSimDevice): """A class whose ESI description file can be moved (for tests).""" - @classmethod - def set_device_xml_dir(cls, path): - # Tests generate customized ESI file in temp directory; provide a hook - # to point fixtures to it - cls.xml_base_dir = path + alt_xml_description = None @classmethod - def xml_description_path(cls): - if not hasattr(cls, "xml_base_dir"): - return super().xml_description_path() - return cls.xml_base_dir / cls.device_xml_dir / cls.xml_description_fname + def read_device_sdos_from_esi(cls, LcId="1033"): + sdo_data = dict() + for dev in cls.get_model(): + conf = dev.config_class + dev_sdo_data = conf.get_device_sdos_from_esi( + None, dev.alt_xml_description, LcId=LcId + ) + sdo_data.update(dev_sdo_data) + return sdo_data @classmethod - def orig_xml_description_path(cls): - return super().xml_description_path() + def read_device_dcs_from_esi(cls, LcId="1033"): + dcs_data = dict() + for dev in cls.get_model(): + conf = dev.config_class + dev_dcs_data = conf.get_device_dcs_from_esi( + None, dev.alt_xml_description, LcId=LcId + ) + dcs_data.update(dev_dcs_data) + return dcs_data diff --git a/hw_device_mgr/ethercat/tests/test_device.py b/hw_device_mgr/ethercat/tests/test_device.py index 479b950a..3692bfee 100644 --- a/hw_device_mgr/ethercat/tests/test_device.py +++ b/hw_device_mgr/ethercat/tests/test_device.py @@ -13,6 +13,8 @@ class TestEtherCATDevice(BaseEtherCATTestClass, _TestCiA402Device): def test_xml_description_path(self): for cls in self.device_model_classes: - esi_path = cls.xml_description_path() - print(esi_path) - assert esi_path.exists() + assert cls.xml_description_fname + if cls.xml_description_package is None: + assert "/" in cls.xml_description_fname + else: + assert "/" not in cls.xml_description_fname diff --git a/hw_device_mgr/ethercat/xml_reader.py b/hw_device_mgr/ethercat/xml_reader.py index 051734d4..2f5b3882 100644 --- a/hw_device_mgr/ethercat/xml_reader.py +++ b/hw_device_mgr/ethercat/xml_reader.py @@ -1,16 +1,23 @@ from .sdo import EtherCATSDO +from ..config_io import ConfigIO +from ..logging import Logging from lxml import etree from pprint import pprint +from functools import lru_cache __all__ = ("EtherCATXMLReader",) -class EtherCATXMLReader: +class EtherCATXMLReader(ConfigIO): """Parse EtherCAT Slave Information "ESI" XML files.""" sdo_class = EtherCATSDO _device_registry = dict() - _fpath_registry = dict() + + logger = Logging.getLogger(__name__) + + default_datatypes_package = "hw_device_mgr.ethercat" + default_datatypes_resource = "esi_base_types.xml" @classmethod def str_to_int(cls, s): @@ -19,12 +26,18 @@ def str_to_int(cls, s): else: return int(s, 10) + @classmethod + def uint(cls, num, numbits=16): + dtc = cls.sdo_class.data_type_class + return getattr(dtc, f"uint{numbits}")(num) + @property def data_type_class(self): return self.sdo_class.data_type_class - def __init__(self, LcId="1033"): + def __init__(self, tree, LcId="1033"): """Init object with locale ID.""" + self.tree = tree self.LcId = LcId # Discard anything not in this locale def safe_set(self, dst, key, val, prefix=None): @@ -107,7 +120,7 @@ def read_object(self, obj, subindex=None): 'Key "%s" value "%s" should start with "#x"' % (key, subobj.text) ) - res[key] = self.str_to_int(subobj.text) + res[key] = self.uint(self.str_to_int(subobj.text)) elif key in {"MinValue", "MaxValue", "DefaultValue"}: # e.g. -32767, 00 (?!?), #x0001 t = subobj.text @@ -129,6 +142,10 @@ def read_object(self, obj, subindex=None): res[key] = subobj.text.rstrip() elif key in {"BitSize", "BitOffs", "SubIdx", "LBound", "Elements"}: res[key] = int(subobj.text) + elif key in {"SubIndex", "BitLen"}: # RxPdo, TxPdo + res[key] = int(subobj.text) + elif key in {"DataType"} and len(subobj) == 0: # RxPdo, TxPdo + res[key] = subobj.text elif key in {"Backup", "Setting"}: res[key] = int(subobj.text) elif key in {"Info", "Flags", "ArrayInfo"}: @@ -167,15 +184,13 @@ def vendor_xml(self): # vendors = self.tree.xpath("/EtherCATInfo/Vendor") if len(vendors) != 1: - raise RuntimeError( - f"{len(vendors)} sections in {self.fpath}" - ) + raise RuntimeError(f"{len(vendors)} sections in XML") return vendors[0] @property def vendor_id(self): id_str = self.vendor_xml.xpath("Id")[0].text # Should only ever be one - return self.data_type_class.uint16(self.str_to_int(id_str)) + return self.uint(self.str_to_int(id_str)) @property def devices_xml(self): @@ -264,7 +279,7 @@ def expand_subitems(self, subitems): expanded_subitems.append(new_subitem) return expanded_subitems - def massage_type(self, otype): + def massage_type(self, otype, **add_keys): """Parse a `DataType` object.""" if "SubItems" in otype: otype["OldSubItems"] = old_subitems = otype.pop("SubItems") @@ -273,6 +288,10 @@ def massage_type(self, otype): key = otype["TypeName"] if key in self.datatypes: # Sanity raise RuntimeError("Duplicate datatype '%s'" % key) + if "Name" in otype: + raise RuntimeError('Found "Name" attr in type') + if add_keys: + self.safe_update(otype, add_keys) self.datatypes[key] = otype def read_datatypes(self, device): @@ -288,9 +307,16 @@ def read_datatypes(self, device): "Profile/Dictionary/DataTypes/DataType[SubItem]" ): self.massage_type(self.read_object(dt)) - for i in self.datatypes.values(): - if "Name" in i: - raise RuntimeError('Found "Name" attr in type') + + def read_default_datatypes(self): + """Read default datatypes for ESI files without them.""" + rsrc = (self.default_datatypes_package, self.default_datatypes_resource) + with self.open_resource(*rsrc) as f: + tree = etree.parse(f) + dts = tree.xpath("/DataTypes/DataType") + assert len(dts), f"Unable to parse {rsrc}" + for dt in tree.xpath("/DataTypes/DataType"): + self.massage_type(self.read_object(dt), from_defaults=True) @classmethod def is_base_type(cls, name): @@ -307,21 +333,19 @@ def type_data_list(self, o): otype = self.datatypes[type_name].copy() # Manipulated below otypes = [] for i in range(len(otype.get("SubItems", range(1)))): - otypes.append(self.type_data(o, i)) + otypes.append(self.type_data(type_name, i)) return otypes - def type_data(self, o, type_idx): + def type_data(self, type_name, type_idx=0): """ Return type data for an object. Include `SubItem` objects if present. """ - type_name = o["Type"] otype = self.datatypes[type_name].copy() # Manipulated below if "SubItems" in otype: subitems = otype.pop("SubItems") if type_idx >= len(subitems): - pprint(o) pprint(otype) pprint(subitems) raise RuntimeError( @@ -380,10 +404,12 @@ def read_objects(self, device): Populate type data. """ - sdos = list() + sdos = dict() # Read data types first self.read_datatypes(device) + if not self.datatypes: + self.read_default_datatypes() # Build object dictionary for obj in device.xpath("Profile/Dictionary/Objects/Object"): @@ -412,7 +438,7 @@ def read_objects(self, device): ecat_type = self.data_type_class.by_name(type_name) except KeyError as e: print(self.data_type_class._name_re_registry) - raise KeyError(f"Reading {self.fpath}: {str(e)}") + raise KeyError(f"Reading XML: {str(e)}") self.safe_set(osub, "DataType", ecat_type) # Flatten out Flags, Info @@ -451,19 +477,76 @@ def read_objects(self, device): osub.pop(a, None) # Add to objects dict - # subindex = osub.setdefault("SubIdx", 0) - # self.print_shit(index, subindex, ecat_type, osub) - sdos.append(osub) + ix = ( + self.uint(osub["Index"]), + self.uint(osub.get("SubIdx", 0), 8), + ) + assert ix not in sdos, f"Duplicate SDO {ix}: {osub}" + sdos[ix] = osub # print(f"Unused: {list(self._unused.keys())}") return sdos - @property - def tree(self): - if hasattr(self, "_tree"): - return self._tree - with self.fpath.open() as f: - self._tree = etree.parse(f) - return self._tree + def munge_pdo_entry(self, pdo_entry, pdo_type): + # Munge an RxPdo/TxPdo Entry to be treated as SDO + o = self.read_object(pdo_entry) + dtc = self.data_type_class + # Munge field names & values + o["Index"] = self.uint(o["Index"]) + if o["Index"] == 0x00: + return o # Some Elmo ESI [RT]xPdo final entry is zero + o["SubIdx"] = self.uint(o.get("SubIndex", 0x00), 8) + o["Type"] = o.pop("DataType") + o["DataType"] = dtc.by_name(o["Type"]) + if "BitLen" not in o: + pprint(o) + raise KeyError("No 'BitLen' subelement in PDO") + o["BitSize"] = o.pop("BitLen") + # Add implicit fields + o["Access"] = "rw" if pdo_type == "RxPdo" else "ro" + o["PdoMapping"] = "R" if pdo_type == "RxPdo" else "T" + o["from_pdo"] = pdo_type + return o + + def munge_pdo(self, obj, pdo_type): + o = dict( + SubIdx=0x00, + Type="USINT", + DataType=self.data_type_class.uint8, + BitSize="8", + Access="ro", + Name="SubIndex 000", + ) + for subobj in obj: + if subobj.tag == "Index": + o["Index"] = self.uint(self.str_to_int(subobj.text)) + elif subobj.tag == "Name": + o["IndexName"] = subobj.text.rstrip() + elif subobj.tag in {"Entry", "Exclude"}: + pass + else: + raise RuntimeError(f"Unknown {pdo_type} tag {subobj.tag}") + return o + + def read_fixed_pdo_entries(self, device, sdo_data): + pdos = dict() + for pdo_type in ("RxPdo", "TxPdo"): + # Parse RxPdo & TxPdo elements + for obj in device.xpath(f"{pdo_type}"): + data = self.munge_pdo(obj, pdo_type) + ix = (data.get("Index"), data.get("SubIndex", 0)) + assert ix not in pdos, f"Duplicate PDO mapping: {data}" + if ix not in sdo_data: + pdos[ix] = data + # Parse RxPdo & TxPdo elements Entry child elements + for obj in device.xpath(f"{pdo_type}[@Fixed='1']/Entry"): + data = self.munge_pdo_entry(obj, pdo_type) + ix = (data.get("Index"), data.get("SubIndex", 0)) + if ix[0] == 0x00: + continue # Some Elmo ESI [RT]xPdo final entry is zero + assert ix not in pdos, f"Duplicate PDO entry: {data}" + if ix not in sdo_data: + pdos[ix] = data + return pdos sdo_translations = dict( # Translate SDO data from read_objects() to SDOs.add_sdo() args @@ -487,32 +570,91 @@ def add_sdo(self, sdos, data): sdo[key_dst] = data.get(key_src, None) sdo["ro"] = sdo.pop("access", "ro") == "ro" sdo["data_type"] = sdo["data_type"].shared_name - dtc = self.data_type_class - idx = sdo["index"] = dtc.uint16(sdo.pop("index")) - subidx = sdo["subindex"] = dtc.uint8(sdo.pop("subindex") or 0) + idx = sdo["index"] = self.uint(sdo.pop("index")) + subidx = sdo["subindex"] = self.uint(sdo.pop("subindex") or 0, 8) + assert (idx, subidx) not in sdos sdos[idx, subidx] = sdo - def add_device_descriptions(self, fpath): - """Parse ESI file and cache device information.""" - if fpath in self._fpath_registry: - return self._fpath_registry[fpath] + # DC definitions + # + # + # + # DC + # DC-Synchron + # #x300 + # 0 + # 0 + # 0 + # + # + + def read_dc_opmodes(self, device): + """Parse XML `` tags into simple Python object.""" + opmodes = list() + for obj in device.xpath("Dc/OpMode"): + opmode = dict() + for subobj in obj: + key = subobj.tag + if key in { + "AssignActivate", + "CycleTimeSync0", + "ShiftTimeSync0", + "CycleTimeSync1", + }: + opmode[key] = self.str_to_int(subobj.text) + else: + opmode[key] = subobj.text + opmodes.append(opmode) + return opmodes + + def device_model_id(self, device_xml): + device_type = self.read_device_type(device_xml) + product_code = self.str_to_int(device_type.get("ProductCode")) + model_id = tuple( + self.uint(i, 32) for i in (self.vendor_id, product_code) + ) + return model_id - self.fpath = fpath + @lru_cache + def parse_sdos(self): + """Parse device SDO info from ESI XML.""" model_sdos = dict() for dxml in self.devices_xml: - sdos = dict() - device_type = self.read_device_type(dxml) - product_code = self.str_to_int(device_type.get("ProductCode")) - model_id = (self.vendor_id, product_code) - model_id = tuple(self.data_type_class.uint32(i) for i in model_id) - self._device_registry[model_id] = sdos - model_sdos[model_id] = sdos + model_id = self.device_model_id(dxml) + reg = self._device_registry.setdefault(model_id, dict()) + model_sdos[model_id] = reg["sdos"] = dict() sdo_data = self.read_objects(dxml) - for sd in sdo_data: - self.add_sdo(sdos, sd) - self._fpath_registry[fpath] = model_sdos + for sd in sdo_data.values(): + self.add_sdo(reg["sdos"], sd) + pdo_data = self.read_fixed_pdo_entries(dxml, sdo_data) + for pd in pdo_data.values(): + self.add_sdo(reg["sdos"], pd) + return model_sdos + @lru_cache + def parse_dc_opmodes(self): + """Parse device DC OpModes info from ESI XML.""" + dc_opmodes = dict() + for dxml in self.devices_xml: + model_id = self.device_model_id(dxml) + opmodes = self.read_dc_opmodes(dxml) + assert isinstance(opmodes, list) + dc_opmodes[model_id] = opmodes + return dc_opmodes + + @classmethod + @lru_cache + def read_from_resource(cls, package, resource, LcId="1033"): + cls.logger.info(f"Reading ESI from ({package}, {resource})") + with cls.open_resource(package, resource) as f: + tree = etree.parse(f) + return cls(tree, LcId=LcId) + @classmethod - def sdos(cls, model_id): - return cls._device_registry[model_id] + @lru_cache + def read_from_path(cls, fpath, LcId="1033"): + print(f"Reading ESI from {fpath}") + with cls.open_path(fpath) as f: + tree = etree.parse(f) + return cls(tree, LcId=LcId) diff --git a/hw_device_mgr/hal/data_types.py b/hw_device_mgr/hal/data_types.py index cb56dd49..a89cd4b5 100644 --- a/hw_device_mgr/hal/data_types.py +++ b/hw_device_mgr/hal/data_types.py @@ -11,15 +11,22 @@ class HALDataType(DataType, HALMixin): int8=dict(hal_type=HALMixin.HAL_S32), int16=dict(hal_type=HALMixin.HAL_S32), int32=dict(hal_type=HALMixin.HAL_S32), - int64=dict(hal_type=HALMixin.HAL_S64), uint8=dict(hal_type=HALMixin.HAL_U32), uint16=dict(hal_type=HALMixin.HAL_U32), uint32=dict(hal_type=HALMixin.HAL_U32), - uint64=dict(hal_type=HALMixin.HAL_U64), float=dict(hal_type=HALMixin.HAL_FLOAT), double=dict(hal_type=HALMixin.HAL_FLOAT), # No HAL_STR type ) + have_64 = hasattr(HALMixin, "HAL_S64") + if have_64: + # Machinekit HAL has 64-bit int types, but not LCNC + subtype_data.update( + dict( + int64=dict(hal_type=HALMixin.HAL_S64), + uint64=dict(hal_type=HALMixin.HAL_U64), + ) + ) @classmethod def hal_type_str(cls): diff --git a/hw_device_mgr/hal/device.py b/hw_device_mgr/hal/device.py index f2be2dc7..6de2a154 100644 --- a/hw_device_mgr/hal/device.py +++ b/hw_device_mgr/hal/device.py @@ -1,6 +1,7 @@ from ..device import Device, SimDevice from .base import HALMixin from .data_types import HALDataType +from functools import cached_property class HALPinDevice(Device, HALMixin): @@ -14,26 +15,31 @@ class HALPinDevice(Device, HALMixin): command_out=(HALMixin.HAL_OUT, ""), ) - # Prepend this to HAL pin names - dev_pin_prefix = "d" - - @property + @cached_property def compname(self): return self.comp.getprefix() def pin_name(self, interface, pname): return self.pin_prefix + self.pin_interfaces[interface][1] + pname - def init(self, *, comp, **kwargs): + @cached_property + def pin_prefix(self): + """ + HAL pin prefix for this device. + + Pin prefix is computed by separating numeric components of the + device `address` string with `.` and adding a final `.`, e.g. + `(0,5)` -> `0.5.`. + """ + return f"{self.addr_slug}{self.slug_separator}" + + def init(self, /, comp, **kwargs): super().init(**kwargs) self.comp = comp # Get specs for all pins in all interfaces; shared pin names must match, # except for direction, which becomes HAL_IO if different all_specs = dict() - self.pin_prefix = ( - "" if self.index is None else f"{self.dev_pin_prefix}{self.index}_" - ) for iface, params in self.pin_interfaces.items(): for base_pname, new_spec in self.iface_pin_specs(iface).items(): if base_pname not in all_specs: diff --git a/hw_device_mgr/hal/tests/test_data_types.py b/hw_device_mgr/hal/tests/test_data_types.py index b4f2b23e..5307b3a8 100644 --- a/hw_device_mgr/hal/tests/test_data_types.py +++ b/hw_device_mgr/hal/tests/test_data_types.py @@ -20,6 +20,11 @@ class TestHALDataType(BaseHALTestClass, _TestDataType): def test_hal_type_str(self): for shared_name, exp_str in self.sname_to_typestr.items(): + if shared_name not in self.data_type_class.subtype_data: + # LinuxCNC HAL doesn't have 64-bit int types + assert shared_name.endswith("64") + print(f"Skipping 64-bit int type {shared_name}") + continue cls = self.data_type_class.by_shared_name(shared_name) exp_int = self.data_type_class.hal_enum(exp_str[4:]) cls_str, cls_int = (cls.hal_type_str(), cls.hal_type) diff --git a/hw_device_mgr/lcec/command.py b/hw_device_mgr/lcec/command.py index 050efbe0..9c9bd146 100644 --- a/hw_device_mgr/lcec/command.py +++ b/hw_device_mgr/lcec/command.py @@ -17,7 +17,9 @@ class LCECCommand(EtherCATCommand): def _parse_output(cls, resp, kwargs): return resp - def _ethercat(self, *args, log_lev="debug", dry_run=False): + def _ethercat( + self, *args, log_lev="debug", dry_run=False, stderr_to_devnull=False + ): """ Run IgH EtherCAT Master `ethercat` utility. @@ -30,8 +32,9 @@ def _ethercat(self, *args, log_lev="debug", dry_run=False): return getattr(self.logger, log_lev)(" ".join(cmd_args)) + stderr = subprocess.DEVNULL if stderr_to_devnull else None try: - resp = subprocess.check_output(cmd_args) + resp = subprocess.check_output(cmd_args, stderr=stderr) except subprocess.CalledProcessError as e: raise EtherCATCommandException(str(e)) @@ -40,10 +43,12 @@ def _ethercat(self, *args, log_lev="debug", dry_run=False): _device_location_re = re.compile(r"=== Master ([0-9]), Slave ([0-9]+) ") - def scan_bus(self, bus=None): + def scan_bus(self, bus=None, **kwargs): bus = self.default_bus if bus is None else bus devices = list() - output = self._ethercat("slaves", f"--master={bus}", "--verbose") + output = self._ethercat( + "slaves", f"--master={bus}", "--verbose", **kwargs + ) for line in output: line = line.strip() if line.startswith("==="): @@ -84,7 +89,9 @@ def master_nic(self, bus=None): else: return None - def upload(self, address=None, index=None, subindex=0, datatype=None): + def upload( + self, address=None, index=None, subindex=0, datatype=None, **kwargs + ): index = self.data_type_class.uint16(index) subindex = self.data_type_class.uint16(subindex) output = self._ethercat( @@ -94,11 +101,14 @@ def upload(self, address=None, index=None, subindex=0, datatype=None): f"0x{index:04X}", f"0x{subindex:02X}", f"--type={datatype.igh_type}", + **kwargs, ) - # FIXME Handle non-int types - val_hex, val = output[0].split(" ", 1) - val = int(val, 10) - return val + if datatype.shared_name == "str": + return output[0] + else: + val_hex, val = output[0].split(" ", 1) + val = int(val, 10) + return val def download( self, @@ -107,18 +117,19 @@ def download( subindex=0, value=None, datatype=None, - dry_run=False, + **kwargs, ): self._ethercat( "download", f"--master={address[0]}", f"--position={address[1]}", + f"--type={datatype.igh_type}", + "--", f"0x{index:04X}", f"0x{subindex:02X}", str(value), - f"--type={datatype.igh_type}", log_lev="info", - dry_run=dry_run, + **kwargs, ) diff --git a/hw_device_mgr/lcec/config.py b/hw_device_mgr/lcec/config.py index d8ca37f1..e54ecd11 100644 --- a/hw_device_mgr/lcec/config.py +++ b/hw_device_mgr/lcec/config.py @@ -3,6 +3,7 @@ from .xml_reader import LCECXMLReader from .sdo import LCECSDO from .command import LCECCommand, LCECSimCommand +from lxml import etree class LCECConfig(EtherCATConfig): @@ -13,6 +14,108 @@ class LCECConfig(EtherCATConfig): sdo_class = LCECSDO command_class = LCECCommand + @classmethod + def gen_ethercat_xml(cls, bus_configs=dict()): + """ + Generate the `ethercat.xml` config file for lcec. + + The `bus_configs` should be a dictionary of + `master_idx:(appTimePeriod, refClockSyncCycles)`. + """ + # Convert bus_configs keys to ints (YAML wants str type) + for key in list(bus_configs): + bus_configs[str(key)] = bus_configs.pop(key) + # Scan bus once + devs = cls.scan_bus() + # Set up XML top level elements: [...] + xml = etree.Element("masters") + masters = dict() + for dev in devs: + if dev.bus in masters: + continue + bus_conf = bus_configs.get(dev.bus, dict()) + bus_conf["idx"] = str(dev.bus) + atp = str(bus_conf.get("appTimePeriod", 1000000)) + bus_conf["appTimePeriod"] = atp + rcsc = str(bus_conf.get("refClockSyncCycles", 1)) + bus_conf["refClockSyncCycles"] = rcsc + master = etree.Element("master", **bus_conf) + xml.append(master) + masters[dev.bus] = (master, bus_conf) + # Set up elements & their child elements + for dev in devs: + master, bus_conf = masters[dev.bus] + slave_xml = etree.Element( + "slave", + idx=str(dev.position), + type="generic", + vid=str(dev.vendor_id), + pid=str(dev.product_code), + configPdos="true", + ) + master.append(slave_xml) + config = cls.gen_config(dev.model_id, dev.address) + # + if dev.dcs(): + assign_activate = max( + [dc["AssignActivate"] for dc in dev.dcs()] + ) + s0s_default = int(int(bus_conf["appTimePeriod"]) / 2) + s0s = config.get("dc_conf", dict()).get( + "sync0Shift", s0s_default + ) + etree.SubElement( + slave_xml, + "dcConf", + assignActivate=hex(assign_activate), + sync0Cycle="*1", + sync0Shift=str(s0s), + ) + # + for sm_ix, sm_data in config["sync_manager"].items(): + assert "dir" in sm_data + assert sm_data["dir"] in {"in", "out"} + sm_xml = etree.Element( + "syncManager", idx=sm_ix, dir=sm_data["dir"] + ) + slave_xml.append(sm_xml) + if not sm_data.get("pdo_mapping", None): + continue + sdo = dev.sdo(sm_data["pdo_mapping"]["index"]) + pdo_xml = etree.Element("pdo", idx=str(sdo.index)) + sm_xml.append(pdo_xml) + for entry in sm_data["pdo_mapping"]["entries"]: + sdo = dev.sdo(entry["index"]) + dt = sdo.data_type + pdo_entry_xml = etree.Element( + "pdoEntry", + idx=str(sdo.index), + subIdx=str(sdo.subindex), + bitLen=str(dt.num_bits), + ) + pdo_xml.append(pdo_entry_xml) + if "name" in entry: + pdo_entry_xml.set("halPin", entry["name"]) + pdo_entry_xml.set("halType", dt.hal_type_str()[4:]) + else: + # complexEntry + pdo_entry_xml.set("halType", "complex") + for bit in entry["bits"]: + complex_entry_xml = etree.Element( + "complexEntry", bitLen="1" + ) + pdo_entry_xml.append(complex_entry_xml) + if bit is None: # Unused bit + continue + elif isinstance(bit, dict): # Dict of attributes + for k, v in bit.items(): + complex_entry_xml.set(k, str(v)) + else: # Pin name; assume 1 bit + complex_entry_xml.set("halType", "bit") + complex_entry_xml.set("halPin", bit) + + return etree.tostring(xml, pretty_print=True) + class LCECSimConfig(LCECConfig, EtherCATSimConfig): command_class = LCECSimCommand diff --git a/hw_device_mgr/lcec/data_types.py b/hw_device_mgr/lcec/data_types.py index 48027e83..cf7de9a9 100644 --- a/hw_device_mgr/lcec/data_types.py +++ b/hw_device_mgr/lcec/data_types.py @@ -11,12 +11,18 @@ class LCECDataType(EtherCATDataType, HALDataType): int8=dict(igh_type="int8"), int16=dict(igh_type="int16"), int32=dict(igh_type="int32"), - int64=dict(igh_type="int64"), uint8=dict(igh_type="uint8"), uint16=dict(igh_type="uint16"), uint32=dict(igh_type="uint32"), - uint64=dict(igh_type="uint64"), float=dict(igh_type="float"), double=dict(igh_type="double"), - # Strings not usable by `ethercat` tool + str=dict(igh_type="string"), ) + if HALDataType.have_64: + # Machinekit HAL has 64-bit int types, but not LCNC + subtype_data.update( + dict( + int64=dict(igh_type="int64"), + uint64=dict(igh_type="uint64"), + ) + ) diff --git a/hw_device_mgr/lcec/tests/base_test_class.py b/hw_device_mgr/lcec/tests/base_test_class.py index 0f343ab9..f81d61ef 100644 --- a/hw_device_mgr/lcec/tests/base_test_class.py +++ b/hw_device_mgr/lcec/tests/base_test_class.py @@ -45,8 +45,9 @@ def mock_ethercat_command(self): commands. Patches `subprocess.check_output()`. """ - def emulate_ethercat_command(args): + def emulate_ethercat_command(args, **kwargs): print(f'mocking command: {" ".join(args)}') + print(f" subprocess.check_output kwargs: {repr(kwargs)}") # Parse out args, kwargs assert args.pop(0) == "ethercat" cmd = args.pop(0) diff --git a/hw_device_mgr/lcec/tests/bogus_devices/device.py b/hw_device_mgr/lcec/tests/bogus_devices/device.py index 7f547978..a1c8967d 100644 --- a/hw_device_mgr/lcec/tests/bogus_devices/device.py +++ b/hw_device_mgr/lcec/tests/bogus_devices/device.py @@ -6,6 +6,7 @@ class BogusLCECDevice(LCECSimDevice, RelocatableESIDevice): category = "bogus_lcec_devices" vendor_id = 0xB090C0 + xml_description_package = "hw_device_mgr.devices.device_xml" class BogusLCECV1Servo(BogusLCECDevice, CiA402SimDevice): diff --git a/hw_device_mgr/lcec/tests/bogus_devices/device_xml b/hw_device_mgr/lcec/tests/bogus_devices/device_xml deleted file mode 120000 index 7de15941..00000000 --- a/hw_device_mgr/lcec/tests/bogus_devices/device_xml +++ /dev/null @@ -1 +0,0 @@ -../../../ethercat/tests/bogus_devices/device_xml \ No newline at end of file diff --git a/hw_device_mgr/lcec/tests/ethercat.conf.xml b/hw_device_mgr/lcec/tests/ethercat.conf.xml new file mode 100644 index 00000000..6c2712f4 --- /dev/null +++ b/hw_device_mgr/lcec/tests/ethercat.conf.xml @@ -0,0 +1,97 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/hw_device_mgr/lcec/tests/test_config.py b/hw_device_mgr/lcec/tests/test_config.py index 3ee6526c..662b6231 100644 --- a/hw_device_mgr/lcec/tests/test_config.py +++ b/hw_device_mgr/lcec/tests/test_config.py @@ -2,7 +2,37 @@ TestEtherCATConfig as _TestEtherCATConfig, ) from .base_test_class import BaseLCECTestClass +from lxml import etree class TestLCECConfig(BaseLCECTestClass, _TestEtherCATConfig): - pass + + ethercat_conf_xml_package = "hw_device_mgr.lcec.tests" + ethercat_conf_xml_resource = "ethercat.conf.xml" + + def test_gen_ethercat_xml(self, config_cls, tmp_path): + # Read expected conf.xml + rsrc = (self.ethercat_conf_xml_package, self.ethercat_conf_xml_resource) + with self.open_resource(*rsrc) as f: + expected_xml = etree.parse(f) + etree.strip_tags(expected_xml, etree.Comment) # Clean out comments + expected_str = etree.tostring(expected_xml).decode() + expected_lines = expected_str.splitlines() + print(f"Comparing lcec conf from {rsrc}") + + # Generate conf.xml + conf = config_cls.gen_ethercat_xml(dict()).decode() + assert conf + conf_lines = conf.splitlines() + print("Generated ethercat.conf.xml:") + print(conf) + + # Compare lines + for ix, expected_line in enumerate(expected_lines): + print(f"{ix} expected: {expected_line}") + assert ix < len(conf_lines) + conf_line = conf_lines[ix] + print(f"{ix} conf: {conf_line}") + assert conf_line == expected_line + # Compare number of lines + assert len(conf_lines) == len(expected_lines) diff --git a/hw_device_mgr/lcec/tests/test_data_types.py b/hw_device_mgr/lcec/tests/test_data_types.py index a0b9677a..b05e6357 100644 --- a/hw_device_mgr/lcec/tests/test_data_types.py +++ b/hw_device_mgr/lcec/tests/test_data_types.py @@ -25,6 +25,11 @@ class TestLCECDataType( def test_igh_type_attr(self): for shared_name in self.defined_shared_types: + if shared_name not in self.data_type_class.subtype_data: + # LinuxCNC HAL doesn't have 64-bit int types + assert shared_name.endswith("64") + print(f"Skipping 64-bit int type {shared_name}") + continue cls = self.data_type_class.by_shared_name(shared_name) print("cls:", cls) assert hasattr(cls, "igh_type") diff --git a/hw_device_mgr/lcec/tests/test_device.py b/hw_device_mgr/lcec/tests/test_device.py index 51cb78ca..0d613301 100644 --- a/hw_device_mgr/lcec/tests/test_device.py +++ b/hw_device_mgr/lcec/tests/test_device.py @@ -19,7 +19,7 @@ class TestLCECDevice(BaseLCECTestClass, _TestEtherCATDevice, _TestHALDevice): ] @pytest.fixture - def obj(self, device_cls, sim_device_data, sdo_data, mock_halcomp): + def obj(self, sim_device_data, mock_halcomp): self.obj = self.device_model_cls( address=sim_device_data["test_address"] ) diff --git a/hw_device_mgr/mgr/mgr.py b/hw_device_mgr/mgr/mgr.py index 5d30c4ad..93e5c498 100644 --- a/hw_device_mgr/mgr/mgr.py +++ b/hw_device_mgr/mgr/mgr.py @@ -15,6 +15,7 @@ class HWDeviceMgr(FysomGlobalMixin, Device): data_type_class = CiA402Device.data_type_class device_base_class = CiA402Device device_classes = None + slug_separator = "" @classmethod def device_model_id(cls): @@ -89,7 +90,7 @@ def scan_devices(cls, **kwargs): def init_device_instances(self, **kwargs): for i, dev in enumerate(self.devices): - dev.init(index=i, **kwargs) + dev.init(**kwargs) self.logger.info(f"Adding device #{i}: {dev}") #################################################### @@ -402,7 +403,7 @@ def fsm_finalize_command(self, e): #################################################### # Execution - def run(self): + def run_loop(self): """Program main loop.""" update_period = 1.0 / self.mgr_config.get("update_rate", 10.0) while not self.shutdown: @@ -424,6 +425,21 @@ def run(self): continue time.sleep(update_period) + def run(self): + """Program main.""" + try: + self.run_loop() + except KeyboardInterrupt: + self.logger.info("Exiting at keyboard interrupt") + return 0 + except Exception: + self.logger.error("Exiting at unrecoverable exception:") + for line in traceback.format_exc().splitlines(): + self.logger.error(line) + return 1 + self.logger.info("Exiting") + return 0 + def read_update_write(self): """ Read hardware, update controller, write hardware. diff --git a/hw_device_mgr/mgr/tests/base_test_class.py b/hw_device_mgr/mgr/tests/base_test_class.py index d8f82b5a..248c7e5e 100644 --- a/hw_device_mgr/mgr/tests/base_test_class.py +++ b/hw_device_mgr/mgr/tests/base_test_class.py @@ -21,14 +21,16 @@ class BaseMgrTestClass(BaseDevicesTestClass): # major reason for the separate test base classes: to provide relevant # fixtures without dragging in irrelevant tests.) - # test_read_update_write() configuration - read_update_write_yaml = "mgr/tests/read_update_write.cases.yaml" + # test_read_update_write() configuration: + # CiA NMT init online & operational status + read_update_write_package = "hw_device_mgr.mgr.tests" # Manager configuration - mgr_config_yaml = "mgr/tests/bogus_devices/mgr_config.yaml" + mgr_config_package = "hw_device_mgr.mgr.tests.bogus_devices" + mgr_config_yaml = "mgr_config.yaml" # Device model SDOs; for test fixture - device_sdos_yaml = "devices/tests/sim_sdo_data.yaml" + device_sdos_package = "hw_device_mgr.devices.tests" # Manager class device_class = HWDeviceMgrTest @@ -47,8 +49,10 @@ class BaseMgrTestClass(BaseDevicesTestClass): @pytest.fixture def mgr_config(self): - self.mgr_config = self.load_yaml(self.mgr_config_yaml) - return self.mgr_config + rsrc = self.mgr_config_package, self.mgr_config_yaml + mgr_config = self.load_yaml_resource(*rsrc) + assert mgr_config, f"Empty YAML package resource {rsrc}" + return mgr_config @pytest.fixture def device_cls(self, device_config, extra_fixtures): diff --git a/hw_device_mgr/mgr/tests/bogus_devices/device_xml b/hw_device_mgr/mgr/tests/bogus_devices/device_xml deleted file mode 120000 index 19f66975..00000000 --- a/hw_device_mgr/mgr/tests/bogus_devices/device_xml +++ /dev/null @@ -1 +0,0 @@ -../../../devices/device_xml \ No newline at end of file diff --git a/hw_device_mgr/mgr/tests/test_mgr.py b/hw_device_mgr/mgr/tests/test_mgr.py index ac0a5a5f..45739888 100644 --- a/hw_device_mgr/mgr/tests/test_mgr.py +++ b/hw_device_mgr/mgr/tests/test_mgr.py @@ -14,9 +14,6 @@ class TestHWDeviceMgr(BaseMgrTestClass, _TestDevice): *_TestDevice.expected_mro, ] - # Test CiA NMT init: online & operational status - read_update_write_yaml = "mgr/tests/read_update_write.cases.yaml" - @pytest.fixture def obj(self, device_cls, mgr_config, device_config, all_device_data): self.obj = device_cls() diff --git a/hw_device_mgr/mgr_hal/tests/bogus_devices/device_xml b/hw_device_mgr/mgr_hal/tests/bogus_devices/device_xml deleted file mode 120000 index 19f66975..00000000 --- a/hw_device_mgr/mgr_hal/tests/bogus_devices/device_xml +++ /dev/null @@ -1 +0,0 @@ -../../../devices/device_xml \ No newline at end of file diff --git a/hw_device_mgr/mgr_ros/mgr.py b/hw_device_mgr/mgr_ros/mgr.py index 2f720967..6afbb47f 100644 --- a/hw_device_mgr/mgr_ros/mgr.py +++ b/hw_device_mgr/mgr_ros/mgr.py @@ -1,11 +1,10 @@ from ..mgr.mgr import HWDeviceMgr, SimHWDeviceMgr +from ..config_io import ConfigIO import rclpy -import yaml -import os import traceback -class ROSHWDeviceMgr(HWDeviceMgr): +class ROSHWDeviceMgr(HWDeviceMgr, ConfigIO): def get_param(self, name, default=None): if self.ros_node.has_parameter(name): param = self.ros_node.get_parameter(name) @@ -44,23 +43,15 @@ def init_devices(self, **kwargs): device_config_path = self.get_param("device_config_path") assert device_config_path, "No 'device_config_path' param defined" self.logger.info(f"Reading device config from '{device_config_path}'") - assert os.path.exists(device_config_path) - with open(device_config_path, "r") as f: - device_config = yaml.safe_load(f) - assert device_config + device_config = self.load_yaml_path(device_config_path) + assert device_config, f"Empty YAML file '{device_config_path}'" super().init_devices(device_config=device_config, **kwargs) def init_sim_from_rosparams(self, **kwargs): sim_device_data_path = self.get_param("sim_device_data_path") assert sim_device_data_path, "No 'sim_device_data_path' param defined" - assert os.path.exists( - sim_device_data_path - ), f"Device data path doesn't exist: '{sim_device_data_path}'" - self.logger.info( - f"Reading sim device config from {sim_device_data_path}" - ) - with open(sim_device_data_path, "r") as f: - sim_device_data = yaml.safe_load(f) + sim_device_data = self.load_yaml_path(sim_device_data_path) + assert sim_device_data, f"Empty YAML file '{sim_device_data_path}'" self.init_sim(sim_device_data=sim_device_data, **kwargs) def read_update_write(self): diff --git a/hw_device_mgr/mgr_ros/tests/base_test_class.py b/hw_device_mgr/mgr_ros/tests/base_test_class.py index f2929c46..e6f1740b 100644 --- a/hw_device_mgr/mgr_ros/tests/base_test_class.py +++ b/hw_device_mgr/mgr_ros/tests/base_test_class.py @@ -1,5 +1,4 @@ from ...mgr.tests.base_test_class import BaseMgrTestClass -import yaml import pytest try: @@ -58,8 +57,7 @@ def sim_device_data_path(self, tmp_path, mock_rclpy): for d in sim_device_data: d["product_code"] = int(d["product_code"]) d["vendor_id"] = int(d["vendor_id"]) - with open(tmpfile, "w") as f: - f.write(yaml.safe_dump(sim_device_data)) + self.dump_yaml_path(tmpfile, sim_device_data) self.rosparams["sim_device_data_path"] = tmpfile yield tmpfile @@ -73,8 +71,7 @@ def device_config_path(self, tmp_path, device_config, mock_rclpy): dc["product_code"] = int(dc["product_code"]) dc["vendor_id"] = int(dc["vendor_id"]) tmpfile = tmp_path / "device_config.yaml" - with open(tmpfile, "w") as f: - f.write(yaml.safe_dump(device_config)) + self.dump_yaml_path(tmpfile, device_config) self.rosparams["device_config_path"] = tmpfile print(f"Cleaned device config written to {tmpfile}") yield tmpfile diff --git a/hw_device_mgr/mgr_ros/tests/bogus_devices/device_xml b/hw_device_mgr/mgr_ros/tests/bogus_devices/device_xml deleted file mode 120000 index 19f66975..00000000 --- a/hw_device_mgr/mgr_ros/tests/bogus_devices/device_xml +++ /dev/null @@ -1 +0,0 @@ -../../../devices/device_xml \ No newline at end of file diff --git a/hw_device_mgr/mgr_ros/tests/test_mgr.py b/hw_device_mgr/mgr_ros/tests/test_mgr.py index 79e11d28..9fe1bea4 100644 --- a/hw_device_mgr/mgr_ros/tests/test_mgr.py +++ b/hw_device_mgr/mgr_ros/tests/test_mgr.py @@ -11,6 +11,7 @@ class TestROSHWDeviceMgr(BaseROSMgrTestClass, _TestHWDeviceMgr): "ROSSimHWDeviceMgr", "ROSHWDeviceMgr", *_TestHWDeviceMgr.expected_mro[1:], + "ConfigIO", ] rclpy_patches = [ "hw_device_mgr.mgr_ros.mgr.rclpy", diff --git a/hw_device_mgr/mgr_ros_hal/device_xml b/hw_device_mgr/mgr_ros_hal/device_xml deleted file mode 120000 index 08f7d8e8..00000000 --- a/hw_device_mgr/mgr_ros_hal/device_xml +++ /dev/null @@ -1 +0,0 @@ -../devices/device_xml \ No newline at end of file diff --git a/hw_device_mgr/mgr_ros_hal/tests/bogus_devices/device_xml b/hw_device_mgr/mgr_ros_hal/tests/bogus_devices/device_xml deleted file mode 120000 index 19f66975..00000000 --- a/hw_device_mgr/mgr_ros_hal/tests/bogus_devices/device_xml +++ /dev/null @@ -1 +0,0 @@ -../../../devices/device_xml \ No newline at end of file diff --git a/hw_device_mgr/mgr_ros_hal/tests/test_mgr.py b/hw_device_mgr/mgr_ros_hal/tests/test_mgr.py index 87f01d8c..70d6cdaf 100644 --- a/hw_device_mgr/mgr_ros_hal/tests/test_mgr.py +++ b/hw_device_mgr/mgr_ros_hal/tests/test_mgr.py @@ -12,5 +12,7 @@ class TestROSHWDeviceMgr( "ROSHALSimHWDeviceMgr", "ROSHALHWDeviceMgr", *_TestROSHWDeviceMgr.expected_mro[1:3], # ROS{Sim...}HWDeviceMgr - *_TestHALHWDeviceMgr.expected_mro[1:], # HALSimHWDeviceMgr...HALMixin + *_TestHALHWDeviceMgr.expected_mro[1:-1], # HALSimHWDeviceMgr...ABC + "ConfigIO", + _TestHALHWDeviceMgr.expected_mro[-1], # HALMixin ] diff --git a/hw_device_mgr/tests/base_test_class.py b/hw_device_mgr/tests/base_test_class.py index 5d0c7ebf..95718fbe 100644 --- a/hw_device_mgr/tests/base_test_class.py +++ b/hw_device_mgr/tests/base_test_class.py @@ -1,7 +1,5 @@ import pytest -from pathlib import Path -import os -import yaml +from ..config_io import ConfigIO from .bogus_devices.data_types import BogusDataType from .bogus_devices.device import ( BogusDevice, @@ -11,11 +9,12 @@ ) -class BaseTestClass: +class BaseTestClass(ConfigIO): """Base test class providing fixtures for use with `bogus_devices`.""" # Device scan data; for test fixture - sim_device_data_yaml = "tests/sim_devices.yaml" + sim_device_data_package = "hw_device_mgr.tests" + sim_device_data_yaml = "sim_devices.yaml" # Data types # Classes under test in this module @@ -26,20 +25,15 @@ class BaseTestClass: # Sim mode by default sim = True - @classmethod - def load_yaml(cls, fname, return_path=False): - p = Path(__file__).parent.parent.joinpath(fname) - with p.open() as f: - data = yaml.safe_load(f) - return (p, data) if return_path else data - @classmethod def test_category_class(cls, test_category): for dmc in cls.device_model_classes: assert dmc.name if dmc.test_category == test_category: return dmc - raise ValueError(f"No device in test category class '{test_category}'") + raise ValueError( + f"{cls}: No device in test category class '{test_category}'" + ) @classmethod def munge_sim_device_data(cls, sim_device_data): @@ -69,13 +63,17 @@ def init_sim(cls, **kwargs): cls.device_class.clear_devices() cls.device_class.init_sim(**kwargs) + @classmethod + def load_sim_device_data(cls): + rsrc = cls.sim_device_data_package, cls.sim_device_data_yaml + dev_data = cls.load_yaml_resource(*rsrc) + assert dev_data, f"Empty device data in package resource {rsrc}" + return dev_data + @classmethod def init_sim_device_data(cls): - # Set up sim devices: munge YAML data & pass to sim device class - cls.sim_device_data_path, dev_data = cls.load_yaml( - cls.sim_device_data_yaml, True - ) - print(f" Raw sim_device_data from {cls.sim_device_data_path}") + # Set up sim devices: munge data & pass to sim device class + dev_data = cls.load_sim_device_data() return cls.munge_sim_device_data(dev_data) @pytest.fixture @@ -100,24 +98,10 @@ def pytest_generate_tests(self, metafunc): if "sim_device_data" not in metafunc.fixturenames: return - path, sim_device_data = self.load_yaml(self.sim_device_data_yaml, True) - sim_device_data = self.munge_sim_device_data(sim_device_data) + data_raw = self.load_sim_device_data() + sim_device_data = self.munge_sim_device_data(data_raw) vals, ids = (list(), list()) for dev in sim_device_data: ids.append(f"{dev['test_name']}@{dev['test_address']}") vals.append(dev) metafunc.parametrize("sim_device_data", vals, ids=ids, scope="class") - - @pytest.fixture - def fpath(self): - """Fixture that returns test directory.""" - # This line resolves black & pep257 conflicts. :P - - def func(base_name=None): - cwd = os.path.dirname(os.path.dirname(os.path.realpath(__file__))) - if base_name is None: - return cwd - else: - return os.path.join(cwd, base_name) - - return func diff --git a/hw_device_mgr/tests/test_device.py b/hw_device_mgr/tests/test_device.py index a2236bd6..9b48cea9 100644 --- a/hw_device_mgr/tests/test_device.py +++ b/hw_device_mgr/tests/test_device.py @@ -3,7 +3,6 @@ from ..device import Device import subprocess from pprint import pformat -import ruamel.yaml class TestDevice(BaseTestClass): @@ -122,7 +121,7 @@ def obj(self, device_cls, sim_device_data): yield self.obj def test_init(self, obj): - assert hasattr(obj, "index") + pass # Base class init() method does nothing def test_set_sim_feedback(self, obj): res = obj.set_sim_feedback() @@ -136,8 +135,9 @@ def test_set_sim_feedback(self, obj): # - Check expected feedback & command, in & out # Configuration - # - Path to .yaml test cases (relative to `tests/` directory) - read_update_write_yaml = None + # - YAML test cases package resource + read_update_write_package = None # Skip tests if None + read_update_write_yaml = "read_update_write.cases.yaml" # - Translate feedback/command test input params from values # human-readable in .yaml to values matching actual params read_update_write_translate_feedback_in = dict() @@ -338,21 +338,19 @@ def read_update_write_loop(self, test_case): self.set_command_and_check() self.write_and_check() - def test_read_update_write(self, obj, fpath): - test_cases_yaml = getattr(self, "read_update_write_yaml", None) - if test_cases_yaml is None: + def test_read_update_write(self, obj): + if self.read_update_write_package is None: return # No test cases defined for this class - with open(fpath(test_cases_yaml)) as f: - yaml = ruamel.yaml.YAML() - test_cases = yaml.load(f) - print(f"Read test cases from {fpath(test_cases_yaml)}") - + rsrc = (self.read_update_write_package, self.read_update_write_yaml) + test_cases = self.load_yaml_resource(*rsrc) + assert test_cases, f"Empty YAML from package resource {rsrc}" + print(f"Read test cases from package resource {rsrc}") for test_case in test_cases: self.read_update_write_loop(test_case) def test_dot(self, tmp_path): # Test class diagram - gv_file = tmp_path / ".." / f"{self.device_class.category}.gv" + gv_file = tmp_path / f"{self.device_class.category}.gv" assert not gv_file.exists() with gv_file.open("w") as f: f.write(self.device_class.dot())