diff --git a/pus_tc/devs/imtq.py b/pus_tc/devs/imtq.py index c78127a..7d376ab 100644 --- a/pus_tc/devs/imtq.py +++ b/pus_tc/devs/imtq.py @@ -136,10 +136,16 @@ def pack_imtq_test_into(object_id: ObjectIdU32, q: DefaultPusQueueHelper, op_cod if op_code == "9": q.add_log_cmd("IMTQ: Commanding dipole") - x_dipole = 0 - y_dipole = 0 - z_dipole = 0 - duration = 0 # ms + + x_dipole = int(input("Specify X dipole [range [0, 2000] * 10^-4 * Am^2]: ")) + y_dipole = int(input("Specify Y dipole [range [0, 2000] * 10^-4 * Am^2]: ")) + z_dipole = int(input("Specify Z dipole [range [0, 2000] * 10^-4 * Am^2]: ")) + duration = int( + input( + f"Specify torque duration [range [0, {pow(2, 16) - 1}, " + f"0 for continuous generation until update]: " + ) + ) q.add_pus_tc( pack_dipole_command( object_id.as_bytes, x_dipole, y_dipole, z_dipole, duration @@ -184,19 +190,39 @@ def pack_dipole_command( object_id: bytes, x_dipole: int, y_dipole: int, z_dipole: int, duration: int ) -> PusTelecommand: """This function packs the command causing the ISIS IMTQ to generate a dipole. - @param object_id The object id of the IMTQ handler. - @param x_dipole The dipole of the x coil in 10^-4*Am^2 (max. 2000) - @param y_dipole The dipole of the y coil in 10^-4*Am^2 (max. 2000) - @param z_dipole The dipole of the z coil in 10^-4*Am^2 (max. 2000) - @param duration The duration in milliseconds the dipole will be generated by the coils. + :param object_id: The object id of the IMTQ handler. + :param x_dipole: The dipole of the x coil in 10^-4*Am^2 (max. 2000) + :param y_dipole: The dipole of the y coil in 10^-4*Am^2 (max. 2000) + :param z_dipole: The dipole of the z coil in 10^-4*Am^2 (max. 2000) + :param duration: The duration in milliseconds the dipole will be generated by the coils. When set to 0, the dipole will be generated until a new dipole actuation command is sent. """ action_id = ImtqActionIds.start_actuation_dipole command = object_id + action_id + x_dipole = int(round(x_dipole)) + y_dipole = int(round(y_dipole)) + z_dipole = int(round(z_dipole)) + if x_dipole < -2000 or x_dipole > 2000: + raise_dipole_error("X dipole", x_dipole) + if y_dipole < -2000 or y_dipole > 2000: + raise_dipole_error("Y dipole", y_dipole) + if z_dipole < -2000 or z_dipole > 2000: + raise_dipole_error("Z dipole", z_dipole) + duration = int(round(duration)) + if duration < 0 or duration > pow(2, 16) - 1: + raise ValueError( + f"Duration in ms of {duration} smaller than 0 or larger than allowed {pow(2, 16) - 1}" + ) command += struct.pack("!h", x_dipole) command += struct.pack("!h", y_dipole) command += struct.pack("!h", z_dipole) - command += struct.pack("!h", duration) + command += struct.pack("!H", duration) command = PusTelecommand(service=8, subservice=128, app_data=command) return command + + +def raise_dipole_error(dipole_str: str, value: int): + raise ValueError( + f"{dipole_str} {value} negative or larger than maximum allowed 2000 * 10^-4*Am^2" + )