73 lines
2.6 KiB
Python
73 lines
2.6 KiB
Python
# -*- coding: utf-8 -*-
|
|
"""
|
|
@file imtq.py
|
|
@brief Tests for the ISIS IMTQ (Magnettorquer) device handler
|
|
@author J. Meier
|
|
@date 25.03.2021
|
|
"""
|
|
import struct
|
|
from tmtccmd.config.definitions import QueueCommands
|
|
|
|
from tmtccmd.pus_tc.packer import TcQueueT
|
|
from tmtccmd.ecss.tc import PusTelecommand
|
|
from tmtccmd.pus_tc.service_3_housekeeping import make_sid, generate_one_hk_command
|
|
|
|
|
|
class RwSetIds:
|
|
STATUS_SET_ID = 4
|
|
TEMPERATURE_SET_ID = 8
|
|
|
|
|
|
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])
|
|
INIT_RW_CONTROLLER = bytearray([0x0, 0x0, 0x0, 0x05])
|
|
SET_SPEED = bytearray([0x0, 0x0, 0x0, 0x06])
|
|
# Reads temperature from reaction wheel into dataset with id 8
|
|
GET_TEMPERATURE = bytearray([0x0, 0x0, 0x0, 0x08])
|
|
|
|
|
|
class SpeedDefinitions:
|
|
RPM_100 = 1000
|
|
RPM_5000 = 5000
|
|
|
|
|
|
class RampTime:
|
|
MS_1000 = 1000
|
|
|
|
|
|
def pack_single_rw_test_into(object_id: bytearray, tc_queue: TcQueueT, op_code: str) -> TcQueueT:
|
|
tc_queue.appendleft(
|
|
(QueueCommands.PRINT,
|
|
"Testing reaction wheel handler with object id: 0x" + object_id.hex())
|
|
)
|
|
|
|
if op_code == "0" or op_code == "1":
|
|
tc_queue.appendleft((QueueCommands.PRINT, "Reaction Wheel: Set speed"))
|
|
command = pack_set_speed_command(object_id, 0, 1000)
|
|
command = PusTelecommand(service=8, subservice=128, ssc=40, app_data=command)
|
|
tc_queue.appendleft(command.pack_command_tuple())
|
|
|
|
# tc_queue.appendleft((QueueCommands.PRINT, "Reaction Wheel: Init reaction wheel controller"))
|
|
# command = object_id + RwCommandIds.INIT_RW_CONTROLLER
|
|
# command = PusTelecommand(service=8, subservice=128, ssc=41, app_data=command)
|
|
# tc_queue.appendleft(command.pack_command_tuple())
|
|
|
|
return tc_queue
|
|
|
|
|
|
def pack_set_speed_command(object_id: bytearray, speed: int, ramp_time: int) -> bytearray:
|
|
""" 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 reached the commanded speed. Valid times are
|
|
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')
|
|
return command
|