eive-tmtc/eive_tmtc/tmtc/power/pwr_ctrl.py

288 lines
10 KiB
Python
Raw Normal View History

2023-10-09 11:10:39 +02:00
import datetime
import enum
import logging
import struct
from tmtccmd.config.tmtc import (
2023-11-22 13:51:33 +01:00
CmdTreeNode,
)
from tmtccmd.fsfw.tmtc_printer import FsfwTmTcPrinter
from tmtccmd.pus.s20_fsfw_param import create_load_param_cmd
from tmtccmd.pus.s20_fsfw_param_defs import (
create_scalar_double_parameter,
create_scalar_float_parameter,
2023-10-09 11:10:39 +02:00
)
2023-11-10 19:23:06 +01:00
from tmtccmd.pus.s200_fsfw_mode import Mode, pack_mode_command
from tmtccmd.pus.tc.s3_fsfw_hk import (
2023-11-22 13:51:33 +01:00
disable_periodic_hk_command,
enable_periodic_hk_command_with_interval,
2023-10-09 11:10:39 +02:00
generate_one_hk_command,
make_sid,
)
2023-11-22 13:51:33 +01:00
from tmtccmd.tmtc.queue import DefaultPusQueueHelper
2023-10-09 11:10:39 +02:00
2023-11-22 13:51:33 +01:00
from eive_tmtc.config.object_ids import PWR_CONTROLLER
from eive_tmtc.pus_tm.defs import PrintWrapper
2023-10-09 11:10:39 +02:00
_LOGGER = logging.getLogger(__name__)
class SetId(enum.IntEnum):
CORE_HK_SET = 0
ENABLE_PL_SET = 1
# class ActionId(enum.IntEnum):
class ParamId(enum.IntEnum):
BATTERY_INTERNAL_RESISTANCE = 0
BATTERY_MAXIMUM_CAPACITY = 1
COULOMB_COUNTER_VOLTAGE_UPPER_THRESHOLD = 2
MAX_ALLOWED_TIME_DIFF = 3
PAYLOAD_OP_LIMIT_ON = 4
PAYLOAD_OP_LIMIT_LOW = 5
HIGHER_MODES_LIMIT = 6
2023-11-22 13:51:33 +01:00
class OpCode:
OFF = "off"
ON = "on"
NML = "normal"
SET_PARAMETER = "set_parameter"
REQUEST_CORE_HK = "core_hk"
ENABLE_CORE_HK = "core_enable_hk"
DISABLE_CORE_HK = "core_disable_hk"
REQUEST_ENABLE_PL_HK = "enable_pl_hk"
ENABLE_ENABLE_PL_HK = "enable_pl_enable_hk"
DISABLE_ENABLE_PL_HK = "enable_pl_disable_hk"
2023-10-09 11:10:39 +02:00
class Info:
OFF = "PWR Ctrl Mode to OFF"
ON = "PWR Ctrl Mode to ON"
NML = "PWR Ctrl Mode to NORMAL"
SET_PARAMETER = "Set Parameter"
REQUEST_CORE_HK = "Request Core HK once"
ENABLE_CORE_HK = "Enable Core HK Data Generation"
DISABLE_CORE_HK = "Disable Core HK Data Generation"
REQUEST_ENABLE_PL_HK = "Request Enable PL HK once"
ENABLE_ENABLE_PL_HK = "Enable Enable PL HK Data Generation"
DISABLE_ENABLE_PL_HK = "Disable Enable PL HK Data Generation"
2023-11-22 13:51:33 +01:00
def create_pwr_ctrl_node() -> CmdTreeNode:
op_code_strs = [
getattr(OpCode, key) for key in dir(OpCode) if not key.startswith("__")
]
info_strs = [getattr(Info, key) for key in dir(OpCode) if not key.startswith("__")]
combined_dict = dict(zip(op_code_strs, info_strs))
2023-11-22 16:40:27 +01:00
node = CmdTreeNode("pwr_ctrl", "Power Controller", hide_children_for_print=True)
2023-11-22 13:51:33 +01:00
for op_code, info in combined_dict.items():
node.add_child(CmdTreeNode(op_code, info))
return node
2023-11-28 17:34:34 +01:00
def pack_power_ctrl_command(q: DefaultPusQueueHelper, cmd_str: str):
2023-11-22 13:51:33 +01:00
if cmd_str == OpCode.OFF:
2023-10-09 11:10:39 +02:00
q.add_log_cmd(f"{Info.OFF}")
q.add_pus_tc(pack_mode_command(PWR_CONTROLLER, Mode.OFF, 0))
2023-11-22 13:51:33 +01:00
elif cmd_str == OpCode.ON:
2023-10-09 11:10:39 +02:00
q.add_log_cmd(f"{Info.ON}")
q.add_pus_tc(pack_mode_command(PWR_CONTROLLER, Mode.ON, 0))
2023-11-22 13:51:33 +01:00
elif cmd_str == OpCode.NML:
2023-10-09 11:10:39 +02:00
q.add_log_cmd(f"{Info.NML}")
q.add_pus_tc(pack_mode_command(PWR_CONTROLLER, Mode.NORMAL, 0))
2023-11-22 13:51:33 +01:00
elif cmd_str in OpCode.SET_PARAMETER:
2023-10-09 11:10:39 +02:00
q.add_log_cmd(f"{Info.SET_PARAMETER}")
set_pwr_ctrl_param(q)
2023-11-22 13:51:33 +01:00
elif cmd_str == OpCode.REQUEST_CORE_HK:
2023-10-09 11:10:39 +02:00
q.add_log_cmd(Info.REQUEST_CORE_HK)
q.add_pus_tc(
generate_one_hk_command(make_sid(PWR_CONTROLLER, SetId.CORE_HK_SET))
)
2023-11-22 13:51:33 +01:00
elif cmd_str == OpCode.ENABLE_CORE_HK:
2023-10-09 11:10:39 +02:00
interval = float(input("Please specify interval in floating point seconds: "))
q.add_log_cmd(Info.ENABLE_CORE_HK)
cmd_tuple = enable_periodic_hk_command_with_interval(
False, make_sid(PWR_CONTROLLER, SetId.CORE_HK_SET), interval
)
q.add_pus_tc(cmd_tuple[0])
q.add_pus_tc(cmd_tuple[1])
2023-11-22 13:51:33 +01:00
elif cmd_str == OpCode.DISABLE_CORE_HK:
2023-10-09 11:10:39 +02:00
q.add_log_cmd(Info.DISABLE_CORE_HK)
q.add_pus_tc(
disable_periodic_hk_command(
False, make_sid(PWR_CONTROLLER, SetId.CORE_HK_SET)
)
)
2023-11-22 13:51:33 +01:00
elif cmd_str == OpCode.REQUEST_ENABLE_PL_HK:
2023-10-09 11:10:39 +02:00
q.add_log_cmd(Info.REQUEST_ENABLE_PL_HK)
q.add_pus_tc(
generate_one_hk_command(make_sid(PWR_CONTROLLER, SetId.ENABLE_PL_SET))
)
2023-11-22 13:51:33 +01:00
elif cmd_str == OpCode.ENABLE_ENABLE_PL_HK:
2023-10-09 11:10:39 +02:00
interval = float(input("Please specify interval in floating point seconds: "))
q.add_log_cmd(Info.ENABLE_ENABLE_PL_HK)
cmd_tuple = enable_periodic_hk_command_with_interval(
False, make_sid(PWR_CONTROLLER, SetId.ENABLE_PL_SET), interval
)
q.add_pus_tc(cmd_tuple[0])
q.add_pus_tc(cmd_tuple[1])
2023-11-22 13:51:33 +01:00
elif cmd_str == OpCode.DISABLE_ENABLE_PL_HK:
2023-10-09 11:10:39 +02:00
q.add_log_cmd(Info.DISABLE_ENABLE_PL_HK)
q.add_pus_tc(
disable_periodic_hk_command(
False, make_sid(PWR_CONTROLLER, SetId.ENABLE_PL_SET)
)
)
def set_pwr_ctrl_param(q: DefaultPusQueueHelper):
for val in ParamId:
2023-10-09 15:43:03 +02:00
print("{:<2}: {:<20}".format(val, val.name))
2023-10-10 14:32:04 +02:00
param = int(input("Specify parameter to set \n" ""))
2023-10-09 11:10:39 +02:00
match param:
case ParamId.BATTERY_INTERNAL_RESISTANCE:
2023-10-09 15:43:03 +02:00
value = float(input("Specify parameter value to set [Ohm]: "))
2023-10-09 11:10:39 +02:00
q.add_pus_tc(
create_load_param_cmd(
create_scalar_float_parameter(
object_id=PWR_CONTROLLER,
domain_id=0,
unique_id=ParamId.BATTERY_INTERNAL_RESISTANCE,
parameter=value,
)
)
)
case ParamId.BATTERY_MAXIMUM_CAPACITY:
2023-10-09 15:43:03 +02:00
value = float(input("Specify parameter value to set [Ah]: "))
2023-10-09 11:10:39 +02:00
q.add_pus_tc(
create_load_param_cmd(
create_scalar_float_parameter(
object_id=PWR_CONTROLLER,
domain_id=0,
unique_id=ParamId.BATTERY_MAXIMUM_CAPACITY,
parameter=value,
)
)
)
case ParamId.COULOMB_COUNTER_VOLTAGE_UPPER_THRESHOLD:
2023-10-09 15:43:03 +02:00
value = float(input("Specify parameter value to set [V]: "))
2023-10-09 11:10:39 +02:00
q.add_pus_tc(
create_load_param_cmd(
create_scalar_float_parameter(
object_id=PWR_CONTROLLER,
domain_id=0,
unique_id=ParamId.COULOMB_COUNTER_VOLTAGE_UPPER_THRESHOLD,
parameter=value,
)
)
)
case ParamId.MAX_ALLOWED_TIME_DIFF:
2023-10-09 15:43:03 +02:00
value = float(input("Specify parameter value to set [s]: "))
2023-10-09 11:10:39 +02:00
q.add_pus_tc(
create_load_param_cmd(
create_scalar_double_parameter(
object_id=PWR_CONTROLLER,
domain_id=0,
unique_id=ParamId.MAX_ALLOWED_TIME_DIFF,
parameter=value,
)
)
)
case ParamId.PAYLOAD_OP_LIMIT_ON:
2023-10-09 15:43:03 +02:00
value = float(input("Specify parameter value to set [1]: "))
2023-10-09 11:10:39 +02:00
q.add_pus_tc(
create_load_param_cmd(
create_scalar_float_parameter(
object_id=PWR_CONTROLLER,
domain_id=0,
unique_id=ParamId.PAYLOAD_OP_LIMIT_ON,
parameter=value,
)
)
)
case ParamId.PAYLOAD_OP_LIMIT_LOW:
2023-10-09 15:43:03 +02:00
value = float(input("Specify parameter value to set [1]: "))
2023-10-09 11:10:39 +02:00
q.add_pus_tc(
create_load_param_cmd(
create_scalar_float_parameter(
object_id=PWR_CONTROLLER,
domain_id=0,
unique_id=ParamId.PAYLOAD_OP_LIMIT_LOW,
parameter=value,
)
)
)
case ParamId.HIGHER_MODES_LIMIT:
2023-10-09 15:43:03 +02:00
value = float(input("Specify parameter value to set [1]: "))
2023-10-09 11:10:39 +02:00
q.add_pus_tc(
create_load_param_cmd(
create_scalar_float_parameter(
object_id=PWR_CONTROLLER,
domain_id=0,
unique_id=ParamId.HIGHER_MODES_LIMIT,
parameter=value,
)
)
)
def handle_pwr_ctrl_hk_data(
pw: PrintWrapper,
set_id: int,
hk_data: bytes,
packet_time: datetime.datetime,
):
pw.ilog(_LOGGER, f"Received PWR CTRL HK with packet time {packet_time}")
match set_id:
case SetId.CORE_HK_SET:
handle_core_hk_data(pw, hk_data)
case SetId.ENABLE_PL_SET:
handle_enable_pl_data(pw, hk_data)
def handle_core_hk_data(pw: PrintWrapper, hk_data: bytes):
pw.dlog("Received Core HK Set")
fmt_int16 = "!h"
fmt_float = "!f"
inc_len_int16 = struct.calcsize(fmt_int16)
inc_len_float = struct.calcsize(fmt_float)
if len(hk_data) < inc_len_int16 + 2 * inc_len_float:
pw.dlog("Received HK set too small")
return
current_idx = 0
2023-10-09 15:43:03 +02:00
total_battery_current = struct.unpack(
fmt_int16, hk_data[current_idx : current_idx + inc_len_int16]
)[0]
2023-10-09 11:10:39 +02:00
current_idx += inc_len_int16
2023-10-09 15:43:03 +02:00
open_circuit_voltage_charge = struct.unpack(
fmt_float, hk_data[current_idx : current_idx + inc_len_float]
)[0]
2023-10-09 11:10:39 +02:00
current_idx += inc_len_float
2023-10-09 15:43:03 +02:00
coulomb_counter_charge = struct.unpack(
fmt_float, hk_data[current_idx : current_idx + inc_len_float]
)[0]
2023-10-09 11:10:39 +02:00
current_idx += inc_len_float
pw.dlog(f"Total Battery Current: {total_battery_current} [mA]")
2023-10-09 15:43:03 +02:00
pw.dlog(f"Open Circuit Voltage Charge: {open_circuit_voltage_charge*100:8.3f} [%]")
pw.dlog(f"Coulomb Counter Charge: {coulomb_counter_charge*100:8.3f} [%]")
2023-10-09 11:10:39 +02:00
FsfwTmTcPrinter.get_validity_buffer(hk_data[current_idx:], num_vars=3)
def handle_enable_pl_data(pw: PrintWrapper, hk_data: bytes):
pw.dlog("Received Enable PL HK Set")
fmt_uint16 = "!B"
inc_len_uint16 = struct.calcsize(fmt_uint16)
if len(hk_data) < inc_len_uint16:
pw.dlog("Received HK set too small")
return
current_idx = 0
2023-10-09 15:43:03 +02:00
pl_use_allowed = struct.unpack(
fmt_uint16, hk_data[current_idx : current_idx + inc_len_uint16]
)[0]
2023-10-09 11:10:39 +02:00
current_idx += inc_len_uint16
pw.dlog(f"PL Use Allowed: {pl_use_allowed}")
FsfwTmTcPrinter.get_validity_buffer(hk_data[current_idx:], num_vars=1)