def __init__(self, positioner_ids, alpha=None, beta=None, **kwargs): if alpha is not None and beta is not None: data = int_to_bytes(int(alpha)) + int_to_bytes(int(beta)) kwargs["data"] = data super().__init__(positioner_ids, **kwargs)
async def process_goto(self, message): """Process an absolute or relative goto command.""" __, command_id, uid, __ = utils.parse_identifier( message.arbitration_id) command_id = CommandID(command_id) command = command_id.get_command_class() data = message.data alpha_move, beta_move = command.decode(data) # type: ignore if command_id == CommandID.GO_TO_RELATIVE_POSITION: alpha_move += self.position[0] beta_move += self.position[0] target_alpha = self.position[0] + alpha_move target_beta = self.position[1] + beta_move if (target_alpha < 0 or target_beta < 0 or target_alpha > 360 or target_beta > 360): self.reply(command_id, uid, ResponseCode.VALUE_OUT_OF_RANGE) return if alpha_move == 0.0: alpha_move_time = 0.0 else: alpha_move_time = int( utils.get_goto_move_time(alpha_move, self.speed[0]) / TIME_STEP) if beta_move == 0.0: beta_move_time = 0.0 else: beta_move_time = int( utils.get_goto_move_time(beta_move, self.speed[1]) / TIME_STEP) self.reply( command_id, uid, ResponseCode.COMMAND_ACCEPTED, data=[ utils.int_to_bytes(alpha_move_time, "i4") + utils.int_to_bytes(beta_move_time, "i4") ], ) self.status ^= (PositionerStatus.DISPLACEMENT_COMPLETED | PositionerStatus.DISPLACEMENT_COMPLETED_ALPHA | PositionerStatus.DISPLACEMENT_COMPLETED_BETA) self.status |= (PositionerStatus.TRAJECTORY_ALPHA_RECEIVED | PositionerStatus.TRAJECTORY_BETA_RECEIVED) await asyncio.sleep( max(alpha_move * TIME_STEP, beta_move_time * TIME_STEP)) self.position = (target_alpha, target_beta) self.status |= (PositionerStatus.DISPLACEMENT_COMPLETED | PositionerStatus.DISPLACEMENT_COMPLETED_ALPHA | PositionerStatus.DISPLACEMENT_COMPLETED_BETA)
def gotoPosition(posID, alphaDeg, betaDeg): alpha_steps, beta_steps = motor_steps_to_angle(alphaDeg, betaDeg, inverse=True) data = int_to_bytes(alpha_steps) + int_to_bytes(beta_steps) sendMsg(GO_TO_ABSOLUTE_POSITION, posID, data) arb, data = getReply() betaTime = bytes_to_int(data[4:]) alphaTime = bytes_to_int(data[0:4]) assert arb[0] == posID print("times to move", betaTime * 0.0005, alphaTime * 0.0005)
def get_data(n_alpha, n_beta): """Returns the data bytearray from a pair for ``(n_alpha, n_beta)``.""" alpha_positions = int(n_alpha) beta_positions = int(n_beta) assert alpha_positions > 0 and beta_positions > 0 data = int_to_bytes(alpha_positions) + int_to_bytes(beta_positions) return data
def encode(alpha, beta): """Returns the position as a bytearray in positioner units.""" alpha_motor, beta_motor = motor_steps_to_angle(alpha, beta, inverse=True) alpha_bytes = int_to_bytes(int(alpha_motor), "i4") beta_bytes = int_to_bytes(int(beta_motor), "i4") data = alpha_bytes + beta_bytes return data
def __init__( self, positioner_ids: int | List[int], alpha: float | None = None, beta: float | None = None, **kwargs, ): if alpha is not None and beta is not None: assert alpha >= 0 and beta >= 0, "invalid current." data = int_to_bytes(int(alpha)) + int_to_bytes(int(beta)) kwargs["data"] = data super().__init__(positioner_ids, **kwargs)
def __init__( self, positioner_ids: int | List[int], alpha=None, beta=None, **kwargs, ): if alpha is not None and beta is not None: alpha_steps, beta_steps = motor_steps_to_angle(alpha, beta, inverse=True) data = int_to_bytes(int(alpha_steps)) + int_to_bytes(int(beta_steps)) kwargs["data"] = data super().__init__(positioner_ids, **kwargs)
def calculate_positions(positions): """Converts angle-time posions to bytes data.""" positions = numpy.array(positions).astype(numpy.float64) positions[:, 0] = positions[:, 0] / 360.0 * MOTOR_STEPS positions[:, 1] /= TIME_STEP positions = positions.astype(numpy.int32) data = [] for angle, tt in positions: data.append( int_to_bytes(angle, dtype="i4") + int_to_bytes(tt, dtype="i4")) return data
def __init__( self, positioner_ids: int | List[int], moves: int | Dict[int, int], **kwargs, ): if isinstance(moves, int): data = int_to_bytes(moves) elif isinstance(moves, dict): data = {pos: int_to_bytes(moves[pos]) for pos in moves} else: raise TypeError("Invalid data type.") kwargs["data"] = data super().__init__(positioner_ids, **kwargs)
def encode(firmware): """Returns the bytearray encoding the firmware version.""" chunks = firmware.split(".")[::-1] data = b"" for chunk in chunks: data += int_to_bytes(int(chunk), "u1") return data
def __init__( self, positioner_ids: int | List[int], alpha: float | None = None, beta: float | None = None, **kwargs, ): if alpha is not None and beta is not None: alpha_steps, beta_steps = motor_steps_to_angle(alpha, beta, inverse=True) alpha_bytes = int_to_bytes(alpha_steps, dtype="i4") beta_bytes = int_to_bytes(beta_steps, dtype="i4") data = alpha_bytes + beta_bytes kwargs["data"] = data super().__init__(positioner_ids, **kwargs)
async def load_firmware( fps: FPS, firmware_file: str | pathlib.Path, positioners: Optional[List[int]] = None, messages_per_positioner: Optional[int] = None, force: bool = False, show_progressbar: bool = False, progress_callback: Optional[Callable[[int, int], Any]] = None, stop_logging: bool = True, ): """Convenience function to run through the steps of loading a new firmware. This function is a coroutine and not intendend for direct use. Use the ``jaeger`` CLI instead. Parameters ---------- fps `~jaeger.fps.FPS` instance to which the commands will be sent. firmware_file Binary file containing the firmware to load. positioners A list of positioner ids whose firmware to update, or `None` to update all the positioners in ``fps``. messages_per_positioner How many messages to send to each positioner at once. This can improve the performance but also overflow the CAN bus buffer. With the default value of `None`, reverts to the configuration value ``positioner.firmware_messages_per_positioner``. force Forces the firmware load to continue even if some positioners are not responding or are not in bootloader mode. show_progressbar Whether to show a progress bar. progress_callback A function to call as data gets transferred to the positioners. The callback is called with ``(current_chunk, n_chuck)`` where ``current_chunk`` is the number of the data chunk being sent and ``n_chunk`` is the total number of chunks in the data package. stop_logging Disable logging to file for the CAN logger to improve performance. """ if show_progressbar: try: import progressbar except ImportError: warnings.warn( "progressbar2 is not installed. Cannot show a progress bar.", JaegerUserWarning, ) progressbar = None show_progressbar = False else: progressbar = None start_time = time.time() firmware_file = pathlib.Path(firmware_file) assert firmware_file.exists(), "cannot find firmware file" log.info(f"firmware file {firmware_file!s} found.") # Open firmware data as binary. firmware_data = open(firmware_file, "rb") crc32 = zlib.crc32(firmware_data.read()) filesize = os.path.getsize(firmware_file) # Check to make sure all positioners are in bootloader mode. valid_positioners = [] n_bad = 0 for positioner_id in fps.positioners: if positioners is not None and positioner_id not in positioners: continue positioner = fps.positioners[positioner_id] if (not positioner.is_bootloader() or BootloaderStatus.BOOTLOADER_INIT not in positioner.status or BootloaderStatus.UNKNOWN in positioner.status): n_bad += 1 continue valid_positioners.append(positioner) if len(valid_positioners) == 0: raise JaegerError( "no positioners found in bootloader mode or with valid status.") if n_bad > 0: msg = f"{n_bad} positioners not in bootloader mode or state is invalid." if force: warnings.warn(msg + " Proceeding becasuse force=True.", JaegerUserWarning) else: raise JaegerError(msg) log.info("stopping pollers") await fps.pollers.stop() log.info(f"upgrading firmware on {len(valid_positioners)} positioners.") start_firmware_payload = int_to_bytes(filesize) + int_to_bytes(crc32) log.info(f"CRC32: {crc32}") log.info(f"File size: {filesize} bytes") pids = [pos.positioner_id for pos in valid_positioners] cmd = await fps.send_command( CommandID.START_FIRMWARE_UPGRADE, positioner_ids=pids, data=[start_firmware_payload], ) if cmd.status.failed or cmd.status.timed_out: log.error("firmware upgrade failed.") return False # Restore pointer to start of file firmware_data.seek(0) log.info("starting data send.") if stop_logging and can_log.fh: fh_handler = can_log.handlers.pop(can_log.handlers.index(can_log.fh)) else: fh_handler = None chunk_size = 8 n_chunks = int(numpy.ceil(filesize / chunk_size)) with contextlib.ExitStack() as stack: if show_progressbar and progressbar: bar = stack.enter_context( progressbar.ProgressBar(max_value=n_chunks)) else: bar = None messages_default = config["positioner"][ "firmware_messages_per_positioner"] messages_per_positioner = messages_per_positioner or messages_default assert isinstance(messages_per_positioner, int) ii = 0 while True: cmds = [] stop = False for __ in range(messages_per_positioner): chunk = firmware_data.read(chunk_size) packetdata = bytearray(chunk) # packetdata.reverse() # IMPORTANT! no longer needed for P1 if len(packetdata) == 0: stop = True break cmds.append( fps.send_command( CommandID.SEND_FIRMWARE_DATA, positioner_ids=pids, data=[packetdata], timeout=15, )) await asyncio.gather(*cmds) if any(cmd.status.failed or cmd.status.timed_out for cmd in cmds): log.error("firmware upgrade failed.") if fh_handler: can_log.addHandler(fh_handler) return False ii += messages_per_positioner if show_progressbar and bar: if ii < n_chunks: bar.update(ii) if progress_callback: progress_callback(ii, n_chunks) if stop: break log.info("firmware upgrade complete.") if fh_handler: can_log.addHandler(fh_handler) total_time = time.time() - start_time log.info(f"upgrading firmware took {total_time:.2f} s.") return True
def encode(alpha, beta): """Encodes the alpha and beta speed as bytes.""" data_speed = int_to_bytes(int(alpha)) + int_to_bytes(int(beta)) return data_speed
async def process_message(self, msg, positioner_id, command_id, uid): """Processes incoming commands from the bus.""" command_id = CommandID(command_id) command = command_id.get_command_class() if positioner_id == 0 and not command.broadcastable: self.reply( command_id, uid, response_code=ResponseCode.INVALID_BROADCAST_COMMAND, ) return if command_id == CommandID.GET_ID: self.reply(command_id, uid) elif command_id == CommandID.GET_FIRMWARE_VERSION: data_firmware = command.encode(self.firmware) # type: ignore self.reply(command_id, uid, data=data_firmware) elif command_id == CommandID.GET_STATUS: data_status = utils.int_to_bytes(self.status) self.reply(command_id, uid, data=data_status) elif command_id == CommandID.GET_NUMBER_TRAJECTORIES: data_status = utils.int_to_bytes(self.number_trajectories) self.reply(command_id, uid, data=data_status) elif command_id in [ CommandID.GO_TO_ABSOLUTE_POSITION, CommandID.GO_TO_RELATIVE_POSITION, ]: asyncio.create_task(self.process_goto(msg)) elif command_id == CommandID.GET_ACTUAL_POSITION: data_position = command.encode(*self.position) # type: ignore self.reply(command_id, uid, data=data_position) elif command_id == CommandID.SET_SPEED: data_speed = command.encode(*self.speed) # type: ignore self.reply(command_id, uid, data=data_speed) elif command_id == CommandID.START_FIRMWARE_UPGRADE: if not self.is_bootloader(): self.reply( command_id, uid, response_code=ResponseCode.INVALID_COMMAND, ) return try: data = msg.data firmware_size = utils.bytes_to_int(data[0:4], "u4") crc32 = utils.bytes_to_int(data[4:9], "u4") except Exception: self.reply( command_id, uid, response_code=ResponseCode.INVALID_COMMAND, ) return self._firmware_size = firmware_size self._crc32 = crc32 self._firmware_received = b"" self.reply(command_id, uid) elif command_id == CommandID.SEND_FIRMWARE_DATA: asyncio.create_task(self.process_firmware_data(uid, msg.data)) else: # Should be a valid command or CommandID(command_id) would # have failed. Just return OK. self.reply(command_id, uid)
def __init__(self, positioner_ids, margin: int, **kwargs): data = int_to_bytes(int(margin), dtype="i4") kwargs["data"] = data super().__init__(positioner_ids, **kwargs)