eive-tmtc/pus_tc/devs/reaction_wheels.py

230 lines
7.5 KiB
Python
Raw Normal View History

2021-06-25 12:07:16 +02:00
# -*- coding: utf-8 -*-
2022-05-05 01:21:57 +02:00
"""reaction_wheels.py
@brief Tests for the reaction wheel handler
2021-06-25 12:07:16 +02:00
@author J. Meier
@date 20.06.2021
2021-06-25 12:07:16 +02:00
"""
2021-06-28 14:08:04 +02:00
import struct
2022-07-04 15:22:53 +02:00
from tmtccmd.tc import QueueHelper
2022-05-10 18:34:15 +02:00
from tmtccmd.tc.pus_3_fsfw_hk import (
generate_one_hk_command,
generate_one_diag_command,
make_sid,
)
2021-10-01 10:55:56 +02:00
from spacepackets.ecss.tc import PusTelecommand
2022-05-05 16:23:17 +02:00
from tmtccmd.tc.pus_200_fsfw_modes import pack_mode_data, Modes, Subservices
2022-05-05 01:21:57 +02:00
from config.definitions import CustomServiceList
class OpCodesDevs:
SPEED = ["0", "speed"]
ON = ["1", "on"]
NML = ["2", "nml"]
OFF = ["3", "off"]
2022-05-10 18:34:15 +02:00
GET_STATUS = ["4", "status"]
GET_TM = ["5", "tm"]
2022-05-05 01:21:57 +02:00
class InfoDevs:
SPEED = "Set speed"
ON = "Set On"
NML = "Set Normal"
OFF = "Set Off"
2022-05-10 18:34:15 +02:00
GET_STATUS = "Get Status HK"
2022-05-05 01:21:57 +02:00
GET_TM = "Get TM HK"
class OpCodesAss:
ON = ["0", "on"]
NML = ["1", "nml"]
OFF = ["2", "off"]
class InfoAss:
ON = "Mode On: 3/4 RWs min. on"
NML = "Mode Normal: 3/4 RWs min. normal"
OFF = "Mode Off: All RWs off"
2021-06-25 12:07:16 +02:00
class RwSetIds:
STATUS_SET_ID = 4
TEMPERATURE_SET_ID = 8
2022-05-05 01:21:57 +02:00
LAST_RESET = 2
TM_SET = 9
2021-06-25 12:07:16 +02:00
class RwCommandIds:
RESET_MCU = bytearray([0x0, 0x0, 0x0, 0x01])
# Reads status information from reaction wheel into dataset with id 4
GET_RW_STATUS = bytearray([0x0, 0x0, 0x0, 0x04])
2021-06-28 14:08:04 +02:00
INIT_RW_CONTROLLER = bytearray([0x0, 0x0, 0x0, 0x05])
2021-06-25 12:07:16 +02:00
SET_SPEED = bytearray([0x0, 0x0, 0x0, 0x06])
# Reads temperature from reaction wheel into dataset with id 8
GET_TEMPERATURE = bytearray([0x0, 0x0, 0x0, 0x08])
2021-06-30 15:08:43 +02:00
GET_TM = bytearray([0x0, 0x0, 0x0, 0x09])
2021-06-25 12:07:16 +02:00
class SpeedDefinitions:
RPM_100 = 1000
RPM_5000 = 5000
class RampTime:
2021-06-25 15:25:22 +02:00
MS_1000 = 1000
2021-06-25 12:07:16 +02:00
2022-05-05 01:21:57 +02:00
def add_rw_cmds(cmd_dict: ServiceOpCodeDictT):
op_code_dict = dict()
add_op_code_entry(
op_code_dict=op_code_dict, info=InfoDevs.SPEED, keys=OpCodesDevs.SPEED
)
add_op_code_entry(op_code_dict=op_code_dict, info=InfoDevs.ON, keys=OpCodesDevs.ON)
add_op_code_entry(
op_code_dict=op_code_dict, info=InfoDevs.OFF, keys=OpCodesDevs.OFF
)
add_op_code_entry(
op_code_dict=op_code_dict, info=InfoDevs.NML, keys=OpCodesDevs.NML
)
2022-05-10 18:34:15 +02:00
add_op_code_entry(
op_code_dict=op_code_dict, info=InfoDevs.GET_STATUS, keys=OpCodesDevs.GET_STATUS
)
2022-05-05 01:21:57 +02:00
add_op_code_entry(
op_code_dict=op_code_dict, info=InfoDevs.GET_TM, keys=OpCodesDevs.GET_TM
)
add_service_op_code_entry(
srv_op_code_dict=cmd_dict,
name=CustomServiceList.REACTION_WHEEL_1.value,
op_code_entry=op_code_dict,
info="Reaction Wheel 1",
)
add_service_op_code_entry(
srv_op_code_dict=cmd_dict,
name=CustomServiceList.REACTION_WHEEL_2.value,
op_code_entry=op_code_dict,
info="Reaction Wheel 2",
)
add_service_op_code_entry(
srv_op_code_dict=cmd_dict,
name=CustomServiceList.REACTION_WHEEL_3.value,
op_code_entry=op_code_dict,
info="Reaction Wheel 3",
)
add_service_op_code_entry(
srv_op_code_dict=cmd_dict,
name=CustomServiceList.REACTION_WHEEL_4.value,
op_code_entry=op_code_dict,
info="Reaction Wheel 4",
)
op_code_dict = dict()
add_op_code_entry(op_code_dict=op_code_dict, info=InfoAss.ON, keys=OpCodesAss.ON)
add_op_code_entry(op_code_dict=op_code_dict, info=InfoAss.NML, keys=OpCodesAss.NML)
add_op_code_entry(op_code_dict=op_code_dict, info=InfoAss.OFF, keys=OpCodesAss.OFF)
add_service_op_code_entry(
srv_op_code_dict=cmd_dict,
name=CustomServiceList.RW_ASSEMBLY.value,
op_code_entry=op_code_dict,
info="Reaction Wheel Assembly",
)
2022-01-18 14:03:56 +01:00
def pack_single_rw_test_into(
2022-07-04 15:22:53 +02:00
object_id: bytes, rw_idx: int, q: QueueHelper, op_code: str
2022-01-18 14:03:56 +01:00
) -> TcQueueT:
2022-05-05 01:21:57 +02:00
if op_code in OpCodesDevs.SPEED:
2021-06-29 16:10:03 +02:00
speed = int(input("Specify speed [0.1 RPM]: "))
ramp_time = int(input("Specify ramp time [ms]: "))
2022-07-04 15:22:53 +02:00
q.add_log_cmd(f"RW {rw_idx}: {InfoDevs.SPEED}")
q.add_pus_tc(pack_set_speed_command(object_id, speed, ramp_time))
2021-06-25 12:07:16 +02:00
2022-05-05 01:21:57 +02:00
if op_code in OpCodesDevs.ON:
2022-07-04 15:22:53 +02:00
q.add_log_cmd(f"RW {rw_idx}: {InfoDevs.ON}")
2022-05-05 01:21:57 +02:00
mode_data = pack_mode_data(object_id, Modes.ON, 0)
2022-07-04 15:22:53 +02:00
q.add_pus_tc(PusTelecommand(service=200, subservice=1, app_data=mode_data))
2021-06-29 16:10:03 +02:00
2022-05-05 01:21:57 +02:00
if op_code in OpCodesDevs.NML:
2022-07-04 15:22:53 +02:00
q.add_log_cmd(f"RW {rw_idx}: {InfoDevs.NML}")
2022-05-05 01:21:57 +02:00
mode_data = pack_mode_data(object_id, Modes.NORMAL, 0)
2022-07-04 15:22:53 +02:00
q.add_pus_tc(PusTelecommand(service=200, subservice=1, app_data=mode_data))
2021-06-28 14:08:04 +02:00
2022-05-05 01:21:57 +02:00
if op_code in OpCodesDevs.OFF:
2022-07-04 15:22:53 +02:00
q.add_log_cmd(f"RW {rw_idx}: {InfoDevs.OFF}")
2022-05-05 01:21:57 +02:00
mode_data = pack_mode_data(object_id, Modes.OFF, 0)
2022-07-04 15:22:53 +02:00
q.add_pus_tc(PusTelecommand(service=200, subservice=1, app_data=mode_data))
2021-06-30 15:08:43 +02:00
2022-05-05 01:21:57 +02:00
if op_code in OpCodesDevs.GET_TM:
2022-07-04 15:22:53 +02:00
q.add_log_cmd(f"RW {rw_idx}: {InfoDevs.GET_TM}")
q.add_pus_tc(
generate_one_hk_command(
sid=make_sid(object_id=object_id, set_id=RwSetIds.TM_SET)
)
2022-01-18 14:03:56 +01:00
)
2022-05-10 18:34:15 +02:00
if op_code in OpCodesDevs.GET_STATUS:
2022-07-04 15:22:53 +02:00
q.add_log_cmd(f"RW {rw_idx}: {InfoDevs.GET_STATUS}")
q.add_pus_tc(
generate_one_diag_command(
sid=make_sid(object_id=object_id, set_id=RwSetIds.STATUS_SET_ID)
)
2022-05-10 18:34:15 +02:00
)
2022-07-04 15:22:53 +02:00
return q
2021-06-25 12:07:16 +02:00
2022-05-05 01:21:57 +02:00
def pack_rw_ass_cmds(tc_queue: TcQueueT, object_id: bytes, op_code: str):
if op_code in OpCodesAss.OFF:
2022-05-05 16:23:17 +02:00
data = pack_mode_data(object_id=object_id, mode=Modes.OFF, submode=0)
2022-05-10 18:34:15 +02:00
cmd = PusTelecommand(
service=200, subservice=Subservices.TC_MODE_COMMAND, app_data=data
)
2022-05-05 16:23:17 +02:00
tc_queue.appendleft(cmd.pack_command_tuple())
2022-05-05 01:21:57 +02:00
if op_code in OpCodesAss.ON:
2022-05-05 16:23:17 +02:00
data = pack_mode_data(object_id=object_id, mode=Modes.ON, submode=0)
2022-05-10 18:34:15 +02:00
cmd = PusTelecommand(
service=200, subservice=Subservices.TC_MODE_COMMAND, app_data=data
)
2022-05-05 16:23:17 +02:00
tc_queue.appendleft(cmd.pack_command_tuple())
2022-05-05 01:21:57 +02:00
if op_code in OpCodesAss.NML:
2022-05-05 16:23:17 +02:00
data = pack_mode_data(object_id=object_id, mode=Modes.NORMAL, submode=0)
2022-05-10 18:34:15 +02:00
cmd = PusTelecommand(
service=200, subservice=Subservices.TC_MODE_COMMAND, app_data=data
)
2022-05-05 16:23:17 +02:00
tc_queue.appendleft(cmd.pack_command_tuple())
2022-05-05 01:21:57 +02:00
2022-05-25 11:10:57 +02:00
def pack_set_speed_command(
2022-07-04 15:22:53 +02:00
object_id: bytes, speed: int, ramp_time_ms: int
2022-05-25 11:10:57 +02:00
) -> PusTelecommand:
2022-01-18 14:03:56 +01:00
"""With this function a command is packed to set the speed of a reaction wheel
2022-05-05 02:00:18 +02:00
:param object_id: The object id of the reaction wheel handler.
:param speed: Valid speeds are [-65000, -1000] and [1000, 65000]. Values are
2022-05-05 01:21:57 +02:00
specified in 0.1 * RPM
2022-05-25 15:31:45 +02:00
:param ramp_time_ms: The time after which the reaction wheel will reach the commanded speed.
2022-05-05 01:21:57 +02:00
Valid times are 10 - 10000 ms
2021-06-25 12:07:16 +02:00
"""
2022-05-25 15:31:45 +02:00
if speed > 0:
if speed < 1000 or speed > 65000:
raise ValueError(
"Invalid RW speed specified. "
"Allowed range is [1000, 65000] 0.1 * RPM"
)
2022-06-02 18:51:40 +02:00
elif speed < 0:
2022-05-25 15:31:45 +02:00
if speed < -65000 or speed > -1000:
raise ValueError(
"Invalid RW speed specified. "
"Allowed range is [-65000, -1000] 0.1 * RPM"
)
2022-06-02 18:51:40 +02:00
else:
# Speed is 0
pass
2022-05-25 15:31:45 +02:00
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")
2021-06-25 12:07:16 +02:00
command_id = RwCommandIds.SET_SPEED
command = bytearray()
2022-05-05 01:21:57 +02:00
command += object_id + command_id
2022-01-18 14:03:56 +01:00
command = command + struct.pack("!i", speed)
2022-05-25 15:31:45 +02:00
command = command + ramp_time_ms.to_bytes(length=2, byteorder="big")
2022-07-04 15:22:53 +02:00
command = PusTelecommand(service=8, subservice=128, app_data=command)
2021-06-25 12:07:16 +02:00
return command