From 8207612caca922cf54864226b833a249d9653054 Mon Sep 17 00:00:00 2001 From: John Morris Date: Wed, 8 Jun 2022 16:52:22 -0500 Subject: [PATCH 01/88] Base tests: Minor updates Fix test_dot test. External projects may have mgr classes without a separate category, i.e. in the `all` class, which causes collisions. I'm not sury why this was ever this way in the first place. Other changes for cleanliness only; nothing fixed: - Remove redundant piece of conditional - Improve assertion exception messages --- hw_device_mgr/tests/base_test_class.py | 4 +++- hw_device_mgr/tests/test_device.py | 2 +- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/hw_device_mgr/tests/base_test_class.py b/hw_device_mgr/tests/base_test_class.py index 5d0c7ebf..cc0fbca0 100644 --- a/hw_device_mgr/tests/base_test_class.py +++ b/hw_device_mgr/tests/base_test_class.py @@ -39,7 +39,9 @@ def test_category_class(cls, test_category): 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): diff --git a/hw_device_mgr/tests/test_device.py b/hw_device_mgr/tests/test_device.py index a2236bd6..946677a0 100644 --- a/hw_device_mgr/tests/test_device.py +++ b/hw_device_mgr/tests/test_device.py @@ -352,7 +352,7 @@ def test_read_update_write(self, obj, fpath): 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()) From 04063cc6b3a0dee6e0ea0a972f29c540a657e8ea Mon Sep 17 00:00:00 2001 From: John Morris Date: Fri, 15 Jul 2022 12:04:14 -0500 Subject: [PATCH 02/88] base class: Reset `feeback_in` interface in `ready()` method ...and `update()` that interface from `get_feedback()`. This fixes issues with the manager class. It also shows that for a more intuitive interface, interfaces should be reset from a `reset()` method in the base `read()`, and `update()` renamed to `set()`. --- hw_device_mgr/device.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/hw_device_mgr/device.py b/hw_device_mgr/device.py index e1a2a98f..288e1566 100644 --- a/hw_device_mgr/device.py +++ b/hw_device_mgr/device.py @@ -95,6 +95,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.""" @@ -369,7 +370,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.""" From 3f159f558cfa550c0a5f10cbfd5694c4d76c3de0 Mon Sep 17 00:00:00 2001 From: John Morris Date: Wed, 14 Sep 2022 14:28:12 -0500 Subject: [PATCH 03/88] cia_301: Pass `**kwargs` through config to command class Will be used by subclasses --- hw_device_mgr/cia_301/command.py | 5 ++++- hw_device_mgr/cia_301/config.py | 7 +++++-- 2 files changed, 9 insertions(+), 3 deletions(-) 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..1d4149d6 100644 --- a/hw_device_mgr/cia_301/config.py +++ b/hw_device_mgr/cia_301/config.py @@ -108,7 +108,7 @@ def sdo(self, ix): # 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 +116,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 +129,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 +142,7 @@ def download(self, sdo, val, dry_run=False): subindex=sdo.subindex, value=val, datatype=sdo.data_type, + **kwargs, ) # From 2ec70fe430f79036e89d6fb1a0387b4fd08a35bd Mon Sep 17 00:00:00 2001 From: John Morris Date: Wed, 14 Sep 2022 14:33:35 -0500 Subject: [PATCH 04/88] lcec: Add option to suppress `ethercat` cmd stderr output When dumping drive params, uploading some objects is expected to fail; suppressing stderr silences the cryptic & out of context error messages printed by the `ethercat` command --- hw_device_mgr/lcec/command.py | 22 ++++++++++++++------- hw_device_mgr/lcec/tests/base_test_class.py | 3 ++- 2 files changed, 17 insertions(+), 8 deletions(-) diff --git a/hw_device_mgr/lcec/command.py b/hw_device_mgr/lcec/command.py index 050efbe0..fe31a724 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,6 +101,7 @@ 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) @@ -107,7 +115,7 @@ def download( subindex=0, value=None, datatype=None, - dry_run=False, + **kwargs, ): self._ethercat( "download", @@ -118,7 +126,7 @@ def download( str(value), f"--type={datatype.igh_type}", log_lev="info", - dry_run=dry_run, + **kwargs, ) 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) From 006300929a5688e4a6c12c493721178db2ee0a80 Mon Sep 17 00:00:00 2001 From: John Morris Date: Wed, 14 Sep 2022 11:35:56 -0500 Subject: [PATCH 05/88] cia_301: Add method to dump drive params to config object The `dump_param_values()` method uploads all device SDO values and returns in a dict of `{sdo_obj : value}` pairs. --- hw_device_mgr/cia_301/config.py | 19 ++++++++++++++++++- 1 file changed, 18 insertions(+), 1 deletion(-) diff --git a/hw_device_mgr/cia_301/config.py b/hw_device_mgr/cia_301/config.py index 1d4149d6..7543fd81 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: @@ -98,12 +99,28 @@ 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] + 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 # From 86f5e7abba82e1030c50acf8500e85cb03005b51 Mon Sep 17 00:00:00 2001 From: John Morris Date: Sat, 11 Jun 2022 14:24:22 -0500 Subject: [PATCH 06/88] lcec: Test fixture tweak Remove unneeded fixtures --- hw_device_mgr/lcec/tests/test_device.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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"] ) From 541bb3172a6299cece7008093b85a50458e19ef1 Mon Sep 17 00:00:00 2001 From: John Morris Date: Thu, 21 Jul 2022 17:02:11 -0500 Subject: [PATCH 07/88] lcec: Accept negative numbers when setting int-type params The dash character in e.g. `-1` confuses the `ethercat` utility. Fix up the `ethercat download` command to explicitly signal the end of options. --- hw_device_mgr/lcec/command.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/hw_device_mgr/lcec/command.py b/hw_device_mgr/lcec/command.py index fe31a724..727fb405 100644 --- a/hw_device_mgr/lcec/command.py +++ b/hw_device_mgr/lcec/command.py @@ -121,10 +121,11 @@ def download( "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", **kwargs, ) From 06543b7c576e2f6a710cc7da93637b4c60f74da9 Mon Sep 17 00:00:00 2001 From: John Morris Date: Wed, 14 Sep 2022 14:38:12 -0500 Subject: [PATCH 08/88] lcec: Add parsing of `ethercat upload -t string` output The `LCECCommand.upload()` method can now handle string types. --- hw_device_mgr/lcec/command.py | 10 ++++++---- hw_device_mgr/lcec/data_types.py | 2 +- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/hw_device_mgr/lcec/command.py b/hw_device_mgr/lcec/command.py index 727fb405..9c9bd146 100644 --- a/hw_device_mgr/lcec/command.py +++ b/hw_device_mgr/lcec/command.py @@ -103,10 +103,12 @@ def upload( 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, diff --git a/hw_device_mgr/lcec/data_types.py b/hw_device_mgr/lcec/data_types.py index 48027e83..dfb9439f 100644 --- a/hw_device_mgr/lcec/data_types.py +++ b/hw_device_mgr/lcec/data_types.py @@ -18,5 +18,5 @@ class LCECDataType(EtherCATDataType, HALDataType): 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"), ) From fb7286ec2b9a31aca258d20dc944ba792d35cbc2 Mon Sep 17 00:00:00 2001 From: John Morris Date: Fri, 24 Jun 2022 14:50:01 -0500 Subject: [PATCH 09/88] mgr: Catch KeyboardInterrupt in main loop --- hw_device_mgr/mgr/mgr.py | 17 ++++++++++++++++- 1 file changed, 16 insertions(+), 1 deletion(-) diff --git a/hw_device_mgr/mgr/mgr.py b/hw_device_mgr/mgr/mgr.py index 5d30c4ad..cc80eb8c 100644 --- a/hw_device_mgr/mgr/mgr.py +++ b/hw_device_mgr/mgr/mgr.py @@ -402,7 +402,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 +424,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. From d6c8ea82e43f620e48b09fb66dfe0fed5b6ea173 Mon Sep 17 00:00:00 2001 From: John Morris Date: Tue, 7 Jun 2022 16:32:57 -0500 Subject: [PATCH 10/88] cia_301: Redo device_config munging The `munge_config()` method was too rigid, requiring each key to be named. It also clobbered bits of the original. Instead, copy the whole raw device config, avoiding skipped and clobbered keys, and munge just the bits that need munging. --- hw_device_mgr/cia_301/config.py | 27 ++++++++++++--------------- 1 file changed, 12 insertions(+), 15 deletions(-) diff --git a/hw_device_mgr/cia_301/config.py b/hw_device_mgr/cia_301/config.py index 7543fd81..a18e73b4 100644 --- a/hw_device_mgr/cia_301/config.py +++ b/hw_device_mgr/cia_301/config.py @@ -207,24 +207,21 @@ 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 @@ -245,7 +242,7 @@ def config(self): else: raise KeyError(f"No config for device at {self.address}") # Prune & cache config - self._config = self.munge_config(conf) + self._config = self.munge_config(conf, self.position) # Return cached config return self._config From 9e2db0a36c7e6dda6f331c46b77b3661524f7795 Mon Sep 17 00:00:00 2001 From: John Morris Date: Fri, 15 Jul 2022 12:01:53 -0500 Subject: [PATCH 11/88] cia_402: Tweak log messages --- hw_device_mgr/cia_402/device.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/hw_device_mgr/cia_402/device.py b/hw_device_mgr/cia_402/device.py index bbdd2c6f..22253b6a 100644 --- a/hw_device_mgr/cia_402/device.py +++ b/hw_device_mgr/cia_402/device.py @@ -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 From dbc667906524b30956ddacc4e7be87c0fa2093b8 Mon Sep 17 00:00:00 2001 From: John Morris Date: Thu, 21 Jul 2022 16:59:18 -0500 Subject: [PATCH 12/88] cia_402: Don't print redundant "Goal not reached" logs --- hw_device_mgr/cia_402/device.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/hw_device_mgr/cia_402/device.py b/hw_device_mgr/cia_402/device.py index 22253b6a..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 = { From e12f9efdce9cde53399cfdcd4077f96f752f22c2 Mon Sep 17 00:00:00 2001 From: John Morris Date: Thu, 23 Jun 2022 19:06:02 -0500 Subject: [PATCH 13/88] errors: Fix class bitrot --- hw_device_mgr/errors/device.py | 26 +++++++++++++++----------- 1 file changed, 15 insertions(+), 11 deletions(-) diff --git a/hw_device_mgr/errors/device.py b/hw_device_mgr/errors/device.py index 0dcfc544..41014c1b 100644 --- a/hw_device_mgr/errors/device.py +++ b/hw_device_mgr/errors/device.py @@ -16,11 +16,14 @@ class ErrorDevice(Device): device_error_dir = "device_err" - 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 @@ -49,22 +52,23 @@ def error_descriptions(cls): 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) - if not error_code: - self.feedback.update(**self.no_error) - return + def get_feedback(self): + fb_out = super().get_feedback() + if not fb_out.get("error_code"): + 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( + self.feedback_out.update( description=f"Unknown error code {error_code}", advice="Please consult with hardware vendor", error_code=error_code, ) - return + return fb_out - self.feedback.update(error_code=error_code, **error_info) + self.feedback_out.update(error_code=error_code, **error_info) + return fb_out class ErrorSimDevice(ErrorDevice, SimDevice): From 9c8d29764af7f1cb81e6e012b895bf220d1f0dde Mon Sep 17 00:00:00 2001 From: John Morris Date: Thu, 21 Jul 2022 17:04:32 -0500 Subject: [PATCH 14/88] errors: When error code changes, log error code & description --- hw_device_mgr/errors/device.py | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/hw_device_mgr/errors/device.py b/hw_device_mgr/errors/device.py index 41014c1b..2a43b4b0 100644 --- a/hw_device_mgr/errors/device.py +++ b/hw_device_mgr/errors/device.py @@ -54,20 +54,26 @@ def error_descriptions(cls): def get_feedback(self): fb_out = super().get_feedback() - if not fb_out.get("error_code"): + error_code = self.feedback_in.get("error_code") + if not error_code: 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_out.update( + fb_out.update( description=f"Unknown error code {error_code}", advice="Please consult with hardware vendor", error_code=error_code, ) return fb_out - - self.feedback_out.update(error_code=error_code, **error_info) + 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 From f044723a5b003991c7c1b1fa5b7ec232eb5e5de9 Mon Sep 17 00:00:00 2001 From: John Morris Date: Tue, 13 Sep 2022 11:13:46 -0500 Subject: [PATCH 15/88] devices: Update SV660 ESI, adding extra objects from manual Add objects shown in manual (and seen on drives) but not in original ESI from Inovance - 2002-06h, 2002-07h: Stop mode settings - 2004-18h: Forced DO output in non-OP state - 2005-08h, 2005-0Ah: Electronic gear ratio settings - 200D-03h: Offline autotuning setting --- .../device_xml/SV660_EOE_1Axis_V9.12.xml | 120 ++++++++++++++++++ 1 file changed, 120 insertions(+) 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 From 4d5cb1d283b63acd00152800aecfa418dafd32b4 Mon Sep 17 00:00:00 2001 From: John Morris Date: Wed, 29 Jun 2022 13:23:27 -0500 Subject: [PATCH 16/88] device base class: Replace `index` attribute with `addr_slug` The `index` attribute was unused other than to compute pin names in the `HALDevice` class. Replace it with a generic string used for generated identifiers. --- hw_device_mgr/device.py | 21 +++++++++++++++++++-- hw_device_mgr/tests/test_device.py | 2 +- 2 files changed, 20 insertions(+), 3 deletions(-) diff --git a/hw_device_mgr/device.py b/hw_device_mgr/device.py index 288e1566..096d40e0 100644 --- a/hw_device_mgr/device.py +++ b/hw_device_mgr/device.py @@ -3,6 +3,8 @@ 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 +38,7 @@ def __init__(self, address=None): self.address = address self.init_interfaces() - def init(self, index=None): + def init(self): """ Initialize device. @@ -44,7 +46,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 +64,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 diff --git a/hw_device_mgr/tests/test_device.py b/hw_device_mgr/tests/test_device.py index 946677a0..ca7afae9 100644 --- a/hw_device_mgr/tests/test_device.py +++ b/hw_device_mgr/tests/test_device.py @@ -122,7 +122,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() From 9829c98afd2736073c55c170ecac0f4ae0c3c923 Mon Sep 17 00:00:00 2001 From: John Morris Date: Wed, 29 Jun 2022 13:36:28 -0500 Subject: [PATCH 17/88] hal: Generate HAL pin prefix from `address` attribute The base `Device` class `index` attribute, unused outside this class, was removed. Generate HAL pins by munging the `address` attribute instead. --- hw_device_mgr/hal/device.py | 22 ++++++++++++++-------- 1 file changed, 14 insertions(+), 8 deletions(-) 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: From 54d997b79e30adc678eb0d5ef1b6d264c0ad2c06 Mon Sep 17 00:00:00 2001 From: John Morris Date: Wed, 18 May 2022 16:47:10 -0500 Subject: [PATCH 18/88] hal: Conditionally use 64-bit int data types LinuxCNC doesn't support these --- hw_device_mgr/hal/data_types.py | 11 +++++++++-- hw_device_mgr/hal/tests/test_data_types.py | 5 +++++ hw_device_mgr/lcec/data_types.py | 10 ++++++++-- hw_device_mgr/lcec/tests/test_data_types.py | 5 +++++ 4 files changed, 27 insertions(+), 4 deletions(-) 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/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/data_types.py b/hw_device_mgr/lcec/data_types.py index dfb9439f..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"), 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/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") From 968030e70b926ed993eda1e15cf3196b33163f15 Mon Sep 17 00:00:00 2001 From: John Morris Date: Mon, 19 Sep 2022 18:03:03 -0500 Subject: [PATCH 19/88] mgr: Following base class, remove `index` arg from `init()` --- hw_device_mgr/mgr/mgr.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/hw_device_mgr/mgr/mgr.py b/hw_device_mgr/mgr/mgr.py index cc80eb8c..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}") #################################################### From de3d1298ae2f822408737fbdb42dcac02a96527d Mon Sep 17 00:00:00 2001 From: John Morris Date: Thu, 19 May 2022 00:29:46 -0500 Subject: [PATCH 20/88] config_io: Initial commit Add a general `ConfigIO` class. It covers several cases, broken down into categories: - Config file source: - POSIX path for reading or writing - Python `importlib` resource (possibly in an "egg" zip file) - Config file use case: - Open as IOStream object (can use as context manager; has `read()`, etc. methods) - Read (path + resource) or write (path only) YAML data This will clean up config data handling for both the device manager classes and their tests. It makes sharing config data from package resources easier, both internally within this package, and also for use by external packages (the latter being the main motivation for this change). --- hw_device_mgr/config_io.py | 39 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 39 insertions(+) create mode 100644 hw_device_mgr/config_io.py 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 From ddbd99b20e5b94c4190a07d58f82b2f6f18d595c Mon Sep 17 00:00:00 2001 From: John Morris Date: Thu, 19 May 2022 00:29:46 -0500 Subject: [PATCH 21/88] base class: Migrate YAML access to ConfigIO and `importlib` refs --- hw_device_mgr/device.py | 16 ++++----- hw_device_mgr/tests/base_test_class.py | 48 ++++++++------------------ hw_device_mgr/tests/test_device.py | 20 +++++------ 3 files changed, 30 insertions(+), 54 deletions(-) diff --git a/hw_device_mgr/device.py b/hw_device_mgr/device.py index 096d40e0..2f9ed336 100644 --- a/hw_device_mgr/device.py +++ b/hw_device_mgr/device.py @@ -1,5 +1,4 @@ import abc -from importlib.resources import path as imp_path from .logging import Logging from .interface import Interface from .data_types import DataType @@ -132,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}>" @@ -356,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: diff --git a/hw_device_mgr/tests/base_test_class.py b/hw_device_mgr/tests/base_test_class.py index cc0fbca0..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,13 +25,6 @@ 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: @@ -71,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 @@ -102,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 ca7afae9..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): @@ -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,15 +338,13 @@ 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) From 5d3d4cbbe5173a9416e24d46434d2f20e7748cbc Mon Sep 17 00:00:00 2001 From: John Morris Date: Thu, 19 May 2022 00:29:46 -0500 Subject: [PATCH 22/88] cia_301: Migrate YAML access to ConfigIO and `importlib` refs --- .../cia_301/tests/base_test_class.py | 72 ++++++++++++------- .../{bogus_devices => }/sim_sdo_data.yaml | 0 hw_device_mgr/cia_301/tests/test_device.py | 4 +- 3 files changed, 50 insertions(+), 26 deletions(-) rename hw_device_mgr/cia_301/tests/{bogus_devices => }/sim_sdo_data.yaml (100%) 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..936ba1ef 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,12 @@ 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" # Classes under test in this module data_type_class = CiA301DataType @@ -42,18 +44,27 @@ class BaseCiA301TestClass(BaseTestClass): @classmethod def init_sim(cls, **kwargs): + """Create sim device objects with configured SDOs.""" if cls.pass_init_sim_device_sdos: # 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" Raw sdo_data from {cls.sdo_data_resource()}") kwargs["sdo_data"] = cls.munge_sdo_data(sdo_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: device_cls = cls.test_category_class(conf["test_category"]) @@ -84,30 +95,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 @@ -217,6 +232,17 @@ 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 + def pytest_generate_tests(self, metafunc): # Dynamic parametrization from sim_device_data_yaml: # - _sim_device_data: iterate through `sim_device_data` list @@ -224,10 +250,8 @@ 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) names = list() vals, ids = (list(), list()) if "_sim_device_data" in metafunc.fixturenames: diff --git a/hw_device_mgr/cia_301/tests/bogus_devices/sim_sdo_data.yaml b/hw_device_mgr/cia_301/tests/sim_sdo_data.yaml similarity index 100% rename from hw_device_mgr/cia_301/tests/bogus_devices/sim_sdo_data.yaml rename to hw_device_mgr/cia_301/tests/sim_sdo_data.yaml 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): From ba77ba8773d99b1cbf48c9d3a28d839a6b568144 Mon Sep 17 00:00:00 2001 From: John Morris Date: Sun, 5 Jun 2022 12:13:20 -0500 Subject: [PATCH 23/88] cia_402: Migrate YAML access to ConfigIO and `importlib` refs --- hw_device_mgr/cia_402/tests/base_test_class.py | 3 ++- hw_device_mgr/cia_402/tests/test_device.py | 5 +++-- 2 files changed, 5 insertions(+), 3 deletions(-) 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) From d9a544183bf2b90a11405184a19f373b06f2623c Mon Sep 17 00:00:00 2001 From: John Morris Date: Thu, 19 May 2022 00:29:46 -0500 Subject: [PATCH 24/88] errors: Migrate YAML access to ConfigIO and `importlib` refs --- hw_device_mgr/errors/device.py | 8 +++----- hw_device_mgr/errors/tests/test_device.py | 1 + 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/hw_device_mgr/errors/device.py b/hw_device_mgr/errors/device.py index 2a43b4b0..79c433ca 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. @@ -45,9 +45,7 @@ def error_descriptions(cls): 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) + err_yaml = cls.load_yaml(path) for err_code_str, err_data in err_yaml.items(): errs[int(err_code_str, 0)] = err_data return cls._error_descriptions[cls.name] diff --git a/hw_device_mgr/errors/tests/test_device.py b/hw_device_mgr/errors/tests/test_device.py index 98e106da..a447bf0b 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 From 2c9618f94504de61fe6dcc4cf17c09aa1051bcc6 Mon Sep 17 00:00:00 2001 From: John Morris Date: Fri, 3 Jun 2022 16:44:47 -0500 Subject: [PATCH 25/88] errors: Use importlib to read device error data files Following previous commit for `errors`. --- hw_device_mgr/errors/device.py | 18 ++++++++---------- .../errors/tests/bogus_devices/device.py | 4 ++++ .../tests/bogus_devices/device_err/__init__.py | 0 ...servo.yaml => bogus_v1_v2_error_servo.yaml} | 0 .../device_err/bogus_v2_error_servo.yaml | 1 - hw_device_mgr/errors/tests/test_device.py | 2 +- 6 files changed, 13 insertions(+), 12 deletions(-) create mode 100644 hw_device_mgr/errors/tests/bogus_devices/device_err/__init__.py rename hw_device_mgr/errors/tests/bogus_devices/device_err/{bogus_v1_error_servo.yaml => bogus_v1_v2_error_servo.yaml} (100%) delete mode 120000 hw_device_mgr/errors/tests/bogus_devices/device_err/bogus_v2_error_servo.yaml diff --git a/hw_device_mgr/errors/device.py b/hw_device_mgr/errors/device.py index 79c433ca..69918069 100644 --- a/hw_device_mgr/errors/device.py +++ b/hw_device_mgr/errors/device.py @@ -14,7 +14,8 @@ class ErrorDevice(Device, ConfigIO): strings to feedback. """ - device_error_dir = "device_err" + device_error_package = None + device_error_yaml = None feedback_in_data_types = dict(error_code="uint32") feedback_in_defaults = dict(error_code=0) @@ -29,23 +30,20 @@ class ErrorDevice(Device, ConfigIO): _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(): - err_yaml = cls.load_yaml(path) + 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] 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 a447bf0b..d710e7ce 100644 --- a/hw_device_mgr/errors/tests/test_device.py +++ b/hw_device_mgr/errors/tests/test_device.py @@ -20,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 From 752087ef8b72eef39fd19dbdbcb429ef8136d3a5 Mon Sep 17 00:00:00 2001 From: John Morris Date: Thu, 9 Jun 2022 17:25:16 -0500 Subject: [PATCH 26/88] cia_301: Add distributed clock configuration --- hw_device_mgr/cia_301/config.py | 13 ++++ hw_device_mgr/cia_301/device.py | 25 ++++++- .../cia_301/tests/base_test_class.py | 72 +++++++++++++++++-- hw_device_mgr/cia_301/tests/dcs_data.yaml | 23 ++++++ hw_device_mgr/cia_301/tests/test_config.py | 10 +++ 5 files changed, 135 insertions(+), 8 deletions(-) create mode 100644 hw_device_mgr/cia_301/tests/dcs_data.yaml diff --git a/hw_device_mgr/cia_301/config.py b/hw_device_mgr/cia_301/config.py index a18e73b4..2304db35 100644 --- a/hw_device_mgr/cia_301/config.py +++ b/hw_device_mgr/cia_301/config.py @@ -29,6 +29,8 @@ 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 @@ -110,6 +112,17 @@ def sdo(self, 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(): + 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: diff --git a/hw_device_mgr/cia_301/device.py b/hw_device_mgr/cia_301/device.py index e76132e4..22fc9ed9 100644 --- a/hw_device_mgr/cia_301/device.py +++ b/hw_device_mgr/cia_301/device.py @@ -98,6 +98,28 @@ def add_device_sdos(cls, sdo_data): """ cls.config_class.add_device_sdos(cls.munge_sdo_data(sdo_data)) + @classmethod + def munge_dcs_data(cls, dcs_data): + # Turn per-model name DCs data from YAML into per-model_id DCs data + res = dict() + for model_name, dcs in dcs_data.items(): + device_cls = cls.get_model_by_name(model_name) + model_id = device_cls.device_model_id() + res[model_id] = dcs + assert res + assert None not in res + return res + + @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(cls.munge_dcs_data(dcs_data)) + @classmethod def get_device(cls, address=None, **kwargs): registry = cls._address_registry.setdefault(cls.name, dict()) @@ -178,10 +200,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 936ba1ef..bf385275 100644 --- a/hw_device_mgr/cia_301/tests/base_test_class.py +++ b/hw_device_mgr/cia_301/tests/base_test_class.py @@ -27,6 +27,10 @@ class BaseCiA301TestClass(BaseTestClass): 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 sdo_class = CiA301SDO @@ -39,17 +43,21 @@ 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): """Create sim device objects with configured SDOs.""" - if cls.pass_init_sim_device_sdos: + if cls.pass_init_sim_device_description: # Init sim SDO data sdo_data = cls.load_sdo_data() print(f" Raw 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" Raw dcs_data from {cls.dcs_data_resource()}") + kwargs["dcs_data"] = cls.munge_dcs_data(dcs_data) # Init sim device data super().init_sim(**kwargs) @@ -243,6 +251,42 @@ def load_sdo_data(cls): 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.name] = 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 @@ -252,6 +296,7 @@ def pytest_generate_tests(self, metafunc): # *Note all three cases are mutually exclusive 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: @@ -259,19 +304,32 @@ 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.name]) + if "_dcs_data" in metafunc.fixturenames: + dev_vals.append(dcs_data[device_cls.name]) + 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(): vals.append(device_sdos) ids.append(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/dcs_data.yaml b/hw_device_mgr/cia_301/tests/dcs_data.yaml new file mode 100644 index 00000000..bb80ba24 --- /dev/null +++ b/hw_device_mgr/cia_301/tests/dcs_data.yaml @@ -0,0 +1,23 @@ +bogus_v1_servo: + - 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 +bogus_v2_servo: + - name: DC Sync + desc: DC for synchronization + assignactivate: 0x300 + cycletimesync0: 0 + shifttimesync0: 0 + - name: DC Off + desc: DC unused + assignactivate: 0x0 +bogus_v1_io: [] # No DCs diff --git a/hw_device_mgr/cia_301/tests/test_config.py b/hw_device_mgr/cia_301/tests/test_config.py index df1e4afc..dbbf622e 100644 --- a/hw_device_mgr/cia_301/tests/test_config.py +++ b/hw_device_mgr/cia_301/tests/test_config.py @@ -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("registered models:", list(config_cls._model_dcs)) + print("config_cls._model_dcs:", config_cls._model_dcs) + assert obj.model_id in config_cls._model_dcs + obj_dcs = obj._model_dcs[obj.model_id] + assert len(obj_dcs) == len(dcs_data) + for expected_dc in dcs_data: + assert expected_dc in obj_dcs From 269756840ae0b86627e1fb0e4dac6c2316e9723f Mon Sep 17 00:00:00 2001 From: John Morris Date: Sat, 11 Jun 2022 12:44:53 -0500 Subject: [PATCH 27/88] cia_301: Key SDO, DCs data by model_id, not name Command and config classes don't have access to device class `name` attribute. This was a design error. --- hw_device_mgr/cia_301/config.py | 1 + hw_device_mgr/cia_301/device.py | 18 ++------- .../cia_301/tests/base_test_class.py | 13 ++++--- hw_device_mgr/cia_301/tests/dcs_data.yaml | 38 +++++++++---------- hw_device_mgr/cia_301/tests/test_config.py | 10 ++--- 5 files changed, 34 insertions(+), 46 deletions(-) diff --git a/hw_device_mgr/cia_301/config.py b/hw_device_mgr/cia_301/config.py index 2304db35..55467dc6 100644 --- a/hw_device_mgr/cia_301/config.py +++ b/hw_device_mgr/cia_301/config.py @@ -116,6 +116,7 @@ def sdo(self, ix): 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 diff --git a/hw_device_mgr/cia_301/device.py b/hw_device_mgr/cia_301/device.py index 22fc9ed9..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,18 +98,6 @@ def add_device_sdos(cls, sdo_data): """ cls.config_class.add_device_sdos(cls.munge_sdo_data(sdo_data)) - @classmethod - def munge_dcs_data(cls, dcs_data): - # Turn per-model name DCs data from YAML into per-model_id DCs data - res = dict() - for model_name, dcs in dcs_data.items(): - device_cls = cls.get_model_by_name(model_name) - model_id = device_cls.device_model_id() - res[model_id] = dcs - assert res - assert None not in res - return res - @classmethod def add_device_dcs(cls, dcs_data): """ @@ -118,7 +106,7 @@ def add_device_dcs(cls, dcs_data): Pass to the `Config` class the information needed to configure DCs for this `model_id`. """ - cls.config_class.add_device_dcs(cls.munge_dcs_data(dcs_data)) + cls.config_class.add_device_dcs(dcs_data) @classmethod def get_device(cls, address=None, **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 bf385275..aeab041d 100644 --- a/hw_device_mgr/cia_301/tests/base_test_class.py +++ b/hw_device_mgr/cia_301/tests/base_test_class.py @@ -219,7 +219,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) @@ -282,7 +282,7 @@ def munge_dcs_data(cls, dcs_data): for test_category, dcs in dcs_data.items(): device_cls = cls.test_category_class(test_category) assert device_cls - new_dcs_data[device_cls.name] = dcs + new_dcs_data[device_cls.device_model_id()] = dcs assert new_dcs_data assert None not in new_dcs_data return new_dcs_data @@ -312,18 +312,19 @@ def pytest_generate_tests(self, metafunc): device_cls = self.test_category_class(dev["test_category"]) assert device_cls is not None if "_sdo_data" in metafunc.fixturenames: - dev_vals.append(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.name]) + 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) 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(): diff --git a/hw_device_mgr/cia_301/tests/dcs_data.yaml b/hw_device_mgr/cia_301/tests/dcs_data.yaml index bb80ba24..1c15672d 100644 --- a/hw_device_mgr/cia_301/tests/dcs_data.yaml +++ b/hw_device_mgr/cia_301/tests/dcs_data.yaml @@ -1,23 +1,21 @@ bogus_v1_servo: - - 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 + - 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 - - name: DC Off - desc: DC unused - assignactivate: 0x0 + - 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/test_config.py b/hw_device_mgr/cia_301/tests/test_config.py index dbbf622e..509da02c 100644 --- a/hw_device_mgr/cia_301/tests/test_config.py +++ b/hw_device_mgr/cia_301/tests/test_config.py @@ -74,10 +74,10 @@ def test_write_config_param_values(self, obj, sdo_data): def test_add_device_dcs(self, obj, config_cls, dcs_data): print("model_id:", obj.model_id) - print("registered models:", list(config_cls._model_dcs)) print("config_cls._model_dcs:", config_cls._model_dcs) - assert obj.model_id in config_cls._model_dcs - obj_dcs = obj._model_dcs[obj.model_id] - assert len(obj_dcs) == len(dcs_data) + 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 expected_dc in obj_dcs + assert dict(expected_dc) in obj.dcs() From d12f0f7e62ba3653d90833c70f21e31da13879d1 Mon Sep 17 00:00:00 2001 From: John Morris Date: Wed, 15 Jun 2022 14:18:03 -0500 Subject: [PATCH 28/88] cia_301: Update device config & tests - Generate device config as class method - Update test data for self-consistency --- hw_device_mgr/cia_301/config.py | 45 +- .../cia_301/tests/base_test_class.py | 4 +- .../cia_301/tests/device_config.yaml | 49 +- hw_device_mgr/cia_301/tests/sim_sdo_data.yaml | 524 +++++++++++++++++- hw_device_mgr/cia_301/tests/test_config.py | 8 +- 5 files changed, 561 insertions(+), 69 deletions(-) diff --git a/hw_device_mgr/cia_301/config.py b/hw_device_mgr/cia_301/config.py index 55467dc6..3f2193d8 100644 --- a/hw_device_mgr/cia_301/config.py +++ b/hw_device_mgr/cia_301/config.py @@ -35,7 +35,6 @@ class CiA301Config: 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): @@ -206,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` @@ -239,27 +237,28 @@ def munge_config(cls, config_raw, position): # 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, self.position) - - # 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/tests/base_test_class.py b/hw_device_mgr/cia_301/tests/base_test_class.py index aeab041d..46a44cda 100644 --- a/hw_device_mgr/cia_301/tests/base_test_class.py +++ b/hw_device_mgr/cia_301/tests/base_test_class.py @@ -52,11 +52,11 @@ def init_sim(cls, **kwargs): if cls.pass_init_sim_device_description: # Init sim SDO data sdo_data = cls.load_sdo_data() - print(f" Raw sdo_data from {cls.sdo_data_resource()}") + 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" Raw dcs_data from {cls.dcs_data_resource()}") + 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) diff --git a/hw_device_mgr/cia_301/tests/device_config.yaml b/hw_device_mgr/cia_301/tests/device_config.yaml index 6d755d00..9ada91ac 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,7 +170,7 @@ pdo_mapping: index: 1600h entries: - - index: 7000h + - index: 7000-01h bits: - out0 - out1 @@ -210,7 +181,7 @@ pdo_mapping: index: 1A00h entries: - - index: 6000h + - index: 6000-01h bits: - in0 - in1 diff --git a/hw_device_mgr/cia_301/tests/sim_sdo_data.yaml b/hw_device_mgr/cia_301/tests/sim_sdo_data.yaml index a84b3911..dd74545f 100644 --- a/hw_device_mgr/cia_301/tests/sim_sdo_data.yaml +++ b/hw_device_mgr/cia_301/tests/sim_sdo_data.yaml @@ -1,5 +1,272 @@ # 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 @@ -65,6 +332,234 @@ bogus_v1_servo: 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 @@ -129,4 +624,31 @@ bogus_v2_servo: index_name: Position limit default_value: 500000 pdo_mapping: R -bogus_v1_io: {} # Only PDOs +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 509da02c..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] From 6bf1cbc929c7686ab978d08102be1acb687adf69 Mon Sep 17 00:00:00 2001 From: John Morris Date: Mon, 20 Jun 2022 14:30:50 -0500 Subject: [PATCH 29/88] cia_301: Add trivial test for lcec --- hw_device_mgr/cia_301/tests/device_config.yaml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/hw_device_mgr/cia_301/tests/device_config.yaml b/hw_device_mgr/cia_301/tests/device_config.yaml index 9ada91ac..fa8bc140 100644 --- a/hw_device_mgr/cia_301/tests/device_config.yaml +++ b/hw_device_mgr/cia_301/tests/device_config.yaml @@ -175,6 +175,8 @@ - out0 - out1 - out2 + # Unused bits + - bitLen: 5 '3': # TPDOs: Transmit PDOs dir: in # from drive @@ -186,5 +188,7 @@ - in0 - in1 - in2 + # Unused bits + - bitLen: 5 # (No parameters) From 1bf6c144ba210c3cda4457a30e4c5c05871efaa9 Mon Sep 17 00:00:00 2001 From: John Morris Date: Fri, 3 Jun 2022 16:41:54 -0500 Subject: [PATCH 30/88] devices: Migrate YAML access to ConfigIO and `importlib` refs --- hw_device_mgr/devices/bogus.py | 1 + hw_device_mgr/devices/device_xml/__init__.py | 0 hw_device_mgr/devices/elmo_gold.py | 1 + hw_device_mgr/devices/inovance_is620n.py | 1 + hw_device_mgr/devices/inovance_sv660.py | 1 + hw_device_mgr/devices/itegva_e7x.py | 1 + hw_device_mgr/devices/tests/base_test_class.py | 6 +++--- hw_device_mgr/devices/tests/device_xml | 1 - 8 files changed, 8 insertions(+), 4 deletions(-) create mode 100644 hw_device_mgr/devices/device_xml/__init__.py delete mode 120000 hw_device_mgr/devices/tests/device_xml 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/__init__.py b/hw_device_mgr/devices/device_xml/__init__.py new file mode 100644 index 00000000..e69de29b 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..75223d8b 100644 --- a/hw_device_mgr/devices/tests/base_test_class.py +++ b/hw_device_mgr/devices/tests/base_test_class.py @@ -21,6 +21,6 @@ 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" 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 From ff32bb0b65e593fa363a95a02f9a2bd93a8081f9 Mon Sep 17 00:00:00 2001 From: John Morris Date: Mon, 20 Jun 2022 15:46:49 -0500 Subject: [PATCH 31/88] cia_301: Fix tests to work without `test_category` Real device configs should be usable in tests --- hw_device_mgr/cia_301/tests/base_test_class.py | 3 +++ 1 file changed, 3 insertions(+) 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 46a44cda..8918f4b3 100644 --- a/hw_device_mgr/cia_301/tests/base_test_class.py +++ b/hw_device_mgr/cia_301/tests/base_test_class.py @@ -75,6 +75,9 @@ def munge_device_config(cls, device_config): """ 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) From c2b29143ed02bf492c6fe6218120b9e81864a07d Mon Sep 17 00:00:00 2001 From: John Morris Date: Fri, 3 Jun 2022 16:57:12 -0500 Subject: [PATCH 32/88] ethercat: Migrate YAML and ESI XML to ConfigIO and `importlib` refs In addition to YAML configs, also move XML ESI files. Some rework in `hw_device_mgr.ethercat.xml_reader.EtherCATXMLReader` needed to support this. Use logger instead of `print()` in ESI parser so scripts can control output. --- hw_device_mgr/ethercat/config.py | 9 ++- hw_device_mgr/ethercat/device.py | 25 ++----- .../ethercat/tests/base_test_class.py | 65 +++++++++---------- .../ethercat/tests/bogus_devices/device.py | 1 + .../bogus_devices/device_xml/BogusIO.xml | 1 - .../bogus_devices/device_xml/BogusServo.xml | 1 - .../ethercat/tests/relocatable_esi_device.py | 23 +++---- hw_device_mgr/ethercat/tests/test_device.py | 8 ++- hw_device_mgr/ethercat/xml_reader.py | 53 +++++++++------ 9 files changed, 93 insertions(+), 93 deletions(-) delete mode 120000 hw_device_mgr/ethercat/tests/bogus_devices/device_xml/BogusIO.xml delete mode 120000 hw_device_mgr/ethercat/tests/bogus_devices/device_xml/BogusServo.xml diff --git a/hw_device_mgr/ethercat/config.py b/hw_device_mgr/ethercat/config.py index df5d1898..9edf1b13 100644 --- a/hw_device_mgr/ethercat/config.py +++ b/hw_device_mgr/ethercat/config.py @@ -27,10 +27,15 @@ class EtherCATConfig(CiA301Config): # @classmethod - def get_device_sdos_from_esi(cls, esi_path): + def get_device_sdos_from_esi(cls, package, fname): """Read in device configuration from ESI file at `esi_path`.""" esi_reader = cls.esi_reader_class() - return esi_reader.add_device_descriptions(esi_path) + if package: + return esi_reader.add_device_descriptions_from_resource( + package, fname + ) + else: + return esi_reader.add_device_descriptions_from_path(fname) class EtherCATSimConfig(EtherCATConfig, CiA301SimConfig): diff --git a/hw_device_mgr/ethercat/device.py b/hw_device_mgr/ethercat/device.py index c9f26fe0..9734e043 100644 --- a/hw_device_mgr/ethercat/device.py +++ b/hw_device_mgr/ethercat/device.py @@ -18,7 +18,8 @@ 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 @@ -45,28 +46,14 @@ def set_params_volatile(self, nv=False): Concrete subclasses may optionally implement this """ - @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): 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 + ) sdo_data.update(dev_sdo_data) return sdo_data diff --git a/hw_device_mgr/ethercat/tests/base_test_class.py b/hw_device_mgr/ethercat/tests/base_test_class.py index 2e18ff84..4b53faf9 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 @@ -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..942a8ced 100644 --- a/hw_device_mgr/ethercat/tests/relocatable_esi_device.py +++ b/hw_device_mgr/ethercat/tests/relocatable_esi_device.py @@ -4,18 +4,15 @@ 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 - - @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 + alt_xml_description = None @classmethod - def orig_xml_description_path(cls): - return super().xml_description_path() + def read_device_sdos_from_esi(cls): + 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 + ) + sdo_data.update(dev_sdo_data) + return sdo_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..2d8e7789 100644 --- a/hw_device_mgr/ethercat/xml_reader.py +++ b/hw_device_mgr/ethercat/xml_reader.py @@ -1,16 +1,19 @@ from .sdo import EtherCATSDO +from ..config_io import ConfigIO +from ..logging import Logging from lxml import etree from pprint import pprint __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__) @classmethod def str_to_int(cls, s): @@ -167,9 +170,7 @@ 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 @@ -412,7 +413,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 @@ -457,14 +458,6 @@ def read_objects(self, device): # 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 - sdo_translations = dict( # Translate SDO data from read_objects() to SDOs.add_sdo() args Index="index", @@ -492,12 +485,9 @@ def add_sdo(self, sdos, data): subidx = sdo["subindex"] = dtc.uint8(sdo.pop("subindex") or 0) 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] - - self.fpath = fpath + def add_device_descriptions(self, stream): + """Parse ESI file stream and cache device information.""" + self.tree = etree.parse(stream) model_sdos = dict() for dxml in self.devices_xml: sdos = dict() @@ -510,7 +500,28 @@ def add_device_descriptions(self, fpath): sdo_data = self.read_objects(dxml) for sd in sdo_data: self.add_sdo(sdos, sd) - self._fpath_registry[fpath] = model_sdos + return model_sdos + + @classmethod + def open_device_description_resource(cls, package, resource): + return cls.open_resource(package, resource) + + @classmethod + def open_device_description_path(cls, path, *args, **kwargs): + return cls.open_path(path, *args, **kwargs) + + def add_device_descriptions_from_resource(self, package, resource): + """Parse ESI from package resource.""" + self.logger.info(f"Reading ESI from ({package}, {resource})") + with self.open_device_description_resource(package, resource) as f: + model_sdos = self.add_device_descriptions(f) + return model_sdos + + def add_device_descriptions_from_path(self, fpath): + """Parse ESI from XML file path.""" + self.logger.info(f"Reading ESI from {fpath}") + with self.open_device_description_path(fpath) as f: + model_sdos = self.add_device_descriptions(f) return model_sdos @classmethod From bd4a90c709837efada89c9bb33ce799bdfb4596e Mon Sep 17 00:00:00 2001 From: John Morris Date: Tue, 7 Jun 2022 16:37:14 -0500 Subject: [PATCH 33/88] ethercat: Parse ESI tags; redo file reading logic To best generate lcec `ethercat.xml` files, the device's distributed clock config should come from the ESI file. Adding that showed that the ESI reading methods were tied to the SDO parser and not reusable. Those are now separated out into `classmethod`s, and cached as well. Also, added more complete plumbing for the XML parser `LcId` param. --- hw_device_mgr/ethercat/config.py | 46 ++++++-- hw_device_mgr/ethercat/device.py | 41 +++++-- .../ethercat/tests/base_test_class.py | 2 +- .../ethercat/tests/relocatable_esi_device.py | 15 ++- hw_device_mgr/ethercat/xml_reader.py | 104 ++++++++++++------ 5 files changed, 156 insertions(+), 52 deletions(-) diff --git a/hw_device_mgr/ethercat/config.py b/hw_device_mgr/ethercat/config.py index 9edf1b13..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,19 +24,50 @@ class EtherCATConfig(CiA301Config): command_class = EtherCATCommand # - # Object dictionary + # Device ESI # @classmethod - def get_device_sdos_from_esi(cls, package, fname): - """Read in device configuration from ESI file at `esi_path`.""" - esi_reader = cls.esi_reader_class() + @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: - return esi_reader.add_device_descriptions_from_resource( - package, fname + esi_reader = esi_reader_class.read_from_resource( + package, fname, LcId=LcId ) else: - return esi_reader.add_device_descriptions_from_path(fname) + 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/device.py b/hw_device_mgr/ethercat/device.py index 9734e043..7bc70246 100644 --- a/hw_device_mgr/ethercat/device.py +++ b/hw_device_mgr/ethercat/device.py @@ -26,9 +26,10 @@ class EtherCATDevice(CiA301Device, abc.ABC): 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): @@ -47,20 +48,22 @@ def set_params_volatile(self, nv=False): """ @classmethod - def read_device_sdos_from_esi(cls): + 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( - dev.xml_description_package, dev.xml_description_fname + 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 @@ -68,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 @@ -80,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. @@ -88,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/tests/base_test_class.py b/hw_device_mgr/ethercat/tests/base_test_class.py index 4b53faf9..c7a07dbd 100644 --- a/hw_device_mgr/ethercat/tests/base_test_class.py +++ b/hw_device_mgr/ethercat/tests/base_test_class.py @@ -36,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): diff --git a/hw_device_mgr/ethercat/tests/relocatable_esi_device.py b/hw_device_mgr/ethercat/tests/relocatable_esi_device.py index 942a8ced..816ce635 100644 --- a/hw_device_mgr/ethercat/tests/relocatable_esi_device.py +++ b/hw_device_mgr/ethercat/tests/relocatable_esi_device.py @@ -7,12 +7,23 @@ class RelocatableESIDevice(EtherCATSimDevice): alt_xml_description = None @classmethod - def read_device_sdos_from_esi(cls): + 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 + None, dev.alt_xml_description, LcId=LcId ) sdo_data.update(dev_sdo_data) 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( + None, dev.alt_xml_description, LcId=LcId + ) + dcs_data.update(dev_dcs_data) + return dcs_data diff --git a/hw_device_mgr/ethercat/xml_reader.py b/hw_device_mgr/ethercat/xml_reader.py index 2d8e7789..ddb18e53 100644 --- a/hw_device_mgr/ethercat/xml_reader.py +++ b/hw_device_mgr/ethercat/xml_reader.py @@ -3,6 +3,7 @@ from ..logging import Logging from lxml import etree from pprint import pprint +from functools import lru_cache __all__ = ("EtherCATXMLReader",) @@ -26,8 +27,9 @@ def str_to_int(cls, s): 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): @@ -485,45 +487,81 @@ def add_sdo(self, sdos, data): subidx = sdo["subindex"] = dtc.uint8(sdo.pop("subindex") or 0) sdos[idx, subidx] = sdo - def add_device_descriptions(self, stream): - """Parse ESI file stream and cache device information.""" - self.tree = etree.parse(stream) + # 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")) + uint32 = self.data_type_class.uint32 + model_id = tuple(uint32(i) for i in (self.vendor_id, product_code)) + return model_id + + @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.add_sdo(reg["sdos"], sd) return model_sdos - @classmethod - def open_device_description_resource(cls, package, resource): - return cls.open_resource(package, resource) + @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 - def open_device_description_path(cls, path, *args, **kwargs): - return cls.open_path(path, *args, **kwargs) - - def add_device_descriptions_from_resource(self, package, resource): - """Parse ESI from package resource.""" - self.logger.info(f"Reading ESI from ({package}, {resource})") - with self.open_device_description_resource(package, resource) as f: - model_sdos = self.add_device_descriptions(f) - return model_sdos - - def add_device_descriptions_from_path(self, fpath): - """Parse ESI from XML file path.""" - self.logger.info(f"Reading ESI from {fpath}") - with self.open_device_description_path(fpath) as f: - model_sdos = self.add_device_descriptions(f) - return model_sdos + @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) From fa9ee6c1905055b42e7e6a3a6191c73b4e295a1e Mon Sep 17 00:00:00 2001 From: John Morris Date: Fri, 3 Jun 2022 16:57:12 -0500 Subject: [PATCH 34/88] lcec: Migrate YAML and ESI XML to ConfigIO and `importlib` refs --- hw_device_mgr/lcec/tests/bogus_devices/device.py | 1 + hw_device_mgr/lcec/tests/bogus_devices/device_xml | 1 - 2 files changed, 1 insertion(+), 1 deletion(-) delete mode 120000 hw_device_mgr/lcec/tests/bogus_devices/device_xml 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 From 173e8f2fae28522cb6b2a97d0cf3a0dc35d6d891 Mon Sep 17 00:00:00 2001 From: John Morris Date: Sat, 11 Jun 2022 12:50:39 -0500 Subject: [PATCH 35/88] devices: Add DC tests - Improve ESI Dc OpMode test cases - Add DC test YAML data --- .../devices/device_xml/BogusServo.xml | 18 +++++-- .../devices/tests/base_test_class.py | 1 + hw_device_mgr/devices/tests/dcs_data.yaml | 47 +++++++++++++++++++ 3 files changed, 62 insertions(+), 4 deletions(-) create mode 100644 hw_device_mgr/devices/tests/dcs_data.yaml diff --git a/hw_device_mgr/devices/device_xml/BogusServo.xml b/hw_device_mgr/devices/device_xml/BogusServo.xml index 2c2d6c5e..796c847c 100644 --- a/hw_device_mgr/devices/device_xml/BogusServo.xml +++ b/hw_device_mgr/devices/device_xml/BogusServo.xml @@ -287,13 +287,18 @@ - DC - Distributed Clock + DC Sync + DC for synchronization #x300 0 0 0 + + DC Off + DC unused + 0 + @@ -565,13 +570,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/tests/base_test_class.py b/hw_device_mgr/devices/tests/base_test_class.py index 75223d8b..e44367ec 100644 --- a/hw_device_mgr/devices/tests/base_test_class.py +++ b/hw_device_mgr/devices/tests/base_test_class.py @@ -24,3 +24,4 @@ class BaseDevicesTestClass(BaseLCECTestClass): 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 From d7a9ed48becbc3c10200b69846c48e906e0818f0 Mon Sep 17 00:00:00 2001 From: John Morris Date: Thu, 19 May 2022 00:29:46 -0500 Subject: [PATCH 36/88] mgr: Migrate YAML access to ConfigIO and `importlib` refs --- hw_device_mgr/mgr/tests/base_test_class.py | 16 ++++++++++------ hw_device_mgr/mgr/tests/bogus_devices/device_xml | 1 - hw_device_mgr/mgr/tests/test_mgr.py | 3 --- 3 files changed, 10 insertions(+), 10 deletions(-) delete mode 120000 hw_device_mgr/mgr/tests/bogus_devices/device_xml 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() From f17cbd8b083fd7f05b4cff117d03c92f94652ca4 Mon Sep 17 00:00:00 2001 From: John Morris Date: Tue, 14 Jun 2022 14:15:54 -0500 Subject: [PATCH 37/88] ethercat: ESI partial support for PDO parsing ESI descriptions may have `TxPdo` or `RxPdo` elements with `Entry` subelements referring to dictionary objects not in the `Dictionary` definition. This commit parses those entries and adds them to the SDO list. It also adds default datatypes for an ESI with no `Dictionary` definition at all. Still missing is parsing the PDO mapping from those elements. Also: - Add `BITARR8` data type (iTegva) - Clean up int formatting - Add extra sanity checks --- hw_device_mgr/ethercat/data_types.py | 2 +- hw_device_mgr/ethercat/esi_base_types.xml | 46 ++++++++ hw_device_mgr/ethercat/xml_reader.py | 133 ++++++++++++++++++---- 3 files changed, 160 insertions(+), 21 deletions(-) create mode 100644 hw_device_mgr/ethercat/esi_base_types.xml 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/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/xml_reader.py b/hw_device_mgr/ethercat/xml_reader.py index ddb18e53..2f5b3882 100644 --- a/hw_device_mgr/ethercat/xml_reader.py +++ b/hw_device_mgr/ethercat/xml_reader.py @@ -16,6 +16,9 @@ class EtherCATXMLReader(ConfigIO): 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): if s.startswith("#x"): @@ -23,6 +26,11 @@ 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 @@ -112,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 @@ -134,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"}: @@ -178,7 +190,7 @@ def vendor_xml(self): @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): @@ -267,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") @@ -276,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): @@ -291,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): @@ -310,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( @@ -383,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"): @@ -454,12 +477,77 @@ 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 + 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 Index="index", @@ -482,9 +570,9 @@ 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 # DC definitions @@ -522,8 +610,9 @@ def read_dc_opmodes(self, device): 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")) - uint32 = self.data_type_class.uint32 - model_id = tuple(uint32(i) for i in (self.vendor_id, product_code)) + model_id = tuple( + self.uint(i, 32) for i in (self.vendor_id, product_code) + ) return model_id @lru_cache @@ -535,8 +624,12 @@ def parse_sdos(self): 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: + 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 From 55dbfba7884b5619d64beb3b153578e687024176 Mon Sep 17 00:00:00 2001 From: John Morris Date: Thu, 19 May 2022 00:29:46 -0500 Subject: [PATCH 38/88] mgr_ros: Migrate YAML access to ConfigIO and `importlib` refs --- hw_device_mgr/mgr_ros/mgr.py | 20 ++++++------------- .../mgr_ros/tests/base_test_class.py | 7 ++----- .../mgr_ros/tests/bogus_devices/device_xml | 1 - hw_device_mgr/mgr_ros/tests/test_mgr.py | 1 + 4 files changed, 9 insertions(+), 20 deletions(-) delete mode 120000 hw_device_mgr/mgr_ros/tests/bogus_devices/device_xml diff --git a/hw_device_mgr/mgr_ros/mgr.py b/hw_device_mgr/mgr_ros/mgr.py index 2f720967..d73b2930 100644 --- a/hw_device_mgr/mgr_ros/mgr.py +++ b/hw_device_mgr/mgr_ros/mgr.py @@ -1,11 +1,11 @@ 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 +44,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", From 9bcf0f7e679daca91f2a0d95f493b0ec471aef3e Mon Sep 17 00:00:00 2001 From: John Morris Date: Wed, 15 Jun 2022 15:06:31 -0500 Subject: [PATCH 39/88] lcec: Generate `ethercat.conf.xml` configuration from bus scan --- hw_device_mgr/lcec/config.py | 99 ++++++++++++++++++++++ hw_device_mgr/lcec/tests/ethercat.conf.xml | 94 ++++++++++++++++++++ hw_device_mgr/lcec/tests/test_config.py | 31 ++++++- 3 files changed, 223 insertions(+), 1 deletion(-) create mode 100644 hw_device_mgr/lcec/tests/ethercat.conf.xml diff --git a/hw_device_mgr/lcec/config.py b/hw_device_mgr/lcec/config.py index d8ca37f1..bd0cc860 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,104 @@ 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: + continue + 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/tests/ethercat.conf.xml b/hw_device_mgr/lcec/tests/ethercat.conf.xml new file mode 100644 index 00000000..4ec51f56 --- /dev/null +++ b/hw_device_mgr/lcec/tests/ethercat.conf.xml @@ -0,0 +1,94 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/hw_device_mgr/lcec/tests/test_config.py b/hw_device_mgr/lcec/tests/test_config.py index 3ee6526c..cc42fc04 100644 --- a/hw_device_mgr/lcec/tests/test_config.py +++ b/hw_device_mgr/lcec/tests/test_config.py @@ -2,7 +2,36 @@ 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) + 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) From 02edf82a16f2c4b0ae6e1c3461b27c3d5a925f8a Mon Sep 17 00:00:00 2001 From: John Morris Date: Fri, 3 Jun 2022 16:57:12 -0500 Subject: [PATCH 40/88] mgr_hal: Migrate YAML access to ConfigIO and `importlib` refs --- hw_device_mgr/mgr_hal/tests/bogus_devices/device_xml | 1 - hw_device_mgr/mgr_ros/mgr.py | 1 - 2 files changed, 2 deletions(-) delete mode 120000 hw_device_mgr/mgr_hal/tests/bogus_devices/device_xml 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 d73b2930..6afbb47f 100644 --- a/hw_device_mgr/mgr_ros/mgr.py +++ b/hw_device_mgr/mgr_ros/mgr.py @@ -1,7 +1,6 @@ from ..mgr.mgr import HWDeviceMgr, SimHWDeviceMgr from ..config_io import ConfigIO import rclpy -import os import traceback From 663a4d68e9d4bd8d0ca0fd215fbd9fab821879b6 Mon Sep 17 00:00:00 2001 From: John Morris Date: Mon, 20 Jun 2022 14:29:30 -0500 Subject: [PATCH 41/88] lcec: Add finer control over elements --- hw_device_mgr/lcec/config.py | 10 +++++++--- hw_device_mgr/lcec/tests/ethercat.conf.xml | 3 +++ hw_device_mgr/lcec/tests/test_config.py | 1 + 3 files changed, 11 insertions(+), 3 deletions(-) diff --git a/hw_device_mgr/lcec/config.py b/hw_device_mgr/lcec/config.py index bd0cc860..e54ecd11 100644 --- a/hw_device_mgr/lcec/config.py +++ b/hw_device_mgr/lcec/config.py @@ -105,10 +105,14 @@ def gen_ethercat_xml(cls, bus_configs=dict()): "complexEntry", bitLen="1" ) pdo_entry_xml.append(complex_entry_xml) - if bit is None: + if bit is None: # Unused bit continue - complex_entry_xml.set("halType", "bit") - complex_entry_xml.set("halPin", bit) + 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) diff --git a/hw_device_mgr/lcec/tests/ethercat.conf.xml b/hw_device_mgr/lcec/tests/ethercat.conf.xml index 4ec51f56..6c2712f4 100644 --- a/hw_device_mgr/lcec/tests/ethercat.conf.xml +++ b/hw_device_mgr/lcec/tests/ethercat.conf.xml @@ -1,3 +1,4 @@ + @@ -77,6 +78,7 @@ + @@ -86,6 +88,7 @@ + diff --git a/hw_device_mgr/lcec/tests/test_config.py b/hw_device_mgr/lcec/tests/test_config.py index cc42fc04..662b6231 100644 --- a/hw_device_mgr/lcec/tests/test_config.py +++ b/hw_device_mgr/lcec/tests/test_config.py @@ -15,6 +15,7 @@ def test_gen_ethercat_xml(self, config_cls, tmp_path): 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}") From e502cda92362dbca253491fae34f5267200d2b2f Mon Sep 17 00:00:00 2001 From: John Morris Date: Sun, 5 Jun 2022 12:13:20 -0500 Subject: [PATCH 42/88] mgr_ros_hal: Migrate YAML access to ConfigIO and `importlib` refs --- hw_device_mgr/mgr_ros_hal/device_xml | 1 - hw_device_mgr/mgr_ros_hal/tests/bogus_devices/device_xml | 1 - hw_device_mgr/mgr_ros_hal/tests/test_mgr.py | 4 +++- 3 files changed, 3 insertions(+), 3 deletions(-) delete mode 120000 hw_device_mgr/mgr_ros_hal/device_xml delete mode 120000 hw_device_mgr/mgr_ros_hal/tests/bogus_devices/device_xml 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 ] From be3285257702d19c729bff5139f5d6c6e0ea7047 Mon Sep 17 00:00:00 2001 From: John Morris Date: Wed, 15 Jun 2022 14:28:42 -0500 Subject: [PATCH 43/88] devices: Update Bogus device ESI descriptions - Add PDO mapping objects to `BogusServo.xml` - Add note to `BogusIO.xml` --- hw_device_mgr/devices/device_xml/BogusIO.xml | 1 + .../devices/device_xml/BogusServo.xml | 1495 ++++++++++++++++- 2 files changed, 1478 insertions(+), 18 deletions(-) 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 @@ -104,29 +614,249 @@ - #x6040 - Control word - UINT - 16 + #x1600 + RxPDO Map 1 + DT1600 + 496 - #x0000 - #xFFFF - #x0000 + + 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 + + - - rw - R - + + #x1601 + 2nd receive PDO-Mapping + DT1601 + 80 + - #x6041 - Status word - UINT - 16 + #x1a00 + TxPDO Map 1 + DT1A00 + 496 - #x0000 - #xFFFF - #x0000 + + 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 + UINT + 16 + + #x0000 + #xFFFF + #x0000 + + + rw + R + + + + #x6041 + Status word + UINT + 16 + + #x0000 + #xFFFF + #x0000 ro @@ -348,6 +1078,515 @@ 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 @@ -386,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 From b8449053c7129a4bb66230e3a8890d482b7fd2fa Mon Sep 17 00:00:00 2001 From: John Morris Date: Thu, 23 Jun 2022 16:13:31 -0500 Subject: [PATCH 44/88] devices: Add error message translations to Inovance drives --- hw_device_mgr/devices/device_err/__init__.py | 0 .../devices/device_err/inovance_is620n.yaml | 489 +++++++++++++++++ .../devices/device_err/inovance_sv660n.yaml | 495 ++++++++++++++++++ hw_device_mgr/devices/inovance_is620n.py | 5 +- hw_device_mgr/devices/inovance_sv660.py | 5 +- setup.py | 1 + 6 files changed, 993 insertions(+), 2 deletions(-) create mode 100644 hw_device_mgr/devices/device_err/__init__.py create mode 100644 hw_device_mgr/devices/device_err/inovance_is620n.yaml create mode 100644 hw_device_mgr/devices/device_err/inovance_sv660n.yaml diff --git a/hw_device_mgr/devices/device_err/__init__.py b/hw_device_mgr/devices/device_err/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/hw_device_mgr/devices/device_err/inovance_is620n.yaml b/hw_device_mgr/devices/device_err/inovance_is620n.yaml new file mode 100644 index 00000000..7d27b67c --- /dev/null +++ b/hw_device_mgr/devices/device_err/inovance_is620n.yaml @@ -0,0 +1,489 @@ +# From Inovance drive manual, table 9.2.1 "Fault Code List" (type=1,2) +# and table 9.2.2 "Warning Code List" (type=3) +# +# display: "Display" column; code displayed on drive LED panel +# description: "Fault Name" column +# type: "Type" column; indicates severity, with 1 most severe +# resettable: "Resettable" column; indicates whether resettable +# without power cycle +# error_code: "603Fh (Error Code)" column +# auxiliary_code: "203Fh (Auxiliary Code)" column; may have +# multiple values + +'0x0101': + description: Parameter abnormal + type: 1 + resettable: false + display: 101 + error_code: 0x6320 + auxiliary_code: 0x01010101 +'0x0102': + description: Programmable logic configuration fault + type: 1 + resettable: false + display: 102 + error_code: 0x7500 + auxiliary_code: 0x01020102 +'0x0103': + description: FPGA software version too early + type: 1 + resettable: false + display: 103 + error_code: 0x7500 + auxiliary_code: 0x01030103 +'0x0104': + description: Programmable logic interruption fault + type: 1 + resettable: false + display: 104 + error_code: 0x7500 + auxiliary_code: 0x01040104 0x01000104 0x0E940104 +'0x0105': + description: Internal program abnormal + type: 1 + resettable: false + display: 105 + error_code: 0x6320 + auxiliary_code: 0x01050105 +'0x0108': + description: Parameter storage fault + type: 1 + resettable: false + display: 108 + error_code: 0x5530 + auxiliary_code: 0x01080108 +'0x0111': + description: Group 2000h/2001h parameter abnormal + type: 1 + resettable: false + display: 111 + error_code: 0x6320 + auxiliary_code: 0x01110111 +'0x0120': + description: Product model matching fault + type: 1 + resettable: false + display: 120 + error_code: 0x7122 + auxiliary_code: 0x01200120 +'0x0121': + description: Invalid S-ON command + type: 2 + resettable: true + display: 121 + error_code: 0x5441 + auxiliary_code: 0x01210121 +'0x0122': + description: Product matching fault in absolute position mode + type: 1 + resettable: false + display: 122 + error_code: 0x7122 + auxiliary_code: 0x01220122 +'0x0130': + description: Different DIs allocated with the same function + type: 1 + resettable: true + display: 130 + error_code: 0x6320 + auxiliary_code: 0x01300130 +'0x0131': + description: DO function No. exceeding the number of functions + type: 1 + resettable: true + display: 131 + error_code: 0x6320 + auxiliary_code: 0x01310131 +'0x0136': + description: Data check error or no parameter stored in the motor ROM + type: 1 + resettable: false + display: 136 + error_code: 0x7305 + auxiliary_code: 0x01360136 +'0x0200': + description: Overcurrent 1 + type: 1 + resettable: false + display: 200 + error_code: 0x2311 + auxiliary_code: 0x02000200 +'0x0201': + description: Overcurrent 2 + type: 1 + resettable: false + display: 201 + error_code: 0x2312 + auxiliary_code: 0x02010201 +'0x0207': + description: Shaft D/Q current overflow + type: 1 + resettable: true + display: 207 + error_code: 0x0FFF + auxiliary_code: 0x02070207 +'0x0208': + description: FPGA sampling operation timeout + type: 1 + resettable: false + display: 208 + error_code: 0x0FFF + auxiliary_code: 0x02080208 +'0x0210': + description: Output short-circuit to ground + type: 1 + resettable: false + display: 210 + error_code: 0x2330 + auxiliary_code: 0x02100210 +'0x0220': + description: Phase sequence incorrect + type: 1 + resettable: false + display: 220 + error_code: 0x0FFF + auxiliary_code: 0x02200220 +'0x0234': + description: Runaway + type: 1 + resettable: false + display: 234 + error_code: 0x0FFF + auxiliary_code: 0x02340234 +'0x0400': + description: Main circuit overvoltage + type: 1 + resettable: true + display: 400 + error_code: 0x3210 + auxiliary_code: 0x04000400 +'0x0410': + description: Main circuit undervoltage + type: 1 + resettable: true + display: 410 + error_code: 0x3220 + auxiliary_code: 0x04100410 +'0x0420': + description: Main circuit phase loss + type: 2 + resettable: true + display: 420 + error_code: 0x3130 + auxiliary_code: 0x04200420 +'0x0430': + description: Control power undervoltage + type: 1 + resettable: false + display: 430 + error_code: 0x3120 + auxiliary_code: 0x04300430 +'0x0500': + description: Motor overspeed + type: 1 + resettable: true + display: 500 + error_code: 0x8400 + auxiliary_code: 0x05000500 +'0x0510': + description: Pulse output overspeed + type: 2 + resettable: true + display: 510 + error_code: 0x0FFF + auxiliary_code: 0x05100510 +'0x0602': + description: Angle auto-tuning failure + type: 1 + resettable: true + display: 602 + error_code: 0x0FFF + auxiliary_code: 0x06020602 +'0x0610': + description: Servo drive overload + type: 2 + resettable: true + display: 610 + error_code: 0x3230 + auxiliary_code: 0x06100610 +'0x0620': + description: Motor overload + type: 2 + resettable: true + display: 620 + error_code: 0x3230 + auxiliary_code: 0x06200620 +'0x0630': + description: Motor rotor locked + type: 2 + resettable: true + display: 630 + error_code: 0x7121 + auxiliary_code: 0x06300630 +'0x0650': + description: Heatsink overheat + type: 2 + resettable: true + display: 650 + error_code: 0x4210 + auxiliary_code: 0x06500650 +'0x0731': + description: Encoder battery failed + type: 2 + resettable: true + display: 731 + error_code: 0x7305 + auxiliary_code: 0x07300731 +'0x0732': + description: Encoder multi-turn counting error + type: 2 + resettable: true + display: 733 + error_code: 0x7305 + auxiliary_code: 0x07300732 +'0x0733': + description: Encoder multi-turn counting overflow + type: 2 + resettable: true + display: 735 + error_code: 0x7305 + auxiliary_code: 0x07300733 +'0x0740': + description: Encoder interference + type: 1 + resettable: false + display: 740 + error_code: 0x7305 + auxiliary_code: 0x07400740 + solution: Caused by spurious inputs on the encoder Z wire. Can be caused by drive logic power-off (not an error), encoder wire damage, electromagnetic interference, or a bad encoder. +'0x0770': + description: External encoder scale fault + type: 1 + resettable: true + display: 770 + error_code: 0x7305 + auxiliary_code: 0x07700770 +'0x0A33': + description: Encoder data abnormal + type: 1 + resettable: false + display: A33 + error_code: 0x7305 + auxiliary_code: 0x0A330A33 +'0x0A34': + description: Encoder communication check abnormal + type: 1 + resettable: false + display: A34 + error_code: 0x7305 + auxiliary_code: 0x0A340A34 +'0x0A35': + description: Z signal lost + type: 1 + resettable: false + display: A35 + error_code: 0x7305 + auxiliary_code: 0x0A350A35 +'0x0b00': + description: Position deviation excess + type: 2 + resettable: true + display: B00 + error_code: 0x8611 + auxiliary_code: 0x0b000b00 +'0x0b01': + description: Position reference excess + type: 2 + resettable: YES + display: B01 + error_code: 0x0FFF + auxiliary_code: 0x0b010b01 +'0x0b02': + description: Position deviation exceeding threshold in fully closed-loop + type: 2 + resettable: true + display: B02 + error_code: 0x8611 + auxiliary_code: 0x0b020b02 +'0x0b03': + description: Electronic gear ratio setting exceeding limit + type: 2 + resettable: true + display: B03 + error_code: 0x6320 + auxiliary_code: 0x0b030b03 +'0x0B04': + description: Parameter setting error with fully closed-loop function + type: 2 + resettable: true + display: B04 + error_code: 0x6320 + auxiliary_code: 0x0B040B04 +'0x0d09': + description: Software upper/lower limit setting incorrect* + type: 2 + resettable: true + display: D09 + error_code: 0x6320 + auxiliary_code: 0x0d090d09 +'0x0d10': + description: Home offset setting incorrect* + type: 2 + resettable: true + display: D10 + error_code: 0x6320 + auxiliary_code: 0x0d100d10 +'0x0E07': + description: Network state abnormal switchover + type: 2 + resettable: true + display: E07 + error_code: 0x0FFF + auxiliary_code: 0x0E070E07 +'0x0E08': + description: Synchronization loss* + type: 2 + resettable: true + display: E08 + error_code: 0x0FFF + auxiliary_code: 0x0E080E08 +'0x0E11': + description: XML configuration file not burnt + type: 2 + resettable: true + display: E11 + error_code: 0x0FFF + auxiliary_code: 0x0E110E11 +'0x0E12': + description: Network initialization failure* + type: 2 + resettable: true + display: E12 + error_code: 0x0E12 + auxiliary_code: 0x0E120E12 +'0x0E13': + description: Synchronization cycle setting incorrect* + type: 2 + resettable: true + display: E13 + error_code: 0x0E13 + auxiliary_code: 0x0E130E13 +'0x0E15': + description: Synchronization cycle error being large* + type: 2 + resettable: true + display: E15 + error_code: 0x0E15 + auxiliary_code: 0x0E150E15 +'0x0110': + description: Setting error of frequency-division pulse output + type: 3 + resettable: true + display: 110 + error_code: 0x6320 + auxiliary_code: 0x01100110 +'0x0601': + description: Homing timeout + type: 3 + resettable: true + display: 601 + error_code: 0x0FFF + auxiliary_code: 0x06010601 +'0x0730': + description: Encoder battery warning + type: 3 + resettable: true + display: 730 + error_code: 0x7305 + auxiliary_code: 0x07300730 +'0x0900': + description: DI emergency braking + type: 3 + resettable: true + display: 900 + error_code: 0x5442 + auxiliary_code: 0x09000900 +'0x0909': + description: Motor overload warning + type: 3 + resettable: true + display: 909 + error_code: 0x3230 + auxiliary_code: 0x09090909 +'0x0920': + description: Regenerative resistor overload + type: 3 + resettable: true + display: 920 + error_code: 0x3210 + auxiliary_code: 0x09200920 +'0x0922': + description: Resistance of external braking resistor too small + type: 3 + resettable: true + display: 922 + error_code: 0x6320 + auxiliary_code: 0x09220922 +'0x0939': + description: Motor power cable breaking + type: 3 + resettable: true + display: 939 + error_code: 0x3331 + auxiliary_code: 0x09390939 +'0x0941': + description: Parameter modification taking effect only after power-on again + type: 3 + resettable: true + display: 941 + error_code: 0x6320 + auxiliary_code: 0x09410941 +'0x0942': + description: Parameter storage too frequent + type: 3 + resettable: true + display: 942 + error_code: 0x7600 + auxiliary_code: 0x09420942 +'0x0950': + description: Positive limit switch warning + type: 3 + resettable: true + display: 950 + error_code: 0x5443 + auxiliary_code: 0x09500950 +'0x0952': + description: Negative limit switch warning + type: 3 + resettable: true + display: 952 + error_code: 0x5444 + auxiliary_code: 0x09520952 +'0x0980': + description: Encoder internal fault + type: 3 + resettable: true + display: 980 + error_code: 0x7305 + auxiliary_code: 0x09800980 +'0x0990': + description: Power input phase loss warning + type: 3 + resettable: true + display: 990 + error_code: 0x3130 + auxiliary_code: 0x09900990 +'0x0998': + description: Homing mode setting incorrect + type: 3 + resettable: true + display: 998 + error_code: 0x0FFF + auxiliary_code: 0x09980998 +'0x0A40': + description: Parameter auto-tuning failure + type: 3 + resettable: true + display: A40 + error_code: 0x0FFF + auxiliary_code: 0x0A400A40 diff --git a/hw_device_mgr/devices/device_err/inovance_sv660n.yaml b/hw_device_mgr/devices/device_err/inovance_sv660n.yaml new file mode 100644 index 00000000..b5be4da5 --- /dev/null +++ b/hw_device_mgr/devices/device_err/inovance_sv660n.yaml @@ -0,0 +1,495 @@ +# From Inovance SV660N "Advanced User Guide", table 9.2 "Communication +# Faults and Warning Codes" +# +# Parameter 603Fh shows the basic error code, but the +# manufacturer-specific SV660N parameter 2008-2Eh "Inner error code" +# shows an extended error value. The manual's table 9.2 shows +# e.g. "fault" E101, with "display" broken down into E101.0 and E101.1 +# with different "name" and occasional differences in "fault range" +# column ("type" and "resettable or not" columns appear not to +# change). +# +# key: "Display" column code with decimal prepended (as 2008-2Eh) +# display: "Display" column; code displayed on drive LED panel +# description: "Name" column +# type: "Type" column; indicates severity, with 1 most severe +# resettable: "Resettable" column; indicates whether resettable +# without power cycle + +'0x0101': + display: E101.0 + description: System parameter error + type: 1 + resettable: false +'0x1101': + display: E101.1 + description: Parameters in group 2000h/2001h being abnormal + type: 1 + resettable: false +'0x0102': + display: E102.0 + description: Logic configuration fault + type: 1 + resettable: false +'0x8102': + display: E102.8 + description: Software version mismatch + type: 1 + resettable: false +'0x1104': + display: E104.1 + description: MCU running timeout + type: 1 + resettable: false +'0x2104': + display: E104.2 + description: Current loop running timeout + type: 1 + resettable: false +'0x4104': + display: E104.4 + description: Reference update timeout + type: 1 + resettable: false +'0x0105': + display: E105.0 + description: Internal program error + type: 1 + resettable: false +'0x0108': + display: E108.0 + description: Parameter write timeout + type: 1 + resettable: true +'0x1108': + display: E108.1 + description: Parameter read timeout + type: 1 + resettable: true +'0x2108': + display: E108.2 + description: Invalid check on data written in EEPROM + type: 1 + resettable: true +'0x3108': + display: E108.3 + description: Invalid check on data read in EEPROM + type: 1 + resettable: true +'0x0120': + display: E120.0 + description: Unknown encoder type + type: 1 + resettable: false +'0x1120': + display: E120.1 + description: Unknown motor model + type: 1 + resettable: false +'0x2120': + display: E120.2 + description: Unknown drive model + type: 1 + resettable: false +'0x5120': + display: E120.5 + description: Mismatch of the motor current and drive current + type: 1 + resettable: false +'0x6120': + description: Mismatch of FPGA and motor model + type: 1 + resettable: false +'0x0122': + display: E122.0 + description: Multi-turn absolute encoder setting error + type: 2 + resettable: true +'0x1122': + display: E122.1 + description: Different DIs allocated with the same function + type: 2 + resettable: true +'0x3122': + display: E122.3 + description: Upper limit invalid + type: 2 + resettable: true +'0x0136': + display: E136.0 + description: Encoder parameter error + type: 1 + resettable: false +'0x1136': + display: E136.1 + description: Encoder communication error + type: 1 + resettable: false +'0x0150': + display: E150.0 + description: STO signal input protection activated + type: 1 + resettable: true +'0x1150': + display: E150.1 + description: STO signal input error + type: 1 + resettable: true +'0x2150': + display: E150.2 + description: Abnormal voltage detected + type: 1 + resettable: true +'0x3150': + display: E150.3 + description: STO upstream optocoupler detection failure + type: 1 + resettable: true +'0x4150': + display: E150.4 + description: PWM Buffer detection failure + type: 1 + resettable: true +'0x0201': + display: E201.0 + description: Phase-P overcurrent + type: 1 + resettable: false +'0x1201': + display: E201.1 + description: Phase-U overcurrent + type: 1 + resettable: false +'0x2201': + display: E201.2 + description: Phase-V overcurrent + type: 1 + resettable: false +'0x4201': + display: E201.4 + description: Phase-N overcurrent + type: 1 + resettable: false +'0x0208': + display: E208.0 + description: MCU position reference updated frequently + type: 1 + resettable: true +'0x2208': + display: E208.2 + description: Encoder communication timeout + type: 1 + resettable: true +'0x3208': + display: E208.3 + description: Current sampling fault + type: 1 + resettable: true +'0x4208': + display: E208.4 + description: FPGA current loop operation timeout + type: 1 + resettable: true +'0x0210': + display: E210.0 + description: Output shorted to ground + type: 1 + resettable: false +'0x0234': + display: E234.0 + description: Runaway protection + type: 1 + resettable: false +'0x0400': + display: E400.0 + description: Main circuit overvoltage + type: 1 + resettable: true +'0x0410': + display: E410.0 + description: Main circuit undervoltage + type: 1 + resettable: true +'0x0420': + display: E420.0 + description: Phase loss + type: 2 + resettable: true +'0x1420': + display: E420.1 + description: PL signal error + type: 2 + resettable: true +'0x0500': + display: E500.0 + description: Motor overspeed + type: 1 + resettable: true +'0x1500': + display: E500.1 + description: Speed feedback overflow + type: 1 + resettable: true +'0x0602': + display: E602.0 + description: Angle auto-tuning error + type: 1 + resettable: true +'0x2602': + display: E602.2 + description: Wrong UVW phase sequence detected during angle auto-tuning + type: 1 + resettable: true +'0x0620': + display: E620.0 + description: Motor overload + type: 1 + resettable: true +'0x0630': + display: E630.0 + description: Locked rotor + type: 1 + resettable: true +'0x0640': + display: E640.0 + description: IGBT over-temperature + type: 1 + resettable: true +'0x0650': + display: E650.0 + description: Heatsink over-temperature + type: 1 + resettable: true +'0x0661': + display: E661.0 + description: Auto-tuned gain values too low +'0x0731': + display: E731.0 + description: Encoder battery failure + type: 2 + resettable: true +'0x0733': + display: E733.0 + description: Encoder multi-turn counting error + type: 2 + resettable: true +'0x0735': + display: E735.0 + description: Encoder multi-turn counting overflow + type: 2 + resettable: true +'0x2740': + display: E740.2 + description: Absolute encoder error + type: 1 + resettable: false +'0x3740': + display: E740.3 + description: Absolute encoder single-turn calculation error + type: 1 + resettable: false +'0x6740': + display: E740.6 + description: Encoder write error + type: 1 + resettable: false +'0x0B00': + display: EB00.0 + description: Encoder read/write check error + type: 2 + resettable: true +'0x0A33': + display: EA33.0 + description: Position deviation too large + type: 1 + resettable: false +'0x1B00': + display: EB00.1 + description: Position deviation overflow + type: 2 + resettable: true +'0x1B01': + display: EB01.1 + description: Position reference increment too large for once + type: 2 + resettable: true +'0x2B01': + display: EB01.2 + description: Position reference increment too large continuously + type: 2 + resettable: true +'0x3B01': + display: EB01.3 + description: Reference overflow + type: 2 + resettable: true +'0x4B01': + display: EB01.4 + description: Reference value beyond the single-turn position limits in the absolute mode + type: 2 + resettable: true +'0x0E09': + display: EE09.0 + description: Software limit setting error + type: 2 + resettable: true +'0x1E09': + display: EE09.1 + description: Home setting error + type: 2 + resettable: true +'0x2E09': + display: EE09.2 + description: Gear ratio over the limit + type: 2 + resettable: true +'0x3E09': + display: EE09.3 + description: No synchronization signal + type: 2 + resettable: true +'0x5E09': + display: EE09.5 + description: PDO mapping over the limit + type: 2 + resettable: true +'0x0E08': + display: EE08.0 + description: Synchronization loss + type: 2 + resettable: true +'0x1E08': + display: EE08.1 + description: Network status switchover error + type: 2 + resettable: true +'0x2E08': + display: EE08.2 + description: IRQ loss + type: 2 + resettable: true +'0x0E11': + display: EE11.0 + description: ESI check error + type: 2 + resettable: true +'0x1E11': + display: EE11.1 + description: Unsuccessful reading of EEPROM + type: 2 + resettable: true +'0x2E11': + display: EE11.2 + description: Unsuccessful update of EEPROM + type: 2 + resettable: true +'0x0E12': + display: EE12.0 + description: External devices of EtherCAT being abnormal + type: 1 + resettable: false +'0x0E13': + display: EE13.0 + description: Synchronization cycle setting error + type: 2 + resettable: true +'0x0E15': + display: EE15.0 + description: Synchronization cycle error too large + type: 2 + resettable: true +'0x0121': + display: E121.0 + description: Invalid S-ON command + type: 3 + resettable: true +'0x0600': + display: E600.0 + description: Inertia auto-tuning failure + type: 3 + resettable: true +'0x0601': + display: E601.0 + description: Homing warning + type: 3 + resettable: true +'0x1601': + display: E601.1 + description: Home switch error + type: 3 + resettable: true +'0x0730': + display: E730.0 + description: Encoder battery warning + type: 3 + resettable: true +'0x0900': + display: E900.0 + description: Emergency stop + type: 3 + resettable: true +'0x0902': + display: E902.0 + description: Invalid DI setting + type: 1 + resettable: true +'0x1902': + display: E902.1 + description: Invalid DO setting + type: 1 + resettable: true +'0x2902': + display: E902.2 + description: Invalid torque reached setting + type: 1 + resettable: true +'0x0908': + display: E908.0 + description: Invalid check byte of model identification + type: 1 + resettable: true +'0x0909': + display: E909.0 + description: Motor overload + type: 1 + resettable: true +'0x0920': + display: E920.0 + description: Regenerative resistor overload + type: 1 + resettable: true +'0x0922': + display: E922.0 + description: Resistance of external regenerative resistor too small + type: 1 + resettable: true +'0x0924': + display: E924.0 + description: Braking transistor over-temperature + type: 1 + resettable: true +'0x0941': + display: E941.0 + description: Parameter modifications not activated + type: 1 + resettable: true +'0x0942': + display: E942.0 + description: Parameter saved frequently + type: 1 + resettable: true +'0x0950': + display: E950.0 + description: Forward overtravel + type: 1 + resettable: true +'0x0952': + display: E952.0 + description: Reverse overtravel + type: 1 + resettable: true +'0x4E09': + display: EE09.4 + description: Homing method setting error + type: 1 + resettable: true diff --git a/hw_device_mgr/devices/inovance_is620n.py b/hw_device_mgr/devices/inovance_is620n.py index 6e310e05..7cd4ebf2 100644 --- a/hw_device_mgr/devices/inovance_is620n.py +++ b/hw_device_mgr/devices/inovance_is620n.py @@ -1,14 +1,17 @@ from ..ethercat.device import EtherCATDevice from ..cia_402.device import CiA402Device +from ..errors.device import ErrorDevice -class InovanceIS620N(EtherCATDevice, CiA402Device): +class InovanceIS620N(EtherCATDevice, CiA402Device, ErrorDevice): """Inovance IS620N servo drives.""" vendor_id = 0x00100000 product_code = 0x000C0108 xml_description_package = "hw_device_mgr.devices.device_xml" xml_description_fname = "IS620N_v2.6.7.xml" + device_error_package = "hw_device_mgr.devices.device_err" + device_error_yaml = "inovance_is620n.yaml" def set_params_volatile(self, nv=False): # 0: params not updated diff --git a/hw_device_mgr/devices/inovance_sv660.py b/hw_device_mgr/devices/inovance_sv660.py index 95089a74..3e94602b 100644 --- a/hw_device_mgr/devices/inovance_sv660.py +++ b/hw_device_mgr/devices/inovance_sv660.py @@ -1,14 +1,17 @@ from ..ethercat.device import EtherCATDevice from ..cia_402.device import CiA402Device +from ..errors.device import ErrorDevice -class InovanceSV660(EtherCATDevice, CiA402Device): +class InovanceSV660(EtherCATDevice, CiA402Device, ErrorDevice): """Inovance SV660 servo drives.""" vendor_id = 0x00100000 product_code = 0x000C010D xml_description_package = "hw_device_mgr.devices.device_xml" xml_description_fname = "SV660_EOE_1Axis_V9.12.xml" + device_error_package = "hw_device_mgr.devices.device_err" + device_error_yaml = "inovance_sv660n.yaml" def set_params_volatile(self, nv=False): # 0: params not updated diff --git a/setup.py b/setup.py index fc43bfce..93117cca 100644 --- a/setup.py +++ b/setup.py @@ -33,6 +33,7 @@ + [f"hw_device_mgr.{p}.tests.bogus_devices" for p in pkgs_bd] + [ "hw_device_mgr.devices.device_xml", + "hw_device_mgr.devices.device_err", ] ) From facf102c472b0c7f4ad57c9eeb2a379732da1d2c Mon Sep 17 00:00:00 2001 From: John Morris Date: Thu, 23 Jun 2022 19:05:36 -0500 Subject: [PATCH 45/88] cia_402: Fix tests for devices inheriting ErrorDevice --- hw_device_mgr/cia_402/tests/test_device.py | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/hw_device_mgr/cia_402/tests/test_device.py b/hw_device_mgr/cia_402/tests/test_device.py index d69d5cb2..b90be26a 100644 --- a/hw_device_mgr/cia_402/tests/test_device.py +++ b/hw_device_mgr/cia_402/tests/test_device.py @@ -9,6 +9,13 @@ class TestCiA402Device(_TestCiA301Device, BaseCiA402TestClass): # to simplify tests & test cases def read_update_write_conv_test_data(self): + if "error_code" in self.obj.feedback_out.get(): + # Account for some devices that inherit from ErrorDevice + self.test_data["feedback_in"].setdefault("error_code", 0x00000000) + self.test_data["feedback_out"].setdefault("error_code", 0x00000000) + self.test_data["feedback_out"].setdefault("description", "No error") + self.test_data["feedback_out"].setdefault("advice", "No error") + if not self.is_402_device: return uint16 = self.device_class.data_type_class.uint16 From 9ffd5b702c822251831fc38bc9beed96eac9b090 Mon Sep 17 00:00:00 2001 From: John Morris Date: Tue, 28 Jun 2022 14:42:33 -0500 Subject: [PATCH 46/88] base class: Add "fault" to device feedback_out interface --- hw_device_mgr/device.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/hw_device_mgr/device.py b/hw_device_mgr/device.py index 2f9ed336..c4525b84 100644 --- a/hw_device_mgr/device.py +++ b/hw_device_mgr/device.py @@ -22,7 +22,9 @@ class Device(abc.ABC): command_out_data_types = dict() feedback_in_defaults = dict() - feedback_out_defaults = dict(goal_reached=True, goal_reason="Reached") + feedback_out_defaults = dict( + goal_reached=True, goal_reason="Reached", fault=False + ) command_in_defaults = dict() command_out_defaults = dict() From 249cb1564687a78edee97efdc243bb51298f8267 Mon Sep 17 00:00:00 2001 From: John Morris Date: Thu, 23 Jun 2022 19:04:55 -0500 Subject: [PATCH 47/88] mgr: Fix tests for devices inheriting ErrorDevice --- hw_device_mgr/mgr/tests/test_mgr.py | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/hw_device_mgr/mgr/tests/test_mgr.py b/hw_device_mgr/mgr/tests/test_mgr.py index 45739888..65494e45 100644 --- a/hw_device_mgr/mgr/tests/test_mgr.py +++ b/hw_device_mgr/mgr/tests/test_mgr.py @@ -38,6 +38,22 @@ def test_category_registry(self): pass def read_update_write_conv_test_data(self): + for i, d in enumerate(self.obj.devices): + if "error_code" in d.feedback_out.get(): + # Account for some devices that inherit from ErrorDevice + self.test_data["feedback_in"].setdefault( + f"drive_{i}_error_code", 0x00000000 + ) + self.test_data["feedback_out"].setdefault( + f"drive_{i}_error_code", 0x00000000 + ) + self.test_data["feedback_out"].setdefault( + f"drive_{i}_description", "No error" + ) + self.test_data["feedback_out"].setdefault( + f"drive_{i}_advice", "No error" + ) + uint16 = self.device_class.data_type_class.uint16 control_mode_int = CiA402Device.control_mode_int for data in (self.test_data, self.ovr_data): From d59d920629c8c66186c24af3223fdb4846ff2c0c Mon Sep 17 00:00:00 2001 From: John Morris Date: Tue, 28 Jun 2022 14:47:38 -0500 Subject: [PATCH 48/88] cia_301: Add 'fault' to feedback_out test cases --- hw_device_mgr/cia_301/tests/read_update_write.cases.yaml | 1 + 1 file changed, 1 insertion(+) diff --git a/hw_device_mgr/cia_301/tests/read_update_write.cases.yaml b/hw_device_mgr/cia_301/tests/read_update_write.cases.yaml index 079dc193..4f962cb8 100644 --- a/hw_device_mgr/cia_301/tests/read_update_write.cases.yaml +++ b/hw_device_mgr/cia_301/tests/read_update_write.cases.yaml @@ -11,6 +11,7 @@ oper: False goal_reached: False goal_reason: Offline + fault: False command_in: online: True oper: True From 0ee3b34460fb5cede2f9bf85027ef5b2b79b2de1 Mon Sep 17 00:00:00 2001 From: John Morris Date: Wed, 29 Jun 2022 17:44:50 -0500 Subject: [PATCH 49/88] cia_402: Set `fault` feedback on status word fault bit --- hw_device_mgr/cia_402/device.py | 14 ++++++++------ .../cia_402/tests/read_update_write.cases.yaml | 4 ++++ 2 files changed, 12 insertions(+), 6 deletions(-) diff --git a/hw_device_mgr/cia_402/device.py b/hw_device_mgr/cia_402/device.py index 8ccf35f4..5c637338 100644 --- a/hw_device_mgr/cia_402/device.py +++ b/hw_device_mgr/cia_402/device.py @@ -65,13 +65,13 @@ def control_mode_str(cls, mode): # Status word bits not used for CiA402 state machine operation may # have other purposes sw_extra_bits = dict( - # READY_TO_SWITCH_ON=0, # (CiA402) - # SWITCH_ON=1, # (CiA402) - # OPERATION_ENABLED=2, # (CiA402) - # FAULT=3, # (CiA402) + READY_TO_SWITCH_ON=0, # (CiA402) + SWITCH_ON=1, # (CiA402) + OPERATION_ENABLED=2, # (CiA402) + FAULT=3, # (CiA402) VOLTAGE_ENABLED=4, - # QUICK_STOP_ACTIVE=5, # (CiA402) - # SWITCH_ON_DISABLED=6, # (CiA402) + QUICK_STOP_ACTIVE=5, # (CiA402) + SWITCH_ON_DISABLED=6, # (CiA402) WARNING=7, # (CiA402) MANUFACTURER_SPECIFIC_1=8, REMOTE=9, @@ -171,6 +171,8 @@ def get_feedback(self): if sf.get(flag_name, False) != flag_val: goal_reached = False goal_reasons.append(f"state flag {flag_name} != {not flag_val}") + if self.test_sw_bit(sw, "FAULT"): + self.feedback_out.update(fault=True) if not goal_reached: goal_reason = "; ".join(goal_reasons) diff --git a/hw_device_mgr/cia_402/tests/read_update_write.cases.yaml b/hw_device_mgr/cia_402/tests/read_update_write.cases.yaml index 76e0aa90..525af957 100644 --- a/hw_device_mgr/cia_402/tests/read_update_write.cases.yaml +++ b/hw_device_mgr/cia_402/tests/read_update_write.cases.yaml @@ -171,6 +171,7 @@ status_word: 0x001F state: FAULT REACTION ACTIVE transition: TRANSITION_13 + fault: True goal_reached: False goal_reason: state FAULT REACTION ACTIVE (0x0000001F) != OPERATION ENABLED command_out: @@ -197,6 +198,7 @@ status_word: 0x0050 state: SWITCH ON DISABLED transition: TRANSITION_15 + fault: False goal_reason: state SWITCH ON DISABLED (0x00000050) != OPERATION ENABLED command_out: control_word: 0x0006 @@ -228,6 +230,7 @@ status_word: 0x001F state: FAULT REACTION ACTIVE transition: TRANSITION_13 + fault: True goal_reason: state FAULT REACTION ACTIVE (0x0000001F) != OPERATION ENABLED command_out: control_word: 0x0000 @@ -269,6 +272,7 @@ feedback_out: status_word: 0x0050 state: SWITCH ON DISABLED + fault: False transition: TRANSITION_15 command_out: control_word: 0x0000 From 15e0ca6fb082ca31844ebcced6d43ff5be8e6917 Mon Sep 17 00:00:00 2001 From: John Morris Date: Wed, 29 Jun 2022 15:38:46 -0500 Subject: [PATCH 50/88] cia_301: Move device parameter init into config class This needs to be done here instead of the `DeviceMgr` classes to keep the latter device agnostic. --- hw_device_mgr/cia_301/config.py | 77 ++++++++++++++++++---- hw_device_mgr/cia_301/device.py | 9 ++- hw_device_mgr/cia_301/tests/test_config.py | 4 +- 3 files changed, 72 insertions(+), 18 deletions(-) diff --git a/hw_device_mgr/cia_301/config.py b/hw_device_mgr/cia_301/config.py index 3f2193d8..d8cf9276 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, CiA301CommandException from .sdo import CiA301SDO +from ..logging import Logging from functools import cached_property @@ -27,6 +28,10 @@ class CiA301Config: command_class = CiA301Command sdo_class = CiA301SDO + init_params_nv = True + + logger = Logging(__name__) + # 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 @@ -150,22 +155,24 @@ def upload(self, sdo, **kwargs): ) return sdo.data_type(res_raw) - def download(self, sdo, val, dry_run=False, **kwargs): + def download(self, sdo, val, dry_run=False, force=False, **kwargs): # Get SDO object sdo = self.sdo(sdo) - # Check before setting value to avoid unnecessary NVRAM writes - res_raw = self.command().upload( - address=self.address, - index=sdo.index, - subindex=sdo.subindex, - datatype=sdo.data_type, - **kwargs, - ) - if sdo.data_type(res_raw) == val: - return # SDO value already correct + if not force: + # Check before setting value to avoid unnecessary NVRAM writes + res_raw = self.command().upload( + address=self.address, + index=sdo.index, + subindex=sdo.subindex, + datatype=sdo.data_type, + **kwargs, + ) + if sdo.data_type(res_raw) == val: + return # SDO value already correct if dry_run: self.logger.info(f"Dry run: download {val} to {sdo}") return + self.logger.info(f"{self} param download {sdo} = {val}") self.command().download( address=self.address, index=sdo.index, @@ -260,9 +267,53 @@ def gen_config(cls, model_id, address): def config(self): return self.gen_config(self.model_id, self.address) - def write_config_param_values(self): + def get_device_params_nv(self): + """ + Return whether device is in non-volatile params mode. + + Drives with parameter volatile/non-volatile mode must overload + this. + """ + return False + + def set_device_params_nv(self, nv=True, dry_run=False): + """ + Set device params to non-volatile/volatile mode. + + Drives with parameter volatile/non-volatile mode must overload + this. + """ + pass + + def initialize_params(self, dry_run=False): + if self.init_params_nv: + # To save NVRAM wear, don't write if all params are correct + all_correct = True + for sdo, value in self.config["param_values"].items(): + if self.upload(sdo) != value: + all_correct = False + break + if all_correct: + self.logger.info(f"{self} param values already correct") + return + # Save current NV mode setting & set NV mode + self._old_device_params_nv = self.get_device_params_nv() + self.logger.info(f"{self} setting device params in NV mode)") + self.set_device_params_nv(dry_run=dry_run) + else: + self.logger.info(f"{self} setting device params in volatile mode)") + + # Something needs changing for sdo, value in self.config["param_values"].items(): - self.download(sdo, value) + self.download(sdo, value, dry_run=dry_run, force=True) + + if self.init_params_nv and not self._old_device_params_nv: + self.logger.info( + f"{self} returning device params to volatile mode)" + ) + self.set_device_params_nv( + nv=self._old_device_params_nv, dry_run=dry_run + ) # # Scan bus device config factory diff --git a/hw_device_mgr/cia_301/device.py b/hw_device_mgr/cia_301/device.py index 2f76238e..93ad501c 100644 --- a/hw_device_mgr/cia_301/device.py +++ b/hw_device_mgr/cia_301/device.py @@ -1,6 +1,7 @@ from ..device import Device, SimDevice from .config import CiA301Config, CiA301SimConfig from .data_types import CiA301DataType +from functools import cached_property, lru_cache class CiA301Device(Device): @@ -30,6 +31,7 @@ def __init__(self, address=None, **kwargs): super().__init__(address=address, **kwargs) @classmethod + @lru_cache def device_model_id(cls): """ Return unique device model identifier. @@ -40,7 +42,7 @@ def device_model_id(cls): model_id = cls.vendor_id, cls.product_code return cls.config_class.format_model_id(model_id) - @property + @cached_property def model_id(self): return self.device_model_id() @@ -49,8 +51,9 @@ def set_device_config(cls, device_config): assert device_config cls.config_class.set_device_config(device_config) - def write_config_param_values(self): - self.config.write_config_param_values() + def init(self, **kwargs): + super().init(**kwargs) + self.config.initialize_params() def get_feedback(self): fb_out = super().get_feedback() diff --git a/hw_device_mgr/cia_301/tests/test_config.py b/hw_device_mgr/cia_301/tests/test_config.py index 866c6614..b6f8e294 100644 --- a/hw_device_mgr/cia_301/tests/test_config.py +++ b/hw_device_mgr/cia_301/tests/test_config.py @@ -55,7 +55,7 @@ def test_upload_download(self, obj, sdo_data): obj.download(sdo_ix, val + 1) assert obj.upload(sdo_ix) == val + 1 - def test_write_config_param_values(self, obj, sdo_data): + def test_initialize_params(self, obj, sdo_data): # Test fixture data: At least one config param value should # be different from default to make this test meaningful. (IO # devices have no config values, so ignore those.) @@ -68,7 +68,7 @@ def test_write_config_param_values(self, obj, sdo_data): # something_different |= (dev_val != conf_val) assert something_different or not obj.config["param_values"] - obj.write_config_param_values() + obj.initialize_params() for sdo_ix, val in obj.config["param_values"].items(): assert obj.upload(sdo_ix) == val From 898960d815b11e07018ce1d97c60d5b47e982e85 Mon Sep 17 00:00:00 2001 From: John Morris Date: Fri, 1 Jul 2022 12:55:13 -0500 Subject: [PATCH 51/88] cia_301: Separate out SDO loading to new method in tests The `ethercat` module can override this, since SDOs are loaded from the ESI instead of YAML files --- .../cia_301/tests/base_test_class.py | 29 ++++++++++--------- 1 file changed, 15 insertions(+), 14 deletions(-) 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 8918f4b3..9424476b 100644 --- a/hw_device_mgr/cia_301/tests/base_test_class.py +++ b/hw_device_mgr/cia_301/tests/base_test_class.py @@ -43,23 +43,24 @@ class BaseCiA301TestClass(BaseTestClass): BogusCiA301V1IO, ) - # Whether to pass SDO/DC data to device_class.init_sim() - pass_init_sim_device_description = True - @classmethod def init_sim(cls, **kwargs): """Create sim device objects with configured SDOs.""" - if cls.pass_init_sim_device_description: - # Init sim SDO data - 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) + super().init_sim(**cls.init_sim_sdo_kwargs(**kwargs)) + + @classmethod + def init_sim_sdo_kwargs(cls, **kwargs): + """Load SDO and DC data to pass to parent init_sim().""" + # Init sim SDO data + 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) + # Send back the result + return kwargs @classmethod def munge_device_config(cls, device_config): From 7480d91638f73852d9687f683d81f15690b8f349 Mon Sep 17 00:00:00 2001 From: John Morris Date: Wed, 29 Jun 2022 15:58:07 -0500 Subject: [PATCH 52/88] ethercat: Remove device param NV methods These are pushed to the `cia_301` config base class & implemented in the `devices` module. --- hw_device_mgr/ethercat/device.py | 17 ++--------------- 1 file changed, 2 insertions(+), 15 deletions(-) diff --git a/hw_device_mgr/ethercat/device.py b/hw_device_mgr/ethercat/device.py index 7bc70246..5d50166d 100644 --- a/hw_device_mgr/ethercat/device.py +++ b/hw_device_mgr/ethercat/device.py @@ -10,9 +10,8 @@ class EtherCATDevice(CiA301Device, abc.ABC): Device instances are addressed by `(master, position)`. - Device model subclasses have matching XML description, methods - (e.g. set params volatile) and other features specific to that - model. + Device model subclasses have matching XML description and other features + specific to that model. """ # Resource names for locating device description XML and error files @@ -39,14 +38,6 @@ def master(self): def position(self): return self.address[1] - @abc.abstractmethod - def set_params_volatile(self, nv=False): - """ - Set device params volatile or non-volatile. - - Concrete subclasses may optionally implement this - """ - @classmethod def read_device_sdos_from_esi(cls, LcId="1033"): sdo_data = dict() @@ -96,10 +87,6 @@ class EtherCATSimDevice(EtherCATDevice, CiA301SimDevice): def __init__(self, **kwargs): super().__init__(**kwargs) - self.params_volatile = False - - def set_params_volatile(self, nv=False): - self.params_volatile = not nv @classmethod def init_sim(cls, LcId="1033", **kwargs): From 26705a0df5cda9fd3bb1e7a4efa4035e100dd5e6 Mon Sep 17 00:00:00 2001 From: John Morris Date: Fri, 1 Jul 2022 12:57:05 -0500 Subject: [PATCH 53/88] ethercat: Override SDO loading in tests --- hw_device_mgr/ethercat/tests/base_test_class.py | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/hw_device_mgr/ethercat/tests/base_test_class.py b/hw_device_mgr/ethercat/tests/base_test_class.py index c7a07dbd..44a54747 100644 --- a/hw_device_mgr/ethercat/tests/base_test_class.py +++ b/hw_device_mgr/ethercat/tests/base_test_class.py @@ -36,7 +36,12 @@ class BaseEtherCATTestClass(BaseCiA402TestClass): ) ] ) - pass_init_sim_device_description = False # Data from ESI file + + @classmethod + def init_sim_sdo_kwargs(cls, **kwargs): + """SDO and DC data are generated from ESI.""" + # Send back the result + return kwargs @property def model_id_clone_map(self): From 77f42c4e2e6103026ce6de601c8a41c8d106d620 Mon Sep 17 00:00:00 2001 From: John Morris Date: Wed, 29 Jun 2022 15:56:46 -0500 Subject: [PATCH 54/88] devices: Move Inovance device param NV methods to config class --- hw_device_mgr/devices/inovance_is620n.py | 36 +++++++++++++++++++----- hw_device_mgr/devices/inovance_sv660.py | 36 +++++++++++++++++++----- 2 files changed, 58 insertions(+), 14 deletions(-) diff --git a/hw_device_mgr/devices/inovance_is620n.py b/hw_device_mgr/devices/inovance_is620n.py index 7cd4ebf2..4d99af15 100644 --- a/hw_device_mgr/devices/inovance_is620n.py +++ b/hw_device_mgr/devices/inovance_is620n.py @@ -1,8 +1,36 @@ from ..ethercat.device import EtherCATDevice +from ..ethercat.config import EtherCATConfig from ..cia_402.device import CiA402Device from ..errors.device import ErrorDevice +class InovanceIS620NConfig(EtherCATConfig): + """Inovance IS620N servo drive config.""" + + # Device params non-volatile setting in 200C-0Eh: + # 0: params not updated + # 1: 2000h series changed from serial or EtherCAT saved + # 2: 6000h series changed from EtherCAT (only) saved + # 3: 2000h and 6000h series changed from EtherCAT (only) saved + + def get_device_params_nv(self): + return self.config.upload("200C-0Eh") == 3 + + def set_device_params_nv(self, nv=True, dry_run=False): + curr_setting = self.get_device_params_nv() + if curr_setting != nv: + self.logger.info( + f"{self} setting params {'non' if nv else ''}volatile mode" + ) + self.config.download( + "200C-0Eh", (0, 3)[nv], force=True, dry_run=dry_run + ) + else: + self.logger.info( + f"{self} params already {'non' if nv else ''}volatile mode" + ) + + class InovanceIS620N(EtherCATDevice, CiA402Device, ErrorDevice): """Inovance IS620N servo drives.""" @@ -12,10 +40,4 @@ class InovanceIS620N(EtherCATDevice, CiA402Device, ErrorDevice): xml_description_fname = "IS620N_v2.6.7.xml" device_error_package = "hw_device_mgr.devices.device_err" device_error_yaml = "inovance_is620n.yaml" - - def set_params_volatile(self, nv=False): - # 0: params not updated - # 1: 2000h series changed from serial or EtherCAT saved - # 2: 6000h series changed from EtherCAT (only) saved - # 3: 2000h and 6000h series changed from EtherCAT (only) saved - self.config.download("200C-0Eh", (0, 3)[nv]) + config_class = InovanceIS620NConfig diff --git a/hw_device_mgr/devices/inovance_sv660.py b/hw_device_mgr/devices/inovance_sv660.py index 3e94602b..1948b1bf 100644 --- a/hw_device_mgr/devices/inovance_sv660.py +++ b/hw_device_mgr/devices/inovance_sv660.py @@ -1,8 +1,36 @@ from ..ethercat.device import EtherCATDevice +from ..ethercat.config import EtherCATConfig from ..cia_402.device import CiA402Device from ..errors.device import ErrorDevice +class InovanceSV660Config(EtherCATConfig): + """Inovance SV660 servo drive config.""" + + # Device params non-volatile setting in 200E-02h: + # 0: params not updated + # 1: 2000h series changed from serial or EtherCAT saved + # 2: 6000h series changed from EtherCAT (only) saved + # 3: 2000h and 6000h series changed from EtherCAT (only) saved + + def get_device_params_nv(self): + return self.config.upload("200E-02h") == 3 + + def set_device_params_nv(self, nv=True, dry_run=False): + curr_setting = self.get_device_params_nv() + if curr_setting != nv: + self.logger.info( + f"{self} setting params {'non' if nv else ''}volatile mode" + ) + self.config.download( + "200E-02h", (0, 3)[nv], force=True, dry_run=dry_run + ) + else: + self.logger.info( + f"{self} params already {'non' if nv else ''}volatile mode" + ) + + class InovanceSV660(EtherCATDevice, CiA402Device, ErrorDevice): """Inovance SV660 servo drives.""" @@ -12,10 +40,4 @@ class InovanceSV660(EtherCATDevice, CiA402Device, ErrorDevice): xml_description_fname = "SV660_EOE_1Axis_V9.12.xml" device_error_package = "hw_device_mgr.devices.device_err" device_error_yaml = "inovance_sv660n.yaml" - - def set_params_volatile(self, nv=False): - # 0: params not updated - # 1: 2000h series changed from serial or EtherCAT saved - # 2: 6000h series changed from EtherCAT (only) saved - # 3: 2000h and 6000h series changed from EtherCAT (only) saved - self.config.download("200E-02h", (0, 3)[nv]) + config_class = InovanceSV660Config From c196dc8f9be64b27098581a3d2f6c161dea972d8 Mon Sep 17 00:00:00 2001 From: John Morris Date: Tue, 28 Jun 2022 16:48:41 -0500 Subject: [PATCH 55/88] cia_402: Perform homing in Device class The `HWDeviceMgr` class is meant to be abstracted from the details of drive operation so that it can manage a wide range of devices. However, it has been directly managing the control & status word flags, which are specific to CiA 402 devices. Moreover, this makes the manager's state machine very much more complex, with many steps between e.g. stop -> start, start -> home, etc. This commit moves the homing routine into the drive class, replacing the control & status word flags with `home_request` command and `home_success` and `home_error` feedback. --- hw_device_mgr/cia_402/device.py | 115 ++++++++++----- .../tests/read_update_write.cases.yaml | 131 +++++++----------- 2 files changed, 135 insertions(+), 111 deletions(-) diff --git a/hw_device_mgr/cia_402/device.py b/hw_device_mgr/cia_402/device.py index 5c637338..b06d1295 100644 --- a/hw_device_mgr/cia_402/device.py +++ b/hw_device_mgr/cia_402/device.py @@ -3,6 +3,7 @@ CiA301DataType, CiA301SimDevice, ) +from time import time class CiA402Device(CiA301Device): @@ -13,11 +14,14 @@ class CiA402Device(CiA301Device): state machine to the goal `state` (e.g. `OPERATION ENABLED`), and will manage other goals. - Goal parameters: + Command parameters: - `state`: The CiA 402 goal state - `control_mode`: Drive control mode, e.g. `MODE_CSP` - - `control_word_flags`: Control word bits to set - - `state_flags`: Status word bits to match for `goal_reached` feedback + - `home_request`: Command homing operation + + Feedback parameters: + - `home_success`: Drive completed homing successfully + - `home_error`: Drive reports homing error """ data_types = CiA301DataType @@ -37,6 +41,8 @@ class CiA402Device(CiA301Device): MODE_CST = 10 DEFAULT_CONTROL_MODE = MODE_CSP + home_timeout = 15.0 # seconds + @classmethod def control_mode_int(cls, mode): """ @@ -72,7 +78,7 @@ def control_mode_str(cls, mode): VOLTAGE_ENABLED=4, QUICK_STOP_ACTIVE=5, # (CiA402) SWITCH_ON_DISABLED=6, # (CiA402) - WARNING=7, # (CiA402) + WARNING=7, MANUFACTURER_SPECIFIC_1=8, REMOTE=9, TARGET_REACHED=10, @@ -80,7 +86,7 @@ def control_mode_str(cls, mode): OPERATION_MODE_SPECIFIC_1=12, # HM=HOMING_ATTAINED OPERATION_MODE_SPECIFIC_2=13, # HM=HOMING_ERROR; others=FOLLOWING_ERROR MANUFACTURER_SPECIFIC_2=14, - HOMING_COMPLETED=15, + MANUFACTURER_SPECIFIC_3=15, ) # Incoming feedback from drives: param_name : data_type @@ -99,14 +105,25 @@ def control_mode_str(cls, mode): feedback_out_defaults = dict( state="START", transition=None, - state_flags={bit: False for bit in sw_extra_bits}, + home_success=False, + home_error=False, **feedback_in_defaults, ) feedback_out_initial_values = dict( status_word=0, control_mode_fb="MODE_NA", + home_success=False, + home_error=False, ) + @classmethod + def test_sw_bit(cls, word, name): + return bool(word & (1 << cls.sw_extra_bits[name])) + + @classmethod + def test_cw_bit(cls, word, name): + return bool(word & (1 << cls.cw_extra_bits[name])) + def get_feedback(self): fb_out = super().get_feedback() @@ -126,7 +143,7 @@ def get_feedback(self): cm_str = self.control_mode_str(cm) self.feedback_out.update(status_word=sw, control_mode_fb=cm_str) cm_cmd = self.command_in.get("control_mode") - if cm_str != cm_cmd: + if cm != self.MODE_HM and cm_str != cm_cmd: goal_reached = False goal_reasons.append(f"control_mode {cm_str} != {cm_cmd}") @@ -160,25 +177,44 @@ def get_feedback(self): else: self.feedback_out.update(transition=None) - # Calculate 'state_flags' feedback from status word - sf = { - flag: bool(sw & (1 << bitnum)) - for flag, bitnum in self.sw_extra_bits.items() - } - self.feedback_out.update(state_flags=sf) - sf_cmd = self.command_in.get("state_flags") - for flag_name, flag_val in sf_cmd.items(): - if sf.get(flag_name, False) != flag_val: + # Calculate homing status + home_success = home_error = False + if self.command_in.get("home_request"): + if self.test_sw_bit(sw, "OPERATION_MODE_SPECIFIC_2"): + # HOMING_ERROR bit set + home_error = True + goal_reached = False + goal_reasons.append("homing error") + elif time() - self._home_request_start > self.home_timeout: goal_reached = False - goal_reasons.append(f"state flag {flag_name} != {not flag_val}") + goal_reasons.append( + f"homing timeout after {self.home_timeout}s" + ) + self.feedback_out.update(fault=True) + home_error = True + if self.test_sw_bit(sw, "OPERATION_MODE_SPECIFIC_1"): + # HOMING_ATTAINED bit set + home_success = True + elif cm != self.MODE_HM: + goal_reached = False + goal_reasons.append( + f"homing requested, but still in {cm_str} mode" + ) + else: + goal_reached = False + goal_reasons.append("homing not complete") + self.feedback_out.update( + home_success=home_success, home_error=home_error + ) if self.test_sw_bit(sw, "FAULT"): self.feedback_out.update(fault=True) if not goal_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}") + self.logger.debug( + f"Device {self.address}: Goal not reached: {goal_reason}" + ) return fb_out state_bits = { @@ -199,8 +235,7 @@ def get_feedback(self): command_in_defaults = dict( state="SWITCH ON DISABLED", control_mode=DEFAULT_CONTROL_MODE, - control_word_flags=dict(), - state_flags=dict(), # Required, even if not commanded + home_request=False, ) # ------- Command out ------- @@ -306,9 +341,25 @@ def _get_next_control_word(self): # Transitioning to next state control_word = self._get_transition_control_word() + # Check for home request + home_request = False + if self.command_in.get("home_request"): + if self.command_in.changed("home_request"): + # New request + self._home_request_start = time() + self.logger.info(f"{self}: Homing operation requested") + if self.feedback_out.get("control_mode_fb") == "MODE_HM": + # Only set HOMING_START control word bit in MODE_HM + home_request = True + elif self.command_in.changed("home_request"): # home_request cleared + self.logger.info(f"{self}: Homing operation complete") + # Add flags and return - flags = self.command_in.get("control_word_flags") - return self._add_control_word_flags(control_word, **flags) + return self._add_control_word_flags( + control_word, + # OPERATION_MODE_SPECIFIC_1 in MODE_HM = HOMING_START + OPERATION_MODE_SPECIFIC_1=home_request, + ) # Map drive states to control word that maintains the state. # `None` indicates hold state N/A in automatic transition states @@ -427,6 +478,9 @@ def _add_control_word_flags(cls, control_word, **flags): return control_word def _get_next_control_mode(self): + # If `home_request` is set, command homing mode + if self.command_in.get("home_request"): + return self.MODE_HM # Get control_mode from command_in; translate e.g. "MODE_CSP" to 8 cm = self.command_in.get("control_mode") return self.control_mode_int(cm) @@ -473,9 +527,12 @@ def set_sim_feedback(self): # Status word (incl. flags) status_word = 0x0000 if state == "START" else self.state_bits[state][1] - cw_flags = self._get_control_word_flags(control_word) + homing_attained = ( # In MODE_HM and HOMING_START set + self.feedback_in.get("control_mode_fb") == self.MODE_HM + and self.test_cw_bit(control_word, "OPERATION_MODE_SPECIFIC_1") + ) sw_flags = dict( - HOMING_COMPLETED=cw_flags["OPERATION_MODE_SPECIFIC_1"], + OPERATION_MODE_SPECIFIC_1=homing_attained, VOLTAGE_ENABLED=sfb.get("online"), ) status_word = self._add_status_word_flags(status_word, **sw_flags) @@ -553,11 +610,3 @@ def _add_status_word_flags(cls, status_word, **flags): status_word &= ~operand & 0xFFFF return status_word - - @classmethod - def _get_control_word_flags(cls, control_word): - # Get flags by name from control word; used in set_sim_feedback - flags = dict() - for name, bitnum in cls.cw_extra_bits.items(): - flags[name] = bool(control_word & 1 << bitnum) - return flags diff --git a/hw_device_mgr/cia_402/tests/read_update_write.cases.yaml b/hw_device_mgr/cia_402/tests/read_update_write.cases.yaml index 525af957..421c3a90 100644 --- a/hw_device_mgr/cia_402/tests/read_update_write.cases.yaml +++ b/hw_device_mgr/cia_402/tests/read_update_write.cases.yaml @@ -19,18 +19,10 @@ status_word: 0x0000 control_mode_fb: MODE_NA # Undefined state: START - state_flags: - VOLTAGE_ENABLED: False - WARNING: False - MANUFACTURER_SPECIFIC_1: False - REMOTE: False - TARGET_REACHED: False - INTERNAL_LIMIT_ACTIVE: False - OPERATION_MODE_SPECIFIC_1: False - OPERATION_MODE_SPECIFIC_2: False - MANUFACTURER_SPECIFIC_2: False - HOMING_COMPLETED: False transition: null + home_success: False + home_error: False + fault: False goal_reached: False goal_reason: Offline command_in: @@ -40,8 +32,7 @@ # CiA 402 state: SWITCH ON DISABLED control_mode: MODE_CSP - control_word_flags: {} - state_flags: {} + home_request: False command_out: # CiA 402 control_word: 0x0000 @@ -75,17 +66,6 @@ oper: True # CiA 402 status_word: 0x0010 # VOLTAGE_ENABLED - state_flags: # Set VOLTAGE_ENABLED - VOLTAGE_ENABLED: True - WARNING: False - MANUFACTURER_SPECIFIC_1: False - REMOTE: False - TARGET_REACHED: False - INTERNAL_LIMIT_ACTIVE: False - OPERATION_MODE_SPECIFIC_1: False - OPERATION_MODE_SPECIFIC_2: False - MANUFACTURER_SPECIFIC_2: False - HOMING_COMPLETED: False state: 'NOT READY TO SWITCH ON' transition: TRANSITION_0 goal_reason: control_mode MODE_NA != MODE_CSP; state NOT READY TO SWITCH ON (0x00000010) != SWITCH ON DISABLED @@ -404,25 +384,13 @@ - desc: "Disable disabled: Hold SWITCH ON DISABLED x2" # -# Controller walks drive through homing operation -# command_mode -> MODE_HM +# Controller moves to OPERATION ENABLED, then commands homing operation # SWITCH ON DISABLED -> OPERATION ENABLED +# command_mode -> MODE_HM # control_word set OPERATION_MODE_SPECIFIC_1 (HOMING_START) bit # status_word HOMING_COMPLETED bit set # OPERATION ENABLED -> SWITCH ON DISABLED -- desc: "Home: Set MODE_HM" - command_in: - # Controller commands homing mode - control_mode: MODE_HM - command_out: - control_mode: MODE_HM - sim_feedback: - control_mode_fb: MODE_HM - desc: "Home: Command OPERATION ENABLED; 402 state machine transition hold" - feedback_in: - control_mode_fb: MODE_HM - feedback_out: - control_mode_fb: MODE_HM command_in: # Controller commands enable state: OPERATION ENABLED @@ -455,7 +423,7 @@ control_word: 0x000F sim_feedback: status_word: 0x0037 -- desc: "Home: Command HOMING_START; 402 state machine transition 4" +- desc: "Home: 402 state machine transition 4; command home_request & MODE_HM" feedback_in: status_word: 0x0037 feedback_out: @@ -466,65 +434,72 @@ goal_reason: Reached command_in: # Controller commands homing start - control_word_flags: - OPERATION_MODE_SPECIFIC_1: True + home_request: True command_out: - control_word: 0x001F # OPERATION ENABLED + OPERATION_MODE_SPECIFIC_1 + control_mode: MODE_HM sim_feedback: - status_word: 0x8037 # OPERATION ENABLED + HOMING_COMPLETED -- desc: "Home: Homing completed; Command SWITCH ON DISABLED; 402 state machine transition hold" + control_mode_fb: MODE_HM +- desc: "Home: Drive MODE_HM; Command HOMING_START" feedback_in: - status_word: 0x8037 - feedback_out: - status_word: 0x8037 - state_flags: # HOMING_COMPLETED set - VOLTAGE_ENABLED: True - WARNING: False - MANUFACTURER_SPECIFIC_1: False - REMOTE: False - TARGET_REACHED: False - INTERNAL_LIMIT_ACTIVE: False - OPERATION_MODE_SPECIFIC_1: False - OPERATION_MODE_SPECIFIC_2: False - MANUFACTURER_SPECIFIC_2: False - HOMING_COMPLETED: True + control_mode_fb: MODE_HM + feedback_out: + control_mode_fb: MODE_HM transition: null + goal_reached: False + goal_reason: homing not complete + command_out: + control_word: 0x001F # OPERATION ENABLED + HOMING_START + sim_feedback: + status_word: 0x1037 # OPERATION ENABLED + HOMING_ATTAINED +- desc: "Home: HOMING_ATTAINED & home_success set; Command no home_request" + feedback_in: + status_word: 0x1037 # OPERATION ENABLED + HOMING_ATTAINED + feedback_out: + status_word: 0x1037 # OPERATION ENABLED + HOMING_ATTAINED + home_success: True + goal_reached: True + goal_reason: Reached command_in: + home_request: False + command_out: + control_word: 0x000F # OPERATION ENABLED + control_mode: MODE_CSP + sim_feedback: + control_mode_fb: MODE_CSP + status_word: 0x0037 # OPERATION ENABLED +- desc: "Home: Command disable" + feedback_in: + status_word: 0x0037 # OPERATION ENABLED + control_mode_fb: MODE_CSP + feedback_out: + status_word: 0x0037 # OPERATION ENABLED + control_mode_fb: MODE_CSP + home_success: False + command_in: + # Controller commands disable state: SWITCH ON DISABLED - control_word_flags: {} # Clear flags command_out: - control_word: 0x0002 + control_word: 0x0002 # QUICK STOP ACTIVE sim_feedback: - status_word: 0x0017 -- desc: "Home: Command disable; 402 state machine transition 11" + status_word: 0x0017 # QUICK STOP ACTIVE + VOLTAGE_ENABLED +- desc: "Home: 402 state machine transition 11" feedback_in: - status_word: 0x0017 + status_word: 0x0017 # QUICK STOP ACTIVE + VOLTAGE_ENABLED feedback_out: status_word: 0x0017 state: QUICK STOP ACTIVE transition: TRANSITION_11 - state_flags: # HOMING_COMPLETED cleared - VOLTAGE_ENABLED: True - WARNING: False - MANUFACTURER_SPECIFIC_1: False - REMOTE: False - TARGET_REACHED: False - INTERNAL_LIMIT_ACTIVE: False - OPERATION_MODE_SPECIFIC_1: False - OPERATION_MODE_SPECIFIC_2: False - MANUFACTURER_SPECIFIC_2: False - HOMING_COMPLETED: False goal_reached: False goal_reason: state QUICK STOP ACTIVE (0x00000017) != SWITCH ON DISABLED command_out: - control_word: 0x0000 + control_word: 0x0000 # SWITCH ON DISABLED sim_feedback: - status_word: 0x0050 + status_word: 0x0050 # SWITCH ON DISABLED + VOLTAGE_ENABLED - desc: "Home: 402 state machine transition 12" feedback_in: - status_word: 0x0050 + status_word: 0x0050 # SWITCH ON DISABLED + VOLTAGE_ENABLED feedback_out: - status_word: 0x0050 + status_word: 0x0050 # SWITCH ON DISABLED + VOLTAGE_ENABLED state: SWITCH ON DISABLED transition: TRANSITION_12 goal_reached: True From 6a9168103ec663ffa3a89ea52801c1bde84559c5 Mon Sep 17 00:00:00 2001 From: John Morris Date: Fri, 1 Jul 2022 13:40:15 -0500 Subject: [PATCH 56/88] base device class: Init interfaces in init(), not constructor Move this step until later for the manager classes, which augment the base interface attributes --- hw_device_mgr/device.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/hw_device_mgr/device.py b/hw_device_mgr/device.py index c4525b84..5b052b8e 100644 --- a/hw_device_mgr/device.py +++ b/hw_device_mgr/device.py @@ -37,17 +37,16 @@ class Device(abc.ABC): def __init__(self, address=None): self.address = address - self.init_interfaces() def init(self): """ - Initialize device. + Initialize device and interfaces. Subclasses may implement `init()` for extra initialization outside the constructor. Implementations should always call `super().init()`. """ - pass + self.init_interfaces() @classmethod def merge_dict_attrs(cls, attr): From fa28ba85dc2dfc5e4b12cf23c24b184c392d6545 Mon Sep 17 00:00:00 2001 From: John Morris Date: Fri, 1 Jul 2022 14:09:43 -0500 Subject: [PATCH 57/88] interface: Enable adding attributes after init The manager classes will need this to add per-drive attributes. Also add method to retrieve the data type of an attribute. --- hw_device_mgr/interface.py | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/hw_device_mgr/interface.py b/hw_device_mgr/interface.py index 505b9862..25d4f767 100644 --- a/hw_device_mgr/interface.py +++ b/hw_device_mgr/interface.py @@ -3,12 +3,22 @@ class Interface: def __init__(self, name, defaults=None, data_types=None): self.name = name - self.data_types = data_types or dict() + self.data_types = data_types.copy() if data_types else dict() self.defaults = self.set_types(**(defaults or dict())) self.values = dict() # Make attribute exist self.set(**defaults) # Set values to defaults self.set(**defaults) # Set old values to defaults + def add_attribute(self, attr, default, data_type): + assert ( + attr not in self.defaults + ), f"Attempt to redefine attribute '{attr}' in interface {self.name}" + self.data_types[attr] = data_type + kwargs = {attr: default} + self.defaults.update(self.set_types(**kwargs)) + self.set(**kwargs) # Set value to default + self.set(**kwargs) # Set old value to default + def set_types(self, **values): """Set data types for values if a type is defined for that value.""" for key, data_type in self.data_types.items(): @@ -27,6 +37,9 @@ def update(self, **values): def get(self, key=None): return self.values if key is None else self.values[key] + def get_data_type(self, key): + return self.data_types[key] + def changed(self, key, return_vals=False): val = self.values[key] val_old = self.values_old[key] From 38876467f4c833452851a5df4daafcb9aa882866 Mon Sep 17 00:00:00 2001 From: John Morris Date: Wed, 29 Jun 2022 16:18:17 -0500 Subject: [PATCH 58/88] mgr: Simplify init_devices() Device `init()` no longer takes args, so that can be collapsed into the `HWDeviceMgr.init_devices()` method. --- hw_device_mgr/mgr/mgr.py | 17 +++++------------ 1 file changed, 5 insertions(+), 12 deletions(-) diff --git a/hw_device_mgr/mgr/mgr.py b/hw_device_mgr/mgr/mgr.py index 93e5c498..d11fff39 100644 --- a/hw_device_mgr/mgr/mgr.py +++ b/hw_device_mgr/mgr/mgr.py @@ -74,25 +74,18 @@ def init_devices( Scan devices and configure with data in device configuration """ # Pass config to Config class and scan devices - assert device_config - self.init_device_classes(device_config=device_config) - self.devices = self.scan_devices(**kwargs) - self.init_device_instances(**device_init_kwargs) - - def init_device_classes(self, device_config=None): - assert device_config + assert device_config, "Empty device configuration" self.device_config = device_config self.device_base_class.set_device_config(device_config) + self.devices = self.scan_devices(**kwargs) + for dev in self.devices: + dev.init() + self.logger.info(f"Initialized device {dev}") @classmethod def scan_devices(cls, **kwargs): return cls.device_base_class.scan_devices(**kwargs) - def init_device_instances(self, **kwargs): - for i, dev in enumerate(self.devices): - dev.init(**kwargs) - self.logger.info(f"Adding device #{i}: {dev}") - #################################################### # Drive state FSM From f3263b9d0665b7460f2eedb1ed59b024c659b2eb Mon Sep 17 00:00:00 2001 From: John Morris Date: Fri, 1 Jul 2022 14:10:49 -0500 Subject: [PATCH 59/88] base device class: Add data types to feedback_out interface Will be used by manager classes --- hw_device_mgr/device.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/hw_device_mgr/device.py b/hw_device_mgr/device.py index 5b052b8e..3ea80d36 100644 --- a/hw_device_mgr/device.py +++ b/hw_device_mgr/device.py @@ -17,7 +17,7 @@ class Device(abc.ABC): data_type_class = DataType feedback_in_data_types = dict() - feedback_out_data_types = dict() + feedback_out_data_types = dict(goal_reached="bit", goal_reason="str", fault="bit") command_in_data_types = dict() command_out_data_types = dict() @@ -347,7 +347,7 @@ def sim_device_data_address(cls, sim_device_data): return sim_device_data["position"] @classmethod - def init_sim(cls, *, sim_device_data): + def init_sim(cls, /, sim_device_data): """ Create sim device objects for tests. From d140e422b2d7b17dd6837391d3916ddf0b47337b Mon Sep 17 00:00:00 2001 From: John Morris Date: Fri, 1 Jul 2022 14:12:16 -0500 Subject: [PATCH 60/88] cia_402: Add data types to all interfaces; make transition `int` Make the `transition` attribute an `int8` type and use `-1` instead of `None`. This will work better with the HAL classes, and is more efficient. --- hw_device_mgr/cia_402/device.py | 187 +++++++++--------- .../tests/read_update_write.cases.yaml | 118 +++++------ hw_device_mgr/cia_402/tests/test_device.py | 10 - 3 files changed, 151 insertions(+), 164 deletions(-) diff --git a/hw_device_mgr/cia_402/device.py b/hw_device_mgr/cia_402/device.py index b06d1295..efaad3bb 100644 --- a/hw_device_mgr/cia_402/device.py +++ b/hw_device_mgr/cia_402/device.py @@ -43,15 +43,6 @@ class CiA402Device(CiA301Device): home_timeout = 15.0 # seconds - @classmethod - def control_mode_int(cls, mode): - """ - Translate control mode to integer value. - - E.g. `MODE_CSP` to `int` `8`; pass `int` back unchanged. - """ - return getattr(cls, mode) if isinstance(mode, str) else mode - @classmethod def control_mode_str(cls, mode): """ @@ -102,16 +93,17 @@ def control_mode_str(cls, mode): ) # Outgoing feedback to controller: param_name : inital_value + feedback_out_data_types = dict( + **feedback_in_data_types, + state="str", + transition="int8", + home_success="bit", + home_error="bit", + ) feedback_out_defaults = dict( - state="START", - transition=None, - home_success=False, - home_error=False, **feedback_in_defaults, - ) - feedback_out_initial_values = dict( - status_word=0, - control_mode_fb="MODE_NA", + state="START", + transition=-1, home_success=False, home_error=False, ) @@ -130,7 +122,7 @@ def get_feedback(self): # If lower layer goals not reached (not operational), set # default feedback ("START" state) if not fb_out.get("goal_reached"): - self.feedback_out.update(**self.feedback_out_initial_values) + self.feedback_out.update(**self.feedback_out_defaults) return self.feedback_out # Goal reached vars @@ -139,13 +131,14 @@ def get_feedback(self): # Status word, control mode from fb in sw = self.feedback_in.get("status_word") - cm = fb_out.get("control_mode_fb") - cm_str = self.control_mode_str(cm) - self.feedback_out.update(status_word=sw, control_mode_fb=cm_str) + cm = self.feedback_in.get("control_mode_fb") + self.feedback_out.update(status_word=sw, control_mode_fb=cm) cm_cmd = self.command_in.get("control_mode") - if cm != self.MODE_HM and cm_str != cm_cmd: + if cm != self.MODE_HM and cm != cm_cmd: goal_reached = False - goal_reasons.append(f"control_mode {cm_str} != {cm_cmd}") + cm_str = self.control_mode_str(cm) + cm_cmd_str = self.control_mode_str(cm_cmd) + goal_reasons.append(f"control_mode {cm_str} != {cm_cmd_str}") # Calculate 'state' feedback for state, bits in self.state_bits.items(): @@ -159,7 +152,7 @@ def get_feedback(self): f"Unknown status word 0x{sw:X}; " f"state {self.feedback_out.get('state')} unchanged" ) - if self._get_next_transition() is not None: + if self._get_next_transition() >= 0: goal_reached = False state_cmd = self.command_in.get("state") sw = self.feedback_in.get("status_word") @@ -168,14 +161,14 @@ def get_feedback(self): # Calculate 'transition' feedback new_st, old_st = self.feedback_out.changed("state", return_vals=True) if (old_st, new_st) == ("START", "NOT READY TO SWITCH ON"): - self.feedback_out.update(transition="TRANSITION_0") + self.feedback_out.update(transition=0) elif new_st == "FAULT REACTION ACTIVE": - self.feedback_out.update(transition="TRANSITION_13") + self.feedback_out.update(transition=13) elif self._get_next_state(curr_state=old_st) == new_st: next_trans = self._get_next_transition(curr_state=old_st) self.feedback_out.update(transition=next_trans) else: - self.feedback_out.update(transition=None) + self.feedback_out.update(transition=-1) # Calculate homing status home_success = home_error = False @@ -237,13 +230,18 @@ def get_feedback(self): control_mode=DEFAULT_CONTROL_MODE, home_request=False, ) + command_in_data_types = dict( + state="str", + control_mode="int8", + home_request="bit", + ) # ------- Command out ------- goal_paths = { # These dicts map the current state to [next state, # transition] to arrive at some goal state. When the - # transition is `None`, the final state has been reached. + # transition is -1, the final state has been reached. # Some transitions happen automatically; those are marked with # a `None` value in the `transitions` dict. # @@ -253,60 +251,60 @@ def get_feedback(self): # subclasses "SWITCHED ON": { # Drives in OPERATION ENABLED move to QUICK STOP ACTIVE - "OPERATION ENABLED": ["QUICK STOP ACTIVE", "TRANSITION_11"], - "QUICK STOP ACTIVE": ["SWITCH ON DISABLED", "TRANSITION_12"], + "OPERATION ENABLED": ["QUICK STOP ACTIVE", 11], + "QUICK STOP ACTIVE": ["SWITCH ON DISABLED", 12], # Transition other drives to SWITCHED ON - "START": ["NOT READY TO SWITCH ON", "TRANSITION_0"], - "NOT READY TO SWITCH ON": ["SWITCH ON DISABLED", "TRANSITION_1"], - "SWITCH ON DISABLED": ["READY TO SWITCH ON", "TRANSITION_2"], - "READY TO SWITCH ON": ["SWITCHED ON", "TRANSITION_3"], - "SWITCHED ON": ["SWITCHED ON", None], # End state - "FAULT": ["SWITCH ON DISABLED", "TRANSITION_15"], - "FAULT REACTION ACTIVE": ["FAULT", "TRANSITION_14"], + "START": ["NOT READY TO SWITCH ON", 0], + "NOT READY TO SWITCH ON": ["SWITCH ON DISABLED", 1], + "SWITCH ON DISABLED": ["READY TO SWITCH ON", 2], + "READY TO SWITCH ON": ["SWITCHED ON", 3], + "SWITCHED ON": ["SWITCHED ON", -1], # End state + "FAULT": ["SWITCH ON DISABLED", 15], + "FAULT REACTION ACTIVE": ["FAULT", 14], }, "OPERATION ENABLED": { # Drives transition to OPERATION ENABLED; note the # Hal402Mgr always brings drives to SWITCHED ON state # first before setting OPERATION ENABLED goal state - "SWITCHED ON": ["OPERATION ENABLED", "TRANSITION_4"], - "OPERATION ENABLED": ["OPERATION ENABLED", None], # End - "START": ["START", "TRANSITION_0"], - "NOT READY TO SWITCH ON": ["SWITCH ON DISABLED", "TRANSITION_1"], - "SWITCH ON DISABLED": ["READY TO SWITCH ON", "TRANSITION_2"], - "READY TO SWITCH ON": ["SWITCHED ON", "TRANSITION_3"], - "FAULT": ["SWITCH ON DISABLED", "TRANSITION_15"], - "FAULT REACTION ACTIVE": ["FAULT", "TRANSITION_14"], - "QUICK STOP ACTIVE": ["SWITCH ON DISABLED", "TRANSITION_12"], + "SWITCHED ON": ["OPERATION ENABLED", 4], + "OPERATION ENABLED": ["OPERATION ENABLED", -1], # End + "START": ["START", 0], + "NOT READY TO SWITCH ON": ["SWITCH ON DISABLED", 1], + "SWITCH ON DISABLED": ["READY TO SWITCH ON", 2], + "READY TO SWITCH ON": ["SWITCHED ON", 3], + "FAULT": ["SWITCH ON DISABLED", 15], + "FAULT REACTION ACTIVE": ["FAULT", 14], + "QUICK STOP ACTIVE": ["SWITCH ON DISABLED", 12], }, # These tr'ns take longer from OPERATION ENABLED -> SWITCH ON DISABLED - # 'OPERATION ENABLED': ['SWITCHED ON', 'TRANSITION_5'], - # 'SWITCHED ON': ['READY TO SWITCH ON', 'TRANSITION_6'], - # 'READY TO SWITCH ON': ['SWITCH ON DISABLED', 'TRANSITION_7'] + # 'OPERATION ENABLED': ['SWITCHED ON', 5], + # 'SWITCHED ON': ['READY TO SWITCH ON', 6], + # 'READY TO SWITCH ON': ['SWITCH ON DISABLED', 7] "SWITCH ON DISABLED": { - "START": ["NOT READY TO SWITCH ON", "TRANSITION_0"], - "NOT READY TO SWITCH ON": ["SWITCH ON DISABLED", "TRANSITION_1"], - "SWITCH ON DISABLED": ["SWITCH ON DISABLED", None], # End State - "READY TO SWITCH ON": ["SWITCH ON DISABLED", "TRANSITION_7"], - "SWITCHED ON": ["SWITCH ON DISABLED", "TRANSITION_10"], - "FAULT REACTION ACTIVE": ["FAULT", "TRANSITION_14"], - "FAULT": ["SWITCH ON DISABLED", "TRANSITION_15"], - "OPERATION ENABLED": ["QUICK STOP ACTIVE", "TRANSITION_11"], - "QUICK STOP ACTIVE": ["SWITCH ON DISABLED", "TRANSITION_12"], + "START": ["NOT READY TO SWITCH ON", 0], + "NOT READY TO SWITCH ON": ["SWITCH ON DISABLED", 1], + "SWITCH ON DISABLED": ["SWITCH ON DISABLED", -1], # End State + "READY TO SWITCH ON": ["SWITCH ON DISABLED", 7], + "SWITCHED ON": ["SWITCH ON DISABLED", 10], + "FAULT REACTION ACTIVE": ["FAULT", 14], + "FAULT": ["SWITCH ON DISABLED", 15], + "OPERATION ENABLED": ["QUICK STOP ACTIVE", 11], + "QUICK STOP ACTIVE": ["SWITCH ON DISABLED", 12], }, # Fault state has three possible final states; see inline notes "FAULT": { # Drives in FAULT state remain in that state - "FAULT REACTION ACTIVE": ["FAULT", "TRANSITION_14"], - "FAULT": ["FAULT", None], # End state + "FAULT REACTION ACTIVE": ["FAULT", 14], + "FAULT": ["FAULT", -1], # End state # Drives in OPERATION ENABLED quick stop & disable - "OPERATION ENABLED": ["QUICK STOP ACTIVE", "TRANSITION_11"], - "QUICK STOP ACTIVE": ["SWITCH ON DISABLED", "TRANSITION_12"], + "OPERATION ENABLED": ["QUICK STOP ACTIVE", 11], + "QUICK STOP ACTIVE": ["SWITCH ON DISABLED", 12], # Drives in all other states transition to SWITCH ON DISABLED - "START": ["NOT READY TO SWITCH ON", "TRANSITION_0"], - "NOT READY TO SWITCH ON": ["SWITCH ON DISABLED", "TRANSITION_1"], - "SWITCH ON DISABLED": ["SWITCH ON DISABLED", None], # End state - "READY TO SWITCH ON": ["SWITCH ON DISABLED", "TRANSITION_7"], - "SWITCHED ON": ["SWITCH ON DISABLED", "TRANSITION_10"], + "START": ["NOT READY TO SWITCH ON", 0], + "NOT READY TO SWITCH ON": ["SWITCH ON DISABLED", 1], + "SWITCH ON DISABLED": ["SWITCH ON DISABLED", -1], # End state + "READY TO SWITCH ON": ["SWITCH ON DISABLED", 7], + "SWITCHED ON": ["SWITCH ON DISABLED", 10], }, } @@ -334,7 +332,7 @@ def set_command(self, **kwargs): def _get_next_control_word(self): # Get base control word - if self._get_next_transition() is None: + if self._get_next_transition() < 0: # Holding current state control_word = self._get_hold_state_control_word() else: @@ -348,7 +346,7 @@ def _get_next_control_word(self): # New request self._home_request_start = time() self.logger.info(f"{self}: Homing operation requested") - if self.feedback_out.get("control_mode_fb") == "MODE_HM": + if self.feedback_out.get("control_mode_fb") == self.MODE_HM: # Only set HOMING_START control word bit in MODE_HM home_request = True elif self.command_in.changed("home_request"): # home_request cleared @@ -389,30 +387,30 @@ def _get_hold_state_control_word(self): # Map transition to the control word that would effect the # transition; None indicates automatic transition where control # word is ignored - transitions = dict( - TRANSITION_0=None, # START->NOT READY TO SWITCH ON - TRANSITION_1=None, # NOT READY TO SWITCH ON->SWITCH ON DISABLED - TRANSITION_2=0x0006, # SWITCH ON DISABLED->READY TO SWITCH ON - TRANSITION_3=0x0007, # READY TO SWITCH ON->SWITCHED ON - TRANSITION_4=0x000F, # SWITCHED ON->OPERATION ENABLED - TRANSITION_5=0x0007, # OPERATION ENABLED->SWITCHED ON - TRANSITION_6=0x0006, # SWITCHED ON->READY TO SWITCH ON - TRANSITION_7=0x0000, # READY TO SWITCH ON->SWITCH ON DISABLED - TRANSITION_8=0x0006, # OPERATION ENABLED->READY TO SWITCH ON - TRANSITION_9=0x0000, # OPERATION ENABLED->SWITCH ON DISABLED - TRANSITION_10=0x0000, # SWITCHED ON->SWITCH ON DISABLED - TRANSITION_11=0x0002, # OPERATION ENABLED->QUICK STOP ACTIVE - TRANSITION_12=0x0000, # QUICK STOP ACTIVE->SWITCH ON DISABLED * - TRANSITION_13=None, # (Any)->FAULT REACTION ACTIVE - TRANSITION_14=None, # FAULT REACTION ACTIVE->FAULT - TRANSITION_15=0x0080, # FAULT->SWITCH ON DISABLED ** - TRANSITION_16=0x000F, # QUICK STOP ACTIVE->OPERATION ENABLED * + transitions = { + 0: None, # START->NOT READY TO SWITCH ON + 1: None, # NOT READY TO SWITCH ON->SWITCH ON DISABLED + 2: 0x0006, # SWITCH ON DISABLED->READY TO SWITCH ON + 3: 0x0007, # READY TO SWITCH ON->SWITCHED ON + 4: 0x000F, # SWITCHED ON->OPERATION ENABLED + 5: 0x0007, # OPERATION ENABLED->SWITCHED ON + 6: 0x0006, # SWITCHED ON->READY TO SWITCH ON + 7: 0x0000, # READY TO SWITCH ON->SWITCH ON DISABLED + 8: 0x0006, # OPERATION ENABLED->READY TO SWITCH ON + 9: 0x0000, # OPERATION ENABLED->SWITCH ON DISABLED + 10: 0x0000, # SWITCHED ON->SWITCH ON DISABLED + 11: 0x0002, # OPERATION ENABLED->QUICK STOP ACTIVE + 12: 0x0000, # QUICK STOP ACTIVE->SWITCH ON DISABLED * + 13: None, # (Any)->FAULT REACTION ACTIVE + 14: None, # FAULT REACTION ACTIVE->FAULT + 15: 0x0080, # FAULT->SWITCH ON DISABLED ** + 16: 0x000F, # QUICK STOP ACTIVE->OPERATION ENABLED * # * Transitions 12, 16: Set 605Ah "Quick stop option code" value: - # - 0: coast to stop; automatic TRANSITION_12 - # - 1-3: stop @ 2007-10h torque; automatic TRANSITION_12 - # - 5-7: stop @ 2007-10h torque; hold; then can TRANSITION_12 or 16 + # - 0: coast to stop; automatic transition 12 + # - 1-3: stop @ 2007-10h torque; automatic transition 12 + # - 5-7: stop @ 2007-10h torque; hold; then can transition 12 or 16 # ** Transition 15: Fault cleared on rising edge of bit 7 - ) + } # Control word bits not used for CiA402 state machine operation # may have other purposes @@ -438,7 +436,7 @@ def _get_hold_state_control_word(self): def _get_transition_control_word(self): # Look up next transition and return control word to effect it transition = self._get_next_transition() - if transition is None: + if transition < 0: # Goal state reached; shouldn't be here raise ValueError( "BUG: No transition control word when goal reached" @@ -481,9 +479,8 @@ def _get_next_control_mode(self): # If `home_request` is set, command homing mode if self.command_in.get("home_request"): return self.MODE_HM - # Get control_mode from command_in; translate e.g. "MODE_CSP" to 8 - cm = self.command_in.get("control_mode") - return self.control_mode_int(cm) + # Get control_mode from command_in + return self.command_in.get("control_mode") class CiA402SimDevice(CiA402Device, CiA301SimDevice): diff --git a/hw_device_mgr/cia_402/tests/read_update_write.cases.yaml b/hw_device_mgr/cia_402/tests/read_update_write.cases.yaml index 421c3a90..c96dc914 100644 --- a/hw_device_mgr/cia_402/tests/read_update_write.cases.yaml +++ b/hw_device_mgr/cia_402/tests/read_update_write.cases.yaml @@ -10,16 +10,16 @@ oper: False # CiA 402: Default values before NMT operational status_word: 0x0000 - control_mode_fb: MODE_NA # Undefined + control_mode_fb: 0 # Undefined feedback_out: # CiA 301 online: False oper: False # CiA 402: Default values before NMT operational status_word: 0x0000 - control_mode_fb: MODE_NA # Undefined + control_mode_fb: 0 # Undefined state: START - transition: null + transition: -1 home_success: False home_error: False fault: False @@ -31,19 +31,19 @@ oper: True # CiA 402 state: SWITCH ON DISABLED - control_mode: MODE_CSP + control_mode: 8 # MODE_CSP home_request: False command_out: # CiA 402 control_word: 0x0000 - control_mode: MODE_CSP + control_mode: 8 # MODE_CSP sim_feedback: # CiA 301 online: True oper: False # CiA 402 status_word: 0x0010 # VOLTAGE_ENABLED - control_mode_fb: MODE_NA # Undefined + control_mode_fb: 0 # Undefined - desc: "Init: 301 NMT state machine pre-op" feedback_in: # CiA 301 @@ -67,26 +67,26 @@ # CiA 402 status_word: 0x0010 # VOLTAGE_ENABLED state: 'NOT READY TO SWITCH ON' - transition: TRANSITION_0 + transition: 0 goal_reason: control_mode MODE_NA != MODE_CSP; state NOT READY TO SWITCH ON (0x00000010) != SWITCH ON DISABLED sim_feedback: # CiA 402 status_word: 0x0050 # SWITCH ON DISABLED + VOLTAGE_ENABLED - control_mode_fb: MODE_CSP + control_mode_fb: 8 # MODE_CSP - desc: "Init: 402 state machine transition 1" feedback_in: status_word: 0x0050 # SWITCH ON DISABLED + VOLTAGE_ENABLED - control_mode_fb: MODE_CSP + control_mode_fb: 8 # MODE_CSP feedback_out: status_word: 0x0050 # SWITCH ON DISABLED + VOLTAGE_ENABLED - control_mode_fb: MODE_CSP + control_mode_fb: 8 # MODE_CSP state: SWITCH ON DISABLED - transition: TRANSITION_1 + transition: 1 goal_reached: True goal_reason: Reached - desc: "Init: Hold state x1" feedback_out: - transition: null + transition: -1 - desc: "Init: Hold state x2" # @@ -107,7 +107,7 @@ feedback_out: status_word: 0x0031 state: READY TO SWITCH ON - transition: TRANSITION_2 + transition: 2 goal_reached: False goal_reason: state READY TO SWITCH ON (0x00000031) != OPERATION ENABLED command_out: @@ -120,19 +120,19 @@ feedback_out: status_word: 0x0033 state: SWITCHED ON - transition: TRANSITION_3 + transition: 3 goal_reason: state SWITCHED ON (0x00000033) != OPERATION ENABLED command_out: control_word: 0x000F sim_feedback: - status_word: 0x0037 + status_word: 0x0037 # OPERATION ENABLED + VOLTAGE_ENABLED - desc: "Enable: 402 state machine transition 4" feedback_in: - status_word: 0x0037 + status_word: 0x0037 # OPERATION ENABLED + VOLTAGE_ENABLED feedback_out: - status_word: 0x0037 + status_word: 0x0037 # OPERATION ENABLED + VOLTAGE_ENABLED state: OPERATION ENABLED - transition: TRANSITION_4 + transition: 4 goal_reached: True goal_reason: Reached @@ -150,7 +150,7 @@ feedback_out: status_word: 0x001F state: FAULT REACTION ACTIVE - transition: TRANSITION_13 + transition: 13 fault: True goal_reached: False goal_reason: state FAULT REACTION ACTIVE (0x0000001F) != OPERATION ENABLED @@ -164,7 +164,7 @@ feedback_out: status_word: 0x0018 state: FAULT - transition: TRANSITION_14 + transition: 14 goal_reason: state FAULT (0x00000018) != OPERATION ENABLED command_out: # Goal is still OPERATION ENABLED, so automatically clear fault @@ -177,7 +177,7 @@ feedback_out: status_word: 0x0050 state: SWITCH ON DISABLED - transition: TRANSITION_15 + transition: 15 fault: False goal_reason: state SWITCH ON DISABLED (0x00000050) != OPERATION ENABLED command_out: @@ -190,7 +190,7 @@ feedback_out: status_word: 0x0031 state: READY TO SWITCH ON - transition: TRANSITION_2 + transition: 2 goal_reason: state READY TO SWITCH ON (0x00000031) != OPERATION ENABLED command_out: control_word: 0x0007 @@ -209,7 +209,7 @@ feedback_out: status_word: 0x001F state: FAULT REACTION ACTIVE - transition: TRANSITION_13 + transition: 13 fault: True goal_reason: state FAULT REACTION ACTIVE (0x0000001F) != OPERATION ENABLED command_out: @@ -222,14 +222,14 @@ feedback_out: status_word: 0x0018 state: FAULT - transition: TRANSITION_14 + transition: 14 goal_reason: state FAULT (0x00000018) != OPERATION ENABLED command_in: # Controller sees FAULT state and commands hold at that state state: FAULT - desc: "Hold fault: Hold state x1" feedback_out: - transition: null + transition: -1 goal_reached: True goal_reason: Reached - desc: "Hold fault: Hold state x2" @@ -253,12 +253,12 @@ status_word: 0x0050 state: SWITCH ON DISABLED fault: False - transition: TRANSITION_15 + transition: 15 command_out: control_word: 0x0000 - desc: "Disable: Hold state x1" feedback_out: - transition: null + transition: -1 goal_reached: True goal_reason: Reached - desc: "Disable: Hold state x2" @@ -291,7 +291,7 @@ feedback_out: status_word: 0x0031 state: READY TO SWITCH ON - transition: TRANSITION_2 + transition: 2 goal_reached: False goal_reason: state READY TO SWITCH ON (0x00000031) != SWITCHED ON command_out: @@ -304,12 +304,12 @@ feedback_out: status_word: 0x0033 state: SWITCHED ON - transition: TRANSITION_3 + transition: 3 goal_reached: True goal_reason: Reached - desc: "Enable multi: Hold SWITCHED ON while waiting for other drives x1" feedback_out: - transition: null + transition: -1 - desc: "Enable multi: Hold SWITCHED ON while waiting for other drives x2" # @@ -328,19 +328,19 @@ command_out: control_word: 0x000F sim_feedback: - status_word: 0x0037 + status_word: 0x0037 # OPERATION ENABLED + VOLTAGE_ENABLED - desc: "Enable multi: 402 state machine transition 4" feedback_in: - status_word: 0x0037 + status_word: 0x0037 # OPERATION ENABLED + VOLTAGE_ENABLED feedback_out: - status_word: 0x0037 + status_word: 0x0037 # OPERATION ENABLED + VOLTAGE_ENABLED state: OPERATION ENABLED - transition: TRANSITION_4 + transition: 4 goal_reached: True goal_reason: Reached - desc: "Enable multi: Hold OPERATION ENABLED x1" feedback_out: - transition: null + transition: -1 - desc: "Enable multi: Hold OPERATION ENABLED x2" # @@ -362,7 +362,7 @@ feedback_out: status_word: 0x0017 state: QUICK STOP ACTIVE - transition: TRANSITION_11 + transition: 11 goal_reached: False goal_reason: state QUICK STOP ACTIVE (0x00000017) != FAULT command_out: @@ -375,12 +375,12 @@ feedback_out: status_word: 0x0050 state: SWITCH ON DISABLED - transition: TRANSITION_12 + transition: 12 goal_reached: True goal_reason: Reached - desc: "Disable disabled: Hold SWITCH ON DISABLED x1" feedback_out: - transition: null + transition: -1 - desc: "Disable disabled: Hold SWITCH ON DISABLED x2" # @@ -404,7 +404,7 @@ feedback_out: status_word: 0x0031 state: READY TO SWITCH ON - transition: TRANSITION_2 + transition: 2 goal_reached: False goal_reason: state READY TO SWITCH ON (0x00000031) != OPERATION ENABLED command_out: @@ -417,34 +417,34 @@ feedback_out: status_word: 0x0033 state: SWITCHED ON - transition: TRANSITION_3 + transition: 3 goal_reason: state SWITCHED ON (0x00000033) != OPERATION ENABLED command_out: control_word: 0x000F sim_feedback: - status_word: 0x0037 + status_word: 0x0037 # OPERATION ENABLED + VOLTAGE_ENABLED - desc: "Home: 402 state machine transition 4; command home_request & MODE_HM" feedback_in: - status_word: 0x0037 + status_word: 0x0037 # OPERATION ENABLED + VOLTAGE_ENABLED feedback_out: - status_word: 0x0037 + status_word: 0x0037 # OPERATION ENABLED + VOLTAGE_ENABLED state: OPERATION ENABLED - transition: TRANSITION_4 + transition: 4 goal_reached: True goal_reason: Reached command_in: # Controller commands homing start home_request: True command_out: - control_mode: MODE_HM + control_mode: 6 # MODE_HM sim_feedback: - control_mode_fb: MODE_HM + control_mode_fb: 6 # MODE_HM - desc: "Home: Drive MODE_HM; Command HOMING_START" feedback_in: - control_mode_fb: MODE_HM + control_mode_fb: 6 # MODE_HM feedback_out: - control_mode_fb: MODE_HM - transition: null + control_mode_fb: 6 # MODE_HM + transition: -1 goal_reached: False goal_reason: homing not complete command_out: @@ -463,17 +463,17 @@ home_request: False command_out: control_word: 0x000F # OPERATION ENABLED - control_mode: MODE_CSP + control_mode: 8 # MODE_CSP sim_feedback: - control_mode_fb: MODE_CSP - status_word: 0x0037 # OPERATION ENABLED + control_mode_fb: 8 # MODE_CSP + status_word: 0x0037 # OPERATION ENABLED + VOLTAGE_ENABLED - desc: "Home: Command disable" feedback_in: - status_word: 0x0037 # OPERATION ENABLED - control_mode_fb: MODE_CSP + status_word: 0x0037 # OPERATION ENABLED + VOLTAGE_ENABLED + control_mode_fb: 8 # MODE_CSP feedback_out: - status_word: 0x0037 # OPERATION ENABLED - control_mode_fb: MODE_CSP + status_word: 0x0037 # OPERATION ENABLED + VOLTAGE_ENABLED + control_mode_fb: 8 # MODE_CSP home_success: False command_in: # Controller commands disable @@ -488,7 +488,7 @@ feedback_out: status_word: 0x0017 state: QUICK STOP ACTIVE - transition: TRANSITION_11 + transition: 11 goal_reached: False goal_reason: state QUICK STOP ACTIVE (0x00000017) != SWITCH ON DISABLED command_out: @@ -501,10 +501,10 @@ feedback_out: status_word: 0x0050 # SWITCH ON DISABLED + VOLTAGE_ENABLED state: SWITCH ON DISABLED - transition: TRANSITION_12 + transition: 12 goal_reached: True goal_reason: Reached - desc: "Home: Hold SWITCH ON DISABLED x1" feedback_out: - transition: null + transition: -1 - desc: "Home: Hold SWITCH ON DISABLED x2" diff --git a/hw_device_mgr/cia_402/tests/test_device.py b/hw_device_mgr/cia_402/tests/test_device.py index b90be26a..513ad257 100644 --- a/hw_device_mgr/cia_402/tests/test_device.py +++ b/hw_device_mgr/cia_402/tests/test_device.py @@ -21,16 +21,6 @@ def read_update_write_conv_test_data(self): uint16 = self.device_class.data_type_class.uint16 for data in (self.test_data, self.ovr_data): for intf, intf_data in data.items(): - # Translate control_mode, e.g. MODE_CSP -> 8 - cm = intf_data.get("control_mode", None) - if cm is not None and intf == "command_out": - cm = self.obj.control_mode_int(cm) - intf_data["control_mode"] = cm - cmf = intf_data.get("control_mode_fb", None) - if cmf is not None and intf in ("feedback_in", "sim_feedback"): - cmf = self.obj.control_mode_int(cmf) - intf_data["control_mode_fb"] = cmf - # Format status_word, control_word for readability, e.g. 0x000F for key in ("status_word", "control_word"): if key in intf_data: From b6bac0cc734e24ddf57706cff5c279f0b775a384 Mon Sep 17 00:00:00 2001 From: John Morris Date: Fri, 1 Jul 2022 14:24:09 -0500 Subject: [PATCH 61/88] errors: Add types to feedback_out interface --- hw_device_mgr/errors/device.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/hw_device_mgr/errors/device.py b/hw_device_mgr/errors/device.py index 69918069..0a41da72 100644 --- a/hw_device_mgr/errors/device.py +++ b/hw_device_mgr/errors/device.py @@ -23,6 +23,9 @@ class ErrorDevice(Device, ConfigIO): feedback_out_defaults = dict( error_code=0, description="No error", advice="No error" ) + feedback_out_data_types = dict( + error_code="uint32", description="str", advice="str" + ) no_error = feedback_out_defaults From 415d3be5101d632fc65c2e279711399335ed46b2 Mon Sep 17 00:00:00 2001 From: John Morris Date: Sun, 10 Jul 2022 13:52:23 -0500 Subject: [PATCH 62/88] base class: Add `DebugInterface` class for tests The `DebugInterface` class adds strict checking to the `Interface` class `update()` method to ensure no unexpected interface keys are added. --- hw_device_mgr/device.py | 7 +++++-- hw_device_mgr/interface.py | 3 +++ hw_device_mgr/tests/bogus_devices/device.py | 2 ++ hw_device_mgr/tests/interface.py | 10 ++++++++++ hw_device_mgr/tests/test_device.py | 2 +- 5 files changed, 21 insertions(+), 3 deletions(-) create mode 100644 hw_device_mgr/tests/interface.py diff --git a/hw_device_mgr/device.py b/hw_device_mgr/device.py index 3ea80d36..cb6f48e4 100644 --- a/hw_device_mgr/device.py +++ b/hw_device_mgr/device.py @@ -15,9 +15,12 @@ class Device(abc.ABC): logger = Logging.getLogger(__name__) data_type_class = DataType + interface_class = Interface feedback_in_data_types = dict() - feedback_out_data_types = dict(goal_reached="bit", goal_reason="str", fault="bit") + feedback_out_data_types = dict( + goal_reached="bit", goal_reason="str", fault="bit" + ) command_in_data_types = dict() command_out_data_types = dict() @@ -89,7 +92,7 @@ def init_interfaces(self): defaults[k] = v.copy() dt_names = self.merge_dict_attrs(f"{name}_data_types") data_types = {k: dt_name2cls(v) for k, v in dt_names.items()} - intfs[name] = Interface(name, defaults, data_types) + intfs[name] = self.interface_class(name, defaults, data_types) def interface(self, name): return self._interfaces[name] diff --git a/hw_device_mgr/interface.py b/hw_device_mgr/interface.py index 25d4f767..c16150ab 100644 --- a/hw_device_mgr/interface.py +++ b/hw_device_mgr/interface.py @@ -26,6 +26,9 @@ def set_types(self, **values): values[key] = data_type(values[key]) return values + def keys(self): + return self.values.keys() + def set(self, **values): self.values_old = self.values self.values = self.defaults.copy() diff --git a/hw_device_mgr/tests/bogus_devices/device.py b/hw_device_mgr/tests/bogus_devices/device.py index 8e73f129..20da6a9c 100644 --- a/hw_device_mgr/tests/bogus_devices/device.py +++ b/hw_device_mgr/tests/bogus_devices/device.py @@ -1,9 +1,11 @@ from ...device import SimDevice +from ..interface import DebugInterface class BogusDevice(SimDevice): """Bogus-Bus device class.""" + interface_class = DebugInterface category = "bogus_bus_device" diff --git a/hw_device_mgr/tests/interface.py b/hw_device_mgr/tests/interface.py new file mode 100644 index 00000000..1df981c7 --- /dev/null +++ b/hw_device_mgr/tests/interface.py @@ -0,0 +1,10 @@ +from ..interface import Interface + + +class DebugInterface(Interface): + """Interface for testing that verifies `update()` keys.""" + + def update(self, **values): + for key in values: + assert key in self.defaults, f"Unknown interface key {key}" + super().update(**values) diff --git a/hw_device_mgr/tests/test_device.py b/hw_device_mgr/tests/test_device.py index 9b48cea9..5813efe6 100644 --- a/hw_device_mgr/tests/test_device.py +++ b/hw_device_mgr/tests/test_device.py @@ -125,7 +125,7 @@ def test_init(self, obj): def test_set_sim_feedback(self, obj): res = obj.set_sim_feedback() - assert res.__class__.__name__ == "Interface" + assert res.__class__.__name__ == "DebugInterface" ######################################### # Test read()/update()/write() integration From 30919d31831ea1830106de3d8e64170a97b55725 Mon Sep 17 00:00:00 2001 From: John Morris Date: Sun, 10 Jul 2022 13:59:21 -0500 Subject: [PATCH 63/88] base class: Updates to `test_read_update_write` test - Add a `munge_interface_data()` method that lets test subclasses override to process test case before sending to device `set_command()`; this will be used in the manager classes to turn generic `d.0.status_word` into `d0.10.status_word` - Tweak override data handling for efficiency: update all overrides at once rather than looping through one key:val at a time - Move `test_dot()` function out of `test_read_update_write()` section --- hw_device_mgr/tests/test_device.py | 42 ++++++++++++++++-------------- 1 file changed, 23 insertions(+), 19 deletions(-) diff --git a/hw_device_mgr/tests/test_device.py b/hw_device_mgr/tests/test_device.py index 5813efe6..c0b5a33b 100644 --- a/hw_device_mgr/tests/test_device.py +++ b/hw_device_mgr/tests/test_device.py @@ -127,6 +127,19 @@ def test_set_sim_feedback(self, obj): res = obj.set_sim_feedback() assert res.__class__.__name__ == "DebugInterface" + def test_dot(self, tmp_path): + # Test class diagram + 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()) + subprocess.check_call(["dot", "-Tpng", "-O", gv_file]) + # All class diagrams + gv_file = tmp_path / ".." / "all.gv" + with gv_file.open("w") as f: + f.write(Device.dot()) + subprocess.check_call(["dot", "-Tpng", "-O", gv_file]) + ######################################### # Test read()/update()/write() integration # @@ -219,9 +232,14 @@ def get_feedback_and_check(self): # ) assert self.check_interface_values("feedback_out") + def munge_interface_data(self, interface): + # Do any test data manipulation before sending to interface; subclasses + # may override + return self.test_data[interface] + def set_command_and_check(self): print("\n*** Running object set_command()") - self.obj.set_command(**self.test_data["command_in"]) + self.obj.set_command(**self.munge_interface_data("command_in")) assert self.check_interface_values("command_in") assert self.check_interface_values("command_out") print("\n*** Overriding command_out") @@ -242,17 +260,16 @@ def post_write_actions(self): # Utilities # - def override_interface_param(self, interface, key, val): + def override_interface_param(self, interface, ovr_data): intf = self.obj.interface(interface) - intf.update(**{key: val}) + intf.update(**ovr_data) def override_data(self, interface): ovr_data = self.ovr_data.get(interface, dict()) if not ovr_data: - print(f" {interface}: No overrides") + print(f" {interface}: {{}} (no overrides)") return - for key, val in ovr_data.items(): - self.override_interface_param(interface, key, val) + self.override_interface_param(interface, ovr_data) self.print_dict(ovr_data, interface, indent=2) # self.print_dict(intf_data, interface, indent=2) @@ -347,16 +364,3 @@ def test_read_update_write(self, obj): 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" - assert not gv_file.exists() - with gv_file.open("w") as f: - f.write(self.device_class.dot()) - subprocess.check_call(["dot", "-Tpng", "-O", gv_file]) - # All class diagrams - gv_file = tmp_path / ".." / "all.gv" - with gv_file.open("w") as f: - f.write(Device.dot()) - subprocess.check_call(["dot", "-Tpng", "-O", gv_file]) From 436af23596355b56991cd9edbd564adfd306d653 Mon Sep 17 00:00:00 2001 From: John Morris Date: Sun, 10 Jul 2022 14:10:01 -0500 Subject: [PATCH 64/88] errors: Update for recent Interface changes --- hw_device_mgr/errors/tests/bogus_devices/device.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/hw_device_mgr/errors/tests/bogus_devices/device.py b/hw_device_mgr/errors/tests/bogus_devices/device.py index 1380e477..d86e48dc 100644 --- a/hw_device_mgr/errors/tests/bogus_devices/device.py +++ b/hw_device_mgr/errors/tests/bogus_devices/device.py @@ -1,7 +1,9 @@ from ...device import ErrorSimDevice +from ....tests.interface import DebugInterface class BogusErrorDevice(ErrorSimDevice): + interface_class = DebugInterface category = "bogus_error_devices" device_error_package = "hw_device_mgr.errors.tests.bogus_devices.device_err" From 14c73f42f5397669375d54a68bf4b5537ca583e5 Mon Sep 17 00:00:00 2001 From: John Morris Date: Sun, 10 Jul 2022 14:10:33 -0500 Subject: [PATCH 65/88] cia_301: Update for recent Interface changes - Use `DebugInterface` in tests - Define `feedback_out` interface keys --- hw_device_mgr/cia_301/device.py | 3 +++ hw_device_mgr/cia_301/tests/bogus_devices/device.py | 2 ++ 2 files changed, 5 insertions(+) diff --git a/hw_device_mgr/cia_301/device.py b/hw_device_mgr/cia_301/device.py index 93ad501c..af79e7fe 100644 --- a/hw_device_mgr/cia_301/device.py +++ b/hw_device_mgr/cia_301/device.py @@ -20,6 +20,9 @@ class CiA301Device(Device): feedback_in_data_types = dict(online="bit", oper="bit") feedback_in_defaults = dict(online=False, oper=False) + feedback_out_data_types = feedback_in_data_types + feedback_out_defaults = feedback_in_defaults + def __init__(self, address=None, **kwargs): if isinstance(address, self.config_class): self.config = address diff --git a/hw_device_mgr/cia_301/tests/bogus_devices/device.py b/hw_device_mgr/cia_301/tests/bogus_devices/device.py index d0b97fa0..89527399 100644 --- a/hw_device_mgr/cia_301/tests/bogus_devices/device.py +++ b/hw_device_mgr/cia_301/tests/bogus_devices/device.py @@ -1,10 +1,12 @@ from ...device import CiA301SimDevice +from ....tests.interface import DebugInterface # Categories class BogusCiA301Device(CiA301SimDevice): """Bogo Co CANopen 301 device class.""" + interface_class = DebugInterface category = "bogus_cia301_device" vendor_id = 0xB090C0 From 1c8a0e5bd498362aa9714b089451832e4e001450 Mon Sep 17 00:00:00 2001 From: John Morris Date: Sun, 10 Jul 2022 14:11:27 -0500 Subject: [PATCH 66/88] cia_402: Updates for recent Interface changes --- hw_device_mgr/cia_402/tests/bogus_devices/device.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/hw_device_mgr/cia_402/tests/bogus_devices/device.py b/hw_device_mgr/cia_402/tests/bogus_devices/device.py index 71ea41f6..cbd52a43 100644 --- a/hw_device_mgr/cia_402/tests/bogus_devices/device.py +++ b/hw_device_mgr/cia_402/tests/bogus_devices/device.py @@ -1,10 +1,12 @@ from ...device import CiA402SimDevice from ....cia_301.device import CiA301SimDevice +from ....tests.interface import DebugInterface class BogusCiA402Device(CiA301SimDevice): """Bogo Co CANopen 402 (except IO) device class.""" + interface_class = DebugInterface category = "bogus_cia402_devices" vendor_id = 0xB090C0 From 1bf7a4c08c9e724fb48f61c0a54d74f9c12ecd45 Mon Sep 17 00:00:00 2001 From: John Morris Date: Sun, 10 Jul 2022 14:11:39 -0500 Subject: [PATCH 67/88] devices: Updates for recent Interfaces changes --- hw_device_mgr/devices/tests/devices.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/hw_device_mgr/devices/tests/devices.py b/hw_device_mgr/devices/tests/devices.py index 0adc7d07..eca87d02 100644 --- a/hw_device_mgr/devices/tests/devices.py +++ b/hw_device_mgr/devices/tests/devices.py @@ -4,9 +4,11 @@ from ..inovance_is620n import InovanceIS620N from ..inovance_sv660 import InovanceSV660 from ..eve_xcr_e import EVEXCRE +from ...tests.interface import DebugInterface class DevicesForTest(LCECSimDevice): + interface_class = DebugInterface category = "devices_for_test" From 84d5597e3154283e48c0fe654aa6993cb587eec7 Mon Sep 17 00:00:00 2001 From: John Morris Date: Sun, 10 Jul 2022 14:11:52 -0500 Subject: [PATCH 68/88] ethercat: Updates for recent Interfaces changes --- hw_device_mgr/ethercat/tests/bogus_devices/device.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/hw_device_mgr/ethercat/tests/bogus_devices/device.py b/hw_device_mgr/ethercat/tests/bogus_devices/device.py index c8bfbfa7..6cfc1140 100644 --- a/hw_device_mgr/ethercat/tests/bogus_devices/device.py +++ b/hw_device_mgr/ethercat/tests/bogus_devices/device.py @@ -1,8 +1,10 @@ from ..relocatable_esi_device import RelocatableESIDevice from ....cia_402.device import CiA402SimDevice +from ....tests.interface import DebugInterface class BogusEtherCATDevice(RelocatableESIDevice): + interface_class = DebugInterface category = "bogus_ethercat_devices" vendor_id = 0xB090C0 xml_description_package = "hw_device_mgr.devices.device_xml" From 90b4d8aa117184912fa5098b18ef887273fea50d Mon Sep 17 00:00:00 2001 From: John Morris Date: Sun, 10 Jul 2022 14:12:23 -0500 Subject: [PATCH 69/88] hal: Updates for recent Interface & test changes - Use DebugInterface in tests - Update for test case override data --- hw_device_mgr/hal/tests/bogus_devices/device.py | 2 ++ hw_device_mgr/hal/tests/test_device.py | 15 ++++++++------- 2 files changed, 10 insertions(+), 7 deletions(-) diff --git a/hw_device_mgr/hal/tests/bogus_devices/device.py b/hw_device_mgr/hal/tests/bogus_devices/device.py index f85bfbd4..8574a896 100644 --- a/hw_device_mgr/hal/tests/bogus_devices/device.py +++ b/hw_device_mgr/hal/tests/bogus_devices/device.py @@ -1,8 +1,10 @@ from ...device import HALPinSimDevice from ....cia_301.device import CiA301SimDevice +from ....tests.interface import DebugInterface class BogusHALDevice(HALPinSimDevice, CiA301SimDevice): + interface_class = DebugInterface category = "bogus_hal_device" vendor_id = 0xB090C0 diff --git a/hw_device_mgr/hal/tests/test_device.py b/hw_device_mgr/hal/tests/test_device.py index 992ec135..8dfba825 100644 --- a/hw_device_mgr/hal/tests/test_device.py +++ b/hw_device_mgr/hal/tests/test_device.py @@ -45,15 +45,16 @@ def test_init(self, obj): # Test read()/update()/write() integration # - def override_interface_param(self, interface, key, val): + def override_interface_param(self, interface, ovr_data): intf = self.obj.interface(interface) - intf.update(**{key: val}) + intf.update(**ovr_data) dt_names = self.obj.merge_dict_attrs(f"{interface}_data_types") - dt = dt_names.get(key, None) - if dt is not None: - val = self.obj.data_type_class.by_shared_name(dt)(val) - pname = self.obj.pin_name(interface, key) - self.set_pin(pname, val) + for key, val in ovr_data.items(): + dt = dt_names.get(key, None) + if dt is not None: + val = self.obj.data_type_class.by_shared_name(dt)(val) + pname = self.obj.pin_name(interface, key) + self.set_pin(pname, val) def copy_sim_feedback(self, obj=None): if obj is None: From 5c7ba1ba886151436f1919cfa12a2b715842b889 Mon Sep 17 00:00:00 2001 From: John Morris Date: Sun, 10 Jul 2022 14:14:42 -0500 Subject: [PATCH 70/88] lcec: Updates for recent Interface changes --- hw_device_mgr/lcec/tests/bogus_devices/device.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/hw_device_mgr/lcec/tests/bogus_devices/device.py b/hw_device_mgr/lcec/tests/bogus_devices/device.py index a1c8967d..31e7ebc6 100644 --- a/hw_device_mgr/lcec/tests/bogus_devices/device.py +++ b/hw_device_mgr/lcec/tests/bogus_devices/device.py @@ -1,9 +1,11 @@ from ....ethercat.tests.relocatable_esi_device import RelocatableESIDevice from ...device import LCECSimDevice from ....cia_402.device import CiA402SimDevice +from ....tests.interface import DebugInterface class BogusLCECDevice(LCECSimDevice, RelocatableESIDevice): + interface_class = DebugInterface category = "bogus_lcec_devices" vendor_id = 0xB090C0 xml_description_package = "hw_device_mgr.devices.device_xml" From fe8d25d082b8a7f055aeaf2c65115a621847fa18 Mon Sep 17 00:00:00 2001 From: John Morris Date: Sun, 10 Jul 2022 14:17:48 -0500 Subject: [PATCH 71/88] mgr: Updates for recent Interface changes --- hw_device_mgr/mgr/tests/bogus_devices/mgr.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/hw_device_mgr/mgr/tests/bogus_devices/mgr.py b/hw_device_mgr/mgr/tests/bogus_devices/mgr.py index 940d6385..7402ecda 100644 --- a/hw_device_mgr/mgr/tests/bogus_devices/mgr.py +++ b/hw_device_mgr/mgr/tests/bogus_devices/mgr.py @@ -8,9 +8,11 @@ InovanceSV660, EVEXCRE, ) +from ....tests.interface import DebugInterface class HwMgrTestDevices(EtherCATSimDevice, CiA402SimDevice): + interface_class = DebugInterface category = "hw_mgr_test_devices" @@ -40,6 +42,7 @@ class HwMgrTestEVEXCRE(HwMgrTestDevices, EVEXCRE): class HWDeviceMgrTestCategory(SimHWDeviceMgr): + interface_class = DebugInterface category = "test_hw_device_mgr" device_base_class = HwMgrTestDevices device_classes = ( From 5df9d61b939a15a77e6319bc2958af137491c443 Mon Sep 17 00:00:00 2001 From: John Morris Date: Mon, 11 Jul 2022 17:11:53 -0500 Subject: [PATCH 72/88] mgr_hal: Updates for recent Interfaces changes Use `DebugInterface` in tests --- hw_device_mgr/mgr_hal/tests/bogus_devices/mgr.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/hw_device_mgr/mgr_hal/tests/bogus_devices/mgr.py b/hw_device_mgr/mgr_hal/tests/bogus_devices/mgr.py index 05b02f9a..f8544f0f 100644 --- a/hw_device_mgr/mgr_hal/tests/bogus_devices/mgr.py +++ b/hw_device_mgr/mgr_hal/tests/bogus_devices/mgr.py @@ -8,9 +8,11 @@ InovanceSV660, EVEXCRE, ) +from ....tests.interface import DebugInterface class HALHWMgrTestDevices(LCECSimDevice, CiA402SimDevice): + interface_class = DebugInterface category = "hal_hw_mgr_test_devices" @@ -40,6 +42,7 @@ class HALHWMgrTestEVEXCRE(HALHWMgrTestDevices, EVEXCRE): class HALHWDeviceMgrTestCategory(HALSimHWDeviceMgr): + interface_class = DebugInterface category = "test_hal_hw_device_mgr" data_type_class = HALHWMgrTestDevices.data_type_class device_base_class = HALHWMgrTestDevices From 5f88a0dc18eefe875c3845a592a1275640981f52 Mon Sep 17 00:00:00 2001 From: John Morris Date: Mon, 11 Jul 2022 17:15:40 -0500 Subject: [PATCH 73/88] mgr_ros: Updates for recent Interfaces changes --- hw_device_mgr/mgr_ros/tests/bogus_devices/mgr.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/hw_device_mgr/mgr_ros/tests/bogus_devices/mgr.py b/hw_device_mgr/mgr_ros/tests/bogus_devices/mgr.py index 9b0b85f7..26804482 100644 --- a/hw_device_mgr/mgr_ros/tests/bogus_devices/mgr.py +++ b/hw_device_mgr/mgr_ros/tests/bogus_devices/mgr.py @@ -7,9 +7,11 @@ HwMgrTestInovanceSV660N, HwMgrTestEVEXCRE, ) +from ....tests.interface import DebugInterface class ROSHWDeviceMgrTestCategory(ROSSimHWDeviceMgr): + interface_class = DebugInterface category = "test_ros_hw_device_mgr" device_base_class = HwMgrTestDevices device_classes = ( From daa05a2fb0b6a54ceae630de89d5eff87f199c15 Mon Sep 17 00:00:00 2001 From: John Morris Date: Mon, 11 Jul 2022 17:18:16 -0500 Subject: [PATCH 74/88] mgr_ros_hal: Updates for recent Interfaces changes Use `DebugInterface` in tests --- hw_device_mgr/mgr_ros_hal/tests/bogus_devices/mgr.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/hw_device_mgr/mgr_ros_hal/tests/bogus_devices/mgr.py b/hw_device_mgr/mgr_ros_hal/tests/bogus_devices/mgr.py index 062e7b99..b36b0c63 100644 --- a/hw_device_mgr/mgr_ros_hal/tests/bogus_devices/mgr.py +++ b/hw_device_mgr/mgr_ros_hal/tests/bogus_devices/mgr.py @@ -1,8 +1,10 @@ from ...mgr import ROSHALSimHWDeviceMgr from ....mgr_hal.tests.bogus_devices.mgr import HALHWDeviceMgrTestCategory +from ....tests.interface import DebugInterface class ROSHWDeviceMgrTestCategory(ROSHALSimHWDeviceMgr): + interface_class = DebugInterface category = "test_ros_hal_hw_device_mgr" data_type_class = HALHWDeviceMgrTestCategory.data_type_class device_base_class = HALHWDeviceMgrTestCategory.device_base_class From bc365cbda9113a2f7be57760fafabc55f99e5c00 Mon Sep 17 00:00:00 2001 From: John Morris Date: Wed, 27 Jul 2022 13:07:24 -0500 Subject: [PATCH 75/88] hal: Clean up HAL component initialization Make `init(comp=comp)` arg optional to simplify calling for `HALCompDevice` objects that have already set up `self.comp`. Move `comp.ready()` directly into `HALCompDevice.init()` for cleanliness. --- hw_device_mgr/hal/device.py | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/hw_device_mgr/hal/device.py b/hw_device_mgr/hal/device.py index 6de2a154..f0c09355 100644 --- a/hw_device_mgr/hal/device.py +++ b/hw_device_mgr/hal/device.py @@ -33,9 +33,15 @@ def pin_prefix(self): """ return f"{self.addr_slug}{self.slug_separator}" - def init(self, /, comp, **kwargs): + def init(self, *, comp=None, **kwargs): + # Set (or ensure) self.comp + if comp is not None: + self.comp = comp + else: + assert hasattr(self, "comp") # HALCompDevice already set + + # Run parent init() to populate interfaces 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 @@ -121,9 +127,6 @@ class HALCompDevice(HALPinDevice): def init(self, **kwargs): self.comp = self.hal.component(self.hal_comp_name or self.name) - self.logger.info(f"Initialized '{self.compname}' HAL component") - super().init(comp=self.comp, **kwargs) - - def hal_comp_ready(self): + super().init(**kwargs) self.comp.ready() - self.logger.info("%s: HAL component ready" % self.name) + self.logger.info(f"HAL component '{self.compname}' ready") From 0d501a4542421c9519bc406bd38f16cbf9f74b05 Mon Sep 17 00:00:00 2001 From: John Morris Date: Mon, 11 Jul 2022 17:00:33 -0500 Subject: [PATCH 76/88] hal: Don't create HAL pins for interface keys with no HAL type For interface keys with type like e.g. `str` with no equivalent HAL type, just omit them during HAL comp pin creation. This will be used in the `mgr_hal` module, which creates interface keys like `state_log`, a human-readable string not meant for a HAL pin. --- hw_device_mgr/hal/device.py | 15 ++++++++++++++- hw_device_mgr/hal/tests/test_device.py | 6 ++++++ 2 files changed, 20 insertions(+), 1 deletion(-) diff --git a/hw_device_mgr/hal/device.py b/hw_device_mgr/hal/device.py index f0c09355..ad2f589f 100644 --- a/hw_device_mgr/hal/device.py +++ b/hw_device_mgr/hal/device.py @@ -45,6 +45,7 @@ def init(self, *, comp=None, **kwargs): # Get specs for all pins in all interfaces; shared pin names must match, # except for direction, which becomes HAL_IO if different + self.no_pin_keys = set() # Interface attrs without HAL pins all_specs = dict() for iface, params in self.pin_interfaces.items(): for base_pname, new_spec in self.iface_pin_specs(iface).items(): @@ -83,8 +84,17 @@ def iface_pin_specs(self, iface): data_types = self.merge_dict_attrs(f"{iface}_data_types") res = dict() for base_pname, data_type_name in data_types.items(): + dtype = self.data_type_class.by_shared_name(data_type_name) + if not hasattr(dtype, "hal_type"): + iface_obj = self.interface(iface) + self.logger.debug( + f"Interface '{iface_obj.name}' key '{base_pname}' type" + f" '{data_type_name}' not HAL compatible; not creating pin" + ) + self.no_pin_keys.add(base_pname) + continue pname = self.pin_name(iface, base_pname) - ptype = self.data_type_class.by_shared_name(data_type_name).hal_type + ptype = dtype.hal_type res[pname] = dict(pname=pname, ptype=ptype, pdir=iface_pdir) return res @@ -96,6 +106,7 @@ def read(self): iface_vals = { p: self.pins[self.pin_name(pin_iface, p)].get() for p in self.interface(pin_iface).get() + if p not in self.no_pin_keys } self.interface(pin_iface).set(**iface_vals) @@ -106,6 +117,8 @@ def write(self): if params[0] == self.HAL_IN: continue # Only write HAL_OUT, HAL_IO pins for name, val in self.interface(pin_iface).get().items(): + if name in self.no_pin_keys: + continue pname = self.pin_name(pin_iface, name) self.pins[pname].set(val) diff --git a/hw_device_mgr/hal/tests/test_device.py b/hw_device_mgr/hal/tests/test_device.py index 8dfba825..8ccb89d6 100644 --- a/hw_device_mgr/hal/tests/test_device.py +++ b/hw_device_mgr/hal/tests/test_device.py @@ -36,6 +36,9 @@ def test_init(self, obj): for intf, data in obj.pin_interfaces.items(): print(f"interface: {intf}; data: {data}") for name in obj.interface(intf).get(): + if name in obj.no_pin_keys: + print(f" {name}: no HAL pin") + continue pname = obj.pin_name(intf, name) assert pname in obj.pins pnames.add(pname) @@ -80,6 +83,9 @@ def check_halpin_values(self, obj=None): for iface in ("sim_feedback", "command_out"): print(f" {iface}:") for name, iface_val in obj.interface(iface).get().items(): + if name in obj.no_pin_keys: + print(f" {name}: no HAL pin") + continue pname = obj.pin_name(iface, name) pin_val = self.get_pin(pname) print(f" {pname}={pin_val}, {name}={iface_val}") From ef502551de43716add62b38ac39178c56c15f576 Mon Sep 17 00:00:00 2001 From: John Morris Date: Fri, 1 Jul 2022 14:15:25 -0500 Subject: [PATCH 77/88] hal: Create pins from interfaces Create pins from interfaces, not from pin specs. This is a step toward creating pins for dynamic interfaces whose keys are not necessarily defined in the static interface definitions. The manager needs this for automatic device discovery. Make `init(comp=comp)` arg optional to simplify calling for `HALCompDevice` objects that have already set up `self.comp`. Move `comp.ready()` directly into `HALCompDevice.init()` for cleanliness. --- hw_device_mgr/hal/device.py | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/hw_device_mgr/hal/device.py b/hw_device_mgr/hal/device.py index ad2f589f..a727c69d 100644 --- a/hw_device_mgr/hal/device.py +++ b/hw_device_mgr/hal/device.py @@ -47,7 +47,7 @@ def init(self, *, comp=None, **kwargs): # except for direction, which becomes HAL_IO if different self.no_pin_keys = set() # Interface attrs without HAL pins all_specs = dict() - for iface, params in self.pin_interfaces.items(): + for iface in self.pin_interfaces: for base_pname, new_spec in self.iface_pin_specs(iface).items(): if base_pname not in all_specs: all_specs[base_pname] = new_spec @@ -65,15 +65,15 @@ def init(self, *, comp=None, **kwargs): ) all_specs[base_pname] = spec - # Create all pins from specs + # Create all pins from interfaces self.pins = dict() for base_pname, specs in all_specs.items(): pname, ptype, pdir = specs["pname"], specs["ptype"], specs["pdir"] try: - pin = comp.newpin(pname, ptype, pdir) + pin = self.comp.newpin(pname, ptype, pdir) except Exception as e: raise RuntimeError( - f"Exception creating pin {comp.getprefix()}.{pname}: {e}" + f"Exception creating pin {self.compname}.{pname}: {e}" ) ptypes, pdirs = (self.hal_enum_str(i) for i in (ptype, pdir)) self.pins[base_pname] = pin @@ -83,13 +83,14 @@ def iface_pin_specs(self, iface): iface_pdir = self.pin_interfaces[iface][0] data_types = self.merge_dict_attrs(f"{iface}_data_types") res = dict() - for base_pname, data_type_name in data_types.items(): - dtype = self.data_type_class.by_shared_name(data_type_name) + iface_obj = self.interface(iface) + for base_pname in iface_obj.keys(): + dtype = iface_obj.get_data_type(base_pname) if not hasattr(dtype, "hal_type"): iface_obj = self.interface(iface) self.logger.debug( f"Interface '{iface_obj.name}' key '{base_pname}' type" - f" '{data_type_name}' not HAL compatible; not creating pin" + f" '{dtype.name}' not HAL compatible; not creating pin" ) self.no_pin_keys.add(base_pname) continue From 05048499d5362ef71b86c1bbd4616badd6fb9a91 Mon Sep 17 00:00:00 2001 From: John Morris Date: Wed, 13 Jul 2022 13:30:41 -0500 Subject: [PATCH 78/88] hal: Remove IO pins & simplify logic Remove HAL IO pin capability, which hasn't worked out well in practice. This simplifies a lot of logic in both `init()` and `read()`/`write()`. --- hw_device_mgr/hal/device.py | 114 ++++++++++--------------- hw_device_mgr/hal/tests/test_device.py | 53 ++++++------ 2 files changed, 71 insertions(+), 96 deletions(-) diff --git a/hw_device_mgr/hal/device.py b/hw_device_mgr/hal/device.py index a727c69d..7b47ce0e 100644 --- a/hw_device_mgr/hal/device.py +++ b/hw_device_mgr/hal/device.py @@ -43,85 +43,59 @@ def init(self, *, comp=None, **kwargs): # Run parent init() to populate interfaces super().init(**kwargs) - # Get specs for all pins in all interfaces; shared pin names must match, - # except for direction, which becomes HAL_IO if different - self.no_pin_keys = set() # Interface attrs without HAL pins - all_specs = dict() - for iface in self.pin_interfaces: - for base_pname, new_spec in self.iface_pin_specs(iface).items(): - if base_pname not in all_specs: - all_specs[base_pname] = new_spec + # Create HAL pins for pin interfaces + self.pins = {i: dict() for i in self.pin_interfaces} + self.no_pin_keys = {i: set() for i in self.pins} # Attrs w/o pins + for intf_name, intf_pins in self.pins.items(): + self.logger.debug(f"{self} Init HAL pins; interface {intf_name}:") + intf = self.interface(intf_name) + for base_pname in intf.keys(): + dtype = intf.get_data_type(base_pname) + if not hasattr(dtype, "hal_type"): + self.logger.debug( + f"Interface '{intf_name}' key '{base_pname}' type" + f" '{dtype.name}' not HAL compatible; not creating pin" + ) + self.no_pin_keys[intf_name].add(base_pname) continue - spec = all_specs[base_pname] - for key, new_val in new_spec.items(): - if key == "pdir" and spec["pdir"] != new_val: - spec["pdir"] = self.HAL_IO - continue - if spec[key] != new_val: - raise RuntimeError( - f"Two interfacess' pin '{base_pname}'" - f" spec '{key}' differs," - f" '{spec[key]}' != '{new_val}'" - ) - all_specs[base_pname] = spec - - # Create all pins from interfaces - self.pins = dict() - for base_pname, specs in all_specs.items(): - pname, ptype, pdir = specs["pname"], specs["ptype"], specs["pdir"] - try: - pin = self.comp.newpin(pname, ptype, pdir) - except Exception as e: - raise RuntimeError( - f"Exception creating pin {self.compname}.{pname}: {e}" - ) - ptypes, pdirs = (self.hal_enum_str(i) for i in (ptype, pdir)) - self.pins[base_pname] = pin - self.logger.debug(f"Created HAL pin {pname} {ptypes} {pdirs}") - - def iface_pin_specs(self, iface): - iface_pdir = self.pin_interfaces[iface][0] - data_types = self.merge_dict_attrs(f"{iface}_data_types") - res = dict() - iface_obj = self.interface(iface) - for base_pname in iface_obj.keys(): - dtype = iface_obj.get_data_type(base_pname) - if not hasattr(dtype, "hal_type"): - iface_obj = self.interface(iface) - self.logger.debug( - f"Interface '{iface_obj.name}' key '{base_pname}' type" - f" '{dtype.name}' not HAL compatible; not creating pin" - ) - self.no_pin_keys.add(base_pname) - continue - pname = self.pin_name(iface, base_pname) - ptype = dtype.hal_type - res[pname] = dict(pname=pname, ptype=ptype, pdir=iface_pdir) - return res + pname = self.pin_name(intf_name, base_pname) + ptype = dtype.hal_type + pdir = self.pin_interfaces[intf_name][0] + try: + pin = self.comp.newpin(pname, ptype, pdir) + except Exception as e: + raise RuntimeError( + f"Exception creating pin {self.compname}.{pname}: {e}" + ) + ptypes, pdirs = (self.hal_enum_str(i) for i in (ptype, pdir)) + intf_pins[base_pname] = pin + self.logger.debug(f" {pname}: {ptypes} {pdirs}") def read(self): # Read from HAL pins - for pin_iface, params in self.pin_interfaces.items(): - if params[0] == self.HAL_OUT: - continue # Only read HAL_IN, HAL_IO pins - iface_vals = { - p: self.pins[self.pin_name(pin_iface, p)].get() - for p in self.interface(pin_iface).get() - if p not in self.no_pin_keys - } - self.interface(pin_iface).set(**iface_vals) + super().read() + pins = self.pins["feedback_in"] + vals = {p: pins[p].get() for p in pins.keys()} + self.interface("feedback_in").update(**vals) + if not getattr(self, "read_once", False): + self.read_once = True + self.logger.info( + f"HAL pins read for {self} feedback_in: {list(pins.keys())}" + ) + self.logger.info( + " Interface keys: " + f"{list(self.interface('feedback_in').keys())}" + ) def write(self): # Write to output pins super().write() - for pin_iface, params in self.pin_interfaces.items(): - if params[0] == self.HAL_IN: + for iface, pins in self.pins.items(): + if self.pin_interfaces[iface][0] == self.HAL_IN: continue # Only write HAL_OUT, HAL_IO pins - for name, val in self.interface(pin_iface).get().items(): - if name in self.no_pin_keys: - continue - pname = self.pin_name(pin_iface, name) - self.pins[pname].set(val) + vals = self.interface(iface).get() + for name, pin in pins.items(): + pin.set(vals[name]) class HALPinSimDevice(HALPinDevice, SimDevice): diff --git a/hw_device_mgr/hal/tests/test_device.py b/hw_device_mgr/hal/tests/test_device.py index 8ccb89d6..d88a223c 100644 --- a/hw_device_mgr/hal/tests/test_device.py +++ b/hw_device_mgr/hal/tests/test_device.py @@ -32,17 +32,17 @@ def test_init(self, obj): assert obj.comp is self.mock_halcomp print(f"pins:\n{pformat(obj.pins)}") - pnames = set() - for intf, data in obj.pin_interfaces.items(): - print(f"interface: {intf}; data: {data}") - for name in obj.interface(intf).get(): - if name in obj.no_pin_keys: - print(f" {name}: no HAL pin") + for intf_name, data in obj.pin_interfaces.items(): + names = set() + print(f"interface: {intf_name}; data: {data}") + intf = obj.interface(intf_name) + for name in intf.get(): + if name not in obj.pins[intf_name]: + # Only reason is `str` objects don't map to HAL + assert intf.get_data_type(name).shared_name == "str" continue - pname = obj.pin_name(intf, name) - assert pname in obj.pins - pnames.add(pname) - assert pnames == set(obj.pins.keys()) + names.add(name) + assert names == set(obj.pins[intf_name].keys()) ######################################### # Test read()/update()/write() integration @@ -62,9 +62,7 @@ def override_interface_param(self, interface, ovr_data): def copy_sim_feedback(self, obj=None): if obj is None: obj = self.obj - print( - "\n*** Copying HAL pin values from sim_feedback to feedback_in" - ) + print(f"\n*** Copy HAL pin values sim_feedback -> feedback_in {obj}") for name in obj.sim_feedback.get(): sfbpname = obj.pin_name("sim_feedback", name) fbpname = obj.pin_name("feedback_in", name) @@ -72,24 +70,27 @@ def copy_sim_feedback(self, obj=None): self.set_pin(fbpname, sfb_val) fb_val = self.get_pin(fbpname) assert sfb_val == fb_val + print(f"*** Finished sim_feedback HAL pin copy {obj}") def pre_read_actions(self): self.copy_sim_feedback() - def check_halpin_values(self, obj=None): + def check_halpin_values(self, iface, obj=None): if obj is None: obj = self.obj - print("\n*** Checking halpin values were written as expected") - for iface in ("sim_feedback", "command_out"): - print(f" {iface}:") - for name, iface_val in obj.interface(iface).get().items(): - if name in obj.no_pin_keys: - print(f" {name}: no HAL pin") - continue - pname = obj.pin_name(iface, name) - pin_val = self.get_pin(pname) - print(f" {pname}={pin_val}, {name}={iface_val}") - assert pin_val == iface_val + print(f"\n*** Checking {obj} {iface} halpin values") + pins = obj.pins[iface] + for name, iface_val in obj.interface(iface).get().items(): + if name not in pins: + print(f" {name}: no HAL pin") + continue + pname = obj.pin_name(iface, name) + pin_val = self.get_pin(pname) + print(f" {pname}={pin_val}, {name}={iface_val}") + assert pin_val == iface_val def post_write_actions(self): - self.check_halpin_values() + obj = self.obj + for iface, pins in obj.pins.items(): + if obj.pin_interfaces[iface][0] == obj.HAL_OUT: + self.check_halpin_values(iface, obj) From 3d54c69a9c34aab6ce23a9e2133bed1247a10e6b Mon Sep 17 00:00:00 2001 From: John Morris Date: Sun, 10 Jul 2022 14:17:48 -0500 Subject: [PATCH 79/88] mgr: Rework to expose per-device command_in attributes In order to support homing devices individually, the manager must expose separate `home_request` command for each drive. This is a substantial change, since some device command still comes from the manager, and now some comes from the outside. The manager's `command_in` interface is no longer static, and per-device keys must be added after drives are enumerated during the bus scan. The incoming command is also changed so that command doesn't anymore come in through the `feedback_in` interface and pass through `feedback_out`, and pass into `command_in` after manager munging. As a result, the feedback interfaces also change quite a bit. Because of the dynamic changes to initialization, the many init functions are combined so that using code can simply create the manager object and call the single `init()` method. No more separate `init()`, `init_sim`, `init_devices()`. Many (though not yet enough) CiA 301 & 402 functions were moved out of the manager and into the devices so the manager can be device agnostic and manage more types of devices. This is true of homing (completely removed from the state machine), drive control mode, and status & control word flags. This simplifies the top-level manager logic. It also requires big changes to the tests, whose routines are adapted for this new scheme, and test cases are extensively updated. --- hw_device_mgr/mgr/mgr.py | 336 +++----- hw_device_mgr/mgr/tests/base_test_class.py | 5 + .../mgr/tests/read_update_write.cases.yaml | 782 +++++++----------- hw_device_mgr/mgr/tests/test_mgr.py | 151 ++-- 4 files changed, 541 insertions(+), 733 deletions(-) diff --git a/hw_device_mgr/mgr/mgr.py b/hw_device_mgr/mgr/mgr.py index d11fff39..1d819bb1 100644 --- a/hw_device_mgr/mgr/mgr.py +++ b/hw_device_mgr/mgr/mgr.py @@ -5,6 +5,7 @@ import traceback import time from fysom import FysomGlobalMixin, FysomGlobal, Canceled +from functools import lru_cache class HWDeviceTimeout(RuntimeError): @@ -21,28 +22,11 @@ class HWDeviceMgr(FysomGlobalMixin, Device): def device_model_id(cls): return cls.name - feedback_in_defaults = dict(state_cmd=0, quick_stop=0, reset=0) - feedback_in_data_types = dict( - state_cmd="uint8", quick_stop="bit", reset="bit" - ) - feedback_out_defaults = dict( - state_cmd="init", - quick_stop=0, - ) - - sim_feedback_defaults = feedback_in_defaults - sim_feedback_data_types = feedback_in_data_types - command_in_defaults = dict( state_cmd="init", state_log="", command_complete=False, drive_state="SWITCH ON DISABLED", - drive_control_mode=device_base_class.control_mode_str( - device_base_class.DEFAULT_CONTROL_MODE - ), - drive_control_word_flags=dict(), - drive_state_flags=dict(), reset=False, ) command_out_defaults = dict(state_cmd=0, reset=0) @@ -54,38 +38,82 @@ def device_model_id(cls): def __init__(self): self.state = "init_command" # Used by FysomGlobalMixin super().__init__() - self.fast_track = False - self.mgr_config = None - self.device_config = None - self.shutdown = False - def init(self, mgr_config=None, **kwargs): + def init(self, /, mgr_config, device_config, **kwargs): """Initialize Manager instance.""" self.mgr_config = mgr_config - super().init(**kwargs) + # Pass device config to Config class + assert device_config, "Empty device configuration" + self.device_config = device_config + self.device_base_class.set_device_config(device_config) + # Scan & init devices + self.init_devices(**kwargs) + # Init self + super().init() self.logger.info("Initialization complete") + @classmethod + def init_sim(cls, **kwargs): + cls.device_base_class.init_sim(**kwargs) + def init_devices( - self, *, device_config, device_init_kwargs=dict(), **kwargs + self, + /, + sim_device_data=None, + device_init_kwargs=dict(), + device_scan_kwargs=dict(), ): """ - Populate `HWDeviceMgr` instance `devices` attribute devices. + Initialize devices. - Scan devices and configure with data in device configuration + Populate `HWDeviceMgr` instance `devices` attribute devices with + bus scans. Configure devices with data from device + configuration. """ - # Pass config to Config class and scan devices - assert device_config, "Empty device configuration" - self.device_config = device_config - self.device_base_class.set_device_config(device_config) - self.devices = self.scan_devices(**kwargs) + # Initialize sim device discovery data, if any + self.init_sim_devices(sim_device_data=sim_device_data) + + # Scan and init devices + self.devices = self.scan_devices(**device_scan_kwargs) for dev in self.devices: - dev.init() + dev.init(**device_init_kwargs) self.logger.info(f"Initialized device {dev}") @classmethod def scan_devices(cls, **kwargs): return cls.device_base_class.scan_devices(**kwargs) + # Mapping of device interface names translated <-> manager interfaces; value + # is set of attributes to skip. + device_translated_interfaces = dict( + # - goal_reached/goal_reason used by mgr + feedback_out=set(), + # - Don't expose device `state` cmd, controlled by manager + command_in={"state"}, + ) + + @lru_cache + def dev_prefix(self, dev, prefix="d", suffix=""): + return f"{prefix}{dev.addr_slug}{suffix}" + + + def init_interfaces(self): + """Add per-device interface attributes to feedback & command.""" + super().init_interfaces() + for dev in self.devices: + prefix = self.dev_prefix(dev, suffix=dev.slug_separator) + for name, skip_set in self.device_translated_interfaces.items(): + dev_intf = dev.interface(name) + mgr_intf = self.interface(name) + for attr_name, val in dev_intf.get().items(): + if attr_name in skip_set: + continue + mgr_attr_name = prefix + attr_name + mgr_intf.add_attribute( + mgr_attr_name, val, dev_intf.get_data_type(attr_name) + ) + + #################################################### # Drive state FSM @@ -93,39 +121,26 @@ def scan_devices(cls, **kwargs): initial=dict(state="init_command", event="init_command", defer=False), events=[ # Init state: From initial state - # - init_1: (Nothing) Wait for devices to come online - # - init_2: Check and update drive params + # - init_1: (Nothing) Wait for devices to come online & self-init # - init_complete: Done dict(name="init_command", src="init_command", dst="init_1"), - dict(name="init_2", src="init_1", dst="init_2"), - dict(name="init_complete", src="init_2", dst="init_complete"), + dict(name="init_complete", src="init_1", dst="init_complete"), # Fault state: From any state # - fault: done dict(name="fault_command", src="*", dst="fault_1"), dict(name="fault_complete", src="fault_1", dst="fault_complete"), # Start state: From any state but 'start*' # - start_1: Switch all devices to SWITCHED ON - # - start_2: Set all drive control modes to default - # - start_3: Switch all devices to OPERATION ENABLED + # - start_2: Switch all devices to OPERATION ENABLED # - start_complete: Done dict(name="start_command", src="*", dst="start_1"), dict(name="start_2", src="start_1", dst="start_2"), - dict(name="start_3", src="start_2", dst="start_3"), - dict(name="start_complete", src="start_3", dst="start_complete"), + dict(name="start_complete", src="start_2", dst="start_complete"), # Stop state: From any state but 'stop*' - # - stop_1: Put all devices in SWITCH ON DISABLED and CSP mode + # - stop_1: Put all devices in SWITCH ON DISABLED # - stop_complete: Done dict(name="stop_command", src="*", dst="stop_1"), dict(name="stop_complete", src="stop_1", dst="stop_complete"), - # Home state: From only 'stop_complete' - # - home_1: Set all drive control modes to HM - # - home_2: Switch all devices to OPERATION ENABLED - # - home_3: Set home flag - # - home_complete: Done; issue 'stop' command - dict(name="home_command", src="*", dst="home_1"), - dict(name="home_2", src="home_1", dst="home_2"), - dict(name="home_3", src="home_2", dst="home_3"), - dict(name="home_complete", src="home_3", dst="home_complete"), ], state_field="state", ) @@ -140,22 +155,15 @@ def on_before_init_command(self, e): def on_enter_init_1(self, e): self.logger.info("Waiting for devices to come online before init") - def on_before_init_2(self, e): + def on_before_init_complete(self, e): if self.fsm_check_devices_online(e, "INIT"): - self.logger.info("All devices online; proceeding with init") - return True - else: return False - - def on_enter_init_2(self, e): - self.initialize_devices() - - def on_before_init_complete(self, e): return self.fsm_check_drive_goal_state(e) def on_enter_init_complete(self, e): self.fsm_finalize_command(e) # Automatically return to SWITCH ON DISABLED after init + self.logger.info("Devices all online; commanding stop state") self.command_in.set( state_cmd="stop", state_log="Automatic 'stop' command at init complete", @@ -188,20 +196,12 @@ def on_before_start_command(self, e): return self.fsm_check_command(e) def on_enter_start_1(self, e): - self.fsm_set_required_status_word_flags(e, VOLTAGE_ENABLED=True) self.fsm_set_drive_state_cmd(e, "SWITCHED ON") def on_before_start_2(self, e): return self.fsm_check_drive_goal_state(e) def on_enter_start_2(self, e): - mode = self.device_base_class.DEFAULT_CONTROL_MODE - self.fsm_set_drive_control_mode(e, mode) - - def on_before_start_3(self, e): - return self.fsm_check_drive_goal_state(e) - - def on_enter_start_3(self, e): # Set reset during transition to OPERATION ENABLED e.reset = True self.fsm_set_drive_state_cmd(e, "OPERATION ENABLED") @@ -221,7 +221,6 @@ def on_before_stop_command(self, e): return self.fsm_check_command(e) def on_enter_stop_1(self, e): - self.fsm_set_required_status_word_flags(e, all_clear=True) return self.fsm_set_drive_state_cmd(e, "SWITCH ON DISABLED") def on_before_stop_complete(self, e): @@ -230,47 +229,6 @@ def on_before_stop_complete(self, e): def on_enter_stop_complete(self, e): self.fsm_finalize_command(e) - # - # Home command - # - def on_before_home_command(self, e): - if not e.src == "stop_complete": - self.logger.warning("Unable to home when devices not stopped") - return False - return self.fsm_check_command(e) - - def on_enter_home_1(self, e): - self.fsm_set_drive_control_mode(e, "MODE_HM") - self.fsm_set_required_status_word_flags(e, VOLTAGE_ENABLED=True) - - def on_before_home_2(self, e): - return self.fsm_check_drive_goal_state(e) - - def on_enter_home_2(self, e): - self.fsm_set_drive_state_cmd(e, "OPERATION ENABLED") - - def on_before_home_3(self, e): - return self.fsm_check_drive_goal_state(e) - - def on_enter_home_3(self, e): - # MODE_HM: OPERATION_MODE_SPECIFIC_1 = HOMING_START - # FIXME Is this drive specific? - self.fsm_set_control_word_flags(e, OPERATION_MODE_SPECIFIC_1=True) - self.fsm_set_required_status_word_flags(e, HOMING_COMPLETED=True) - - def on_before_home_complete(self, e): - return self.fsm_check_drive_goal_state(e) - - def on_enter_home_complete(self, e): - self.fsm_finalize_command(e) - self.fsm_set_control_word_flags(e, OPERATION_MODE_SPECIFIC_1=False) - self.fsm_set_required_status_word_flags(e, all_clear=True) - # Automatically return to SWITCH ON DISABLED after homing - self.command_in.set( - state_cmd="stop", - state_log="Automatic 'stop' command at home complete", - ) - # # All states # @@ -291,7 +249,6 @@ def on_change_state(self, e): "init": 0, "stop": 1, "start": 2, - "home": 3, "fault": 4, } @@ -363,31 +320,6 @@ def fsm_set_drive_state_cmd(self, e, state): ) self.command_in.update(drive_state=state) - def fsm_set_drive_control_mode(self, e, mode): - cmd_name = self.fsm_command_from_event(e) - mode_str = self.device_base_class.control_mode_str(mode) - self.logger.info(f"{cmd_name} command: Setting drive mode {mode_str}") - self.command_in.update(drive_control_mode=mode_str) - - def fsm_set_control_word_flags(self, e, **flags): - cmd_name = self.fsm_command_from_event(e) - self.logger.info( - f"{cmd_name} command: Setting control word flags {flags}" - ) - self.command_in.get("drive_control_word_flags").update(flags) - - def fsm_set_required_status_word_flags(self, e, all_clear=False, **flags): - cmd_name = self.fsm_command_from_event(e) - if all_clear: - self.logger.info( - f"{cmd_name} command: Clearing required status word flags" - ) - self.command_in.get("drive_state_flags").clear() - self.logger.info( - f"{cmd_name} command: Setting required status word flags {flags}" - ) - self.command_in.get("drive_state_flags").update(**flags) - def fsm_finalize_command(self, e): cmd_name = self.fsm_command_from_event(e) self.command_in.update(command_complete=True) @@ -399,6 +331,8 @@ def fsm_finalize_command(self, e): def run_loop(self): """Program main loop.""" update_period = 1.0 / self.mgr_config.get("update_rate", 10.0) + self.fast_track = False + self.shutdown = False while not self.shutdown: try: self.read_update_write() @@ -454,26 +388,18 @@ def read_update_write(self): # Map current command to dict of {current_state:next_event} # names; `None` means arrived init=dict( - init_1="init_2", - init_2="init_complete", + init_1="init_complete", init_complete=None, ), start=dict( start_1="start_2", - start_2="start_3", - start_3="start_complete", + start_2="start_complete", start_complete=None, ), stop=dict( stop_1="stop_complete", stop_complete=None, ), - home=dict( - home_1="home_2", - home_2="home_3", - home_3="home_complete", - home_complete=None, - ), fault=dict( fault_1="fault_complete", fault_complete=None, @@ -490,36 +416,32 @@ def get_feedback(self): """Process manager and device external feedback.""" fb_out = super().get_feedback() - # Incoming commanded state - state_cmd = self.cmd_int_to_name_map[self.feedback_in.get("state_cmd")] - quick_stop = self.feedback_in.get("quick_stop") - fb_out.update(state_cmd=state_cmd, quick_stop=quick_stop) - # Get device feedback for dev in self.devices: dev.get_feedback() + # Copy device feedback to mgr feedback, adding prefix + skip = self.device_translated_interfaces["feedback_out"] + dev_intf = dev.interface("feedback_out") + prefix = self.dev_prefix(dev, suffix=dev.slug_separator) + updates = { + f"{prefix}{k}":v + for k,v in dev_intf.get().items() + if k not in skip + } + fb_out.update(**updates) return fb_out - def set_command(self): + def set_command(self, **kwargs): """ Set command for top-level manager and for drives. - - Manager `command_in` is derived from manager & drive - `feedback_out` rather than being passed down via the controller - API. """ # Initialize command in/out interfaces with previous cycle values cmd_in = self.command_in cmd_out = super().set_command(**cmd_in.get()) - # Check for state command from feedback - if self.feedback_in.changed("state_cmd"): - # Other commands from state_cmd fb; can't override fault - cmd_in.update( - state_cmd=self.feedback_out.get("state_cmd"), - state_log="state command feedback changed", - ) + # Update incoming command + cmd_in.update(**kwargs) # Special cases where 'fault' overrides current command: if self.state.startswith("init"): @@ -544,26 +466,20 @@ def set_command(self): state_cmd="fault", state_log=f"Devices at ({fd_addrs}) in FAULT state", ) - elif ( - self.feedback_in.get("quick_stop") - and cmd_in.get("state_cmd") != "fault" + elif self.query_devices(fault=True) and self.query_devices( + fault="changed" ): - # Quick stop feedback high; treat this as a fault command - cmd_in.update( - state_cmd="fault", - state_log="quick_stop pin high", - ) - elif self.query_devices( - state_flags=lambda x: not x["VOLTAGE_ENABLED"] - ) and cmd_in.get("state_cmd") not in ("stop", "fault"): - # Some devices have no motor power + # Devices set `fault` since last update + fds = self.query_devices(fault=True) + fd_addrs = ", ".join(str(d.address) for d in fds) cmd_in.update( state_cmd="fault", - state_log="voltage_enabled bit low: no motor power at drive", + state_log=f"Devices at ({fd_addrs}) set fault", ) + event = self.automatic_next_event() if cmd_in.changed("state_cmd"): - # Received new command to stop/start/home/fault. Try it + # Received new command to stop/start/fault. Try it # by triggering the FSM event; a Canceled exception means # it can't be done, so ignore it. event = f"{cmd_in.get('state_cmd')}_command" @@ -571,10 +487,8 @@ def set_command(self): self.trigger(event, msg=cmd_in.get("state_log")) except Canceled: self.logger.warning(f"Unable to honor {event} command") - - # Attempt automatic transition to next state - event = self.automatic_next_event() - if event is not None: + elif event is not None: + # Attempt automatic transition to next state try: self.trigger( event, msg=f"Automatic transition from {self.state} state" @@ -611,29 +525,38 @@ def automatic_next_event(self): # Drive helpers @classmethod - def init_sim(cls, *, sim_device_data): - cls.device_base_class.init_sim(sim_device_data=sim_device_data) - - def initialize_devices(self): - # Ensure drive parameters are up to date - self.logger.info("Initializing devices") - self.logger.info("- Setting drive params volatile") - for d in self.devices: - d.set_params_volatile() - self.logger.info("- Updating drive params") - for d in self.devices: - d.write_config_param_values() + def init_sim_devices(cls, /, sim_device_data=None, **kwargs): + """ + Run `init_sim()` on devices. + + For configurations that include sim devices (even when the device + manager itself isn't running in sim mode). + """ + if sim_device_data is None: + return # No sim devices to configure + cls.device_base_class.init_sim(sim_device_data=sim_device_data, **kwargs) def set_drive_command(self): - for drive in self.devices: - drive.set_command( - state=self.command_in.get("drive_state"), - control_mode=self.command_in.get("drive_control_mode"), - control_word_flags=self.command_in.get( - "drive_control_word_flags" - ), - state_flags=self.command_in.get("drive_state_flags"), - ) + mgr_vals = self.command_in.get() + skip = self.device_translated_interfaces.get("command_in", set()) + for dev in self.devices: + updates = dict() + if "command_in" in self.device_translated_interfaces: + # Copy mgr command_in to matching device command + dev_command_in = dev.interface("command_in") + prefix = self.dev_prefix(dev, suffix=dev.slug_separator) + dev.set_command( + state=self.command_in.get("drive_state"), + **{ + k:mgr_vals[f"{prefix}{k}"] + for k in dev_command_in.keys() + if k not in skip + } + ) + else: + dev.set_command( + state=self.command_in.get("drive_state"), + ) def query_devices(self, **kwargs): res = list() @@ -652,15 +575,10 @@ def query_devices(self, **kwargs): res.append(dev) return res + def __str__(self): + return f"<{self.name}>" + class SimHWDeviceMgr(HWDeviceMgr, SimDevice): device_base_class = CiA402SimDevice - - def set_sim_feedback(self): - sfb = super().set_sim_feedback() - sfb.update( - state_cmd=self.command_out.get("state_cmd"), - quick_stop=self.feedback_in.get("quick_stop"), - ) - return sfb diff --git a/hw_device_mgr/mgr/tests/base_test_class.py b/hw_device_mgr/mgr/tests/base_test_class.py index 248c7e5e..7a439640 100644 --- a/hw_device_mgr/mgr/tests/base_test_class.py +++ b/hw_device_mgr/mgr/tests/base_test_class.py @@ -54,6 +54,11 @@ def mgr_config(self): assert mgr_config, f"Empty YAML package resource {rsrc}" return mgr_config + @classmethod + def init_sim(cls, **kwargs): + """Create sim device objects with configured SDOs.""" + super().init_sim(**cls.init_sim_sdo_kwargs(**kwargs)) + @pytest.fixture def device_cls(self, device_config, extra_fixtures): # Don't init device_model_classes device_config; HWDeviceMgr does that diff --git a/hw_device_mgr/mgr/tests/read_update_write.cases.yaml b/hw_device_mgr/mgr/tests/read_update_write.cases.yaml index 0003754c..5ea3da3a 100644 --- a/hw_device_mgr/mgr/tests/read_update_write.cases.yaml +++ b/hw_device_mgr/mgr/tests/read_update_write.cases.yaml @@ -5,151 +5,112 @@ # - desc: "Init: 301 NMT state machine init" feedback_in: - # Mgr - reset: 0 - state_cmd: 0 - quick_stop: 0 # Drives # - CiA 301 - drive_x_online: False - drive_x_oper: False + d.x.online: False + d.x.oper: False # CiA 402: Default values before NMT operational - drive_x_status_word: 0x0000 # NOT READY TO SWITCH ON - drive_x_control_mode_fb: MODE_NA # Undefined + d.x.status_word: 0x0000 # NOT READY TO SWITCH ON + d.x.control_mode_fb: 0 # Undefined feedback_out: # Mgr - reset: 0 - state_cmd: init - quick_stop: 0 goal_reached: True goal_reason: Reached + fault: False # Drives # - CiA 301 - drive_x_online: False - drive_x_oper: False + d.x.online: False + d.x.oper: False # - CiA 402: Default values before NMT operational - drive_x_status_word: 0x0000 # NOT READY TO SWITCH ON - drive_x_control_mode_fb: MODE_NA # Undefined - drive_x_state: START - drive_x_state_flags: - VOLTAGE_ENABLED: False - WARNING: False - MANUFACTURER_SPECIFIC_1: False - REMOTE: False - TARGET_REACHED: False - INTERNAL_LIMIT_ACTIVE: False - OPERATION_MODE_SPECIFIC_1: False - OPERATION_MODE_SPECIFIC_2: False - MANUFACTURER_SPECIFIC_2: False - HOMING_COMPLETED: False - drive_x_transition: null - drive_x_goal_reached: False - drive_x_goal_reason: Offline + d.x.status_word: 0x0000 # NOT READY TO SWITCH ON + d.x.control_mode_fb: 0 # Undefined + d.x.state: START + d.x.transition: -1 # No transition + d.x.home_success: False + d.x.home_error: False + d.x.fault: False + d.x.goal_reached: False + d.x.goal_reason: Offline command_in: # Mgr state_cmd: init state_log: "Automatic transition from init_command state" command_complete: False drive_state: SWITCH ON DISABLED - drive_control_mode: MODE_CSP - drive_control_word_flags: {} - drive_state_flags: {} reset: False # Drives # - CiA 402 - drive_x_state: SWITCH ON DISABLED - drive_x_control_mode: MODE_CSP - drive_x_control_word_flags: {} - drive_x_state_flags: {} + d.x.control_mode: 8 # MODE_CSP + d.x.home_request: False command_out: # Mgr state_cmd: 0 # init reset: False # Drives # - CiA 402 - drive_x_control_word: 0x0000 # SWITCH ON DISABLED - drive_x_control_mode: MODE_CSP + d.x.control_word: 0x0000 # SWITCH ON DISABLED + d.x.control_mode: 8 # MODE_CSP sim_feedback: - # Mgr - reset: 0 - state_cmd: 0 # init - quick_stop: 0 # Drives # - CiA 301 - drive_x_online: True - drive_x_oper: False + d.x.online: True + d.x.oper: False # - CiA 402 - drive_x_status_word: 0x0010 # VOLTAGE_ENABLED - drive_x_control_mode_fb: 0 # Undefined + d.x.status_word: 0x0010 # VOLTAGE_ENABLED + d.x.control_mode_fb: 0 # Undefined - desc: "Init: 301 NMT state machine pre-op" feedback_in: # Drives # - CiA 301 - drive_x_online: True + d.x.online: True # - CiA 402 - drive_x_status_word: 0x0010 # VOLTAGE_ENABLED + d.x.status_word: 0x0010 # VOLTAGE_ENABLED feedback_out: # Drives # - CiA 301 - drive_x_online: True - drive_x_goal_reason: Not operational + d.x.online: True + d.x.goal_reason: Not operational sim_feedback: # Drives # - CiA 301 - drive_x_oper: True + d.x.oper: True - desc: "Init: Drives become operational" feedback_in: # Drives # - CiA 301 - drive_x_oper: True + d.x.oper: True feedback_out: # Drives # - CiA 301 - drive_x_oper: True + d.x.oper: True # - CiA 402 - drive_x_state: 'NOT READY TO SWITCH ON' - drive_x_status_word: 0x0010 # VOLTAGE_ENABLED - drive_x_state_flags: - VOLTAGE_ENABLED: True - WARNING: False - MANUFACTURER_SPECIFIC_1: False - REMOTE: False - TARGET_REACHED: False - INTERNAL_LIMIT_ACTIVE: False - OPERATION_MODE_SPECIFIC_1: False - OPERATION_MODE_SPECIFIC_2: False - MANUFACTURER_SPECIFIC_2: False - HOMING_COMPLETED: False - drive_x_transition: TRANSITION_0 - drive_x_goal_reason: control_mode MODE_NA != MODE_CSP; state NOT READY TO SWITCH ON (0x00000010) != SWITCH ON DISABLED + d.x.state: 'NOT READY TO SWITCH ON' + d.x.status_word: 0x0010 # VOLTAGE_ENABLED + d.x.transition: 0 + d.x.goal_reason: control_mode MODE_NA != MODE_CSP; state NOT READY TO SWITCH ON (0x00000010) != SWITCH ON DISABLED sim_feedback: # - CiA 402 - drive_x_status_word: 0x0050 # SWITCH ON DISABLED + VOLTAGE_ENABLED - drive_x_control_mode_fb: MODE_CSP + d.x.status_word: 0x0050 # SWITCH ON DISABLED + VOLTAGE_ENABLED + d.x.control_mode_fb: 8 # MODE_CSP - desc: "Init: 402 state machine transition 1" feedback_in: - drive_x_status_word: 0x0050 # SWITCH ON DISABLED + VOLTAGE_ENABLED - drive_x_control_mode_fb: MODE_CSP + d.x.status_word: 0x0050 # SWITCH ON DISABLED + VOLTAGE_ENABLED + d.x.control_mode_fb: 8 # MODE_CSP feedback_out: - drive_x_status_word: 0x0050 # SWITCH ON DISABLED + VOLTAGE_ENABLED - drive_x_control_mode_fb: MODE_CSP - drive_x_state: SWITCH ON DISABLED - drive_x_transition: TRANSITION_1 - drive_x_goal_reached: True - drive_x_goal_reason: Reached + d.x.status_word: 0x0050 # SWITCH ON DISABLED + VOLTAGE_ENABLED + d.x.control_mode_fb: 8 # MODE_CSP + d.x.state: SWITCH ON DISABLED + d.x.transition: 1 + d.x.goal_reached: True + d.x.goal_reason: Reached command_in: state_cmd: stop state_log: Automatic 'stop' command at init complete command_out: state_cmd: 0x01 # stop - sim_feedback: - state_cmd: 0x01 # stop - desc: "Init: Automatic 'stop' command" - feedback_in: - state_cmd: 0x01 # stop feedback_out: - state_cmd: stop - drive_x_transition: null + d.x.transition: -1 # No transition command_in: state_log: "Automatic transition from init_complete state" - desc: "Init: Complete" @@ -162,74 +123,59 @@ # CiA 402 SWITCH ON DISABLED -> OPERATION ENABLED # - desc: "Enable: Receive external 'start' command" - sim_feedback_set: - # Simulate external start request - state_cmd: 2 # start - feedback_in: - state_cmd: 2 # start - feedback_out: - state_cmd: start command_in: state_cmd: start state_log: state command feedback changed command_complete: False # Controller commands drive state SWITCHED ON drive_state: SWITCHED ON - drive_x_state: SWITCHED ON - # Controller wants VOLTAGE_ENABLED bit set to start drives - drive_state_flags: - VOLTAGE_ENABLED: True - drive_x_state_flags: - VOLTAGE_ENABLED: True command_out: state_cmd: 2 # start - drive_x_control_word: 0x0006 # READY TO SWITCH ON + d.x.control_word: 0x0006 # READY TO SWITCH ON sim_feedback: - state_cmd: 2 # start - drive_x_status_word: 0x0031 # READY TO SWITCH ON + VOLTAGE_ENABLED + d.x.status_word: 0x0031 # READY TO SWITCH ON + VOLTAGE_ENABLED - desc: "Enable: Drives reach READY TO SWITCH ON state" feedback_in: - drive_x_status_word: 0x0031 # READY TO SWITCH ON + VOLTAGE_ENABLED + d.x.status_word: 0x0031 # READY TO SWITCH ON + VOLTAGE_ENABLED feedback_out: - drive_x_status_word: 0x0031 # READY TO SWITCH ON + VOLTAGE_ENABLED - drive_x_state: READY TO SWITCH ON - drive_x_transition: TRANSITION_2 - drive_x_goal_reached: False - drive_x_goal_reason: state READY TO SWITCH ON (0x00000031) != SWITCHED ON + d.x.status_word: 0x0031 # READY TO SWITCH ON + VOLTAGE_ENABLED + d.x.state: READY TO SWITCH ON + d.x.transition: 2 + d.x.goal_reached: False + d.x.goal_reason: state READY TO SWITCH ON (0x00000031) != SWITCHED ON command_out: - drive_x_control_word: 0x0007 # SWITCHED ON + d.x.control_word: 0x0007 # SWITCHED ON sim_feedback: - drive_x_status_word: 0x0033 # SWITCHED ON + VOLTAGE_ENABLED + d.x.status_word: 0x0033 # SWITCHED ON + VOLTAGE_ENABLED - desc: "Enable: Drives reach SWITCHED ON state" feedback_in: - drive_x_status_word: 0x0033 # SWITCHED ON + VOLTAGE_ENABLED + d.x.status_word: 0x0033 # SWITCHED ON + VOLTAGE_ENABLED feedback_out: - drive_x_status_word: 0x0033 # SWITCHED ON + VOLTAGE_ENABLED - drive_x_state: SWITCHED ON - drive_x_transition: TRANSITION_3 + d.x.status_word: 0x0033 # SWITCHED ON + VOLTAGE_ENABLED + d.x.state: SWITCHED ON + d.x.transition: 3 # Reached pre-operational SWITCHED ON goal - drive_x_goal_reached: True - drive_x_goal_reason: Reached + d.x.goal_reached: True + d.x.goal_reason: Reached command_in: # Continue with OPERATION ENABLED goal drive_state: OPERATION ENABLED - drive_x_state: OPERATION ENABLED # Reset high for one cycle reset: True command_out: - drive_x_control_word: 0x000F # OPERATION ENABLED + d.x.control_word: 0x000F # OPERATION ENABLED reset: 1 sim_feedback: - drive_x_status_word: 0x0037 # OPERATION ENABLED + VOLTAGE_ENABLED + d.x.status_word: 0x0037 # OPERATION ENABLED + VOLTAGE_ENABLED - desc: "Enable: Command complete" feedback_in: - drive_x_status_word: 0x0037 # OPERATION ENABLED + VOLTAGE_ENABLED + d.x.status_word: 0x0037 # OPERATION ENABLED + VOLTAGE_ENABLED feedback_out: - drive_x_status_word: 0x0037 # OPERATION ENABLED + VOLTAGE_ENABLED - drive_x_state: OPERATION ENABLED - drive_x_transition: TRANSITION_4 - drive_x_goal_reached: True - drive_x_goal_reason: Reached + d.x.status_word: 0x0037 # OPERATION ENABLED + VOLTAGE_ENABLED + d.x.state: OPERATION ENABLED + d.x.transition: 4 + d.x.goal_reached: True + d.x.goal_reason: Reached command_in: command_complete: True reset: False @@ -237,7 +183,7 @@ reset: 0 - desc: "Enable: Hold state OPERATION ENABLED x1" feedback_out: - drive_x_transition: null + d.x.transition: -1 # No transition - desc: "Enable: Hold state OPERATION ENABLED x2" # @@ -245,163 +191,135 @@ # (In real life, upper layers would hold fault) # OPERATION ENABLED -> FAULT -> SWITCH ON DISABLED -> (towards) OPERATION ENABLED # -- desc: "Sim fault: Drive 1 faults" +- desc: "Sim fault: Drive 1 faults; others enter QUICK STOP ACTIVE" sim_feedback_set: # Simulate the fault - drive_1_status_word: 0x001F # FAULT REACTION ACTIVE + VOLTAGE_ENABLED + d.1.status_word: 0x001F # FAULT REACTION ACTIVE + VOLTAGE_ENABLED feedback_in: - drive_1_status_word: 0x001F # FAULT REACTION ACTIVE + VOLTAGE_ENABLED + d.1.status_word: 0x001F # FAULT REACTION ACTIVE + VOLTAGE_ENABLED feedback_out: - drive_1_status_word: 0x001F # FAULT REACTION ACTIVE + VOLTAGE_ENABLED - drive_1_state: FAULT REACTION ACTIVE - drive_1_transition: TRANSITION_13 - drive_1_goal_reached: False - drive_1_goal_reason: state FAULT REACTION ACTIVE (0x0000001F) != OPERATION ENABLED - command_out: - drive_1_control_word: 0x0000 # SWITCH ON DISABLED - sim_feedback: - drive_1_status_word: 0x0018 # FAULT + VOLTAGE_ENABLED -- desc: "Sim fault: Automatic 'fault' command" - feedback_in: - drive_1_status_word: 0x0018 # FAULT + VOLTAGE_ENABLED - feedback_out: - drive_1_status_word: 0x0018 # FAULT + VOLTAGE_ENABLED - drive_1_state: FAULT - drive_1_transition: TRANSITION_14 - drive_1_goal_reason: state FAULT (0x00000018) != OPERATION ENABLED + d.1.status_word: 0x001F # FAULT REACTION ACTIVE + VOLTAGE_ENABLED + d.1.state: FAULT REACTION ACTIVE + d.1.transition: 13 + d.1.fault: True + d.1.goal_reached: False + d.1.goal_reason: state FAULT REACTION ACTIVE (0x0000001F) != OPERATION ENABLED command_in: - # Manager reacts, setting command to 'fault' state_cmd: fault - state_log: Devices at ((0, 11)) in FAULT state + state_log: Devices at ((0, 11)) set fault command_complete: False drive_state: FAULT - # Manager commands all drives to FAULT state - drive_x_state: FAULT command_out: - # Manager reacts to drive fault and commands all devices to fault state - state_cmd: 4 # fault - # Drives in fault hold state, else command QUICK STOP - drive_1_control_word: 0x0000 # (in FAULT mode, hold state) - drive_x_control_word: 0x0002 # QUICK STOP ACTIVE + state_cmd: 0x04 # fault + d.x.control_word: 0x0002 # QUICK STOP ACTIVE + d.1.control_word: 0x0000 # SWITCH ON DISABLED sim_feedback: - # Manager state command piped back to state command feedback - state_cmd: 4 # fault - drive_x_status_word: 0x0017 # QUICK STOP + VOLTAGE_ENABLED - drive_1_status_word: 0x0018 # FAULT + VOLTAGE_ENABLED + d.x.status_word: 0x0017 # QUICK STOP + VOLTAGE_ENABLED + d.1.status_word: 0x0018 # FAULT + VOLTAGE_ENABLED - desc: "Sim fault: Non-faulted drives enter QUICK STOP ACTIVE" feedback_in: - state_cmd: 4 # fault - drive_x_status_word: 0x0017 # QUICK STOP + VOLTAGE_ENABLED - drive_1_status_word: 0x0018 # FAULT + VOLTAGE_ENABLED + d.x.status_word: 0x0017 # QUICK STOP + VOLTAGE_ENABLED + d.1.status_word: 0x0018 # FAULT + VOLTAGE_ENABLED feedback_out: - state_cmd: fault # drive_1 holds state - drive_1_status_word: 0x0018 # FAULT + VOLTAGE_ENABLED - drive_1_state: FAULT - drive_1_transition: null - drive_1_goal_reached: True - drive_1_goal_reason: Reached + d.1.status_word: 0x0018 # FAULT + VOLTAGE_ENABLED + d.1.state: FAULT + d.1.transition: 14 + d.1.goal_reached: True + d.1.goal_reason: Reached # ...while other drives enter QUICK STOP ACTIVE - drive_x_status_word: 0x0017 # QUICK STOP + VOLTAGE_ENABLED - drive_x_state: QUICK STOP ACTIVE - drive_x_transition: TRANSITION_11 - drive_x_goal_reached: False - drive_x_goal_reason: state QUICK STOP ACTIVE (0x00000017) != FAULT + d.x.status_word: 0x0017 # QUICK STOP + VOLTAGE_ENABLED + d.x.state: QUICK STOP ACTIVE + d.x.transition: 11 + d.x.goal_reached: False + d.x.goal_reason: state QUICK STOP ACTIVE (0x00000017) != FAULT command_in: state_log: Devices at ((0, 11)) in FAULT state command_out: # Manager commands TRANSITION_12 to SWITCH ON DISABLED - drive_x_control_word: 0x0000 # SWITCH ON DISABLED + d.x.control_word: 0x0000 # SWITCH ON DISABLED sim_feedback: - drive_1_status_word: 0x0018 # FAULT + VOLTAGE_ENABLED - drive_x_status_word: 0x0050 # SWITCH ON DISABLED + VOLTAGE_ENABLED + d.1.status_word: 0x0018 # FAULT + VOLTAGE_ENABLED + d.x.status_word: 0x0050 # SWITCH ON DISABLED + VOLTAGE_ENABLED - desc: "Sim fault: Non-faulted drives enter SWITCH ON DISABLED" feedback_in: - drive_1_status_word: 0x0018 # FAULT + VOLTAGE_ENABLED - drive_x_status_word: 0x0050 # SWITCH ON DISABLED + VOLTAGE_ENABLED - feedback_out: - drive_1_transition: null - drive_1_status_word: 0x0018 # FAULT + VOLTAGE_ENABLED - drive_1_state: FAULT - drive_x_status_word: 0x0050 # SWITCH ON DISABLED + VOLTAGE_ENABLED - drive_x_state: SWITCH ON DISABLED - drive_x_transition: TRANSITION_12 - drive_x_goal_reached: True - drive_x_goal_reason: Reached + d.1.status_word: 0x0018 # FAULT + VOLTAGE_ENABLED + d.x.status_word: 0x0050 # SWITCH ON DISABLED + VOLTAGE_ENABLED + feedback_out: + d.1.transition: -1 + d.1.status_word: 0x0018 # FAULT + VOLTAGE_ENABLED + d.1.state: FAULT + d.x.status_word: 0x0050 # SWITCH ON DISABLED + VOLTAGE_ENABLED + d.x.state: SWITCH ON DISABLED + d.x.transition: 12 + d.x.goal_reached: True + d.x.goal_reason: Reached command_in: # Manager has all drives in FAULT mode command_complete: True - desc: "Sim fault: Hold fault command x1" feedback_out: - drive_x_transition: null + d.x.transition: -1 # No transition - desc: "Sim fault: Hold fault command x2" - desc: "Sim fault: External 'start' command" - sim_feedback_set: - # Simulate external start request - state_cmd: 2 # start - feedback_in: - state_cmd: 2 # start - feedback_out: - state_cmd: start command_in: # Manager acts on 'start' command from feedback; commands drives SWITCHED ON state_cmd: start state_log: state command feedback changed command_complete: False drive_state: SWITCHED ON - drive_x_state: SWITCHED ON command_out: state_cmd: 0x02 # start - drive_x_control_word: 0x0006 # READY TO SWITCH ON - drive_1_control_word: 0x0080 # Clear fault + d.x.control_word: 0x0006 # READY TO SWITCH ON + d.1.control_word: 0x0080 # Clear fault sim_feedback: - state_cmd: 0x02 # start - drive_x_status_word: 0x0031 # READY TO SWITCH ON + VOLTAGE_ENABLED - drive_1_status_word: 0x0050 # SWITCH ON DISABLED + VOLTAGE_ENABLED + d.x.status_word: 0x0031 # READY TO SWITCH ON + VOLTAGE_ENABLED + d.1.status_word: 0x0050 # SWITCH ON DISABLED + VOLTAGE_ENABLED - desc: "Sim fault: Drives start toward READY TO SWITCH ON state" feedback_in: - drive_x_status_word: 0x0031 # READY TO SWITCH ON + VOLTAGE_ENABLED - drive_1_status_word: 0x0050 # SWITCH ON DISABLED + VOLTAGE_ENABLED + d.x.status_word: 0x0031 # READY TO SWITCH ON + VOLTAGE_ENABLED + d.1.status_word: 0x0050 # SWITCH ON DISABLED + VOLTAGE_ENABLED feedback_out: # Drive 1 clears fault, entering SWITCH ON DISABLED - drive_1_status_word: 0x0050 # SWITCH ON DISABLED + VOLTAGE_ENABLED - drive_1_state: SWITCH ON DISABLED - drive_1_transition: TRANSITION_15 - drive_1_goal_reached: False - drive_1_goal_reason: state SWITCH ON DISABLED (0x00000050) != SWITCHED ON + d.1.status_word: 0x0050 # SWITCH ON DISABLED + VOLTAGE_ENABLED + d.1.state: SWITCH ON DISABLED + d.1.transition: 15 + d.1.fault: False + d.1.goal_reached: False + d.1.goal_reason: state SWITCH ON DISABLED (0x00000050) != SWITCHED ON # ...while other drives are one step ahead - drive_x_status_word: 0x0031 # READY TO SWITCH ON + VOLTAGE_ENABLED - drive_x_state: READY TO SWITCH ON - drive_x_transition: TRANSITION_2 - drive_x_goal_reached: False - drive_x_goal_reason: state READY TO SWITCH ON (0x00000031) != SWITCHED ON + d.x.status_word: 0x0031 # READY TO SWITCH ON + VOLTAGE_ENABLED + d.x.state: READY TO SWITCH ON + d.x.transition: 2 + d.x.goal_reached: False + d.x.goal_reason: state READY TO SWITCH ON (0x00000031) != SWITCHED ON command_out: - drive_1_control_word: 0x0006 # READY TO SWITCH ON - drive_x_control_word: 0x0007 # SWITCHED ON + d.1.control_word: 0x0006 # READY TO SWITCH ON + d.x.control_word: 0x0007 # SWITCHED ON sim_feedback: - drive_1_status_word: 0x0031 # READY TO SWITCH ON + VOLTAGE_ENABLED - drive_x_status_word: 0x0033 # SWITCHED ON + VOLTAGE_ENABLED + d.1.status_word: 0x0031 # READY TO SWITCH ON + VOLTAGE_ENABLED + d.x.status_word: 0x0033 # SWITCHED ON + VOLTAGE_ENABLED - desc: "Sim fault: Drives continue toward READY TO SWITCH ON state" feedback_in: - drive_1_status_word: 0x0031 # READY TO SWITCH ON + VOLTAGE_ENABLED - drive_x_status_word: 0x0033 # SWITCHED ON + VOLTAGE_ENABLED + d.1.status_word: 0x0031 # READY TO SWITCH ON + VOLTAGE_ENABLED + d.x.status_word: 0x0033 # SWITCHED ON + VOLTAGE_ENABLED feedback_out: # Faulted drive still lagging - drive_1_status_word: 0x0031 # READY TO SWITCH ON + VOLTAGE_ENABLED - drive_1_state: READY TO SWITCH ON - drive_1_transition: TRANSITION_2 - drive_1_goal_reached: False - drive_1_goal_reason: state READY TO SWITCH ON (0x00000031) != SWITCHED ON + d.1.status_word: 0x0031 # READY TO SWITCH ON + VOLTAGE_ENABLED + d.1.state: READY TO SWITCH ON + d.1.transition: 2 + d.1.goal_reached: False + d.1.goal_reason: state READY TO SWITCH ON (0x00000031) != SWITCHED ON # ...while other drives reached SWITCHED ON - drive_x_status_word: 0x0033 # SWITCHED ON + VOLTAGE_ENABLED - drive_x_state: SWITCHED ON - drive_x_transition: TRANSITION_3 - drive_x_goal_reached: True - drive_x_goal_reason: Reached + d.x.status_word: 0x0033 # SWITCHED ON + VOLTAGE_ENABLED + d.x.state: SWITCHED ON + d.x.transition: 3 + d.x.goal_reached: True + d.x.goal_reason: Reached command_out: - drive_1_control_word: 0x0007 # SWITCHED ON + d.1.control_word: 0x0007 # SWITCHED ON sim_feedback: - drive_1_status_word: 0x0033 # SWITCHED ON + VOLTAGE_ENABLED + d.1.status_word: 0x0033 # SWITCHED ON + VOLTAGE_ENABLED # @@ -409,72 +327,62 @@ # - desc: "Hold fault: Drive 2 fault" sim_feedback_set: - drive_2_status_word: 0x001F # FAULT REACTION ACTIVE + VOLTAGE_ENABLED + d.2.status_word: 0x001F # FAULT REACTION ACTIVE + VOLTAGE_ENABLED feedback_in: - drive_2_status_word: 0x001F # FAULT REACTION ACTIVE + VOLTAGE_ENABLED + d.2.status_word: 0x001F # FAULT REACTION ACTIVE + VOLTAGE_ENABLED # Drive 1 reaches commanded SWITCHED ON state - drive_1_status_word: 0x0033 # SWITCHED ON + VOLTAGE_ENABLED - feedback_out: - drive_2_status_word: 0x001F # FAULT REACTION ACTIVE + VOLTAGE_ENABLED - drive_2_state: FAULT REACTION ACTIVE - drive_2_transition: TRANSITION_13 - drive_2_goal_reached: False - drive_2_goal_reason: state FAULT REACTION ACTIVE (0x0000001F) != SWITCHED ON - drive_1_status_word: 0x0033 # SWITCHED ON + VOLTAGE_ENABLED - drive_1_state: SWITCHED ON - drive_1_transition: TRANSITION_3 - drive_1_goal_reached: True - drive_1_goal_reason: Reached - drive_x_transition: null + d.1.status_word: 0x0033 # SWITCHED ON + VOLTAGE_ENABLED + feedback_out: + d.2.status_word: 0x001F # FAULT REACTION ACTIVE + VOLTAGE_ENABLED + d.2.state: FAULT REACTION ACTIVE + d.2.transition: 13 + d.2.fault: True + d.2.goal_reached: False + d.2.goal_reason: state FAULT REACTION ACTIVE (0x0000001F) != SWITCHED ON + d.1.status_word: 0x0033 # SWITCHED ON + VOLTAGE_ENABLED + d.1.state: SWITCHED ON + d.1.transition: 3 + d.1.goal_reached: True + d.1.goal_reason: Reached + d.x.transition: -1 # No transition + command_in: + state_cmd: fault + state_log: Devices at ((0, 12)) set fault + drive_state: FAULT command_out: - drive_2_control_word: 0x0000 # SWITCH ON DISABLED - drive_x_control_word: 0x0007 # SWITCHED ON + state_cmd: 0x04 # fault + d.x.control_word: 0x0000 # SWITCH ON DISABLED sim_feedback: - drive_2_status_word: 0x0018 # FAULT + VOLTAGE_ENABLED - drive_x_status_word: 0x0033 # SWITCHED ON + VOLTAGE_ENABLED + d.2.status_word: 0x0018 # FAULT + VOLTAGE_ENABLED + d.x.status_word: 0x0050 # SWITCH ON DISABLED + VOLTAGE_ENABLED - desc: "Hold fault: Goal->FAULT" feedback_in: - drive_2_status_word: 0x0018 # FAULT + VOLTAGE_ENABLED - drive_x_status_word: 0x0033 # SWITCHED ON + VOLTAGE_ENABLED - feedback_out: - drive_2_status_word: 0x0018 # FAULT + VOLTAGE_ENABLED - drive_2_state: FAULT - drive_2_transition: TRANSITION_14 - drive_2_goal_reason: state FAULT (0x00000018) != SWITCHED ON - drive_1_transition: null + d.2.status_word: 0x0018 # FAULT + VOLTAGE_ENABLED + d.x.status_word: 0x0050 # SWITCH ON DISABLED + VOLTAGE_ENABLED + feedback_out: + d.x.status_word: 0x0050 + d.x.state: SWITCH ON DISABLED + d.x.transition: 10 + d.2.status_word: 0x0018 # FAULT + VOLTAGE_ENABLED + d.2.state: FAULT + d.2.transition: 14 + d.2.goal_reached: True + d.2.goal_reason: Reached command_in: - # Controller sees FAULT state and commands hold at that state + # Controller sees FAULT state and commands hold at that state state_cmd: fault state_log: Devices at ((0, 12)) in FAULT state + command_complete: True drive_state: FAULT - drive_x_state: FAULT command_out: state_cmd: 0x04 # fault - drive_x_control_word: 0x0000 # SWITCH ON DISABLED + d.x.control_word: 0x0000 # SWITCH ON DISABLED sim_feedback: - state_cmd: 0x04 # fault - drive_x_status_word: 0x0050 # SWITCH ON DISABLED + VOLTAGE_ENABLED - drive_2_status_word: 0x0018 # FAULT + VOLTAGE_ENABLED -- desc: "Hold fault: Drives settle in FAULT state" - feedback_in: - state_cmd: 0x04 # fault - drive_x_status_word: 0x0050 # SWITCH ON DISABLED + VOLTAGE_ENABLED - drive_2_status_word: 0x0018 # FAULT + VOLTAGE_ENABLED - feedback_out: - state_cmd: fault - drive_x_status_word: 0x0050 # SWITCH ON DISABLED + VOLTAGE_ENABLED - drive_x_state: SWITCH ON DISABLED - drive_x_transition: TRANSITION_10 - drive_x_goal_reached: True - drive_x_goal_reason: Reached - drive_2_status_word: 0x0018 # FAULT + VOLTAGE_ENABLED - drive_2_state: FAULT - drive_2_transition: null - command_in: - command_complete: True + d.x.status_word: 0x0050 # SWITCH ON DISABLED + VOLTAGE_ENABLED + d.2.status_word: 0x0018 # FAULT + VOLTAGE_ENABLED - desc: "Hold fault: Hold state x1" feedback_out: - drive_x_transition: null + d.x.transition: -1 # No transition - desc: "Hold fault: Hold state x2" @@ -483,43 +391,33 @@ # FAULT -> SWITCH ON DISABLED # - desc: "Disable: Command disable" - sim_feedback_set: - # Simulate external stop request - state_cmd: 1 # stop - feedback_in: - state_cmd: 1 # stop - feedback_out: - state_cmd: stop command_in: state_cmd: stop state_log: state command feedback changed drive_state: SWITCH ON DISABLED - drive_state_flags: {} # Controller commands disable - drive_x_state: SWITCH ON DISABLED - drive_x_state_flags: {} command_out: state_cmd: 0x01 # stop - drive_2_control_word: 0x0080 # Clear fault - drive_x_control_word: 0x0000 # SWITCH ON DISABLED + d.2.control_word: 0x0080 # Clear fault + d.x.control_word: 0x0000 # SWITCH ON DISABLED sim_feedback: - state_cmd: 0x01 # stop - drive_x_status_word: 0x0050 # SWITCH ON DISABLED + VOLTAGE_ENABLED + d.x.status_word: 0x0050 # SWITCH ON DISABLED + VOLTAGE_ENABLED - desc: "Disable: 402 state machine transition 15" feedback_in: - drive_x_status_word: 0x0050 # SWITCH ON DISABLED + VOLTAGE_ENABLED + d.x.status_word: 0x0050 # SWITCH ON DISABLED + VOLTAGE_ENABLED feedback_out: - drive_x_transition: null - drive_x_status_word: 0x0050 # SWITCH ON DISABLED + VOLTAGE_ENABLED - drive_x_state: SWITCH ON DISABLED - drive_2_transition: TRANSITION_15 + d.x.transition: -1 # No transition + d.x.status_word: 0x0050 # SWITCH ON DISABLED + VOLTAGE_ENABLED + d.x.state: SWITCH ON DISABLED + d.2.fault: False + d.2.transition: 15 command_out: - drive_x_control_word: 0x0000 # SWITCH ON DISABLED + d.x.control_word: 0x0000 # SWITCH ON DISABLED - desc: "Disable: Hold state x1" feedback_out: - drive_x_transition: null - drive_x_goal_reached: True - drive_x_goal_reason: Reached + d.x.transition: -1 # No transition + d.x.goal_reached: True + d.x.goal_reason: Reached - desc: "Disable: Hold state x2" # @@ -527,246 +425,188 @@ # disabled state # - desc: "Ext quick_stop noop: Command FAULT" - sim_feedback_set: - quick_stop: True - feedback_in: - quick_stop: 1 - feedback_out: - quick_stop: 1 command_in: # Controller commands FAULT state_cmd: fault state_log: quick_stop pin high drive_state: FAULT - drive_x_state: FAULT command_out: state_cmd: 0x04 # fault - sim_feedback: - state_cmd: 0x04 # fault - quick_stop: 1 - desc: "Ext fault noop: Clear quick stop and hold fault state" - sim_feedback_set: - quick_stop: True - feedback_in: - state_cmd: 0x04 # fault - feedback_out: - state_cmd: fault command_in: state_log: state command feedback changed - desc: "Ext fault noop: Hold fault state" - desc: "Ext fault noop: External stop command, but quick_stop still set" - sim_feedback_set: - # Simulate external stop request - state_cmd: 1 # stop - feedback_in: - state_cmd: 1 # stop - feedback_out: - state_cmd: stop command_in: state_log: quick_stop pin high - desc: "Ext fault noop: Manager forces fault command" - feedback_in: - state_cmd: 4 # fault - feedback_out: - state_cmd: fault command_in: state_log: state command feedback changed - desc: "Ext fault noop: Hold fault state" - desc: "Ext fault noop: External quick_stop pin cleared and stop command" - sim_feedback_set: - # Simulate external stop request - state_cmd: 1 # stop - quick_stop: False - feedback_in: - state_cmd: 0x01 # stop - quick_stop: 0 - feedback_out: - state_cmd: stop - quick_stop: 0 command_in: state_cmd: stop drive_state: SWITCH ON DISABLED - drive_x_state: SWITCH ON DISABLED command_out: state_cmd: 0x01 # stop - sim_feedback: - state_cmd: 0x01 # stop - quick_stop: 0 - desc: "Ext fault noop: Hold stop state" # -# Controller walks drive through homing operation -# command_mode -> MODE_HM +# Controller moves to OPERATION ENABLED, then commands homing +# operation on first two drives +# # SWITCH ON DISABLED -> OPERATION ENABLED +# command_mode -> MODE_HM # control_word set OPERATION_MODE_SPECIFIC_1 (HOMING_START) bit # status_word HOMING_COMPLETED bit set # OPERATION ENABLED -> SWITCH ON DISABLED -- desc: "Home: Set MODE_HM" - sim_feedback_set: - # Simulate external stop request - state_cmd: 3 # home - feedback_in: - state_cmd: 0x03 - feedback_out: - state_cmd: home +- desc: "Home: Command OPERATION ENABLED" command_in: - state_cmd: home + state_cmd: start command_complete: False - drive_state: OPERATION ENABLED - drive_x_state: OPERATION ENABLED - drive_control_mode: MODE_HM - drive_x_control_mode: MODE_HM - drive_state_flags: - VOLTAGE_ENABLED: True - drive_x_state_flags: - VOLTAGE_ENABLED: True + # Controller commands drive state SWITCHED ON + drive_state: SWITCHED ON command_out: - state_cmd: 0x03 # home - drive_x_control_word: 0x0006 # READY TO SWITCH ON - drive_x_control_mode: MODE_HM + state_cmd: 2 # start + d.x.control_word: 0x0006 # READY TO SWITCH ON sim_feedback: - state_cmd: 3 # home - drive_x_status_word: 0x0031 # READY TO SWITCH ON + VOLTAGE_ENABLED - drive_x_control_mode_fb: MODE_HM -- desc: "Home: Command OPERATION ENABLED; 402 state machine transition hold" + d.x.status_word: 0x0031 # READY TO SWITCH ON + VOLTAGE_ENABLED +- desc: "Home: Command OPERATION ENABLED; 402 state machine transition 2" feedback_in: - drive_x_control_mode_fb: MODE_HM - drive_x_status_word: 0x0031 # READY TO SWITCH ON + VOLTAGE_ENABLED + d.x.status_word: 0x0031 # READY TO SWITCH ON + VOLTAGE_ENABLED feedback_out: - drive_x_control_mode_fb: MODE_HM - drive_x_status_word: 0x0031 # READY TO SWITCH ON + VOLTAGE_ENABLED - drive_x_state: READY TO SWITCH ON - drive_x_transition: TRANSITION_2 - drive_x_goal_reached: False - drive_x_goal_reason: state READY TO SWITCH ON (0x00000031) != OPERATION ENABLED - command_in: - # Controller commands enable - drive_x_state: OPERATION ENABLED + d.x.status_word: 0x0031 # READY TO SWITCH ON + VOLTAGE_ENABLED + d.x.state: READY TO SWITCH ON + d.x.transition: 2 + d.x.goal_reached: False + d.x.goal_reason: state READY TO SWITCH ON (0x00000031) != SWITCHED ON command_out: - drive_x_control_word: 0x0007 # SWITCHED ON + d.x.control_word: 0x0007 # SWITCHED ON sim_feedback: - drive_x_status_word: 0x0033 # SWITCHED ON + VOLTAGE_ENABLED + d.x.status_word: 0x0033 # SWITCHED ON + VOLTAGE_ENABLED - desc: "Home: 402 state machine transition 3" feedback_in: - drive_x_status_word: 0x0033 # SWITCHED ON + VOLTAGE_ENABLED + d.x.status_word: 0x0033 # SWITCHED ON + VOLTAGE_ENABLED feedback_out: - drive_x_status_word: 0x0033 # SWITCHED ON + VOLTAGE_ENABLED - drive_x_state: SWITCHED ON - drive_x_transition: TRANSITION_3 - drive_x_goal_reason: state SWITCHED ON (0x00000033) != OPERATION ENABLED + d.x.status_word: 0x0033 # SWITCHED ON + VOLTAGE_ENABLED + d.x.state: SWITCHED ON + d.x.transition: 3 + d.x.goal_reached: True + d.x.goal_reason: Reached + command_in: + drive_state: OPERATION ENABLED + reset: True command_out: - drive_x_control_word: 0x000F # OPERATION ENABLED + reset: 1 + d.x.control_word: 0x000F # OPERATION ENABLED sim_feedback: - drive_x_status_word: 0x0037 # OPERATION ENABLED + VOLTAGE_ENABLED -- desc: "Home: Command HOMING_START; 402 state machine transition 4" + d.x.status_word: 0x0037 # OPERATION ENABLED + VOLTAGE_ENABLED +- desc: "Home: 402 state machine transition 4; command home_request & MODE_HM" feedback_in: - drive_x_status_word: 0x0037 # OPERATION ENABLED + VOLTAGE_ENABLED + d.x.status_word: 0x0037 # OPERATION ENABLED + VOLTAGE_ENABLED feedback_out: - drive_x_status_word: 0x0037 # OPERATION ENABLED + VOLTAGE_ENABLED - drive_x_state: OPERATION ENABLED - drive_x_transition: TRANSITION_4 - drive_x_goal_reached: True - drive_x_goal_reason: Reached + d.x.status_word: 0x0037 # OPERATION ENABLED + VOLTAGE_ENABLED + d.x.state: OPERATION ENABLED + d.x.transition: 4 + d.x.goal_reached: True + d.x.goal_reason: Reached command_in: + command_complete: True + reset: False # Controller commands homing start - drive_control_word_flags: - OPERATION_MODE_SPECIFIC_1: True - drive_state_flags: - VOLTAGE_ENABLED: True - HOMING_COMPLETED: True - drive_x_control_word_flags: - OPERATION_MODE_SPECIFIC_1: True - drive_x_state_flags: - VOLTAGE_ENABLED: True - HOMING_COMPLETED: True + d.0.home_request: True + d.1.home_request: True + command_out: + reset: 0 + d.0.control_mode: 6 # MODE_HM + d.1.control_mode: 6 # MODE_HM + sim_feedback: + d.0.control_mode_fb: 6 # MODE_HM + d.1.control_mode_fb: 6 # MODE_HM +- desc: "Home: Drive MODE_HM; Command HOMING_START" + feedback_in: + d.0.control_mode_fb: 6 # MODE_HM + d.1.control_mode_fb: 6 # MODE_HM + feedback_out: + d.x.transition: -1 # No transition + d.0.control_mode_fb: 6 # MODE_HM + d.1.control_mode_fb: 6 # MODE_HM + d.0.goal_reached: False + d.1.goal_reached: False + d.0.goal_reason: homing not complete + d.1.goal_reason: homing not complete + command_out: + d.0.control_word: 0x001F # OPERATION ENABLED + HOMING_START + d.1.control_word: 0x001F # OPERATION ENABLED + HOMING_START + sim_feedback: + d.0.status_word: 0x1037 # OPERATION ENABLED + HOMING_ATTAINED + d.1.status_word: 0x1037 # OPERATION ENABLED + HOMING_ATTAINED +- desc: "Home: HOMING_ATTAINED & home_success set; Command no home_request" + feedback_in: + d.0.status_word: 0x1037 # OPERATION ENABLED + HOMING_ATTAINED + d.1.status_word: 0x1037 # OPERATION ENABLED + HOMING_ATTAINED + feedback_out: + d.0.status_word: 0x1037 # OPERATION ENABLED + HOMING_ATTAINED + d.1.status_word: 0x1037 # OPERATION ENABLED + HOMING_ATTAINED + d.0.home_success: True + d.1.home_success: True + d.x.goal_reached: True + d.x.goal_reason: Reached + command_in: + d.x.home_request: False command_out: - drive_x_control_word: 0x001F # OPERATION ENABLED + OPERATION_MODE_SPECIFIC_1 + d.x.control_word: 0x000F # OPERATION ENABLED + d.x.control_mode: 8 # MODE_CSP sim_feedback: - drive_x_status_word: 0x8037 # OPERATION ENABLED + HOMING_COMPLETED + VOLTAGE_ENABLED + d.x.control_mode_fb: 8 # MODE_CSP + d.x.status_word: 0x0037 # OPERATION ENABLED + VOLTAGE_ENABLED - desc: "Home: Homing completed; command SWITCH ON DISABLED" feedback_in: - drive_x_status_word: 0x8037 # OPERATION ENABLED + HOMING_COMPLETED + VOLTAGE_ENABLED + d.x.status_word: 0x0037 # OPERATION ENABLED + VOLTAGE_ENABLED + d.x.control_mode_fb: 8 # MODE_CSP feedback_out: - drive_x_status_word: 0x8037 # OPERATION ENABLED + HOMING_COMPLETED + VOLTAGE_ENABLED - drive_x_state_flags: # HOMING_COMPLETED set - VOLTAGE_ENABLED: True - WARNING: False - MANUFACTURER_SPECIFIC_1: False - REMOTE: False - TARGET_REACHED: False - INTERNAL_LIMIT_ACTIVE: False - OPERATION_MODE_SPECIFIC_1: False - OPERATION_MODE_SPECIFIC_2: False - MANUFACTURER_SPECIFIC_2: False - HOMING_COMPLETED: True - drive_x_transition: null - drive_x_goal_reached: True - drive_x_goal_reason: Reached + d.x.status_word: 0x0037 # OPERATION ENABLED + VOLTAGE_ENABLED + d.x.transition: -1 # No transition + d.x.control_mode_fb: 8 # MODE_CSP + d.x.home_success: False command_in: state_cmd: stop state_log: Automatic 'stop' command at home complete drive_state: SWITCH ON DISABLED - drive_control_mode: MODE_CSP - drive_state_flags: {} # Clear HOMING_COMPLETED, VOLTAGE_ENABLED - drive_x_state: SWITCH ON DISABLED - drive_x_state_flags: {} # Clear HOMING_COMPLETED, VOLTAGE_ENABLED - drive_x_control_mode: MODE_CSP - drive_control_word_flags: - OPERATION_MODE_SPECIFIC_1: False - drive_x_control_word_flags: # Clear OPERATION_MODE_SPECIFIC_1 - OPERATION_MODE_SPECIFIC_1: False command_out: - state_cmd: 0x01 - drive_x_control_word: 0x0002 # QUICK STOP - drive_x_control_mode: MODE_CSP - sim_feedback: state_cmd: 0x01 # stop - drive_x_status_word: 0x0017 # QUICK STOP ACTIVE + VOLTAGE_ENABLED - drive_x_control_mode_fb: MODE_CSP + d.x.control_word: 0x0002 # QUICK STOP + sim_feedback: + d.x.status_word: 0x0017 # QUICK STOP ACTIVE + VOLTAGE_ENABLED - desc: "Home: Command disable; 402 state machine transition 11" feedback_in: - state_cmd: 0x01 # stop - drive_x_status_word: 0x0017 # QUICK STOP ACTIVE + VOLTAGE_ENABLED - drive_x_control_mode_fb: MODE_CSP + d.x.status_word: 0x0017 # QUICK STOP ACTIVE + VOLTAGE_ENABLED feedback_out: - state_cmd: stop - drive_x_status_word: 0x0017 # QUICK STOP ACTIVE + VOLTAGE_ENABLED - drive_x_state: QUICK STOP ACTIVE - drive_x_state_flags: - VOLTAGE_ENABLED: True - WARNING: False - MANUFACTURER_SPECIFIC_1: False - REMOTE: False - TARGET_REACHED: False - INTERNAL_LIMIT_ACTIVE: False - OPERATION_MODE_SPECIFIC_1: False - OPERATION_MODE_SPECIFIC_2: False - MANUFACTURER_SPECIFIC_2: False - HOMING_COMPLETED: False # Cleared - drive_x_transition: TRANSITION_11 - drive_x_control_mode_fb: MODE_CSP - drive_x_goal_reached: False - drive_x_goal_reason: state QUICK STOP ACTIVE (0x00000017) != SWITCH ON DISABLED + d.x.status_word: 0x0017 # QUICK STOP ACTIVE + VOLTAGE_ENABLED + d.x.state: QUICK STOP ACTIVE + d.x.transition: 11 + d.x.goal_reached: False + d.x.goal_reason: state QUICK STOP ACTIVE (0x00000017) != SWITCH ON DISABLED command_in: state_log: Automatic transition from home_complete state command_out: - drive_x_control_word: 0x0000 # SWITCH ON DISABLED + d.x.control_word: 0x0000 # SWITCH ON DISABLED sim_feedback: - drive_x_status_word: 0x0050 # SWITCH ON DISABLED + VOLTAGE_ENABLED + d.x.status_word: 0x0050 # SWITCH ON DISABLED + VOLTAGE_ENABLED - desc: "Home: 402 state machine transition 12" feedback_in: - drive_x_status_word: 0x0050 # SWITCH ON DISABLED + VOLTAGE_ENABLED + d.x.status_word: 0x0050 # SWITCH ON DISABLED + VOLTAGE_ENABLED feedback_out: - drive_x_status_word: 0x0050 # SWITCH ON DISABLED + VOLTAGE_ENABLED - drive_x_state: SWITCH ON DISABLED - drive_x_transition: TRANSITION_12 - drive_x_goal_reached: True - drive_x_goal_reason: Reached + d.x.status_word: 0x0050 # SWITCH ON DISABLED + VOLTAGE_ENABLED + d.x.state: SWITCH ON DISABLED + d.x.transition: 12 + d.x.goal_reached: True + d.x.goal_reason: Reached command_in: command_complete: True - desc: "Home: Hold SWITCH ON DISABLED x1" feedback_out: - drive_x_transition: null + d.x.transition: -1 # No transition - desc: "Home: Hold SWITCH ON DISABLED x2" diff --git a/hw_device_mgr/mgr/tests/test_mgr.py b/hw_device_mgr/mgr/tests/test_mgr.py index 65494e45..b1da9eec 100644 --- a/hw_device_mgr/mgr/tests/test_mgr.py +++ b/hw_device_mgr/mgr/tests/test_mgr.py @@ -1,8 +1,8 @@ from .base_test_class import BaseMgrTestClass from ...tests.test_device import TestDevice as _TestDevice -from ...cia_402.device import CiA402Device import re import pytest +from functools import lru_cache, cached_property class TestHWDeviceMgr(BaseMgrTestClass, _TestDevice): @@ -17,12 +17,36 @@ class TestHWDeviceMgr(BaseMgrTestClass, _TestDevice): @pytest.fixture def obj(self, device_cls, mgr_config, device_config, all_device_data): self.obj = device_cls() - self.obj.init(mgr_config=mgr_config) - self.obj.init_sim(sim_device_data=all_device_data.values()) - self.obj.init_devices(device_config=device_config) + self.obj.init( + mgr_config=mgr_config, + device_config=device_config, + sim_device_data=all_device_data.values(), + ) yield self.obj - drive_key_re = re.compile(r"^drive_([x0-9])_(.*)$") + test_case_key_re = re.compile(r"^d\.([x0-9])\.(.*)$") + drive_interface_key_re = re.compile(r"^d([0-9])\.([0-9]+)\.(.*)$") + + @cached_property + def drive_addr_to_index_map(self): + return {d.address: i for i, d in enumerate(self.obj.devices)} + + @lru_cache + def obj_interface_to_test_case_key(self, interface_key): + # Translate test object interface key `d0.16.control_mode` to test case + # key `d.6.control_mode` + m = self.drive_interface_key_re.match(interface_key) + if m is None: + return interface_key # Doesn't match fmt. `d.6.control_mode` + address = tuple(map(int, m.groups()[:-1])) # (0, 16) + index = self.drive_addr_to_index_map[address] # 6 + key = m.groups()[-1] # "control_mode" + return f"d.{index}.{key}" + + def obj_interface_to_test_case(self, data): + return { + self.obj_interface_to_test_case_key(k): v for k, v in data.items() + } def test_init(self, obj, all_device_data): super().test_init(obj) @@ -42,73 +66,113 @@ def read_update_write_conv_test_data(self): if "error_code" in d.feedback_out.get(): # Account for some devices that inherit from ErrorDevice self.test_data["feedback_in"].setdefault( - f"drive_{i}_error_code", 0x00000000 + f"d.{i}.error_code", 0x00000000 ) self.test_data["feedback_out"].setdefault( - f"drive_{i}_error_code", 0x00000000 + f"d.{i}.error_code", 0x00000000 ) self.test_data["feedback_out"].setdefault( - f"drive_{i}_description", "No error" + f"d.{i}.description", "No error" ) self.test_data["feedback_out"].setdefault( - f"drive_{i}_advice", "No error" + f"d.{i}.advice", "No error" ) uint16 = self.device_class.data_type_class.uint16 - control_mode_int = CiA402Device.control_mode_int for data in (self.test_data, self.ovr_data): for intf, intf_data in data.items(): for key in intf_data.keys(): # Test drive keys - match = self.drive_key_re.match(key) + match = self.test_case_key_re.match(key) if not match: continue # Only testing drive keys - dkey = match.group(2) - if dkey == "control_mode" and intf == "command_out": - # Translate control_mode, e.g. MODE_CSP -> 8 - intf_data[key] = control_mode_int(intf_data[key]) - elif dkey == "control_mode_fb" and intf in ( - "sim_feedback", - "feedback_in", - ): - # Translate control_mode, e.g. MODE_CSP -> 8 - intf_data[key] = control_mode_int(intf_data[key]) - elif dkey in ("status_word", "control_word"): + if match.group(2) in ("status_word", "control_word"): # Format status_word, control_word for # readability, e.g. 0x000F intf_data[key] = uint16(intf_data[key]) def munge_test_case_data(self, test_case, dst, suffix=""): - # Expand drive_x_foo -> drive_1_foo, etc.; don't clobber + # Expand d.x.foo -> d.1.foo, etc.; don't clobber # specific case for intf in self.device_class.interface_names: values = test_case.get(intf + suffix, dict()) intf_dst = dst.setdefault(intf, dict()) for key, val in values.items(): - match = self.drive_key_re.match(key) + match = self.test_case_key_re.match(key) if not match: intf_dst[key] = val - continue + continue # Regular attribute, not d.x.foo d_ix, d_key = match.groups() if d_ix == "x": for i, dev in enumerate(self.obj.devices): - dev_key = f"drive_{i}_{d_key}" + dev_key = f"d.{i}.{d_key}" if dev_key not in values: intf_dst[dev_key] = val else: - intf_dst[key] = val + d_ix = int(d_ix) + dev_key = f"d.{d_ix}.{d_key}" + intf_dst[dev_key] = val + + def munge_interface_data(self, interface): + # Translate test case key `d.6.control_mode` to object interface key + # `d0.16.control_mode` + data_raw = super().munge_interface_data(interface) + data = dict() + for key, val in data_raw.items(): + m = self.test_case_key_re.match(key) + if m is None: + # Doesn't match fmt. `d.6.control_mode` + data[key] = val + else: + index, key = m.groups() # ("6", "control_mode") + dev = self.obj.devices[int(index)] + pfx = self.obj.dev_prefix(dev, suffix=dev.slug_separator) + dev_key = f"{pfx}{key}" # "d0.16.control_mode" + data[dev_key] = val + + return data + + def override_interface_param(self, interface, ovr_data): + for key, val in ovr_data.items(): + match = self.test_case_key_re.match(key) + if match: + index, key = match.groups() + intf = self.obj.devices[int(index)].interface(interface) + intf.update(**{key: val}) + else: + super().override_interface_param(interface, {key: val}) + + def check_interface_values(self, interface, indent=4): + if interface in {"feedback_out", "command_in"}: + # Higher level interfaces to application + return self.check_interface_values_higher(interface, indent=indent) + + else: + # Lower level interfaces close to hardware interface + return self.check_interface_values_lower(interface, indent=indent) + + def check_interface_values_higher(self, interface, indent=4): + # Check feedback_in, command_out, sim interfaces + expected = self.test_data[interface] + actual_raw = self.obj.interface(interface).get() + actual = self.obj_interface_to_test_case(actual_raw) + if super().check_data_values(interface, expected, actual, indent): + return True + else: + print(f"FAILURE at {self.test_desc}") + return False def split_drive_data(self, data): # The manager is a device, and each of obj.devices is a # device. The test cases are crammed together with device - # keys prefixed with `drive_x_`, meaning it applies to all - # drives, or prefixed with `drive_0_`, meaning it applies to + # keys prefixed with `d.x.`, meaning it applies to all + # drives, or prefixed with `d.0.`, meaning it applies to # the first drive, or prefixed with nothing, meaning it # applies to the manager. mgr_data = data.copy() device_data = list([dict() for d in self.obj.devices]) - for drive_key in list(mgr_data.keys()): - match = self.drive_key_re.match(drive_key) + for drive_key, val in list(mgr_data.items()): + match = self.test_case_key_re.match(drive_key) if not match: continue # Not a drive_key val = mgr_data.pop(drive_key) @@ -116,16 +180,9 @@ def split_drive_data(self, data): device_data[int(index)][key] = val return mgr_data, device_data - def override_interface_param(self, interface, key, val): - match = self.drive_key_re.match(key) - if match: - index, key = match.groups() - intf = self.obj.devices[int(index)].interface(interface) - intf.update(**{key: val}) - else: - super().override_interface_param(interface, key, val) + def check_interface_values_lower(self, interface, indent=4): + # Check feedback_in, command_out, sim interfaces - def check_interface_values(self, interface, indent=4): # Prepare expected data expected = self.test_data[interface] mgr_expected, device_expected = self.split_drive_data(expected) @@ -147,21 +204,9 @@ def check_interface_values(self, interface, indent=4): actual = device.interface(interface).get() # self.print_dict(actual, f"drive_{i}", indent=4) passing &= self.check_data_values( - "", device_expected[i], actual, indent, f"drive_{i}_" + "", device_expected[i], actual, indent=indent, prefix=f"d.{i}." ) if not passing: print(f"FAILURE at {self.test_desc}") return passing - - def set_command_and_check(self): - print("\n*** Running object set_command()") - # Mgr command_in is internally generated; no args to set_command() - # (Drive command is set by mgr; no need to call that separately) - self.obj.set_command() - success = self.check_interface_values("command_in") - success = self.check_interface_values("command_out") and success - assert success - print("\n*** Overriding command_out") - self.override_data("command_out") - # self.print_dict(self.test_data, "Test data (after override)") From ab7797c75b107a0ad6b5aebf9e2f05732e5c0058 Mon Sep 17 00:00:00 2001 From: John Morris Date: Mon, 11 Jul 2022 14:13:07 -0500 Subject: [PATCH 80/88] mgr: Move several keys from command_in to command_out interface Another artifact of the previous screwey design where the manager command was passed through the `feedback_in` interface, the keys `state_log`, `command_complete`, `reset` and `drive_state` were part of the `command_in` interface. This commit moves them to the `command_out` interface where they logically belong. In a similar way, this commit also fixes the handling of the `state_cmd` key: The previous screwey design overrode that key in the `command_in` interface, when it should have been overridden only in the `command_out` interface. This doesn't make sense, and also breaks tests when `command_in` is set from test data, but then changes before the interface is checked (again) against test data. --- hw_device_mgr/mgr/mgr.py | 160 +++++++++++------- .../mgr/tests/read_update_write.cases.yaml | 114 +++++-------- 2 files changed, 137 insertions(+), 137 deletions(-) diff --git a/hw_device_mgr/mgr/mgr.py b/hw_device_mgr/mgr/mgr.py index 1d819bb1..26569023 100644 --- a/hw_device_mgr/mgr/mgr.py +++ b/hw_device_mgr/mgr/mgr.py @@ -24,13 +24,24 @@ def device_model_id(cls): command_in_defaults = dict( state_cmd="init", - state_log="", + ) + command_in_data_types = dict( + state_cmd="str", + ) + command_out_defaults = dict( + state_cmd=0, + state_log="(Uninitialized)", command_complete=False, + reset=0, drive_state="SWITCH ON DISABLED", - reset=False, ) - command_out_defaults = dict(state_cmd=0, reset=0) - command_out_data_types = dict(state_cmd="uint8", reset="bit") + command_out_data_types = dict( + state_cmd="uint8", + state_log="str", + command_complete="bit", + reset="bit", + drive_state="str", + ) #################################################### # Initialization @@ -164,8 +175,8 @@ def on_enter_init_complete(self, e): self.fsm_finalize_command(e) # Automatically return to SWITCH ON DISABLED after init self.logger.info("Devices all online; commanding stop state") - self.command_in.set( - state_cmd="stop", + self.command_out.update( + state_cmd=self.cmd_name_to_int_map["stop"], state_log="Automatic 'stop' command at init complete", ) @@ -238,8 +249,8 @@ def on_change_state(self, e): # Set/clear reset command reset = getattr(e, "reset", False) - self.command_in.update(reset=reset) - if self.command_in.changed("reset"): + self.command_out.update(reset=reset) + if self.command_out.changed("reset"): self.logger.info(f"Reset command set to {reset}") # @@ -254,6 +265,10 @@ def on_change_state(self, e): cmd_int_to_name_map = {v: k for k, v in cmd_name_to_int_map.items()} + @property + def state_cmd_str(self): + return self.cmd_int_to_name_map[self.command_out.get("state_cmd")] + def timer_start(self, timeout=None): if timeout is None: timeout = self.mgr_config.get("goal_state_timeout", 30.0) @@ -263,8 +278,9 @@ def timer_check_overrun(self, msg): if not hasattr(self, "_timeout") or time.time() <= self._timeout: return - msg = f"{self.command_in.get('state_cmd')} timeout: {msg}" - self.command_in.set(state_cmd="fault", state_log=msg) + msg = f"{self.state_cmd_str} timeout: {msg}" + fault_cmd = self.cmd_name_to_int_map["fault"] + self.command_out.update(state_cmd=fault_cmd, state_log=msg) del self._timeout raise HWDeviceTimeout(msg) @@ -276,26 +292,31 @@ def fsm_check_devices_online(self, e, state): return self.query_devices(oper=False) def fsm_check_command(self, e, timeout=None): - cmd_name = self.fsm_command_from_event(e) + state_cmd_str = self.fsm_command_from_event(e) + state_cmd = self.cmd_name_to_int_map[state_cmd_str] if ( e.src.startswith("init") and e.src != "init_complete" - ) and cmd_name != "init": + ) and state_cmd_str != "init": # Don't preempt init (fault) - msg = f"Ignoring {cmd_name} command in init state {e.src}" - self.command_in.set(state_cmd="init", state_log=msg) - if self.command_in.changed("state_cmd"): + msg = f"Ignoring {state_cmd_str} command in init state {e.src}" + state_cmd = self.cmd_name_to_int_map["init"] + self.command_out.update(state_cmd=state_cmd, state_log=msg) + if self.command_out.changed("state_cmd"): self.logger.warning(msg) return False - elif e.src != f"{cmd_name}_command" and e.src.startswith(cmd_name): + elif e.src != f"{state_cmd_str}_command" and e.src.startswith( + state_cmd_str + ): # Already running self.logger.warning( - f"Ignoring {cmd_name} command from state {e.src}" + f"Ignoring {state_cmd_str} command from state {e.src}" ) return False else: - self.logger.info(f"Received {cmd_name} command: {e.msg}") - self.command_in.set(state_cmd=cmd_name, state_log=e.msg) - self.command_in.update(command_complete=False) + self.logger.info(f"Received {state_cmd_str} command: {e.msg}") + self.command_out.update( + state_cmd=state_cmd, state_log=e.msg, command_complete=False + ) self.timer_start( timeout=timeout or self.mgr_config.get("goal_state_timeout", 5.0) @@ -318,11 +339,11 @@ def fsm_set_drive_state_cmd(self, e, state): self.logger.info( f"{cmd_name} command: Setting drive state command to {state}" ) - self.command_in.update(drive_state=state) + self.command_out.update(drive_state=state) def fsm_finalize_command(self, e): cmd_name = self.fsm_command_from_event(e) - self.command_in.update(command_complete=True) + self.command_out.update(command_complete=True) self.logger.info(f"Command {cmd_name} completed") #################################################### @@ -342,8 +363,9 @@ def run_loop(self): self.logger.error("Ignoring unexpected exception; details:") for line in traceback.format_exc().splitlines(): self.logger.error(line) - self.command_in.set( - state_cmd="fault", msg_log="Unexpected exception" + self.command_out.update( + state_cmd=self.cmd_name_to_int_map["fault"], + msg_log="Unexpected exception", ) if self.fast_track: # This update included a state transition; skip @@ -414,34 +436,33 @@ def read(self): def get_feedback(self): """Process manager and device external feedback.""" - fb_out = super().get_feedback() + mgr_fb_out = super().get_feedback() # Get device feedback for dev in self.devices: - dev.get_feedback() - # Copy device feedback to mgr feedback, adding prefix - skip = self.device_translated_interfaces["feedback_out"] - dev_intf = dev.interface("feedback_out") + dev_fb_out = dev.get_feedback() prefix = self.dev_prefix(dev, suffix=dev.slug_separator) updates = { + # Copy device fb_out to mgr fb_out, adding prefix f"{prefix}{k}":v - for k,v in dev_intf.get().items() - if k not in skip + for k,v in dev_fb_out.get().items() + # ...but skip these keys + if k not in self.device_translated_interfaces["feedback_out"] } - fb_out.update(**updates) + mgr_fb_out.update(**updates) - return fb_out + return mgr_fb_out def set_command(self, **kwargs): """ Set command for top-level manager and for drives. """ - # Initialize command in/out interfaces with previous cycle values - cmd_in = self.command_in - cmd_out = super().set_command(**cmd_in.get()) - - # Update incoming command - cmd_in.update(**kwargs) + # Initialize command out interface with previous values; this could + # clobber parent class updates for regular device classes, but this + # isn't a regular device and it inherits directly from `Device` + old_cmd_out = self.command_out.get().copy() + cmd_out = super().set_command(**kwargs) + cmd_out.update(**old_cmd_out) # Special cases where 'fault' overrides current command: if self.state.startswith("init"): @@ -452,8 +473,8 @@ def set_command(self, **kwargs): # Treat devices not operational as a fault fds = self.query_devices(oper=False) fd_addrs = ", ".join(str(d.address) for d in fds) - cmd_in.update( - state_cmd="fault", + cmd_out.update( + state_cmd=self.cmd_name_to_int_map["fault"], state_log=f"Devices at ({fd_addrs}) not online and operational", ) elif self.query_devices(state="FAULT") and self.query_devices( @@ -462,8 +483,8 @@ def set_command(self, **kwargs): # Devices went into FAULT state since last update fds = self.query_devices(state="FAULT") fd_addrs = ", ".join(str(d.address) for d in fds) - cmd_in.update( - state_cmd="fault", + cmd_out.update( + state_cmd=self.cmd_name_to_int_map["fault"], state_log=f"Devices at ({fd_addrs}) in FAULT state", ) elif self.query_devices(fault=True) and self.query_devices( @@ -472,26 +493,46 @@ def set_command(self, **kwargs): # Devices set `fault` since last update fds = self.query_devices(fault=True) fd_addrs = ", ".join(str(d.address) for d in fds) - cmd_in.update( - state_cmd="fault", + cmd_out.update( + state_cmd=self.cmd_name_to_int_map["fault"], state_log=f"Devices at ({fd_addrs}) set fault", ) + elif kwargs.get("state_cmd", None) is None: + pass # Use previous state_cmd value + elif kwargs["state_cmd"] not in self.cmd_name_to_int_map: + state_cmd = kwargs["state_cmd"] + self.logger.error(f"Invalid state command, '{state_cmd}'") + cmd_out.update( + state_cmd=self.cmd_name_to_int_map["fault"], + state_log=f"Invalid state command, '{state_cmd}'", + ) + else: + # Take state_cmd from kwargs + state_cmd = kwargs["state_cmd"] + cmd_out.update( + state_cmd=self.cmd_name_to_int_map[state_cmd], + ) + if cmd_out.changed("state_cmd"): + # Assume external command + cmd_out.update( + state_log=f"External command '{state_cmd}'", + ) - event = self.automatic_next_event() - if cmd_in.changed("state_cmd"): + if cmd_out.changed("state_cmd"): # Received new command to stop/start/fault. Try it # by triggering the FSM event; a Canceled exception means # it can't be done, so ignore it. - event = f"{cmd_in.get('state_cmd')}_command" + event = f"{self.state_cmd_str}_command" try: - self.trigger(event, msg=cmd_in.get("state_log")) + self.trigger(event, msg=cmd_out.get("state_log")) except Canceled: self.logger.warning(f"Unable to honor {event} command") - elif event is not None: + elif self.automatic_next_event() is not None: # Attempt automatic transition to next state try: self.trigger( - event, msg=f"Automatic transition from {self.state} state" + self.automatic_next_event(), + msg=f"Automatic transition from {self.state} state", ) except Canceled: # `on_before_{event}()` method returned `False`, @@ -502,10 +543,7 @@ def set_command(self, **kwargs): # State transition succeeded; fast-track the next update self.fast_track = True - # Set command, incl. drive command, and return - state_cmd = self.cmd_name_to_int_map[cmd_in.get("state_cmd")] - reset = cmd_in.get("reset") - cmd_out.update(state_cmd=state_cmd, reset=reset) + # Set drive command and return self.set_drive_command() return cmd_out @@ -516,9 +554,9 @@ def write(self): dev.write() def automatic_next_event(self): - state_cmd = self.command_in.get("state_cmd") - state_map = self.fsm_next_state_map[state_cmd] - event = state_map.get(self.state, f"{state_cmd}_command") + state_cmd_str = self.state_cmd_str + state_map = self.fsm_next_state_map[state_cmd_str] + event = state_map.get(self.state, f"{state_cmd_str}_command") return event #################################################### @@ -542,11 +580,11 @@ def set_drive_command(self): for dev in self.devices: updates = dict() if "command_in" in self.device_translated_interfaces: - # Copy mgr command_in to matching device command + # Copy mgr command_out to matching device command_in dev_command_in = dev.interface("command_in") prefix = self.dev_prefix(dev, suffix=dev.slug_separator) dev.set_command( - state=self.command_in.get("drive_state"), + state=self.command_out.get("drive_state"), **{ k:mgr_vals[f"{prefix}{k}"] for k in dev_command_in.keys() @@ -555,7 +593,7 @@ def set_drive_command(self): ) else: dev.set_command( - state=self.command_in.get("drive_state"), + state=self.command_out.get("drive_state"), ) def query_devices(self, **kwargs): diff --git a/hw_device_mgr/mgr/tests/read_update_write.cases.yaml b/hw_device_mgr/mgr/tests/read_update_write.cases.yaml index 5ea3da3a..75181af6 100644 --- a/hw_device_mgr/mgr/tests/read_update_write.cases.yaml +++ b/hw_device_mgr/mgr/tests/read_update_write.cases.yaml @@ -34,10 +34,6 @@ command_in: # Mgr state_cmd: init - state_log: "Automatic transition from init_command state" - command_complete: False - drive_state: SWITCH ON DISABLED - reset: False # Drives # - CiA 402 d.x.control_mode: 8 # MODE_CSP @@ -45,6 +41,9 @@ command_out: # Mgr state_cmd: 0 # init + state_log: "Automatic transition from init_command state" + command_complete: False + drive_state: SWITCH ON DISABLED reset: False # Drives # - CiA 402 @@ -105,16 +104,18 @@ d.x.goal_reason: Reached command_in: state_cmd: stop - state_log: Automatic 'stop' command at init complete command_out: state_cmd: 0x01 # stop + state_log: Automatic 'stop' command at init complete + command_complete: 1 - desc: "Init: Automatic 'stop' command" feedback_out: d.x.transition: -1 # No transition - command_in: + command_out: state_log: "Automatic transition from init_complete state" + command_complete: 0 - desc: "Init: Complete" - command_in: + command_out: command_complete: True - desc: "Init: Hold state" @@ -125,12 +126,12 @@ - desc: "Enable: Receive external 'start' command" command_in: state_cmd: start - state_log: state command feedback changed - command_complete: False + command_out: # Controller commands drive state SWITCHED ON drive_state: SWITCHED ON - command_out: state_cmd: 2 # start + state_log: External command 'start' + command_complete: False d.x.control_word: 0x0006 # READY TO SWITCH ON sim_feedback: d.x.status_word: 0x0031 # READY TO SWITCH ON + VOLTAGE_ENABLED @@ -157,14 +158,12 @@ # Reached pre-operational SWITCHED ON goal d.x.goal_reached: True d.x.goal_reason: Reached - command_in: + command_out: # Continue with OPERATION ENABLED goal drive_state: OPERATION ENABLED # Reset high for one cycle - reset: True - command_out: - d.x.control_word: 0x000F # OPERATION ENABLED reset: 1 + d.x.control_word: 0x000F # OPERATION ENABLED sim_feedback: d.x.status_word: 0x0037 # OPERATION ENABLED + VOLTAGE_ENABLED - desc: "Enable: Command complete" @@ -176,10 +175,8 @@ d.x.transition: 4 d.x.goal_reached: True d.x.goal_reason: Reached - command_in: - command_complete: True - reset: False command_out: + command_complete: True reset: 0 - desc: "Enable: Hold state OPERATION ENABLED x1" feedback_out: @@ -206,11 +203,11 @@ d.1.goal_reason: state FAULT REACTION ACTIVE (0x0000001F) != OPERATION ENABLED command_in: state_cmd: fault + command_out: + state_cmd: 0x04 # fault state_log: Devices at ((0, 11)) set fault command_complete: False drive_state: FAULT - command_out: - state_cmd: 0x04 # fault d.x.control_word: 0x0002 # QUICK STOP ACTIVE d.1.control_word: 0x0000 # SWITCH ON DISABLED sim_feedback: @@ -233,9 +230,8 @@ d.x.transition: 11 d.x.goal_reached: False d.x.goal_reason: state QUICK STOP ACTIVE (0x00000017) != FAULT - command_in: - state_log: Devices at ((0, 11)) in FAULT state command_out: + state_log: Devices at ((0, 11)) in FAULT state # Manager commands TRANSITION_12 to SWITCH ON DISABLED d.x.control_word: 0x0000 # SWITCH ON DISABLED sim_feedback: @@ -254,7 +250,7 @@ d.x.transition: 12 d.x.goal_reached: True d.x.goal_reason: Reached - command_in: + command_out: # Manager has all drives in FAULT mode command_complete: True - desc: "Sim fault: Hold fault command x1" @@ -265,11 +261,11 @@ command_in: # Manager acts on 'start' command from feedback; commands drives SWITCHED ON state_cmd: start - state_log: state command feedback changed - command_complete: False - drive_state: SWITCHED ON command_out: state_cmd: 0x02 # start + state_log: External command 'start' + command_complete: False + drive_state: SWITCHED ON d.x.control_word: 0x0006 # READY TO SWITCH ON d.1.control_word: 0x0080 # Clear fault sim_feedback: @@ -347,10 +343,10 @@ d.x.transition: -1 # No transition command_in: state_cmd: fault - state_log: Devices at ((0, 12)) set fault - drive_state: FAULT command_out: state_cmd: 0x04 # fault + state_log: Devices at ((0, 12)) set fault + drive_state: FAULT d.x.control_word: 0x0000 # SWITCH ON DISABLED sim_feedback: d.2.status_word: 0x0018 # FAULT + VOLTAGE_ENABLED @@ -371,11 +367,11 @@ command_in: # Controller sees FAULT state and commands hold at that state state_cmd: fault + command_out: + state_cmd: 0x04 # fault state_log: Devices at ((0, 12)) in FAULT state command_complete: True drive_state: FAULT - command_out: - state_cmd: 0x04 # fault d.x.control_word: 0x0000 # SWITCH ON DISABLED sim_feedback: d.x.status_word: 0x0050 # SWITCH ON DISABLED + VOLTAGE_ENABLED @@ -393,11 +389,11 @@ - desc: "Disable: Command disable" command_in: state_cmd: stop - state_log: state command feedback changed - drive_state: SWITCH ON DISABLED - # Controller commands disable command_out: state_cmd: 0x01 # stop + state_log: External command 'stop' + command_complete: False + drive_state: SWITCH ON DISABLED d.2.control_word: 0x0080 # Clear fault d.x.control_word: 0x0000 # SWITCH ON DISABLED sim_feedback: @@ -412,6 +408,7 @@ d.2.fault: False d.2.transition: 15 command_out: + command_complete: True d.x.control_word: 0x0000 # SWITCH ON DISABLED - desc: "Disable: Hold state x1" feedback_out: @@ -420,38 +417,6 @@ d.x.goal_reason: Reached - desc: "Disable: Hold state x2" -# -# Controller commands FAULT in reaction to quick_stop input; hold -# disabled state -# -- desc: "Ext quick_stop noop: Command FAULT" - command_in: - # Controller commands FAULT - state_cmd: fault - state_log: quick_stop pin high - drive_state: FAULT - command_out: - state_cmd: 0x04 # fault -- desc: "Ext fault noop: Clear quick stop and hold fault state" - command_in: - state_log: state command feedback changed -- desc: "Ext fault noop: Hold fault state" -- desc: "Ext fault noop: External stop command, but quick_stop still set" - command_in: - state_log: quick_stop pin high -- desc: "Ext fault noop: Manager forces fault command" - command_in: - state_log: state command feedback changed -- desc: "Ext fault noop: Hold fault state" -- desc: "Ext fault noop: External quick_stop pin cleared and stop command" - command_in: - state_cmd: stop - drive_state: SWITCH ON DISABLED - command_out: - state_cmd: 0x01 # stop -- desc: "Ext fault noop: Hold stop state" - - # # Controller moves to OPERATION ENABLED, then commands homing # operation on first two drives @@ -464,11 +429,12 @@ - desc: "Home: Command OPERATION ENABLED" command_in: state_cmd: start + command_out: + state_cmd: 2 # start + state_log: External command 'start' command_complete: False # Controller commands drive state SWITCHED ON drive_state: SWITCHED ON - command_out: - state_cmd: 2 # start d.x.control_word: 0x0006 # READY TO SWITCH ON sim_feedback: d.x.status_word: 0x0031 # READY TO SWITCH ON + VOLTAGE_ENABLED @@ -494,10 +460,8 @@ d.x.transition: 3 d.x.goal_reached: True d.x.goal_reason: Reached - command_in: - drive_state: OPERATION ENABLED - reset: True command_out: + drive_state: OPERATION ENABLED reset: 1 d.x.control_word: 0x000F # OPERATION ENABLED sim_feedback: @@ -512,12 +476,11 @@ d.x.goal_reached: True d.x.goal_reason: Reached command_in: - command_complete: True - reset: False # Controller commands homing start d.0.home_request: True d.1.home_request: True command_out: + command_complete: True reset: 0 d.0.control_mode: 6 # MODE_HM d.1.control_mode: 6 # MODE_HM @@ -572,10 +535,11 @@ d.x.home_success: False command_in: state_cmd: stop - state_log: Automatic 'stop' command at home complete - drive_state: SWITCH ON DISABLED command_out: state_cmd: 0x01 # stop + state_log: External command 'stop' + command_complete: False + drive_state: SWITCH ON DISABLED d.x.control_word: 0x0002 # QUICK STOP sim_feedback: d.x.status_word: 0x0017 # QUICK STOP ACTIVE + VOLTAGE_ENABLED @@ -588,8 +552,6 @@ d.x.transition: 11 d.x.goal_reached: False d.x.goal_reason: state QUICK STOP ACTIVE (0x00000017) != SWITCH ON DISABLED - command_in: - state_log: Automatic transition from home_complete state command_out: d.x.control_word: 0x0000 # SWITCH ON DISABLED sim_feedback: @@ -603,7 +565,7 @@ d.x.transition: 12 d.x.goal_reached: True d.x.goal_reason: Reached - command_in: + command_out: command_complete: True - desc: "Home: Hold SWITCH ON DISABLED x1" From 3075bcfc62405060e2b040c2af86d00ee2ebb783 Mon Sep 17 00:00:00 2001 From: John Morris Date: Mon, 11 Jul 2022 17:04:07 -0500 Subject: [PATCH 81/88] mgr: Move device interface key creation to `init_devices` method There's a chicken and egg problem in `init()`, where the parent `init()` initializes interfaces, but `init_devices()` hasn't yet created device-related interface keys. This may not yet be fully resolved, but this is a crude attempt that moves device interface key creation to `init_devices()`. It may still be a problem in the HAL manager, since the HAL comp device `init()` method creates the HAL comp, creates interfaces with HAL pins, and then marks the comp ready. --- hw_device_mgr/mgr/mgr.py | 57 ++++++++++++++++++---------------------- 1 file changed, 26 insertions(+), 31 deletions(-) diff --git a/hw_device_mgr/mgr/mgr.py b/hw_device_mgr/mgr/mgr.py index 26569023..31ca8743 100644 --- a/hw_device_mgr/mgr/mgr.py +++ b/hw_device_mgr/mgr/mgr.py @@ -57,10 +57,10 @@ def init(self, /, mgr_config, device_config, **kwargs): assert device_config, "Empty device configuration" self.device_config = device_config self.device_base_class.set_device_config(device_config) - # Scan & init devices - self.init_devices(**kwargs) # Init self super().init() + # Scan & init devices + self.init_devices(**kwargs) self.logger.info("Initialization complete") @classmethod @@ -90,6 +90,20 @@ def init_devices( dev.init(**device_init_kwargs) self.logger.info(f"Initialized device {dev}") + # Add per-device interface attributes to feedback & command + for dev in self.devices: + prefix = self.dev_prefix(dev, suffix=dev.slug_separator) + for name, skip_set in self.device_translated_interfaces.items(): + dev_intf = dev.interface(name) + mgr_intf = self.interface(name) + for attr_name, val in dev_intf.get().items(): + if attr_name in skip_set: + continue + mgr_attr_name = prefix + attr_name + mgr_intf.add_attribute( + mgr_attr_name, val, dev_intf.get_data_type(attr_name) + ) + @classmethod def scan_devices(cls, **kwargs): return cls.device_base_class.scan_devices(**kwargs) @@ -107,24 +121,6 @@ def scan_devices(cls, **kwargs): def dev_prefix(self, dev, prefix="d", suffix=""): return f"{prefix}{dev.addr_slug}{suffix}" - - def init_interfaces(self): - """Add per-device interface attributes to feedback & command.""" - super().init_interfaces() - for dev in self.devices: - prefix = self.dev_prefix(dev, suffix=dev.slug_separator) - for name, skip_set in self.device_translated_interfaces.items(): - dev_intf = dev.interface(name) - mgr_intf = self.interface(name) - for attr_name, val in dev_intf.get().items(): - if attr_name in skip_set: - continue - mgr_attr_name = prefix + attr_name - mgr_intf.add_attribute( - mgr_attr_name, val, dev_intf.get_data_type(attr_name) - ) - - #################################################### # Drive state FSM @@ -444,8 +440,8 @@ def get_feedback(self): prefix = self.dev_prefix(dev, suffix=dev.slug_separator) updates = { # Copy device fb_out to mgr fb_out, adding prefix - f"{prefix}{k}":v - for k,v in dev_fb_out.get().items() + f"{prefix}{k}": v + for k, v in dev_fb_out.get().items() # ...but skip these keys if k not in self.device_translated_interfaces["feedback_out"] } @@ -454,9 +450,7 @@ def get_feedback(self): return mgr_fb_out def set_command(self, **kwargs): - """ - Set command for top-level manager and for drives. - """ + """Set command for top-level manager and for drives.""" # Initialize command out interface with previous values; this could # clobber parent class updates for regular device classes, but this # isn't a regular device and it inherits directly from `Device` @@ -567,18 +561,19 @@ def init_sim_devices(cls, /, sim_device_data=None, **kwargs): """ Run `init_sim()` on devices. - For configurations that include sim devices (even when the device - manager itself isn't running in sim mode). + For configurations that include sim devices (even when the + device manager itself isn't running in sim mode). """ if sim_device_data is None: return # No sim devices to configure - cls.device_base_class.init_sim(sim_device_data=sim_device_data, **kwargs) + cls.device_base_class.init_sim( + sim_device_data=sim_device_data, **kwargs + ) def set_drive_command(self): mgr_vals = self.command_in.get() skip = self.device_translated_interfaces.get("command_in", set()) for dev in self.devices: - updates = dict() if "command_in" in self.device_translated_interfaces: # Copy mgr command_out to matching device command_in dev_command_in = dev.interface("command_in") @@ -586,10 +581,10 @@ def set_drive_command(self): dev.set_command( state=self.command_out.get("drive_state"), **{ - k:mgr_vals[f"{prefix}{k}"] + k: mgr_vals[f"{prefix}{k}"] for k in dev_command_in.keys() if k not in skip - } + }, ) else: dev.set_command( From 69414e329b2baa57df681bfd3b2122088cd11e75 Mon Sep 17 00:00:00 2001 From: John Morris Date: Mon, 11 Jul 2022 17:11:53 -0500 Subject: [PATCH 82/88] mgr_hal: Updates for recent changes in tests Updates for `override_interface_param()` change --- hw_device_mgr/mgr_hal/tests/test_mgr.py | 20 +++++++++++--------- 1 file changed, 11 insertions(+), 9 deletions(-) diff --git a/hw_device_mgr/mgr_hal/tests/test_mgr.py b/hw_device_mgr/mgr_hal/tests/test_mgr.py index 44ea8090..a126964b 100644 --- a/hw_device_mgr/mgr_hal/tests/test_mgr.py +++ b/hw_device_mgr/mgr_hal/tests/test_mgr.py @@ -15,15 +15,17 @@ class TestHALHWDeviceMgr(BaseHALMgrTestClass, _TestHWDeviceMgr, _TestHALDevice): _TestHALDevice.expected_mro[-1], # HalMixin ] - def override_interface_param(self, interface, key, val): - match = self.drive_key_re.match(key) - if match: - index, key = match.groups() - dev = self.obj.devices[int(index)] - pname = dev.pin_name(interface, key) - self.set_pin(pname, val) - else: - super().override_interface_param(interface, key, val) + + def override_interface_param(self, interface, ovr_data): + for key, val in ovr_data.items(): + match = self.test_case_key_re.match(key) + if match: + index, key = match.groups() + dev = self.obj.devices[int(index)] + pname = dev.pin_name(interface, key) + self.set_pin(pname, val) + else: + super().override_interface_param(interface, key, val) def copy_sim_feedback(self): super().copy_sim_feedback() From 3bc44c57baa3e6925f00217a2dd86c1ef65066ee Mon Sep 17 00:00:00 2001 From: John Morris Date: Mon, 11 Jul 2022 17:11:53 -0500 Subject: [PATCH 83/88] mgr_hal: Updates for recent changes in init() --- hw_device_mgr/mgr_hal/mgr.py | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/hw_device_mgr/mgr_hal/mgr.py b/hw_device_mgr/mgr_hal/mgr.py index e87f7416..55353ad2 100644 --- a/hw_device_mgr/mgr_hal/mgr.py +++ b/hw_device_mgr/mgr_hal/mgr.py @@ -9,12 +9,9 @@ class HALHWDeviceMgr(HWDeviceMgr, HALCompDevice): data_type_class = HALCompDevice.data_type_class device_base_class = HALPinDevice - def init_devices(self, **kwargs): - super().init_devices(**kwargs) - self.hal_comp_ready() - - def init_device_instances(self, **kwargs): - super().init_device_instances(comp=self.comp, **kwargs) + def init_devices(self, /, device_init_kwargs=dict(), **kwargs): + device_init_kwargs["comp"] = self.comp + super().init_devices(device_init_kwargs=device_init_kwargs, **kwargs) class HALSimHWDeviceMgr(HALHWDeviceMgr, SimHWDeviceMgr, HALPinSimDevice): From ec4ff20574b6cd5399fbf2a911263748806ced31 Mon Sep 17 00:00:00 2001 From: John Morris Date: Mon, 11 Jul 2022 17:11:53 -0500 Subject: [PATCH 84/88] mgr_hal: Fix HAL pin init and add fb_out and cmd_in HAL pins The `HALHWDeviceMgr` class inherits from `HALCompDevice` before `HWDeviceMgr` so that the former's `init()` function initializes the HAL comp before the latter's `init()` function runs, and afterwards, marks the HAL comp `ready()`. Set empty `slug_separator` to avoid prefixing pins with e.g. `.reset`. --- hw_device_mgr/mgr_hal/mgr.py | 15 ++++++++++++-- hw_device_mgr/mgr_hal/tests/test_mgr.py | 27 ++++++++++++++----------- 2 files changed, 28 insertions(+), 14 deletions(-) diff --git a/hw_device_mgr/mgr_hal/mgr.py b/hw_device_mgr/mgr_hal/mgr.py index 55353ad2..63ce539b 100644 --- a/hw_device_mgr/mgr_hal/mgr.py +++ b/hw_device_mgr/mgr_hal/mgr.py @@ -2,13 +2,24 @@ from ..hal.device import HALCompDevice, HALPinDevice, HALPinSimDevice -class HALHWDeviceMgr(HWDeviceMgr, HALCompDevice): +class HALHWDeviceMgr(HALCompDevice, HWDeviceMgr): """Hardware device manager with HAL pins.""" + # Inherit from `HALCompDevice` first to ensure `HALCompDevice.init()` sets + # `self.comp` first, `HWDeviceMgr.init()` calls `HALPinDevice.init()` then + # `HWDeviceMgr.init_devices()`, then returns to `HALCompDevice.init()` to + # call `comp.ready()`. + hal_comp_name = "hw_device_mgr" - data_type_class = HALCompDevice.data_type_class device_base_class = HALPinDevice + pin_interfaces = dict( + feedback_in=(HALPinDevice.HAL_IN, ""), + feedback_out=(HALPinDevice.HAL_OUT, ""), + command_in=(HALPinDevice.HAL_IN, ""), + command_out=(HALPinDevice.HAL_OUT, ""), + ) + def init_devices(self, /, device_init_kwargs=dict(), **kwargs): device_init_kwargs["comp"] = self.comp super().init_devices(device_init_kwargs=device_init_kwargs, **kwargs) diff --git a/hw_device_mgr/mgr_hal/tests/test_mgr.py b/hw_device_mgr/mgr_hal/tests/test_mgr.py index a126964b..7565a24b 100644 --- a/hw_device_mgr/mgr_hal/tests/test_mgr.py +++ b/hw_device_mgr/mgr_hal/tests/test_mgr.py @@ -8,14 +8,13 @@ class TestHALHWDeviceMgr(BaseHALMgrTestClass, _TestHWDeviceMgr, _TestHALDevice): "HALHWDeviceMgrTestCategory", "HALSimHWDeviceMgr", "HALHWDeviceMgr", - *_TestHWDeviceMgr.expected_mro[1:4], # SimHWDeviceMgr..FysomGlobalMixin "HALCompDevice", # HAL comp (this should be tested, too!) - *_TestHALDevice.expected_mro[:2], # HALPinSim...HALPin - *_TestHWDeviceMgr.expected_mro[4:], # SimDevice...ABC - _TestHALDevice.expected_mro[-1], # HalMixin + _TestHWDeviceMgr.expected_mro[1], # SimHWDeviceMgr + *_TestHALDevice.expected_mro[:2], # HALPinSimDevice...HALPinDevice + *_TestHWDeviceMgr.expected_mro[2:], # HWDeviceMgr...ABC + _TestHALDevice.expected_mro[-1], # HalMixin (skip CiA301, etc.) ] - def override_interface_param(self, interface, ovr_data): for key, val in ovr_data.items(): match = self.test_case_key_re.match(key) @@ -34,18 +33,22 @@ def copy_sim_feedback(self): def post_read_actions(self, obj=None): if obj is None: - super().post_read_actions() - print(" feedback_in pin values:") obj = self.obj + super().post_read_actions() + for dev in obj.devices: + self.post_read_actions(dev) - for name in obj.feedback_in.get(): + print(f" feedback_in pin values: {obj}") + for name in obj.pins["feedback_in"]: pname = obj.pin_name("feedback_in", name) val = self.get_pin(pname) print(f" {pname} = {val}") - assert val == obj.feedback_in.get(pname) + assert val == obj.feedback_in.get(name) print() - def check_halpin_values(self): - super().check_halpin_values() + def post_write_actions(self): + super().post_write_actions() for dev in self.obj.devices: - super().check_halpin_values(obj=dev) + for iface in dev.pins: + if dev.pin_interfaces[iface][0] == dev.HAL_OUT: + self.check_halpin_values(iface, dev) From 5604cef2f6a968bddaa414739c2d9a74626b198b Mon Sep 17 00:00:00 2001 From: John Morris Date: Mon, 11 Jul 2022 17:15:40 -0500 Subject: [PATCH 85/88] mgr_ros: Updates for recent changes in init() Update for `init()` rework: - Update method signature - Fold reading device config and sim device data into main `init()` method - Update tests Also, `update_rate` was moved from class attr to `mgr_config` key. --- hw_device_mgr/mgr_ros/mgr.py | 51 ++++++++++++------- .../mgr_ros/tests/base_test_class.py | 1 + hw_device_mgr/mgr_ros/tests/test_mgr.py | 6 +-- 3 files changed, 35 insertions(+), 23 deletions(-) diff --git a/hw_device_mgr/mgr_ros/mgr.py b/hw_device_mgr/mgr_ros/mgr.py index 6afbb47f..9f788674 100644 --- a/hw_device_mgr/mgr_ros/mgr.py +++ b/hw_device_mgr/mgr_ros/mgr.py @@ -12,12 +12,19 @@ def get_param(self, name, default=None): param = self.ros_node.declare_parameter(name, value=default) return param.value - def init(self, args, **kwargs): + def init(self, /, argv, **kwargs): """ Initialize manager instance. - Init ROS node, shutdown callback and rate object, and read - manager config from ROS params, in addition to base class init + Init ROS node, shutdown callback and rate object, and read manager + config and device config YAML path from ROS params, in addition to base + class init. + + The ROS param `device_config_path` must be a `str` containing the path + to the device configuration YAML file. + + If the ROS param `sim_device_data_path` is non-empty, it must be a `str` + containing the path to a YAML file with the sim device configuration. """ # - Init ROS node node_kwargs = dict( @@ -25,34 +32,39 @@ def init(self, args, **kwargs): automatically_declare_parameters_from_overrides=True, ) - rclpy.init(args=args) + rclpy.init(args=argv) self.ros_node = rclpy.create_node(self.name, **node_kwargs) self.ros_context = rclpy.utilities.get_default_context() self.logger.info(f"Initializing '{self.name}' ROS node") - # - ROS update rate - self.update_rate = self.get_param("update_rate", 10) # - mgr_config + if "mgr_config" in kwargs: + raise TypeError("unexpected 'mgr_config' argument") mgr_config = dict( goal_state_timeout=self.get_param("goal_state_timeout", 2), init_timeout=self.get_param("init_timeout", 5), + update_rate=self.get_param("update_rate", 10), ) - super().init(mgr_config=mgr_config, **kwargs) - self.logger.info(f"Initialized '{self.name}' ROS node") - - def init_devices(self, **kwargs): + # - device_config + if "device_config" in kwargs: + raise TypeError("unexpected 'device_config' argument") 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}'") 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" - 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) + # - sim device data + if "sim_device_data" in kwargs: + raise TypeError("unexpected 'sim_device_data' argument") + sim_device_data_path = self.get_param("sim_device_data_path", None) + if sim_device_data_path is not None: + sim_device_data = self.load_yaml_path(sim_device_data_path) + assert sim_device_data, f"Empty YAML file '{sim_device_data_path}'" + kwargs["sim_device_data"] = sim_device_data + # + super().init( + mgr_config=mgr_config, device_config=device_config, **kwargs + ) + self.logger.info(f"Initialized '{self.name}' ROS node") def read_update_write(self): """ @@ -89,7 +101,8 @@ def run(self): Let `rclpy.node.Node` spinner manage looping and exit """ - self.ros_node.create_timer(1 / self.update_rate, self.read_update_write) + update_period = 1.0 / self.mgr_config.get("update_rate", 10.0) + self.ros_node.create_timer(update_period, self.read_update_write) try: rclpy.spin(self.ros_node) except KeyboardInterrupt: 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 e6f1740b..66fbae59 100644 --- a/hw_device_mgr/mgr_ros/tests/base_test_class.py +++ b/hw_device_mgr/mgr_ros/tests/base_test_class.py @@ -45,6 +45,7 @@ def manager_ros_params(self, mock_rclpy, mgr_config): hdm_params.update(mgr_config) hdm_params.pop("devices") self.rosparams.update(hdm_params) + print(f"manager_ros_params fixture: rosparams = {self.rosparams}") @pytest.fixture def sim_device_data_path(self, tmp_path, mock_rclpy): diff --git a/hw_device_mgr/mgr_ros/tests/test_mgr.py b/hw_device_mgr/mgr_ros/tests/test_mgr.py index 9fe1bea4..a27165c5 100644 --- a/hw_device_mgr/mgr_ros/tests/test_mgr.py +++ b/hw_device_mgr/mgr_ros/tests/test_mgr.py @@ -22,14 +22,12 @@ def obj(self, device_cls): # init_sim() and init_devices() signatures changed, so can't # use parent test class obj fixture self.obj = device_cls() - self.obj.init(list()) - self.obj.init_sim_from_rosparams() - self.obj.init_devices() + self.obj.init(argv=list()) yield self.obj def test_ros_params(self, obj): print(f"self.rosparams:\n{pformat(self.rosparams)}") - assert obj.update_rate == 20 # Defaults to 10 + assert obj.mgr_config.get("update_rate", None) == 20 # Defaults to 10 assert hasattr(obj, "mgr_config") assert "init_timeout" in obj.mgr_config assert hasattr(obj, "device_config") From e41d2258b67dc5ba4e1e6fef7c98e077c7c87893 Mon Sep 17 00:00:00 2001 From: John Morris Date: Mon, 11 Jul 2022 17:18:16 -0500 Subject: [PATCH 86/88] mgr_ros_hal: Updates for recent changes in init() Update entrypoint with simplified `init() call. --- hw_device_mgr/mgr_ros_hal/__main__.py | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/hw_device_mgr/mgr_ros_hal/__main__.py b/hw_device_mgr/mgr_ros_hal/__main__.py index 7a5683f1..89cad93c 100644 --- a/hw_device_mgr/mgr_ros_hal/__main__.py +++ b/hw_device_mgr/mgr_ros_hal/__main__.py @@ -4,12 +4,8 @@ def main(args=None): argv = list(sys.argv[1:]) # Knock off executable name - sim = "--sim" in argv mgr = ROSHALHWDeviceMgr() - mgr.init(argv) - if sim: - mgr.init_sim_from_rosparams() - mgr.init_devices() + mgr.init(argv=argv) mgr.run() From 87305664ae459d5d63f84b66449b1d4ceff28eae Mon Sep 17 00:00:00 2001 From: John Morris Date: Mon, 19 Sep 2022 21:20:06 -0500 Subject: [PATCH 87/88] config_io: Add `resource_path()` methodc --- hw_device_mgr/config_io.py | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/hw_device_mgr/config_io.py b/hw_device_mgr/config_io.py index 03c20b13..7fd3d938 100644 --- a/hw_device_mgr/config_io.py +++ b/hw_device_mgr/config_io.py @@ -1,6 +1,6 @@ import ruamel.yaml from pathlib import Path -from importlib.resources import open_binary +from importlib.resources import open_binary, path class ConfigIO: @@ -15,6 +15,12 @@ def open_resource(cls, package, resource): """Return open file object for importlib package resource.""" return open_binary(package, resource) + @classmethod + def resource_path(cls, package, resource): + """Return resource path as a `str` (may not actually exist).""" + with path(package, resource) as p: + return str(p) + @classmethod def load_yaml_path(cls, path): """Read and return `data` from YAML formatted file `path`.""" From 144f73956ee35cb40b6a7f8e97b4624c29e32a11 Mon Sep 17 00:00:00 2001 From: John Morris Date: Mon, 19 Sep 2022 21:20:34 -0500 Subject: [PATCH 88/88] base class: Print YAML file path in tests --- hw_device_mgr/tests/test_device.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/hw_device_mgr/tests/test_device.py b/hw_device_mgr/tests/test_device.py index 9b48cea9..0b27703f 100644 --- a/hw_device_mgr/tests/test_device.py +++ b/hw_device_mgr/tests/test_device.py @@ -342,9 +342,10 @@ def test_read_update_write(self, obj): if self.read_update_write_package is None: return # No test cases defined for this class rsrc = (self.read_update_write_package, self.read_update_write_yaml) + rsrc_str = self.resource_path(*rsrc) 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}") + assert test_cases, f"Empty YAML from package resource {rsrc_str}" + print(f"Read test cases from package resource {rsrc_str}") for test_case in test_cases: self.read_update_write_loop(test_case)