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)