Merge pull request 'power_handler' (#1) from power_handler into master
Reviewed-on: #1
This commit is contained in:
commit
a30caabacb
25
Cargo.lock
generated
25
Cargo.lock
generated
@ -597,6 +597,26 @@ dependencies = [
|
||||
"libc",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "num_enum"
|
||||
version = "0.5.9"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "8d829733185c1ca374f17e52b762f24f535ec625d2cc1f070e34c8a9068f341b"
|
||||
dependencies = [
|
||||
"num_enum_derive",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "num_enum_derive"
|
||||
version = "0.5.9"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "2be1598bf1c313dcdd12092e3f1920f463462525a21b7b4e11b4168353d0123e"
|
||||
dependencies = [
|
||||
"proc-macro2",
|
||||
"quote",
|
||||
"syn",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "once_cell"
|
||||
version = "1.16.0"
|
||||
@ -792,14 +812,15 @@ dependencies = [
|
||||
|
||||
[[package]]
|
||||
name = "spacepackets"
|
||||
version = "0.5.2"
|
||||
version = "0.5.3"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "a31966b4c2a1ca1902c6d34050648e4fbffeb124144775396422a94340068efd"
|
||||
checksum = "81541fd89a5bc02845a849895d6ed1721235b3eac26fc77010f0a05f53bc4e8a"
|
||||
dependencies = [
|
||||
"chrono",
|
||||
"crc",
|
||||
"delegate",
|
||||
"num-traits",
|
||||
"num_enum",
|
||||
"serde",
|
||||
"zerocopy",
|
||||
]
|
||||
|
||||
383
pyclient/main copy.py
Normal file
383
pyclient/main copy.py
Normal file
@ -0,0 +1,383 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Example client for the sat-rs example application"""
|
||||
import enum
|
||||
import logging
|
||||
import struct
|
||||
import sys
|
||||
import time
|
||||
from typing import Optional
|
||||
import datetime
|
||||
|
||||
import tmtccmd
|
||||
from spacepackets.ecss import PusTelemetry, PusTelecommand, PusVerificator
|
||||
from spacepackets.ecss.pus_17_test import Service17Tm
|
||||
from spacepackets.ecss.pus_1_verification import UnpackParams, Service1Tm
|
||||
from spacepackets.ccsds.time import CdsShortTimestamp
|
||||
|
||||
from tmtccmd import CcsdsTmtcBackend, TcHandlerBase, ProcedureParamsWrapper
|
||||
from tmtccmd.tc.pus_3_fsfw_hk import generate_one_hk_command, make_sid
|
||||
from tmtccmd.tc.pus_11_tc_sched import create_time_tagged_cmd
|
||||
from tmtccmd.core.base import BackendRequest
|
||||
from tmtccmd.pus import VerificationWrapper
|
||||
from tmtccmd.tm import CcsdsTmHandler, SpecificApidHandlerBase
|
||||
from tmtccmd.com import ComInterface
|
||||
from tmtccmd.config import (
|
||||
default_json_path,
|
||||
SetupParams,
|
||||
HookBase,
|
||||
TmtcDefinitionWrapper,
|
||||
CoreServiceList,
|
||||
OpCodeEntry,
|
||||
params_to_procedure_conversion,
|
||||
)
|
||||
from tmtccmd.config import PreArgsParsingWrapper, SetupWrapper
|
||||
from tmtccmd.logging import add_colorlog_console_logger
|
||||
from tmtccmd.logging.pus import (
|
||||
RegularTmtcLogWrapper,
|
||||
RawTmtcTimedLogWrapper,
|
||||
TimedLogWhen,
|
||||
)
|
||||
from tmtccmd.tc import (
|
||||
TcQueueEntryType,
|
||||
ProcedureWrapper,
|
||||
TcProcedureType,
|
||||
FeedWrapper,
|
||||
SendCbParams,
|
||||
DefaultPusQueueHelper,
|
||||
QueueWrapper,
|
||||
)
|
||||
from tmtccmd.tm.pus_5_fsfw_event import Service5Tm
|
||||
from tmtccmd.util import FileSeqCountProvider, PusFileSeqCountProvider
|
||||
from tmtccmd.util.obj_id import ObjectIdDictT
|
||||
|
||||
from tmtccmd.util.tmtc_printer import FsfwTmTcPrinter
|
||||
|
||||
_LOGGER = logging.getLogger(__name__)
|
||||
|
||||
EXAMPLE_PUS_APID = 0x02
|
||||
|
||||
|
||||
class SatRsConfigHook(HookBase):
|
||||
def __init__(self, json_cfg_path: str):
|
||||
super().__init__(json_cfg_path=json_cfg_path)
|
||||
|
||||
def assign_communication_interface(self, com_if_key: str) -> Optional[ComInterface]:
|
||||
from tmtccmd.config.com import (
|
||||
create_com_interface_default,
|
||||
create_com_interface_cfg_default,
|
||||
)
|
||||
|
||||
cfg = create_com_interface_cfg_default(
|
||||
com_if_key=com_if_key,
|
||||
json_cfg_path=self.cfg_path,
|
||||
space_packet_ids=None,
|
||||
)
|
||||
return create_com_interface_default(cfg)
|
||||
|
||||
def get_tmtc_definitions(self) -> TmtcDefinitionWrapper:
|
||||
from tmtccmd.config.globals import get_default_tmtc_defs
|
||||
|
||||
defs = get_default_tmtc_defs()
|
||||
srv_5 = OpCodeEntry()
|
||||
srv_5.add("0", "Event Test")
|
||||
defs.add_service(
|
||||
name=CoreServiceList.SERVICE_5.value,
|
||||
info="PUS Service 5 Event",
|
||||
op_code_entry=srv_5,
|
||||
)
|
||||
srv_17 = OpCodeEntry()
|
||||
srv_17.add("0", "Ping Test")
|
||||
defs.add_service(
|
||||
name=CoreServiceList.SERVICE_17_ALT,
|
||||
info="PUS Service 17 Test",
|
||||
op_code_entry=srv_17,
|
||||
)
|
||||
srv_3 = OpCodeEntry()
|
||||
srv_3.add(HkOpCodes.GENERATE_ONE_SHOT, "Generate AOCS one shot HK")
|
||||
defs.add_service(
|
||||
name=CoreServiceList.SERVICE_3,
|
||||
info="PUS Service 3 Housekeeping",
|
||||
op_code_entry=srv_3,
|
||||
)
|
||||
|
||||
srv_8 = OpCodeEntry()
|
||||
srv_8.add("1", "Camera Image Request")
|
||||
defs.add_service(
|
||||
name=CoreServiceList.SERVICE_8,
|
||||
info="PUS Service 8 Action",
|
||||
op_code_entry=srv_8,
|
||||
)
|
||||
|
||||
srv_11 = OpCodeEntry()
|
||||
srv_11.add("0", "Scheduled TC Test")
|
||||
defs.add_service(
|
||||
name=CoreServiceList.SERVICE_11,
|
||||
info="PUS Service 11 TC Scheduling",
|
||||
op_code_entry=srv_11,
|
||||
)
|
||||
return defs
|
||||
|
||||
def perform_mode_operation(self, tmtc_backend: CcsdsTmtcBackend, mode: int):
|
||||
_LOGGER.info("Mode operation hook was called")
|
||||
pass
|
||||
|
||||
def get_object_ids(self) -> ObjectIdDictT:
|
||||
from tmtccmd.config.objects import get_core_object_ids
|
||||
|
||||
return get_core_object_ids()
|
||||
|
||||
|
||||
class PusHandler(SpecificApidHandlerBase):
|
||||
def __init__(
|
||||
self,
|
||||
verif_wrapper: VerificationWrapper,
|
||||
printer: FsfwTmTcPrinter,
|
||||
raw_logger: RawTmtcTimedLogWrapper,
|
||||
):
|
||||
super().__init__(EXAMPLE_PUS_APID, None)
|
||||
self.printer = printer
|
||||
self.raw_logger = raw_logger
|
||||
self.verif_wrapper = verif_wrapper
|
||||
|
||||
def handle_tm(self, packet: bytes, _user_args: any):
|
||||
try:
|
||||
tm_packet = PusTelemetry.unpack(
|
||||
packet, time_reader=CdsShortTimestamp.empty()
|
||||
)
|
||||
except ValueError as e:
|
||||
_LOGGER.warning("Could not generate PUS TM object from raw data")
|
||||
_LOGGER.warning(f"Raw Packet: [{packet.hex(sep=',')}], REPR: {packet!r}")
|
||||
raise e
|
||||
service = tm_packet.service
|
||||
dedicated_handler = False
|
||||
if service == 1:
|
||||
tm_packet = Service1Tm.unpack(
|
||||
data=packet, params=UnpackParams(CdsShortTimestamp.empty(), 1, 2)
|
||||
)
|
||||
res = self.verif_wrapper.add_tm(tm_packet)
|
||||
if res is None:
|
||||
_LOGGER.info(
|
||||
f"Received Verification TM[{tm_packet.service}, {tm_packet.subservice}] "
|
||||
f"with Request ID {tm_packet.tc_req_id.as_u32():#08x}"
|
||||
)
|
||||
_LOGGER.warning(
|
||||
f"No matching telecommand found for {tm_packet.tc_req_id}"
|
||||
)
|
||||
else:
|
||||
self.verif_wrapper.log_to_console(tm_packet, res)
|
||||
self.verif_wrapper.log_to_file(tm_packet, res)
|
||||
dedicated_handler = True
|
||||
if service == 3:
|
||||
_LOGGER.info("No handling for HK packets implemented")
|
||||
_LOGGER.info(f"Raw packet: 0x[{packet.hex(sep=',')}]")
|
||||
pus_tm = PusTelemetry.unpack(packet, time_reader=CdsShortTimestamp.empty())
|
||||
if pus_tm.subservice == 25:
|
||||
if len(pus_tm.source_data) < 8:
|
||||
raise ValueError("No addressable ID in HK packet")
|
||||
json_str = pus_tm.source_data[8:]
|
||||
dedicated_handler = True
|
||||
if service == 5:
|
||||
tm_packet = Service5Tm.unpack(packet, time_reader=CdsShortTimestamp.empty())
|
||||
if service == 17:
|
||||
tm_packet = Service17Tm.unpack(
|
||||
packet, time_reader=CdsShortTimestamp.empty()
|
||||
)
|
||||
dedicated_handler = True
|
||||
if tm_packet.subservice == 2:
|
||||
self.printer.file_logger.info("Received Ping Reply TM[17,2]")
|
||||
_LOGGER.info("Received Ping Reply TM[17,2]")
|
||||
else:
|
||||
self.printer.file_logger.info(
|
||||
f"Received Test Packet with unknown subservice {tm_packet.subservice}"
|
||||
)
|
||||
_LOGGER.info(
|
||||
f"Received Test Packet with unknown subservice {tm_packet.subservice}"
|
||||
)
|
||||
if tm_packet is None:
|
||||
_LOGGER.info(
|
||||
f"The service {service} is not implemented in Telemetry Factory"
|
||||
)
|
||||
tm_packet = PusTelemetry.unpack(
|
||||
packet, time_reader=CdsShortTimestamp.empty()
|
||||
)
|
||||
self.raw_logger.log_tm(tm_packet)
|
||||
if not dedicated_handler and tm_packet is not None:
|
||||
pass
|
||||
# self.printer.handle_long_tm_print(packet_if=tm_packet, info_if=tm_packet)
|
||||
|
||||
|
||||
def make_addressable_id(target_id: int, unique_id: int) -> bytes:
|
||||
byte_string = bytearray(struct.pack("!I", target_id))
|
||||
byte_string.extend(struct.pack("!I", unique_id))
|
||||
return byte_string
|
||||
|
||||
|
||||
def read_addressable_id(data: bytes) -> tuple[int, int]:
|
||||
target_id = struct.unpack("!I", data[0:4])[0]
|
||||
set_id = struct.unpack("!I", data[4:8])[0]
|
||||
return (target_id, set_id)
|
||||
|
||||
|
||||
class RequestTargetId(enum.IntEnum):
|
||||
ACS = 1
|
||||
PLD = 2
|
||||
|
||||
|
||||
class AcsHkIds(enum.IntEnum):
|
||||
MGM_SET = 1
|
||||
|
||||
|
||||
class HkOpCodes:
|
||||
GENERATE_ONE_SHOT = ["0", "oneshot"]
|
||||
|
||||
|
||||
def make_target_id(target_id: int) -> bytes:
|
||||
byte_string = bytearray(struct.pack("!I", target_id))
|
||||
return byte_string
|
||||
|
||||
|
||||
class TcHandler(TcHandlerBase):
|
||||
def __init__(
|
||||
self,
|
||||
seq_count_provider: FileSeqCountProvider,
|
||||
verif_wrapper: VerificationWrapper,
|
||||
):
|
||||
super(TcHandler, self).__init__()
|
||||
self.seq_count_provider = seq_count_provider
|
||||
self.verif_wrapper = verif_wrapper
|
||||
self.queue_helper = DefaultPusQueueHelper(
|
||||
queue_wrapper=QueueWrapper.empty(),
|
||||
tc_sched_timestamp_len=CdsShortTimestamp.TIMESTAMP_SIZE,
|
||||
seq_cnt_provider=seq_count_provider,
|
||||
pus_verificator=self.verif_wrapper.pus_verificator,
|
||||
default_pus_apid=EXAMPLE_PUS_APID,
|
||||
)
|
||||
|
||||
def send_cb(self, send_params: SendCbParams):
|
||||
entry_helper = send_params.entry
|
||||
if entry_helper.is_tc:
|
||||
if entry_helper.entry_type == TcQueueEntryType.PUS_TC:
|
||||
pus_tc_wrapper = entry_helper.to_pus_tc_entry()
|
||||
raw_tc = pus_tc_wrapper.pus_tc.pack()
|
||||
_LOGGER.info(f"Sending {pus_tc_wrapper.pus_tc}")
|
||||
send_params.com_if.send(raw_tc)
|
||||
elif entry_helper.entry_type == TcQueueEntryType.LOG:
|
||||
log_entry = entry_helper.to_log_entry()
|
||||
_LOGGER.info(log_entry.log_str)
|
||||
|
||||
def queue_finished_cb(self, helper: ProcedureWrapper):
|
||||
if helper.proc_type == TcProcedureType.DEFAULT:
|
||||
def_proc = helper.to_def_procedure()
|
||||
_LOGGER.info(
|
||||
f"Queue handling finished for service {def_proc.service} and "
|
||||
f"op code {def_proc.op_code}"
|
||||
)
|
||||
|
||||
def feed_cb(self, helper: ProcedureWrapper, wrapper: FeedWrapper):
|
||||
q = self.queue_helper
|
||||
q.queue_wrapper = wrapper.queue_wrapper
|
||||
if helper.proc_type == TcProcedureType.DEFAULT:
|
||||
def_proc = helper.to_def_procedure()
|
||||
service = def_proc.service
|
||||
op_code = def_proc.op_code
|
||||
if (
|
||||
service == CoreServiceList.SERVICE_17
|
||||
or service == CoreServiceList.SERVICE_17_ALT
|
||||
):
|
||||
q.add_log_cmd("Sending PUS ping telecommand")
|
||||
return q.add_pus_tc(PusTelecommand(service=17, subservice=1))
|
||||
if service == CoreServiceList.SERVICE_11:
|
||||
q.add_log_cmd("Sending PUS scheduled TC telecommand")
|
||||
crt_time = CdsShortTimestamp.from_now()
|
||||
time_stamp = crt_time + datetime.timedelta(seconds=10)
|
||||
time_stamp = time_stamp.pack()
|
||||
return q.add_pus_tc(
|
||||
create_time_tagged_cmd(
|
||||
time_stamp,
|
||||
PusTelecommand(service=17, subservice=1),
|
||||
apid=EXAMPLE_PUS_APID,
|
||||
)
|
||||
)
|
||||
if service == CoreServiceList.SERVICE_8:
|
||||
q.add_log_cmd("Sending PUS action request telecommand")
|
||||
return q.add_pus_tc(
|
||||
PusTelecommand(service=8, subservice=1, app_data=make_target_id(RequestTargetId.PLD))
|
||||
)
|
||||
if service == CoreServiceList.SERVICE_3:
|
||||
if op_code in HkOpCodes.GENERATE_ONE_SHOT:
|
||||
q.add_log_cmd("Sending HK one shot request")
|
||||
q.add_pus_tc(
|
||||
generate_one_hk_command(
|
||||
make_addressable_id(RequestTargetId.ACS, AcsHkIds.MGM_SET)
|
||||
)
|
||||
)
|
||||
pass
|
||||
|
||||
|
||||
def main():
|
||||
add_colorlog_console_logger(_LOGGER)
|
||||
tmtccmd.init_printout(False)
|
||||
hook_obj = SatRsConfigHook(json_cfg_path=default_json_path())
|
||||
parser_wrapper = PreArgsParsingWrapper()
|
||||
parser_wrapper.create_default_parent_parser()
|
||||
parser_wrapper.create_default_parser()
|
||||
parser_wrapper.add_def_proc_args()
|
||||
params = SetupParams()
|
||||
post_args_wrapper = parser_wrapper.parse(hook_obj, params)
|
||||
proc_wrapper = ProcedureParamsWrapper()
|
||||
if post_args_wrapper.use_gui:
|
||||
post_args_wrapper.set_params_without_prompts(proc_wrapper)
|
||||
else:
|
||||
post_args_wrapper.set_params_with_prompts(proc_wrapper)
|
||||
params.apid = EXAMPLE_PUS_APID
|
||||
setup_args = SetupWrapper(
|
||||
hook_obj=hook_obj, setup_params=params, proc_param_wrapper=proc_wrapper
|
||||
)
|
||||
# Create console logger helper and file loggers
|
||||
tmtc_logger = RegularTmtcLogWrapper()
|
||||
printer = FsfwTmTcPrinter(tmtc_logger.logger)
|
||||
raw_logger = RawTmtcTimedLogWrapper(when=TimedLogWhen.PER_HOUR, interval=1)
|
||||
verificator = PusVerificator()
|
||||
verification_wrapper = VerificationWrapper(
|
||||
verificator, _LOGGER, printer.file_logger
|
||||
)
|
||||
# Create primary TM handler and add it to the CCSDS Packet Handler
|
||||
tm_handler = PusHandler(verification_wrapper, printer, raw_logger)
|
||||
ccsds_handler = CcsdsTmHandler(generic_handler=None)
|
||||
ccsds_handler.add_apid_handler(tm_handler)
|
||||
|
||||
# Create TC handler
|
||||
seq_count_provider = PusFileSeqCountProvider()
|
||||
tc_handler = TcHandler(seq_count_provider, verification_wrapper)
|
||||
tmtccmd.setup(setup_args=setup_args)
|
||||
init_proc = params_to_procedure_conversion(setup_args.proc_param_wrapper)
|
||||
tmtc_backend = tmtccmd.create_default_tmtc_backend(
|
||||
setup_wrapper=setup_args,
|
||||
tm_handler=ccsds_handler,
|
||||
tc_handler=tc_handler,
|
||||
init_procedure=init_proc,
|
||||
)
|
||||
tmtccmd.start(tmtc_backend=tmtc_backend, hook_obj=hook_obj)
|
||||
try:
|
||||
while True:
|
||||
state = tmtc_backend.periodic_op(None)
|
||||
if state.request == BackendRequest.TERMINATION_NO_ERROR:
|
||||
sys.exit(0)
|
||||
elif state.request == BackendRequest.DELAY_IDLE:
|
||||
_LOGGER.info("TMTC Client in IDLE mode")
|
||||
time.sleep(3.0)
|
||||
elif state.request == BackendRequest.DELAY_LISTENER:
|
||||
time.sleep(0.8)
|
||||
elif state.request == BackendRequest.DELAY_CUSTOM:
|
||||
if state.next_delay.total_seconds() <= 0.4:
|
||||
time.sleep(state.next_delay.total_seconds())
|
||||
else:
|
||||
time.sleep(0.4)
|
||||
elif state.request == BackendRequest.CALL_NEXT:
|
||||
pass
|
||||
except KeyboardInterrupt:
|
||||
sys.exit(0)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@ -218,7 +218,7 @@ def make_target_id(target_id: int) -> bytes:
|
||||
byte_string = bytearray(struct.pack("!I", target_id))
|
||||
return byte_string
|
||||
|
||||
def read_addressable_id(data: bytes) -> Tuple[int, int]:
|
||||
def read_addressable_id(data: bytes) -> tuple[int, int]:
|
||||
target_id = struct.unpack("!I", data[0:4])[0]
|
||||
set_id = struct.unpack("!I", data[4:8])[0]
|
||||
return (target_id, set_id)
|
||||
@ -248,6 +248,7 @@ class TcHandler(TcHandlerBase):
|
||||
self.verif_wrapper = verif_wrapper
|
||||
self.queue_helper = DefaultPusQueueHelper(
|
||||
queue_wrapper=None,
|
||||
tc_sched_timestamp_len=7,
|
||||
seq_cnt_provider=seq_count_provider,
|
||||
)
|
||||
|
||||
|
||||
@ -1,2 +1,2 @@
|
||||
# tmtccmd == 3.0.0
|
||||
-e git+https://github.com/robamu-org/tmtccmd@97e5e51101a08b21472b3ddecc2063359f7e307a#egg=tmtccmd
|
||||
tmtccmd == 4.0.0rc0
|
||||
# -e git+https://github.com/robamu-org/tmtccmd@97e5e51101a08b21472b3ddecc2063359f7e307a#egg=tmtccmd
|
||||
|
||||
@ -1,5 +1,4 @@
|
||||
use eurosim_obsw::RequestTargetId;
|
||||
use satrs_core::tmtc::AddressableId;
|
||||
|
||||
pub type CollectionIntervalFactor = u32;
|
||||
|
||||
|
||||
@ -2,7 +2,7 @@
|
||||
#![allow(unused_variables)]
|
||||
#![allow(unused_imports)]
|
||||
|
||||
use crate::device_handler::DeviceState;
|
||||
use crate::pcdu::DeviceState;
|
||||
|
||||
use crate::can_ids::{
|
||||
can_id_to_package_id, package_id_to_can_id, value_to_package_id, DeviceId, PackageId,
|
||||
|
||||
@ -1,9 +1,6 @@
|
||||
use embedded_can::{Id, StandardId};
|
||||
use log::warn;
|
||||
use std::collections::HashMap;
|
||||
use std::fs;
|
||||
use std::sync::mpsc::Sender;
|
||||
use std::thread::Thread;
|
||||
|
||||
pub use num_derive::{FromPrimitive, ToPrimitive};
|
||||
pub use num_traits::{FromPrimitive, ToPrimitive};
|
||||
@ -93,6 +90,39 @@ pub enum DeviceId {
|
||||
All = 23,
|
||||
}
|
||||
|
||||
impl TryFrom<u16> for DeviceId {
|
||||
type Error = ();
|
||||
|
||||
fn try_from(v: u16) -> Result<Self, Self::Error> {
|
||||
match v {
|
||||
x if x==DeviceId::OBC as u16 => Ok(DeviceId::OBC),
|
||||
x if x==DeviceId::PCDU as u16 => Ok(DeviceId::PCDU),
|
||||
x if x==DeviceId::MGM1 as u16 => Ok(DeviceId::MGM1),
|
||||
x if x==DeviceId::MGM2 as u16 => Ok(DeviceId::MGM2),
|
||||
x if x==DeviceId::MGM3 as u16 => Ok(DeviceId::MGM3),
|
||||
x if x==DeviceId::MGM4 as u16 => Ok(DeviceId::MGM4),
|
||||
x if x==DeviceId::SunSensor1 as u16 => Ok(DeviceId::SunSensor1),
|
||||
x if x==DeviceId::SunSensor2 as u16 => Ok(DeviceId::SunSensor2),
|
||||
x if x==DeviceId::SunSensor3 as u16 => Ok(DeviceId::SunSensor3),
|
||||
x if x==DeviceId::SunSensor4 as u16 => Ok(DeviceId::SunSensor4),
|
||||
x if x==DeviceId::SunSensor5 as u16 => Ok(DeviceId::SunSensor5),
|
||||
x if x==DeviceId::SunSensor6 as u16 => Ok(DeviceId::SunSensor6),
|
||||
x if x==DeviceId::StarTracker as u16 => Ok(DeviceId::StarTracker),
|
||||
x if x==DeviceId::MGT1 as u16 => Ok(DeviceId::MGT1),
|
||||
x if x==DeviceId::MGT2 as u16 => Ok(DeviceId::MGT2),
|
||||
x if x==DeviceId::MGT3 as u16 => Ok(DeviceId::MGT3),
|
||||
x if x==DeviceId::MGT4 as u16 => Ok(DeviceId::MGT4),
|
||||
x if x==DeviceId::RWL1 as u16 => Ok(DeviceId::RWL1),
|
||||
x if x==DeviceId::RWL2 as u16 => Ok(DeviceId::RWL2),
|
||||
x if x==DeviceId::RWL3 as u16 => Ok(DeviceId::RWL3),
|
||||
x if x==DeviceId::RWL4 as u16 => Ok(DeviceId::RWL4),
|
||||
x if x==DeviceId::Camera as u16 => Ok(DeviceId::Camera),
|
||||
x if x==DeviceId::All as u16 => Ok(DeviceId::All),
|
||||
_ => Err(()),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Debug)]
|
||||
pub struct PackageModel {
|
||||
package_id: PackageId,
|
||||
|
||||
@ -1,222 +0,0 @@
|
||||
use crate::can_ids::{DeviceId, PackageId, PackageModel, ThreadId};
|
||||
use log::{info, warn};
|
||||
use socketcan::{errors, frame, socket, CanFrame, Socket};
|
||||
use std::collections::HashMap;
|
||||
use std::hash::Hash;
|
||||
use std::sync::mpsc::{Receiver, RecvError, Sender};
|
||||
|
||||
use crate::can::{CanRxHandler, CanTxHandler};
|
||||
pub use num_derive::{FromPrimitive, ToPrimitive};
|
||||
pub use num_traits::{FromPrimitive, ToPrimitive};
|
||||
pub use strum::IntoEnumIterator; // 0.17.1
|
||||
pub use strum_macros::EnumIter; // 0.17.1
|
||||
|
||||
#[derive(Copy, Clone, Debug, PartialEq)]
|
||||
pub enum DeviceState {
|
||||
On,
|
||||
Off,
|
||||
Broken,
|
||||
Unknown,
|
||||
}
|
||||
|
||||
pub struct CanDeviceHandler {
|
||||
device_state_map: HashMap<DeviceId, DeviceState>,
|
||||
can_tx_handler: CanTxHandler,
|
||||
can_rx_receiver: Receiver<PackageModel>,
|
||||
}
|
||||
|
||||
impl CanDeviceHandler {
|
||||
pub fn new(
|
||||
can_tx_handler: CanTxHandler,
|
||||
can_rx_receiver: Receiver<PackageModel>,
|
||||
) -> CanDeviceHandler {
|
||||
let mut device_state_map: HashMap<DeviceId, DeviceState> = HashMap::new();
|
||||
|
||||
for id in DeviceId::iter() {
|
||||
device_state_map.insert(id, DeviceState::Unknown);
|
||||
}
|
||||
CanDeviceHandler {
|
||||
device_state_map,
|
||||
can_tx_handler,
|
||||
can_rx_receiver,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn power_on(&mut self, id: DeviceId) -> Result<(), ()> {
|
||||
if !self.device_state_map.contains_key(&id) {
|
||||
return Err(());
|
||||
}
|
||||
info!("Powering on device {:?}", id);
|
||||
|
||||
let msg_data: [u8; 1] = [id as u8];
|
||||
self.can_tx_handler
|
||||
.tx_socket(PackageId::DevicePowerOnRequest, &msg_data);
|
||||
|
||||
let request_confirmation = self.can_rx_receiver.recv();
|
||||
match request_confirmation {
|
||||
Ok(confirmation) => {
|
||||
if confirmation.package_id() != PackageId::DevicePowerOnRequestConfirmation {
|
||||
warn!("Wrong package ID.");
|
||||
return Err(());
|
||||
}
|
||||
if confirmation.data()[0] != id as u8 {
|
||||
warn!("Wrong device ID.");
|
||||
return Err(());
|
||||
}
|
||||
if confirmation.data()[1] != 1 {
|
||||
warn!("Request unsuccessful.");
|
||||
return Err(());
|
||||
}
|
||||
info!("Power on request confirmation received.");
|
||||
}
|
||||
Err(_) => {
|
||||
warn!("Error receiving package.");
|
||||
return Err(());
|
||||
}
|
||||
}
|
||||
|
||||
let request_success = self.can_rx_receiver.recv();
|
||||
match request_success {
|
||||
Ok(success) => {
|
||||
if success.package_id() != PackageId::DevicePowerOnConfirmation {
|
||||
warn!("Wrong package ID.");
|
||||
return Err(());
|
||||
}
|
||||
if success.data()[0] != id as u8 {
|
||||
warn!("Wrong device ID.");
|
||||
return Err(());
|
||||
}
|
||||
if success.data()[1] != 1 {
|
||||
warn!("Power on unsuccessful.");
|
||||
return Err(());
|
||||
}
|
||||
info!("Power on confirmation received.");
|
||||
}
|
||||
Err(_) => {
|
||||
warn!("Error receiving package.");
|
||||
return Err(());
|
||||
}
|
||||
}
|
||||
|
||||
Ok(())
|
||||
}
|
||||
|
||||
pub fn power_off(&mut self, id: DeviceId) -> Result<(), ()> {
|
||||
if !self.device_state_map.contains_key(&id) {
|
||||
return Err(());
|
||||
}
|
||||
info!("Powering on device {:?}", id);
|
||||
|
||||
let msg_data: [u8; 1] = [id as u8];
|
||||
self.can_tx_handler
|
||||
.tx_socket(PackageId::DevicePowerOffRequest, &msg_data);
|
||||
|
||||
let request_confirmation = self.can_rx_receiver.recv();
|
||||
match request_confirmation {
|
||||
Ok(confirmation) => {
|
||||
if confirmation.package_id() != PackageId::DevicePowerOffRequestConfirmation {
|
||||
warn!("Wrong package ID.");
|
||||
return Err(());
|
||||
}
|
||||
if confirmation.data()[0] != id as u8 {
|
||||
warn!("Wrong device ID.");
|
||||
return Err(());
|
||||
}
|
||||
if confirmation.data()[1] != 1 {
|
||||
warn!("Request unsuccessful.");
|
||||
return Err(());
|
||||
}
|
||||
info!("Power off request confirmation received.");
|
||||
}
|
||||
Err(_) => {
|
||||
warn!("Error receiving package.");
|
||||
return Err(());
|
||||
}
|
||||
}
|
||||
|
||||
let request_success = self.can_rx_receiver.recv();
|
||||
match request_success {
|
||||
Ok(success) => {
|
||||
if success.package_id() != PackageId::DevicePowerOffConfirmation {
|
||||
warn!("Wrong package ID.");
|
||||
return Err(());
|
||||
}
|
||||
if success.data()[0] != id as u8 {
|
||||
warn!("Wrong device ID.");
|
||||
return Err(());
|
||||
}
|
||||
if success.data()[1] != 1 {
|
||||
warn!("Power off unsuccessful.");
|
||||
return Err(());
|
||||
}
|
||||
info!("Power off confirmation received.");
|
||||
}
|
||||
Err(_) => {
|
||||
warn!("Error receiving package.");
|
||||
return Err(());
|
||||
}
|
||||
}
|
||||
|
||||
Ok(())
|
||||
}
|
||||
|
||||
pub fn get_power_states(&mut self) -> HashMap<DeviceId, DeviceState> {
|
||||
for id in DeviceId::iter() {
|
||||
self.update_power_state(id)
|
||||
.expect("Error updating power state.");
|
||||
}
|
||||
self.device_state_map.clone()
|
||||
}
|
||||
|
||||
pub fn get_power_state(&mut self, id: DeviceId) -> Option<&DeviceState> {
|
||||
self.update_power_state(id)
|
||||
.expect("Error updating power state.");
|
||||
self.device_state_map.get(&id)
|
||||
}
|
||||
|
||||
pub fn update_power_state(&mut self, id: DeviceId) -> Result<(), ()> {
|
||||
if !self.device_state_map.contains_key(&id) {
|
||||
return Err(());
|
||||
}
|
||||
|
||||
let msg_data: [u8; 1] = [id as u8];
|
||||
self.can_tx_handler
|
||||
.tx_socket(PackageId::DevicePowerStatusRequest, &msg_data);
|
||||
|
||||
let response = self.can_rx_receiver.recv();
|
||||
if let Ok(response) = response {
|
||||
let data = response.data();
|
||||
|
||||
if data[0] == id as u8 {
|
||||
if data[1] == 1 {
|
||||
*self.device_state_map.get_mut(&id).unwrap() = DeviceState::On;
|
||||
} else if data[1] == 0 {
|
||||
*self.device_state_map.get_mut(&id).unwrap() = DeviceState::Off;
|
||||
} else {
|
||||
return Err(());
|
||||
}
|
||||
}
|
||||
} else {
|
||||
return Err(());
|
||||
}
|
||||
Ok(())
|
||||
}
|
||||
}
|
||||
|
||||
pub fn power_up_sequence(mut device_handler: CanDeviceHandler) -> HashMap<DeviceId, DeviceState> {
|
||||
for id in DeviceId::iter() {
|
||||
device_handler
|
||||
.power_on(id)
|
||||
.expect("Error powering on device.");
|
||||
}
|
||||
device_handler.get_power_states()
|
||||
}
|
||||
|
||||
pub fn power_down_sequence(mut device_handler: CanDeviceHandler) -> HashMap<DeviceId, DeviceState> {
|
||||
for id in DeviceId::iter() {
|
||||
device_handler
|
||||
.power_off(id)
|
||||
.expect("Error powering on device.");
|
||||
}
|
||||
device_handler.get_power_states()
|
||||
}
|
||||
169
src/main.rs
169
src/main.rs
@ -4,14 +4,14 @@ mod cam;
|
||||
mod can;
|
||||
mod can_ids;
|
||||
mod ccsds;
|
||||
mod device_handler;
|
||||
mod pld_handler;
|
||||
mod hk;
|
||||
mod logger;
|
||||
mod pus;
|
||||
mod requests;
|
||||
mod tmtc;
|
||||
mod pcdu;
|
||||
|
||||
use crate::hk::{AcsHkIds, HkRequest};
|
||||
use crate::requests::{Request, RequestWithToken};
|
||||
use crate::tmtc::{
|
||||
core_tmtc_task, OtherArgs, PusTcSource, TcArgs, TcStore, TmArgs, TmFunnel, TmStore, PUS_APID,
|
||||
@ -23,37 +23,37 @@ use satrs_core::event_man::{
|
||||
use satrs_core::events::EventU32;
|
||||
use satrs_core::pool::{LocalPool, PoolCfg, StoreAddr};
|
||||
use satrs_core::pus::event_man::{
|
||||
DefaultPusMgmtBackendProvider, EventReporter, EventRequest, EventRequestWithToken,
|
||||
DefaultPusMgmtBackendProvider, EventReporter, EventRequestWithToken,
|
||||
PusEventDispatcher,
|
||||
};
|
||||
use satrs_core::pus::hk::Subservice;
|
||||
use satrs_core::pus::verification::{
|
||||
MpscVerifSender, VerificationReporterCfg, VerificationReporterWithSender,
|
||||
};
|
||||
use satrs_core::pus::{EcssTmError, EcssTmErrorWithSend, EcssTmSenderCore};
|
||||
use satrs_core::pus::{EcssTmErrorWithSend, EcssTmSenderCore};
|
||||
use satrs_core::seq_count::{
|
||||
SeqCountProviderSyncClonable, SequenceCountProvider, SequenceCountProviderCore,
|
||||
SeqCountProviderSyncClonable,
|
||||
};
|
||||
use satrs_core::{
|
||||
spacepackets::time::cds::TimeProvider,
|
||||
spacepackets::time::TimeWriter,
|
||||
spacepackets::tm::{PusTm, PusTmSecondaryHeader},
|
||||
spacepackets::{SequenceFlags, SpHeader},
|
||||
spacepackets::tm::{PusTm},
|
||||
};
|
||||
use strum::IntoEnumIterator;
|
||||
|
||||
use crate::can_ids::{can_id_to_package_id, load_package_ids, PackageId, PackageModel, ThreadId};
|
||||
use crate::can_ids::{can_id_to_package_id, DeviceId, load_package_ids, PackageId, PackageModel, ThreadId};
|
||||
use embedded_can::{Id, StandardId};
|
||||
use log::{info, warn};
|
||||
use satrs_core::tmtc::tm_helper::PusTmWithCdsShortHelper;
|
||||
use std::collections::HashMap;
|
||||
use std::net::{IpAddr, SocketAddr};
|
||||
use std::sync::mpsc::{channel, RecvError, TryRecvError};
|
||||
use std::sync::{mpsc, Arc, RwLock};
|
||||
use std::sync::mpsc::{channel};
|
||||
use std::sync::{mpsc, Arc, RwLock, Mutex};
|
||||
use std::thread;
|
||||
use std::time::Duration;
|
||||
use satrs_core::power::{SwitchId, SwitchState};
|
||||
//use libc::time64_t;
|
||||
use crate::action::ActionRequest;
|
||||
use crate::cam::CameraRequest;
|
||||
use crate::pld_handler::{CameraHandler, core_pld_task};
|
||||
use crate::pcdu::{core_power_task, PowerSwitcher};
|
||||
|
||||
#[derive(Clone)]
|
||||
struct EventTmSender {
|
||||
@ -189,17 +189,6 @@ fn main() {
|
||||
// get package id hashmap
|
||||
let package_ids_rx = load_package_ids();
|
||||
|
||||
// checks for packet ids
|
||||
println!("{:?}", package_ids_rx[&PackageId::PCDUStatusRequest]);
|
||||
println!(
|
||||
"{:?}",
|
||||
package_ids_rx[&PackageId::CameraImageRequestConfirmation]
|
||||
);
|
||||
let test = can_id_to_package_id(Id::Standard(StandardId::new(65).expect("Invalid Id")));
|
||||
if let Some(id) = test {
|
||||
println!("{:?}", package_ids_rx[&id]);
|
||||
}
|
||||
|
||||
let socket0 = can::CanRxHandler::new_socket("can0", can_senders, package_ids_rx).unwrap();
|
||||
|
||||
info!("Starting TMTC task");
|
||||
@ -217,6 +206,26 @@ fn main() {
|
||||
}
|
||||
});
|
||||
|
||||
let (pcdu_tx, pcdu_rx) = mpsc::channel::<(SwitchId, SwitchState)>();
|
||||
let pcdu_can_tx =
|
||||
can::CanTxHandler::new_socket("can0", ThreadId::PowerThread, load_package_ids()).unwrap();
|
||||
|
||||
|
||||
let mut device_state_map = HashMap::new();
|
||||
for id in DeviceId::iter() {
|
||||
device_state_map.insert(id, SwitchState::Off);
|
||||
}
|
||||
let clonable_device_state_map = Arc::new(Mutex::new(device_state_map));
|
||||
|
||||
let mut power_switcher = PowerSwitcher::new(pcdu_tx, clonable_device_state_map.clone());
|
||||
|
||||
info!("Starting power task");
|
||||
let builder2 = thread::Builder::new().name("PowerThread".into());
|
||||
let jh2 = builder2.spawn(move || {
|
||||
core_power_task(pcdu_rx, pcdu_can_tx, power_can_rx, clonable_device_state_map.clone());
|
||||
});
|
||||
|
||||
|
||||
let package_map_aocs_tx = load_package_ids();
|
||||
let aocs_tm_funnel_tx = tm_funnel_tx.clone();
|
||||
let mut aocs_tm_store = tm_store.clone();
|
||||
@ -319,106 +328,12 @@ fn main() {
|
||||
|
||||
let PLDCanSocket =
|
||||
can::CanTxHandler::new_socket("can0", ThreadId::PLDThread, package_map_pld_tx).unwrap();
|
||||
|
||||
//let mut pcdu_tx_clone = pcdu_tx.clone();
|
||||
println!("Starting Payload Handling task");
|
||||
let builder3 = thread::Builder::new().name("PLDThread".into());
|
||||
let jh3 = builder3.spawn(move || {
|
||||
let mut time_stamp_buf: [u8; 7] = [0; 7];
|
||||
loop {
|
||||
match pld_thread_rx.try_recv() {
|
||||
Ok(request_with_token) => {
|
||||
match request_with_token.0 {
|
||||
Request::ActionRequest(action_id) => {
|
||||
match action_id {
|
||||
ActionRequest::ImageRequest(target_id) => {
|
||||
assert_eq!(target_id, RequestTargetId::PldSubsystem);
|
||||
// get current time stamp
|
||||
let cds_stamp = TimeProvider::from_now_with_u16_days().unwrap();
|
||||
cds_stamp.write_to_bytes(&mut time_stamp_buf);
|
||||
|
||||
// send start verification and get token
|
||||
let start_token = reporter_pld
|
||||
.start_success(request_with_token.1, Some(&time_stamp_buf))
|
||||
.expect("Error sending start success.");
|
||||
|
||||
// make can bus package to camera
|
||||
let data = [1];
|
||||
PLDCanSocket.tx_socket(PackageId::CameraImageRequest, &data);
|
||||
|
||||
//let timeout = Duration::from_millis(400);
|
||||
|
||||
// loop to allow early exit incase verif never arrives
|
||||
|
||||
// wait for request verification
|
||||
loop {
|
||||
match pld_can_rx.recv() {
|
||||
Ok(msg) => {
|
||||
if msg.package_id()
|
||||
== PackageId::CameraImageRequestConfirmation
|
||||
&& msg.data()[0] == 1
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
Err(_) => {
|
||||
warn!("Error receiving Can Bus Message.");
|
||||
}
|
||||
}
|
||||
}
|
||||
// wait for start of execution
|
||||
loop {
|
||||
match pld_can_rx.recv() {
|
||||
Ok(msg) => {
|
||||
if msg.package_id()
|
||||
== PackageId::CameraImageExecutionStart
|
||||
&& msg.data()[0] == 1
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
Err(_) => {
|
||||
warn!("Error receiving Can Bus Message.");
|
||||
}
|
||||
}
|
||||
}
|
||||
// wait for end of execution
|
||||
loop {
|
||||
match pld_can_rx.recv() {
|
||||
Ok(msg) => {
|
||||
if msg.package_id()
|
||||
== PackageId::CameraImageExectutionEnd
|
||||
&& msg.data()[0] == 1
|
||||
{
|
||||
let cds_stamp =
|
||||
TimeProvider::from_now_with_u16_days()
|
||||
.unwrap();
|
||||
cds_stamp.write_to_bytes(&mut time_stamp_buf);
|
||||
|
||||
// send end verification with token
|
||||
reporter_pld
|
||||
.completion_success(
|
||||
start_token,
|
||||
Some(&time_stamp_buf),
|
||||
)
|
||||
.expect("Error sending start success.");
|
||||
break;
|
||||
}
|
||||
}
|
||||
Err(_) => {
|
||||
warn!("Error receiving Can Bus Message.");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
ActionRequest::OrientationRequest(_) => {}
|
||||
_ => {}
|
||||
}
|
||||
}
|
||||
_ => {}
|
||||
}
|
||||
}
|
||||
Err(_) => {}
|
||||
}
|
||||
}
|
||||
core_pld_task(power_switcher.clone(), pld_thread_rx, pld_can_rx, &mut reporter_pld);
|
||||
});
|
||||
|
||||
println!("Starting TM funnel task");
|
||||
@ -444,15 +359,9 @@ fn main() {
|
||||
jh1.unwrap()
|
||||
.join()
|
||||
.expect("Joining CAN Bus Listening thread failed");
|
||||
//jh2.unwrap().join().expect("Joing AOCS thread failed");
|
||||
jh3.unwrap().join().expect("Joing AOCS thread failed");
|
||||
jh4.unwrap().join().expect("Joing AOCS thread failed");
|
||||
|
||||
/*
|
||||
jh1.join().expect("Joining TM Funnel thread failed");
|
||||
jh2.join().expect("Joining Event Manager thread failed");
|
||||
jh3.join().expect("Joining AOCS thread failed");
|
||||
*/
|
||||
jh2.unwrap().join().expect("Joining power thread failed");
|
||||
jh3.unwrap().join().expect("Joining PLD thread failed");
|
||||
jh4.unwrap().join().expect("Joining TM funnel thread failed");
|
||||
}
|
||||
#[derive(Default)]
|
||||
struct MgmData {
|
||||
|
||||
225
src/pcdu.rs
Normal file
225
src/pcdu.rs
Normal file
@ -0,0 +1,225 @@
|
||||
use std::collections::HashMap;
|
||||
use std::sync::mpsc::{Receiver, Sender};
|
||||
use satrs_core::power::{PowerSwitcherCommandSender, PowerSwitchInfo, PowerSwitchProvider, SwitchId, SwitchState};
|
||||
use crate::can::{CanTxHandler};
|
||||
use crate::can_ids::{DeviceId, PackageId, PackageModel};
|
||||
use std::convert::TryFrom;
|
||||
use std::sync::{Arc, Mutex};
|
||||
use std::time::Duration;
|
||||
use std::vec;
|
||||
use log::info;
|
||||
pub use strum::IntoEnumIterator; // 0.17.1
|
||||
pub use strum_macros::EnumIter; // 0.17.1
|
||||
|
||||
#[derive(Copy, Clone, Debug, PartialEq, EnumIter)]
|
||||
pub enum DeviceState {
|
||||
On,
|
||||
Off,
|
||||
SwitchingPower,
|
||||
Setup,
|
||||
Idle,
|
||||
}
|
||||
|
||||
#[derive(Clone)]
|
||||
pub struct PowerSwitcher {
|
||||
switch_tx: Sender<(SwitchId, SwitchState)>,
|
||||
device_state_map: Arc<Mutex<HashMap<DeviceId, SwitchState>>>,
|
||||
}
|
||||
|
||||
pub struct PCDU {
|
||||
switch_rx: Receiver<(SwitchId, SwitchState)>,
|
||||
can_tx: CanTxHandler,
|
||||
can_rx: Receiver<PackageModel>,
|
||||
device_state_map: Arc<Mutex<HashMap<DeviceId, SwitchState>>>,
|
||||
}
|
||||
|
||||
impl PowerSwitcher {
|
||||
pub fn new(switch_tx: Sender<(SwitchId, SwitchState)>, device_state_map: Arc<Mutex<HashMap<DeviceId, SwitchState>>>) -> PowerSwitcher {
|
||||
PowerSwitcher{switch_tx, device_state_map}
|
||||
}
|
||||
}
|
||||
|
||||
impl PowerSwitcherCommandSender for PowerSwitcher {
|
||||
type Error = ();
|
||||
|
||||
fn send_switch_on_cmd(&mut self, switch_id: SwitchId) -> Result<(), Self::Error> {
|
||||
return match self.switch_tx.send((switch_id, SwitchState::On)) {
|
||||
Ok(_) => {Ok(())}
|
||||
Err(_) => {Err(())}
|
||||
}
|
||||
}
|
||||
|
||||
fn send_switch_off_cmd(&mut self, switch_id: SwitchId) -> Result<(), Self::Error> {
|
||||
return match self.switch_tx.send((switch_id, SwitchState::Off)) {
|
||||
Ok(_) => {Ok(())}
|
||||
Err(_) => {Err(())}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl PowerSwitchInfo for PowerSwitcher {
|
||||
type Error = ();
|
||||
|
||||
fn get_switch_state(&mut self, switch_id: SwitchId) -> Result<SwitchState, Self::Error> {
|
||||
let map_locked = self.device_state_map.lock().unwrap();
|
||||
if let Ok(dev_id) = DeviceId::try_from(switch_id) {
|
||||
return if let Some(state) = map_locked.get(&dev_id) {
|
||||
Ok(*state)
|
||||
} else {
|
||||
Err(())
|
||||
}
|
||||
} else {
|
||||
Err(())
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
fn switch_delay_ms(&self) -> u32 {
|
||||
0
|
||||
}
|
||||
}
|
||||
|
||||
impl PowerSwitchProvider for PowerSwitcher {
|
||||
type Error = ();
|
||||
}
|
||||
|
||||
impl PCDU {
|
||||
pub fn new(switch_rx: Receiver<(SwitchId, SwitchState)>, can_tx: CanTxHandler, can_rx: Receiver<PackageModel>, device_state_map: Arc<Mutex<HashMap<DeviceId, SwitchState>>>) -> PCDU{
|
||||
PCDU{switch_rx, can_tx, can_rx, device_state_map}
|
||||
}
|
||||
|
||||
pub fn send_power_on(&mut self, switch_id: SwitchId) -> Result<(), ()> {
|
||||
return if let Ok(dev_id) = DeviceId::try_from(switch_id) {
|
||||
let dev_id_bytes = dev_id as u8;
|
||||
let buf: &[u8] = &dev_id_bytes.to_be_bytes();
|
||||
self.can_tx.tx_socket(PackageId::DevicePowerOnRequest, buf);
|
||||
let mut map_lock = self.device_state_map.lock().unwrap();
|
||||
*map_lock.get_mut(&dev_id).unwrap() = SwitchState::Unknown;
|
||||
self.can_rx.recv();
|
||||
self.can_rx.recv();
|
||||
Ok(())
|
||||
} else {
|
||||
Err(())
|
||||
}
|
||||
}
|
||||
|
||||
pub fn send_power_off(&mut self, switch_id: SwitchId) -> Result<(), ()> {
|
||||
return if let Ok(dev_id) = DeviceId::try_from(switch_id) {
|
||||
let dev_id_bytes = dev_id as u8;
|
||||
let buf: &[u8] = &dev_id_bytes.to_be_bytes();
|
||||
self.can_tx.tx_socket(PackageId::DevicePowerOffRequest, buf);
|
||||
let mut map_lock = self.device_state_map.lock().unwrap();
|
||||
*map_lock.get_mut(&dev_id).unwrap() = SwitchState::Unknown;
|
||||
self.can_rx.recv();
|
||||
self.can_rx.recv();
|
||||
Ok(())
|
||||
} else {
|
||||
Err(())
|
||||
}
|
||||
}
|
||||
|
||||
pub fn update_states_helper(&mut self, dev_id: &DeviceId) -> Result<(), ()> {
|
||||
let switch_id: SwitchId = *dev_id as u16;
|
||||
let dev_id_bytes = *dev_id as u8;
|
||||
let buf: &[u8] = &dev_id_bytes.to_be_bytes();
|
||||
self.can_tx.tx_socket(PackageId::DevicePowerStatusRequest, buf);
|
||||
match self.can_rx.recv_timeout(Duration::from_secs(10)) {
|
||||
Ok(msg) => {
|
||||
if msg.package_id() == PackageId::DevicePowerStatusResponse && msg.data()[0] == dev_id_bytes{
|
||||
info!("received power status response");
|
||||
let mut map_lock = self.device_state_map.lock().unwrap();
|
||||
let mut state: SwitchState;
|
||||
match msg.data()[1] {
|
||||
0 => {
|
||||
state = SwitchState::Off
|
||||
},
|
||||
1 => {
|
||||
state = SwitchState::On
|
||||
},
|
||||
2 => {
|
||||
state = SwitchState::Faulty
|
||||
},
|
||||
_ => {
|
||||
return Err(());
|
||||
}
|
||||
}
|
||||
*map_lock.get_mut(&dev_id).unwrap() = state;
|
||||
info!("{:?}", map_lock);
|
||||
Ok(())
|
||||
} else {
|
||||
Err(())
|
||||
}
|
||||
}
|
||||
Err(_) => {
|
||||
Err(())
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
pub fn update_all_states_helper(&mut self) -> Result<(), ()> {
|
||||
let mut map_lock = self.device_state_map.lock().unwrap();
|
||||
let mut device_list = vec::Vec::new();
|
||||
for key in map_lock.keys() {
|
||||
device_list.push(key.clone());
|
||||
}
|
||||
drop(map_lock);
|
||||
for dev_id in device_list {
|
||||
self.update_states_helper(&dev_id)?;
|
||||
}
|
||||
Ok(())
|
||||
}
|
||||
|
||||
pub fn update_switch_states(&mut self, switch_id: SwitchId) -> Result<(), ()> {
|
||||
return if let Ok(dev_id) = DeviceId::try_from(switch_id) {
|
||||
match dev_id {
|
||||
DeviceId::All => {
|
||||
self.update_all_states_helper()
|
||||
}
|
||||
_ => {
|
||||
self.update_states_helper(&dev_id)
|
||||
}
|
||||
}
|
||||
} else {
|
||||
Err(())
|
||||
}
|
||||
}
|
||||
|
||||
pub fn handle_power_requests(&mut self) -> Result<u16, ()>{
|
||||
let mut i = 0;
|
||||
while let Ok((switch_id, switch_state)) = self.switch_rx.recv() {
|
||||
match switch_state {
|
||||
SwitchState::Off => {
|
||||
match self.send_power_off(switch_id) {
|
||||
Ok(_) => {
|
||||
i = i + 1;
|
||||
}
|
||||
Err(_) => {
|
||||
return Err(());
|
||||
}
|
||||
}
|
||||
}
|
||||
SwitchState::On => {
|
||||
match self.send_power_on(switch_id) {
|
||||
Ok(_) => {
|
||||
i = i + 1;
|
||||
}
|
||||
Err(_) => {
|
||||
return Err(());
|
||||
}
|
||||
}
|
||||
}
|
||||
_ => {}
|
||||
}
|
||||
self.update_switch_states(switch_id);
|
||||
}
|
||||
return Ok(i);
|
||||
}
|
||||
}
|
||||
|
||||
pub fn core_power_task(switch_rx: Receiver<(SwitchId, SwitchState)>, can_tx: CanTxHandler, can_rx: Receiver<PackageModel>, device_state_map: Arc<Mutex<HashMap<DeviceId, SwitchState>>>) {
|
||||
let mut pcdu = PCDU::new(switch_rx, can_tx, can_rx, device_state_map);
|
||||
loop{
|
||||
pcdu.handle_power_requests().unwrap();
|
||||
}
|
||||
}
|
||||
171
src/pld_handler.rs
Normal file
171
src/pld_handler.rs
Normal file
@ -0,0 +1,171 @@
|
||||
use std::sync::mpsc;
|
||||
use std::sync::mpsc::{Receiver, Sender, TryRecvError};
|
||||
use log::info;
|
||||
use satrs_core::power::{PowerSwitcherCommandSender, PowerSwitchInfo, SwitchId, SwitchState};
|
||||
use satrs_core::pus::verification::{StdVerifSenderError, VerificationReporterWithSender};
|
||||
use satrs_core::spacepackets::time::cds::TimeProvider;
|
||||
use satrs_core::spacepackets::time::TimeWriter;
|
||||
use eurosim_obsw::RequestTargetId;
|
||||
use crate::action::ActionRequest;
|
||||
use crate::can::CanTxHandler;
|
||||
use crate::can_ids::{DeviceId, load_package_ids, PackageId, PackageModel, ThreadId};
|
||||
use crate::pld_handler::CameraMode::PictureRequest;
|
||||
use crate::pcdu::{DeviceState, PowerSwitcher};
|
||||
use crate::requests::{Request, RequestWithToken};
|
||||
|
||||
#[derive(Debug, PartialEq, Copy, Clone)]
|
||||
pub enum CameraMode {
|
||||
Idle,
|
||||
PictureRequest,
|
||||
Verification,
|
||||
Start,
|
||||
End,
|
||||
}
|
||||
|
||||
pub struct CameraHandler {
|
||||
power_switcher: PowerSwitcher,
|
||||
camera_device_id: DeviceId,
|
||||
camera_switch_id: SwitchId,
|
||||
device_state: DeviceState,
|
||||
can_tx: CanTxHandler,
|
||||
can_rx: Receiver<PackageModel>,
|
||||
mode: CameraMode,
|
||||
mode_rx: Receiver<CameraMode>,
|
||||
}
|
||||
|
||||
impl CameraHandler {
|
||||
pub fn new(power_switcher: PowerSwitcher, camera_device_id: DeviceId, can_tx: CanTxHandler, can_rx: Receiver<PackageModel>, mode_rx: Receiver<CameraMode>) -> CameraHandler {
|
||||
let camera_switch_id = camera_device_id as u16;
|
||||
CameraHandler{power_switcher, camera_device_id, camera_switch_id, device_state: DeviceState::Off, can_tx, can_rx, mode: CameraMode::Idle, mode_rx}
|
||||
}
|
||||
|
||||
pub fn set_mode(&mut self, mode: CameraMode) {
|
||||
if self.mode == CameraMode::Idle{
|
||||
if mode == CameraMode::PictureRequest {
|
||||
self.mode = PictureRequest;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub fn get_mode(&self) -> CameraMode {
|
||||
self.mode
|
||||
}
|
||||
|
||||
pub fn periodic_op(&mut self) {
|
||||
// Camera Device Handler State Machine
|
||||
|
||||
match self.mode {
|
||||
CameraMode::Idle => {}
|
||||
CameraMode::PictureRequest => {
|
||||
if self.device_state == DeviceState::Off {
|
||||
self.power_switcher.send_switch_on_cmd(self.camera_switch_id).expect("sending switch cmd failed");
|
||||
self.device_state = DeviceState::SwitchingPower;
|
||||
info!("switching power");
|
||||
}
|
||||
if self.device_state == DeviceState::SwitchingPower {
|
||||
if self.power_switcher.get_is_switch_on(self.camera_switch_id).expect("reading switch state failed") {
|
||||
self.device_state = DeviceState::On;
|
||||
info!("device on");
|
||||
}
|
||||
}
|
||||
if self.device_state == DeviceState::On {
|
||||
self.can_tx.tx_socket(PackageId::CameraImageRequest, &[1]);
|
||||
info!("sent camera request");
|
||||
self.mode = CameraMode::Verification;
|
||||
}
|
||||
}
|
||||
CameraMode::Verification => {
|
||||
if self.device_state == DeviceState::On {
|
||||
info!("waiting for image request confirmation");
|
||||
if let Ok(msg) = self.can_rx.recv() {
|
||||
if msg.package_id() == PackageId::CameraImageRequestConfirmation {
|
||||
self.mode = CameraMode::Start;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
CameraMode::Start => {
|
||||
if self.device_state == DeviceState::On {
|
||||
info!("waiting for image start confirmation");
|
||||
if let Ok(msg) = self.can_rx.recv() {
|
||||
if msg.package_id() == PackageId::CameraImageExecutionStart {
|
||||
self.mode = CameraMode::End;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
CameraMode::End => {
|
||||
if self.device_state == DeviceState::On {
|
||||
info!("waiting for image end confirmation");
|
||||
if let Ok(msg) = self.can_rx.recv() {
|
||||
if msg.package_id() == PackageId::CameraImageExectutionEnd {
|
||||
self.power_switcher.send_switch_off_cmd(self.camera_switch_id).expect("sending switch command failed");
|
||||
self.device_state = DeviceState::SwitchingPower;
|
||||
info!("switching power");
|
||||
}
|
||||
}
|
||||
}
|
||||
if self.device_state == DeviceState::SwitchingPower {
|
||||
if !self.power_switcher.get_is_switch_on(self.camera_switch_id).expect("reading switch state failed") {
|
||||
self.device_state = DeviceState::Off;
|
||||
info!("device off");
|
||||
}
|
||||
}
|
||||
if self.device_state == DeviceState::Off {
|
||||
self.mode = CameraMode::Idle;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub fn core_pld_task(power_switcher: PowerSwitcher, pld_thread_rx: Receiver<RequestWithToken>, pld_can_rx: Receiver<PackageModel>, reporter_pld: &mut VerificationReporterWithSender<StdVerifSenderError>) {
|
||||
let (camera_mode_tx, camera_mode_rx) = mpsc::channel();
|
||||
let camera_can_tx =
|
||||
CanTxHandler::new_socket("can0", ThreadId::PLDThread, load_package_ids()).unwrap();
|
||||
|
||||
let mut camera_handler = CameraHandler::new(power_switcher, DeviceId::Camera, camera_can_tx, pld_can_rx, camera_mode_rx);
|
||||
|
||||
let mut time_stamp_buf: [u8; 7] = [0; 7];
|
||||
loop {
|
||||
match pld_thread_rx.try_recv() {
|
||||
Ok(request_with_token) => {
|
||||
match request_with_token.0 {
|
||||
Request::ActionRequest(action_id) => {
|
||||
match action_id {
|
||||
ActionRequest::ImageRequest(target_id) => {
|
||||
assert_eq!(target_id, RequestTargetId::PldSubsystem);
|
||||
camera_handler.set_mode(CameraMode::PictureRequest);
|
||||
|
||||
let cds_stamp = TimeProvider::from_now_with_u16_days().unwrap();
|
||||
cds_stamp.write_to_bytes(&mut time_stamp_buf).unwrap();
|
||||
// send start verification and get token
|
||||
let start_token = reporter_pld
|
||||
.start_success(request_with_token.1, Some(&time_stamp_buf))
|
||||
.expect("Error sending start success.");
|
||||
|
||||
info!("{:?}", camera_handler.get_mode());
|
||||
while camera_handler.get_mode() != CameraMode::Idle {
|
||||
camera_handler.periodic_op();
|
||||
}
|
||||
|
||||
// send end verification with token
|
||||
reporter_pld
|
||||
.completion_success(
|
||||
start_token,
|
||||
Some(&time_stamp_buf),
|
||||
)
|
||||
.expect("Error sending start success.");
|
||||
}
|
||||
ActionRequest::OrientationRequest(_) => {}
|
||||
ActionRequest::PointingRequest(_) => {}
|
||||
}
|
||||
}
|
||||
_ => {}
|
||||
}
|
||||
}
|
||||
Err(_) => {}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
20
src/pus.rs
20
src/pus.rs
@ -1,10 +1,10 @@
|
||||
use crate::hk::{CollectionIntervalFactor, HkRequest};
|
||||
use crate::requests::{Request, RequestWithToken};
|
||||
use crate::tmtc::{PusTcSource, TmStore};
|
||||
use eurosim_obsw::{hk_err, tmtc_err, RequestTargetId};
|
||||
use eurosim_obsw::{hk_err, tmtc_err};
|
||||
use satrs_core::events::EventU32;
|
||||
use satrs_core::pool::StoreAddr;
|
||||
use satrs_core::pus::event::Subservices;
|
||||
use satrs_core::pus::event::Subservice;
|
||||
use satrs_core::pus::event_man::{EventRequest, EventRequestWithToken};
|
||||
use satrs_core::pus::hk;
|
||||
use satrs_core::pus::verification::{
|
||||
@ -14,7 +14,7 @@ use satrs_core::res_code::ResultU16;
|
||||
use satrs_core::tmtc::tm_helper::PusTmWithCdsShortHelper;
|
||||
use satrs_core::tmtc::{AddressableId, PusServiceProvider};
|
||||
use satrs_core::{
|
||||
spacepackets, spacepackets::ecss::PusPacket, spacepackets::tc::PusTc,
|
||||
spacepackets::ecss::PusPacket, spacepackets::tc::PusTc,
|
||||
spacepackets::time::cds::TimeProvider, spacepackets::time::TimeWriter, spacepackets::SpHeader,
|
||||
};
|
||||
use std::cell::RefCell;
|
||||
@ -23,7 +23,6 @@ use crate::action;
|
||||
use crate::action::ActionRequest;
|
||||
use eurosim_obsw::RequestTargetId::{AcsSubsystem, PldSubsystem};
|
||||
use satrs_core::pus::scheduling::PusScheduler;
|
||||
use satrs_core::spacepackets::ecss::PusServiceId::Action;
|
||||
use std::collections::HashMap;
|
||||
use std::rc::Rc;
|
||||
use std::sync::mpsc::Sender;
|
||||
@ -188,13 +187,14 @@ impl PusReceiver {
|
||||
.send(RequestWithToken(Request::HkRequest(request), token))
|
||||
.unwrap_or_else(|_| panic!("Sending HK request {:?} failed", request));
|
||||
};
|
||||
if PusPacket::subservice(pus_tc) == hk::Subservice::TcEnableGeneration as u8 {
|
||||
if PusPacket::subservice(pus_tc) == hk::Subservice::TcEnableHkGeneration as u8 {
|
||||
send_request(HkRequest::Enable(addressable_id));
|
||||
} else if PusPacket::subservice(pus_tc) == hk::Subservice::TcDisableGeneration as u8 {
|
||||
} else if PusPacket::subservice(pus_tc) == hk::Subservice::TcDisableHkGeneration as u8 {
|
||||
send_request(HkRequest::Disable(addressable_id));
|
||||
} else if PusPacket::subservice(pus_tc) == hk::Subservice::TcGenerateOneShotHk as u8 {
|
||||
send_request(HkRequest::OneShot(addressable_id));
|
||||
} else if PusPacket::subservice(pus_tc) == hk::Subservice::TcModifyCollectionInterval as u8
|
||||
} else if PusPacket::subservice(pus_tc)
|
||||
== hk::Subservice::TcModifyHkCollectionInterval as u8
|
||||
{
|
||||
if user_data.len() < 12 {
|
||||
self.update_time_stamp();
|
||||
@ -257,7 +257,7 @@ impl PusReceiver {
|
||||
}
|
||||
let event_id = EventU32::from(u32::from_be_bytes(app_data.try_into().unwrap()));
|
||||
match PusPacket::subservice(pus_tc).try_into() {
|
||||
Ok(Subservices::TcEnableEventGeneration) => {
|
||||
Ok(Subservice::TcEnableEventGeneration) => {
|
||||
self.update_time_stamp();
|
||||
let start_token = send_start_acceptance(&mut self.verif_reporter, &self.time_stamp);
|
||||
self.event_request_tx
|
||||
@ -267,7 +267,7 @@ impl PusReceiver {
|
||||
})
|
||||
.expect("Sending event request failed");
|
||||
}
|
||||
Ok(Subservices::TcDisableEventGeneration) => {
|
||||
Ok(Subservice::TcDisableEventGeneration) => {
|
||||
self.update_time_stamp();
|
||||
let start_token = send_start_acceptance(&mut self.verif_reporter, &self.time_stamp);
|
||||
self.event_request_tx
|
||||
@ -417,8 +417,6 @@ impl PusReceiver {
|
||||
scheduler
|
||||
.insert_wrapped_tc::<TimeProvider>(pus_tc, pool.as_mut())
|
||||
.expect("TODO: panic message");
|
||||
let time =
|
||||
TimeProvider::from_bytes_with_u16_days(&pus_tc.user_data().unwrap()).unwrap();
|
||||
drop(scheduler);
|
||||
|
||||
self.verif_reporter
|
||||
|
||||
@ -1,5 +1,4 @@
|
||||
use crate::action::ActionRequest;
|
||||
use crate::cam::CameraRequest;
|
||||
use crate::hk::HkRequest;
|
||||
use satrs_core::pus::verification::{TcStateAccepted, VerificationToken};
|
||||
|
||||
|
||||
@ -8,7 +8,6 @@ use std::fmt::{Display, Formatter};
|
||||
use std::net::SocketAddr;
|
||||
use std::rc::Rc;
|
||||
use std::sync::mpsc::{Receiver, SendError, Sender, TryRecvError};
|
||||
use std::sync::{Arc, LockResult, Mutex};
|
||||
use std::thread;
|
||||
use std::time::Duration;
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user