diff --git a/pus_tc/devs/reaction_wheels.py b/pus_tc/devs/reaction_wheels.py index 970f964..f127830 100644 --- a/pus_tc/devs/reaction_wheels.py +++ b/pus_tc/devs/reaction_wheels.py @@ -196,20 +196,36 @@ def pack_rw_ass_cmds(tc_queue: TcQueueT, object_id: bytes, op_code: str): def pack_set_speed_command( - object_id: bytes, speed: int, ramp_time: int, ssc: int + object_id: bytes, speed: int, ramp_time_ms: int, ssc: int ) -> PusTelecommand: """With this function a command is packed to set the speed of a reaction wheel :param object_id: The object id of the reaction wheel handler. :param speed: Valid speeds are [-65000, -1000] and [1000, 65000]. Values are specified in 0.1 * RPM - :param ramp_time: The time after which the reaction wheel will reach the commanded speed. + :param ramp_time_ms: The time after which the reaction wheel will reach the commanded speed. Valid times are 10 - 10000 ms :param ssc: Source sequence count """ + if speed > 0: + if speed < 1000 or speed > 65000: + raise ValueError( + "Invalid RW speed specified. " + "Allowed range is [1000, 65000] 0.1 * RPM" + ) + else: + if speed < -65000 or speed > -1000: + raise ValueError( + "Invalid RW speed specified. " + "Allowed range is [-65000, -1000] 0.1 * RPM" + ) + if ramp_time_ms < 0 or ( + ramp_time_ms > 0 and (ramp_time_ms > 10000 or ramp_time_ms < 10) + ): + raise ValueError("Invalid Ramp Speed time. Allowed range is [10-10000] ms") command_id = RwCommandIds.SET_SPEED command = bytearray() command += object_id + command_id command = command + struct.pack("!i", speed) - command = command + ramp_time.to_bytes(length=2, byteorder="big") + command = command + ramp_time_ms.to_bytes(length=2, byteorder="big") command = PusTelecommand(service=8, subservice=128, ssc=ssc, app_data=command) return command diff --git a/pus_tc/system/proc.py b/pus_tc/system/proc.py index 1790416..79e1e24 100644 --- a/pus_tc/system/proc.py +++ b/pus_tc/system/proc.py @@ -1,3 +1,5 @@ +from typing import List + from config.definitions import CustomServiceList from tmtccmd.config import ( QueueCommands, @@ -8,16 +10,6 @@ from tmtccmd.config import ( from tmtccmd.tc.definitions import TcQueueT from tmtccmd.tc.pus_3_fsfw_hk import * -""" -from config.object_ids import ( - BPX_HANDLER_ID, - P60_DOCK_HANDLER, - PDU_1_HANDLER_ID, - PDU_2_HANDLER_ID, - ACU_HANDLER_ID, - CORE_CONTROLLER_ID, -) -""" import config.object_ids as oids from pus_tc.devs.bpx_batt import BpxSetIds from pus_tc.system.core import SetIds as CoreSetIds @@ -146,14 +138,11 @@ def pack_generic_hk_listening_cmds( ) elif one_rw is True: - activate_one_rw_in_sequence( - tc_queue=tc_queue, + activate_all_rws_in_sequence( + tc_queue=tc_queue, test_speed=20000, test_ramp_time=10000, init_ssc=0 ) - elif two_rws is True: - activate_two_rws_in_sequence( - tc_queue=tc_queue, - ) + activate_all_rws_two_consecutively(tc_queue=tc_queue, init_ssc=0) else: pass @@ -221,7 +210,7 @@ def pack_proc_commands(tc_queue: TcQueueT, op_code: str): if op_code in OpCodes.RAD_SEN_FT: key = KAI.RAD_SEN_FT[0] - sid_list.append(make_sid(oids.RAD_SENSOR_ID, RadSetIds.RAD_SEN_CORE)) + sid_list.append(make_sid(oids.RAD_SENSOR_ID, RadSetIds.HK)) pack_generic_hk_listening_cmds( tc_queue=tc_queue, proc_key=key, @@ -652,128 +641,89 @@ def activate_mgts_alternately( tc_queue.appendleft((QueueCommands.WAIT, 40.0)) -def activate_one_rw_in_sequence( - tc_queue: TcQueueT, -): +def rw_speed_cmd_single( + tc_queue: TcQueueT, oid: bytes, init_ssc: int, speed: int, ramp_time: int +) -> int: + command = pack_set_speed_command( + object_id=oid, speed=speed, ramp_time_ms=ramp_time, ssc=init_ssc + ) + init_ssc += 1 + tc_queue.appendleft(command.pack_command_tuple()) + tc_queue.appendleft((QueueCommands.WAIT, 70.0)) + command = pack_set_speed_command( + object_id=oids.RW1_ID, speed=0, ramp_time_ms=ramp_time, ssc=init_ssc + ) + tc_queue.appendleft(command.pack_command_tuple()) + return init_ssc + 1 + + +def rw_speed_up_cmd_consec( + tc_queue: TcQueueT, obids: List[bytes], init_ssc: int, speed: int, ramp_time: int +) -> int: + for oid in obids: + command = pack_set_speed_command( + object_id=oid, speed=speed, ramp_time_ms=ramp_time, ssc=init_ssc + ) + tc_queue.appendleft(command.pack_command_tuple()) + init_ssc += 1 + return init_ssc + + +def rw_speed_down_cmd_consec( + tc_queue: TcQueueT, obids: List[bytes], init_ssc: int, ramp_time: int +) -> int: + for oid in obids: + command = pack_set_speed_command( + object_id=oid, speed=0, ramp_time_ms=ramp_time, ssc=init_ssc + ) + tc_queue.appendleft(command.pack_command_tuple()) + init_ssc += 1 + return init_ssc + + +def activate_all_rws_in_sequence( + tc_queue: TcQueueT, init_ssc: int, test_speed: int, test_ramp_time: int +) -> int: + new_ssc = init_ssc # RW1 speed cmd tc_queue.appendleft((QueueCommands.WAIT, 5.0)) - command = pack_set_speed_command( - object_id=oids.RW1_ID, - speed=20000, - ramp_time=10000, + new_ssc = rw_speed_cmd_single( + tc_queue, oids.RW1_ID, new_ssc, test_speed, test_ramp_time ) - tc_queue.appendleft(command.pack_command_tuple()) - tc_queue.appendleft((QueueCommands.WAIT, 70.0)) - command = pack_set_speed_command( - object_id=oids.RW1_ID, - speed=0, - ramp_time=10000, - ) - tc_queue.appendleft(command.pack_command_tuple()) tc_queue.appendleft((QueueCommands.WAIT, 10.0)) - - # RW2 speed cmd - command = pack_set_speed_command( - object_id=oids.RW2_ID, - speed=20000, - ramp_time=10000, + new_ssc = rw_speed_cmd_single( + tc_queue, oids.RW2_ID, new_ssc, test_speed, test_ramp_time ) - tc_queue.appendleft(command.pack_command_tuple()) - tc_queue.appendleft((QueueCommands.WAIT, 70.0)) - command = pack_set_speed_command( - object_id=oids.RW2_ID, - speed=0, - ramp_time=10000, - ) - tc_queue.appendleft(command.pack_command_tuple()) tc_queue.appendleft((QueueCommands.WAIT, 10.0)) - - # RW3 speed cmd - command = pack_set_speed_command( - object_id=oids.RW3_ID, - speed=20000, - ramp_time=10000, + new_ssc = rw_speed_cmd_single( + tc_queue, oids.RW3_ID, new_ssc, test_speed, test_ramp_time ) - tc_queue.appendleft(command.pack_command_tuple()) - tc_queue.appendleft((QueueCommands.WAIT, 70.0)) - command = pack_set_speed_command( - object_id=oids.RW3_ID, - speed=0, - ramp_time=10000, - ) - tc_queue.appendleft(command.pack_command_tuple()) tc_queue.appendleft((QueueCommands.WAIT, 10.0)) - - # RW4 speed cmd - command = pack_set_speed_command( - object_id=oids.RW4_ID, - speed=20000, - ramp_time=10000, + new_ssc = rw_speed_cmd_single( + tc_queue, oids.RW4_ID, new_ssc, test_speed, test_ramp_time ) - tc_queue.appendleft(command.pack_command_tuple()) - tc_queue.appendleft((QueueCommands.WAIT, 70.0)) - command = pack_set_speed_command( - object_id=oids.RW4_ID, - speed=0, - ramp_time=10000, - ) - tc_queue.appendleft(command.pack_command_tuple()) tc_queue.appendleft((QueueCommands.WAIT, 5.0)) + return new_ssc -def activate_two_rws_in_sequence(tc_queue: TcQueueT): +def activate_all_rws_two_consecutively(tc_queue: TcQueueT, init_ssc: int) -> int: + new_ssc = init_ssc # RW1+3 speed cmd tc_queue.appendleft((QueueCommands.WAIT, 5.0)) - command = pack_set_speed_command( - object_id=oids.RW1_ID, - speed=-20000, - ramp_time=10000, + new_ssc = rw_speed_up_cmd_consec( + tc_queue, [oids.RW1_ID, oids.RW3_ID], new_ssc, -20000, 10000 ) - tc_queue.appendleft(command.pack_command_tuple()) - command = pack_set_speed_command( - object_id=oids.RW3_ID, - speed=-20000, - ramp_time=10000, - ) - tc_queue.appendleft(command.pack_command_tuple()) - tc_queue.appendleft((QueueCommands.WAIT, 70.0)) - command = pack_set_speed_command( - object_id=oids.RW1_ID, - speed=0, - ramp_time=10000, - ) - tc_queue.appendleft(command.pack_command_tuple()) - command = pack_set_speed_command( - object_id=oids.RW3_ID, - speed=0, - ramp_time=10000, - ) - tc_queue.appendleft(command.pack_command_tuple()) tc_queue.appendleft((QueueCommands.WAIT, 10.0)) # RW2+4 speed cmd - command = pack_set_speed_command( - object_id=oids.RW2_ID, - speed=-20000, - ramp_time=10000, + new_ssc = rw_speed_up_cmd_consec( + tc_queue, [oids.RW2_ID, oids.RW4_ID], new_ssc, -20000, 10000 ) - tc_queue.appendleft(command.pack_command_tuple()) - command = pack_set_speed_command( - object_id=oids.RW4_ID, - speed=-20000, - ramp_time=10000, - ) - tc_queue.appendleft(command.pack_command_tuple()) tc_queue.appendleft((QueueCommands.WAIT, 70.0)) - command = pack_set_speed_command( - object_id=oids.RW2_ID, - speed=0, - ramp_time=10000, + new_ssc = rw_speed_down_cmd_consec( + tc_queue, [oids.RW1_ID, oids.RW3_ID], new_ssc, 10000 ) - tc_queue.appendleft(command.pack_command_tuple()) - command = pack_set_speed_command( - object_id=oids.RW4_ID, - speed=0, - ramp_time=10000, - ) - tc_queue.appendleft(command.pack_command_tuple()) tc_queue.appendleft((QueueCommands.WAIT, 5.0)) + new_ssc = rw_speed_down_cmd_consec( + tc_queue, [oids.RW2_ID, oids.RW4_ID], new_ssc, 10000 + ) + return new_ssc