rust/eurosim-obsw
6
1
Fork 0
Code Issues Pull Requests Packages Projects Releases 1 Wiki Activity

finished seq count, aocs handlers, housekeeper, maybe other stuff

Browse Source
This commit is contained in:
lkoester 2023-03-17 11:09:17 +01:00
parent 8ceb2c7a79
commit 51471c0a2d
8 changed files with 864894 additions and 336 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

863835
output.log
View File

File diff suppressed because it is too large Load Diff

56
pyclient/main copy.py
View File

@ -9,6 +9,7 @@ from typing import Optional
import datetime
import json
import pprint
import struct
import tmtccmd
from spacepackets.ecss import PusTelemetry, PusTelecommand, PusVerificator
@ -17,7 +18,7 @@ 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_3_fsfw_hk import generate_one_hk_command, make_sid, create_enable_periodic_hk_command, create_disable_periodic_hk_command
from tmtccmd.tc.pus_11_tc_sched import create_time_tagged_cmd
from tmtccmd.core.base import BackendRequest
from tmtccmd.pus import VerificationWrapper
@ -56,7 +57,7 @@ from tmtccmd.util.tmtc_printer import FsfwTmTcPrinter
_LOGGER = logging.getLogger(__name__)
EXAMPLE_PUS_APID = 0x02
EXAMPLE_PUS_APID = 0x50
class SatRsConfigHook(HookBase):
@ -96,6 +97,8 @@ class SatRsConfigHook(HookBase):
)
srv_3 = OpCodeEntry()
srv_3.add(HkOpCodes.GENERATE_ONE_SHOT, "Generate AOCS one shot HK")
srv_3.add(HkOpCodes.ENABLE_PERIODIC, "Enable Periodic AOCS HK")
srv_3.add(HkOpCodes.DISABLE_PERIODIC, "Disable Periodic AOCS HK")
defs.add_service(
name=CoreServiceList.SERVICE_3,
info="PUS Service 3 Housekeeping",
@ -118,6 +121,15 @@ class SatRsConfigHook(HookBase):
info="PUS Service 11 TC Scheduling",
op_code_entry=srv_11,
)
srv_200 = OpCodeEntry()
srv_200.add("0", "AOCS Mode Idle")
srv_200.add("1", "AOCS Mode On")
defs.add_service(
name = CoreServiceList.SERVICE_200,
info="PUS Custom Service 200 Mode",
op_code_entry=srv_200,
)
return defs
def perform_mode_operation(self, tmtc_backend: CcsdsTmtcBackend, mode: int):
@ -178,9 +190,17 @@ class PusHandler(SpecificApidHandlerBase):
if pus_tm.subservice == 25:
if len(pus_tm.source_data) < 8:
raise ValueError("No addressable ID in HK packet")
json_str = str(pus_tm.source_data[8:].decode('utf8'))
json_object = json.loads(json_str)
pprint.pprint(json_object)
#json_str = str(pus_tm.source_data[8:].decode('utf8'))
#json_object = json.loads(json_str)
#pprint.pprint(json_object)
print(pus_tm.tm_data.hex())
i = 8
print(struct.unpack('d', pus_tm.tm_data[8:16]))
while i < len(pus_tm.tm_data):
print(pus_tm.tm_data[i:i+8].hex())
print(struct.unpack('d', pus_tm.tm_data[i:i+8]))
i += 8
dedicated_handler = True
if service == 5:
tm_packet = Service5Tm.unpack(packet, time_reader=CdsShortTimestamp.empty())
@ -242,12 +262,19 @@ class AcsHkIds(enum.IntEnum):
class HkOpCodes:
GENERATE_ONE_SHOT = ["0", "oneshot"]
ENABLE_PERIODIC = ["1", "enable periodic"]
DISABLE_PERIODIC = ["2", "disable periodic"]
def make_target_id(target_id: int) -> bytes:
byte_string = bytearray(struct.pack("!I", target_id))
return byte_string
def make_mode_submode(mode: int, submode: int) -> bytes:
byte_string = bytearray(struct.pack("!I", mode))
byte_string.extend(struct.pack("!I", submode))
return byte_string
class TcHandler(TcHandlerBase):
def __init__(
@ -294,6 +321,15 @@ class TcHandler(TcHandlerBase):
def_proc = helper.to_def_procedure()
service = def_proc.service
op_code = def_proc.op_code
if service == CoreServiceList.SERVICE_200:
q.add_log_cmd("Sending PUS Mode Request telecommand")
if op_code == "0":
tc = PusTelecommand(service=200, subservice=1, app_data=make_mode_submode(0,0), apid=0x50)
return q.add_pus_tc(tc)
if op_code == "1":
tc = PusTelecommand(service=200, subservice=1, app_data=make_mode_submode(1,0), apid=0x50)
return q.add_pus_tc(tc)
if (
service == CoreServiceList.SERVICE_17
or service == CoreServiceList.SERVICE_17_ALT
@ -346,6 +382,16 @@ class TcHandler(TcHandlerBase):
make_addressable_id(RequestTargetId.ACS, AcsHkIds.MGM_SET)
)
)
if op_code in HkOpCodes.ENABLE_PERIODIC:
q.add_log_cmd("Sending periodic HK request")
tc = create_enable_periodic_hk_command(False, make_addressable_id(RequestTargetId.ACS, AcsHkIds.MGM_SET))
q.add_log_cmd(tc)
q.add_pus_tc(tc)
if op_code in HkOpCodes.DISABLE_PERIODIC:
q.add_log_cmd("Sending periodic HK request")
tc = create_disable_periodic_hk_command(False, make_addressable_id(RequestTargetId.ACS, AcsHkIds.MGM_SET))
q.add_log_cmd(tc)
q.add_pus_tc(tc)
pass

821
src/aocs.rs
View File

@ -1,32 +1,36 @@
use crate::action::ActionRequest;
use crate::aocs::AOCSSensorMode::{Idle, SendingData};
use crate::can_ids::PackageId::AOCSDataStarTracker1;
use crate::can_ids::{DeviceId, PackageId, PackageModel};
use crate::hk::{AOCSSensorData, AocsHousekeeper};
use crate::helpers::{ModeHelper, VerifHelper};
use crate::hk::{AocsDataMap, AocsDataType, AocsHousekeeper, CSS_VOLTAGE_1, CSS_VOLTAGE_2, CSS_VOLTAGE_3, CSS_VOLTAGE_4, CSS_VOLTAGE_5, CSS_VOLTAGE_6, MGM_VOLTAGE_1, MGM_VOLTAGE_2, MGM_VOLTAGE_3, STR_QUATERNION_1, STR_QUATERNION_2, STR_QUATERNION_3, STR_QUATERNION_4};
use crate::power_handler::{DeviceState, PowerSwitcher};
use crate::requests::{Request, RequestWithToken};
use crate::tmtc::TmStore;
use byteorder::{ByteOrder, LittleEndian};
use num_derive::ToPrimitive;
use satrs_core::hk::HkRequest;
use satrs_core::mode::ModeRequest;
use satrs_core::mode::{ModeAndSubmode, ModeRequest};
use satrs_core::pool::StoreAddr;
use satrs_core::power::SwitchId;
use satrs_core::power::{PowerSwitcherCommandSender, SwitchId};
use satrs_core::pus::verification::{
TcStateAccepted, TcStateNone, VerificationReporterWithSender, VerificationToken,
};
use satrs_core::pus::MpscPusInStoreSendError;
use satrs_core::seq_count::SeqCountProviderSyncClonable;
use satrs_core::spacepackets::time::cds::TimeProvider;
use serde::{Deserialize, Serialize};
use std::sync::mpsc::{channel, Receiver, Sender};
use std::sync::mpsc::{channel, Receiver, Sender, TryRecvError};
use std::sync::{Arc, Mutex};
use std::u32;
#[derive(ToPrimitive)]
#[derive(ToPrimitive, PartialEq, Copy, Clone)]
pub enum AOCSSensorMode {
Idle = 0,
SendingData = 1,
}
pub trait AOCSSensorHandler {
pub trait AocsSensorHandler {
type Error;
fn get_package_id(&mut self) -> Result<PackageId, Self::Error>;
@ -48,138 +52,21 @@ pub trait AOCSSensorHandler {
}
}
#[derive(Debug, Copy, Clone, Default, Serialize, Deserialize)]
pub struct MGMData {
axis_1: f64,
axis_2: f64,
axis_3: f64,
}
impl MGMData {
pub fn from_floats(axis_1: f64, axis_2: f64, axis_3: f64) -> MGMData {
MGMData {
axis_1,
axis_2,
axis_3,
}
}
pub fn new() -> MGMData {
MGMData {
axis_1: 0.0,
axis_2: 0.0,
axis_3: 0.0,
}
}
pub fn update(&mut self, axis_1: f64, axis_2: f64, axis_3: f64) {
self.axis_1 = axis_1;
self.axis_2 = axis_2;
self.axis_3 = axis_3;
}
pub fn to_array(&self) -> [f64; 3] {
[self.axis_1, self.axis_2, self.axis_3]
}
pub fn to_tuple(&self) -> (f64, f64, f64) {
(self.axis_1, self.axis_2, self.axis_3)
}
}
#[derive(Debug, Copy, Clone, Default, Serialize, Deserialize)]
pub struct CSSData {
voltage_1: f64,
voltage_2: f64,
voltage_3: f64,
voltage_4: f64,
voltage_5: f64,
voltage_6: f64,
}
impl CSSData {
pub fn from_floats(
voltage_1: f64,
voltage_2: f64,
voltage_3: f64,
voltage_4: f64,
voltage_5: f64,
voltage_6: f64,
) -> CSSData {
CSSData {
voltage_1,
voltage_2,
voltage_3,
voltage_4,
voltage_5,
voltage_6,
}
}
pub fn new() -> CSSData {
CSSData {
voltage_1: 0.0,
voltage_2: 0.0,
voltage_3: 0.0,
voltage_4: 0.0,
voltage_5: 0.0,
voltage_6: 0.0,
}
}
pub fn update(
&mut self,
voltage_1: f64,
voltage_2: f64,
voltage_3: f64,
voltage_4: f64,
voltage_5: f64,
voltage_6: f64,
) {
self.voltage_1 = voltage_1;
self.voltage_2 = voltage_2;
self.voltage_3 = voltage_3;
self.voltage_4 = voltage_4;
self.voltage_5 = voltage_5;
self.voltage_6 = voltage_6;
}
pub fn to_array(&self) -> [f64; 6] {
[
self.voltage_1,
self.voltage_2,
self.voltage_3,
self.voltage_4,
self.voltage_5,
self.voltage_6,
]
}
pub fn to_tuple(&self) -> (f64, f64, f64, f64, f64, f64) {
(
self.voltage_1,
self.voltage_2,
self.voltage_3,
self.voltage_4,
self.voltage_5,
self.voltage_6,
)
}
}
pub struct CSSHandler {
power_switcher: PowerSwitcher,
device_id: DeviceId,
switch_id: SwitchId,
device_state: DeviceState,
device_mode: AOCSSensorMode,
css_data: Arc<Mutex<CSSData>>,
mode: AOCSSensorMode,
aocs_data: Arc<Mutex<AocsDataMap>>,
can_tx: Sender<PackageModel>,
can_rx: Receiver<PackageModel>,
request_rx: Receiver<RequestWithToken>,
verif_helper: VerifHelper,
mode_helper: ModeHelper,
}
impl AOCSSensorHandler for CSSHandler {
impl AocsSensorHandler for CSSHandler {
type Error = ();
fn get_package_id(&mut self) -> Result<PackageId, Self::Error> {
@ -198,73 +85,164 @@ impl CSSHandler {
pub fn new(
power_switcher: PowerSwitcher,
device_id: DeviceId,
switch_id: SwitchId,
device_state: DeviceState,
device_mode: AOCSSensorMode,
css_data: Arc<Mutex<CSSData>>,
aocs_data: Arc<Mutex<AocsDataMap>>,
can_tx: Sender<PackageModel>,
can_rx: Receiver<PackageModel>,
request_rx: Receiver<RequestWithToken>,
) -> CSSHandler {
CSSHandler {
verif_helper: VerifHelper,
mode_helper: ModeHelper,
) -> Self {
let switch_id = device_id as u16;
Self {
power_switcher,
device_id,
switch_id,
device_state,
device_mode,
css_data: Arc::new(Mutex::new(CSSData::default())),
mode: Idle,
aocs_data,
can_tx,
can_rx,
request_rx,
verif_helper,
mode_helper,
}
}
pub fn get_data_ref(&mut self) -> Arc<Mutex<CSSData>> {
self.css_data.clone()
pub fn periodic_op(&mut self) {
self.handle_requests();
self.read_can();
}
pub fn css_core_task(&mut self) {
self.handle_request_messages();
}
fn handle_requests(&mut self) {
if let Ok(req) = self.request_rx.try_recv() {
let (req, start_token) = self
.verif_helper
.start_and_unwrap(req)
.expect("error sending start of execution");
match req {
Request::HkRequest(_) => {
if let Some(token) = start_token {
self.verif_helper
.completion_failure(token)
.expect("error sending completion failure");
}
}
Request::ModeRequest(req) => match req {
ModeRequest::SetMode(mode_submode) => {
if let Some(aocs_mode) = mode_submode_to_aocs_mode(mode_submode) {
self.mode = aocs_mode;
}
fn handle_request_messages(&mut self) {
let request = self.request_rx.try_recv().unwrap();
let token = request.1.unwrap();
match request.0 {
Request::HkRequest(_) => {}
Request::ModeRequest(request) => {
self.handle_mode_request(request, token);
if let Some(token) = start_token {
self.verif_helper
.completion(token)
.expect("error sending completion success");
}
}
ModeRequest::ReadMode => {
self.mode_helper
.make_and_send_mode_reply(aocs_mode_to_mode_submode(self.mode))
.expect("error sending mode reply");
if let Some(token) = start_token {
self.verif_helper
.completion(token)
.expect("error sending completion success");
}
}
ModeRequest::AnnounceMode => {
if let Some(token) = start_token {
self.verif_helper
.completion_failure(token)
.expect("error sending completion failure");
}
}
ModeRequest::AnnounceModeRecursive => {
self.mode_helper
.make_and_send_mode_reply(aocs_mode_to_mode_submode(self.mode))
.expect("error sending mode reply");
if let Some(token) = start_token {
self.verif_helper
.completion(token)
.expect("error sending completion failure");
}
}
},
Request::ActionRequest(_) => {
if let Some(token) = start_token {
self.verif_helper
.completion_failure(token)
.expect("error sending completion failure");
}
}
}
Request::ActionRequest(request) => self.handle_action_request(request, token),
}
}
fn set_mode_reading(&mut self) {
if self.mode == SendingData {
return;
}
fn handle_mode_request(
&mut self,
request: ModeRequest,
token: VerificationToken<TcStateAccepted>,
) {
match request {
ModeRequest::SetMode(mode) => match mode.mode() {
0 => {}
// TODO: currently unrecoverable, should probably be changed
self.power_switcher
.send_switch_on_cmd(self.switch_id)
.expect("error sending switch on cmd");
self.enable_sensor_data_generation()
.expect("error enabling sensor data generation");
self.mode = SendingData;
}
fn set_mode_idle(&mut self) {
if self.mode == Idle {
return;
}
self.disable_sensor_data_generation()
.expect("error disabling sensor data generation");
self.power_switcher
.send_switch_off_cmd(self.switch_id)
.expect("error sending switch off cmd");
self.mode = Idle;
}
fn read_can(&mut self) {
if let Ok(package) = self.can_rx.try_recv() {
let mut map = self.aocs_data.lock().expect("error locking data map");
let float_data = self.decode_sensor_data(package.data());
match package.package_id() {
PackageId::AOCSDataSunSensor1 => {
map.update_value(CSS_VOLTAGE_1, AocsDataType::float_value(float_data))
.expect("error updating value");
}
PackageId::AOCSDataSunSensor2 => {
map.update_value(CSS_VOLTAGE_2, AocsDataType::float_value(float_data))
.expect("error updating value");
}
PackageId::AOCSDataSunSensor3 => {
map.update_value(CSS_VOLTAGE_3, AocsDataType::float_value(float_data))
.expect("error updating value");
}
PackageId::AOCSDataSunSensor4 => {
map.update_value(CSS_VOLTAGE_4, AocsDataType::float_value(float_data))
.expect("error updating value");
}
PackageId::AOCSDataSunSensor5 => {
map.update_value(CSS_VOLTAGE_5, AocsDataType::float_value(float_data))
.expect("error updating value");
}
PackageId::AOCSDataSunSensor6 => {
map.update_value(CSS_VOLTAGE_6, AocsDataType::float_value(float_data))
.expect("error updating value");
}
_ => {}
},
ModeRequest::ReadMode => {}
ModeRequest::AnnounceMode => {}
ModeRequest::AnnounceModeRecursive => {}
}
drop(map);
}
}
fn handle_action_request(
&mut self,
request: ActionRequest,
token: VerificationToken<TcStateAccepted>,
) {
match request {
ActionRequest::ImageRequest(target_id) => {}
ActionRequest::OrientationRequest(_) => {}
ActionRequest::PointingRequest(_) => {}
}
fn decode_sensor_data(&self, buf: &[u8]) -> f64 {
LittleEndian::read_f64(&buf)
}
}
@ -273,14 +251,16 @@ pub struct MGMHandler {
device_id: DeviceId,
switch_id: SwitchId,
device_state: DeviceState,
mode: AOCSSensorMode,
aocs_data: Arc<Mutex<AocsDataMap>>,
can_tx: Sender<PackageModel>,
can_rx: Receiver<PackageModel>,
mode: AOCSSensorMode,
request_rx: Receiver<RequestWithToken>,
mgm_data: Arc<Mutex<MGMData>>,
verif_helper: VerifHelper,
mode_helper: ModeHelper,
}
impl AOCSSensorHandler for MGMHandler {
impl AocsSensorHandler for MGMHandler {
type Error = ();
fn get_package_id(&mut self) -> Result<PackageId, Self::Error> {
@ -299,113 +279,478 @@ impl MGMHandler {
pub fn new(
power_switcher: PowerSwitcher,
device_id: DeviceId,
device_state: DeviceState,
aocs_data: Arc<Mutex<AocsDataMap>>,
can_tx: Sender<PackageModel>,
can_rx: Receiver<PackageModel>,
action_rx: Receiver<RequestWithToken>,
) -> MGMHandler {
request_rx: Receiver<RequestWithToken>,
verif_helper: VerifHelper,
mode_helper: ModeHelper,
) -> Self {
let switch_id = device_id as u16;
MGMHandler {
Self {
power_switcher,
device_id,
switch_id,
device_state: DeviceState::Off,
device_state,
mode: Idle,
aocs_data,
can_tx,
can_rx,
mode: AOCSSensorMode::Idle,
request_rx: action_rx,
mgm_data: Arc::new(Mutex::new(MGMData::new())),
request_rx,
verif_helper,
mode_helper,
}
}
pub fn get_data_ref(&mut self) -> Arc<Mutex<MGMData>> {
self.mgm_data.clone()
}
pub fn periodic_op(&mut self) {
self.update_mode();
self.handle_requests();
self.read_sensor_data();
self.read_can();
}
pub fn update_mode(&mut self) {}
fn handle_requests(&mut self) {
if let Ok(req) = self.request_rx.try_recv() {
let (req, start_token) = self
.verif_helper
.start_and_unwrap(req)
.expect("error sending start of execution");
match req {
Request::HkRequest(_) => {
if let Some(token) = start_token {
self.verif_helper
.completion_failure(token)
.expect("error sending completion failure");
}
}
Request::ModeRequest(req) => match req {
ModeRequest::SetMode(mode_submode) => {
if let Some(aocs_mode) = mode_submode_to_aocs_mode(mode_submode) {
self.mode = aocs_mode;
}
pub fn handle_requests(&mut self) {
if self.device_state == DeviceState::On {
if let Ok(request) = self.request_rx.try_recv() {
match request.0 {
Request::HkRequest(hk_req) => {
//self.handle_hk_request(hk_req);
if let Some(token) = start_token {
self.verif_helper
.completion(token)
.expect("error sending completion success");
}
}
Request::ActionRequest(_action_request) => {
//self.handle_action_request(action_request);
ModeRequest::ReadMode => {
self.mode_helper
.make_and_send_mode_reply(aocs_mode_to_mode_submode(self.mode))
.expect("error sending mode reply");
if let Some(token) = start_token {
self.verif_helper
.completion(token)
.expect("error sending completion success");
}
}
Request::ModeRequest(_mode_request) => {
//self.handle_mode_request(mode_request);
ModeRequest::AnnounceMode => {
if let Some(token) = start_token {
self.verif_helper
.completion_failure(token)
.expect("error sending completion failure");
}
}
ModeRequest::AnnounceModeRecursive => {
self.mode_helper
.make_and_send_mode_reply(aocs_mode_to_mode_submode(self.mode))
.expect("error sending mode reply");
if let Some(token) = start_token {
self.verif_helper
.completion(token)
.expect("error sending completion failure");
}
}
},
Request::ActionRequest(_) => {
if let Some(token) = start_token {
self.verif_helper
.completion_failure(token)
.expect("error sending completion failure");
}
}
}
}
}
pub fn handle_mode_request(&mut self, mode_request: ModeRequest) {
match mode_request {
ModeRequest::SetMode(_) => {}
ModeRequest::ReadMode => {}
ModeRequest::AnnounceMode => {}
ModeRequest::AnnounceModeRecursive => {}
fn set_mode_reading(&mut self) {
if self.mode == SendingData {
return;
}
// TODO: currently unrecoverable, should probably be changed
self.power_switcher
.send_switch_on_cmd(self.switch_id)
.expect("error sending switch on cmd");
self.enable_sensor_data_generation()
.expect("error enabling sensor data generation");
self.mode = SendingData;
}
/*
pub fn handle_hk_request(&mut self, hk_req: HkRequest) {
match hk_req {
HkRequest::OneShot(_) => {
self.can_tx.send(PackageModel::new(PackageId::AOCSDataRequestMGM1, &[2]).unwrap()).unwrap();
}
HkRequest::Enable(_) => {
if !self.sensor_data_enabled {
self.sensor_data_enabled = true;
self.can_tx.send(PackageModel::new(PackageId::AOCSDataRequestMGM1, &[1]).unwrap()).unwrap();
}
}
HkRequest::Disable(_) => {
if self.sensor_data_enabled {
self.sensor_data_enabled = false;
self.can_tx.send(PackageModel::new(PackageId::AOCSDataRequestMGM1, &[0]).unwrap()).unwrap();
}
}
HkRequest::ModifyCollectionInterval(_, _) => {}
fn set_mode_idle(&mut self) {
if self.mode == Idle {
return;
}
self.disable_sensor_data_generation()
.expect("error disabling sensor data generation");
self.power_switcher
.send_switch_off_cmd(self.switch_id)
.expect("error sending switch off cmd");
self.mode = Idle;
}
*/
pub fn read_sensor_data(&mut self) {
fn read_can(&mut self) {
if let Ok(package) = self.can_rx.try_recv() {
let mut map = self.aocs_data.lock().expect("error locking data map");
let float_data = self.decode_sensor_data(package.data());
if let Ok(mut mgm_data) = self.mgm_data.lock() {
match package.package_id() {
PackageId::AOCSDataMGM1 => mgm_data.axis_1 = float_data,
PackageId::AOCSDataMGM2 => mgm_data.axis_2 = float_data,
PackageId::AOCSDataMGM3 => mgm_data.axis_3 = float_data,
_ => {}
match package.package_id() {
PackageId::AOCSDataStarTracker1 => {
map.update_value(STR_QUATERNION_1, AocsDataType::float_value(float_data))
.expect("error updating value");
}
PackageId::AOCSDataStarTracker2 => {
map.update_value(STR_QUATERNION_2, AocsDataType::float_value(float_data))
.expect("error updating value");
}
PackageId::AOCSDataStarTracker3 => {
map.update_value(STR_QUATERNION_3, AocsDataType::float_value(float_data))
.expect("error updating value");
}
PackageId::AOCSDataStarTracker4 => {
map.update_value(STR_QUATERNION_4, AocsDataType::float_value(float_data))
.expect("error updating value");
}
_ => {}
}
drop(map);
}
}
pub fn decode_sensor_data(&mut self, buf: &[u8]) -> f64 {
fn decode_sensor_data(&self, buf: &[u8]) -> f64 {
LittleEndian::read_f64(&buf)
}
//pub fn handle_action_request(&mut self, action_request: ActionRequest) {}
}
pub struct AOCSController {
aocs_housekeeper: AocsHousekeeper,
mgm_handler: MGMHandler,
pub struct STRHandler {
power_switcher: PowerSwitcher,
device_id: DeviceId,
switch_id: SwitchId,
device_state: DeviceState,
mode: AOCSSensorMode,
aocs_data: Arc<Mutex<AocsDataMap>>,
can_tx: Sender<PackageModel>,
can_rx: Receiver<PackageModel>,
request_rx: Receiver<RequestWithToken>,
verif_helper: VerifHelper,
mode_helper: ModeHelper,
}
impl AocsSensorHandler for STRHandler {
type Error = ();
fn get_package_id(&mut self) -> Result<PackageId, Self::Error> {
Ok(PackageId::AOCSDataRequestStarTracker)
}
fn send_message(&mut self, id: PackageId, buf: &[u8]) -> Result<(), Self::Error> {
self.can_tx
.send(PackageModel::new(id, buf).unwrap())
.unwrap();
return Ok(());
}
}
impl STRHandler {
pub fn new(
power_switcher: PowerSwitcher,
device_id: DeviceId,
device_state: DeviceState,
aocs_data: Arc<Mutex<AocsDataMap>>,
can_tx: Sender<PackageModel>,
can_rx: Receiver<PackageModel>,
request_rx: Receiver<RequestWithToken>,
verif_helper: VerifHelper,
mode_helper: ModeHelper,
) -> Self {
let switch_id = device_id as u16;
Self {
power_switcher,
device_id,
switch_id,
device_state,
mode: Idle,
aocs_data,
can_tx,
can_rx,
request_rx,
verif_helper,
mode_helper,
}
}
pub fn periodic_op(&mut self) {
self.handle_requests();
self.read_can();
}
fn handle_requests(&mut self) {
if let Ok(req) = self.request_rx.try_recv() {
let (req, start_token) = self
.verif_helper
.start_and_unwrap(req)
.expect("error sending start of execution");
match req {
Request::HkRequest(_) => {
if let Some(token) = start_token {
self.verif_helper
.completion_failure(token)
.expect("error sending completion failure");
}
}
Request::ModeRequest(req) => match req {
ModeRequest::SetMode(mode_submode) => {
if let Some(aocs_mode) = mode_submode_to_aocs_mode(mode_submode) {
self.mode = aocs_mode;
}
if let Some(token) = start_token {
self.verif_helper
.completion(token)
.expect("error sending completion success");
}
}
ModeRequest::ReadMode => {
self.mode_helper
.make_and_send_mode_reply(aocs_mode_to_mode_submode(self.mode))
.expect("error sending mode reply");
if let Some(token) = start_token {
self.verif_helper
.completion(token)
.expect("error sending completion success");
}
}
ModeRequest::AnnounceMode => {
if let Some(token) = start_token {
self.verif_helper
.completion_failure(token)
.expect("error sending completion failure");
}
}
ModeRequest::AnnounceModeRecursive => {
self.mode_helper
.make_and_send_mode_reply(aocs_mode_to_mode_submode(self.mode))
.expect("error sending mode reply");
if let Some(token) = start_token {
self.verif_helper
.completion(token)
.expect("error sending completion failure");
}
}
},
Request::ActionRequest(_) => {
if let Some(token) = start_token {
self.verif_helper
.completion_failure(token)
.expect("error sending completion failure");
}
}
}
}
}
fn set_mode_reading(&mut self) {
if self.mode == SendingData {
return;
}
// TODO: currently unrecoverable, should probably be changed
self.power_switcher
.send_switch_on_cmd(self.switch_id)
.expect("error sending switch on cmd");
self.enable_sensor_data_generation()
.expect("error enabling sensor data generation");
self.mode = SendingData;
}
fn set_mode_idle(&mut self) {
if self.mode == Idle {
return;
}
self.disable_sensor_data_generation()
.expect("error disabling sensor data generation");
self.power_switcher
.send_switch_off_cmd(self.switch_id)
.expect("error sending switch off cmd");
self.mode = Idle;
}
fn read_can(&mut self) {
if let Ok(package) = self.can_rx.try_recv() {
let mut map = self.aocs_data.lock().expect("error locking data map");
let float_data = self.decode_sensor_data(package.data());
match package.package_id() {
PackageId::AOCSDataMGM1 => {
map.update_value(MGM_VOLTAGE_1, AocsDataType::float_value(float_data))
.expect("error updating value");
}
PackageId::AOCSDataMGM2 => {
map.update_value(MGM_VOLTAGE_2, AocsDataType::float_value(float_data))
.expect("error updating value");
}
PackageId::AOCSDataMGM2 => {
map.update_value(MGM_VOLTAGE_3, AocsDataType::float_value(float_data))
.expect("error updating value");
}
_ => {}
}
drop(map);
}
}
fn decode_sensor_data(&self, buf: &[u8]) -> f64 {
LittleEndian::read_f64(&buf)
}
}
pub struct AocsController {
request_rx: Receiver<RequestWithToken>,
hk_request_tx: Sender<RequestWithToken>,
mgm_request_tx: Sender<RequestWithToken>,
css_request_tx: Sender<RequestWithToken>,
str_request_tx: Sender<RequestWithToken>,
seq_count_provider: SeqCountProviderSyncClonable,
verif_helper: VerifHelper,
mode_helper: ModeHelper,
mode: AOCSSensorMode,
}
impl AocsController {
pub fn new(
request_rx: Receiver<RequestWithToken>,
mgm_request_tx: Sender<RequestWithToken>,
css_request_tx: Sender<RequestWithToken>,
str_request_tx: Sender<RequestWithToken>,
seq_count_provider: SeqCountProviderSyncClonable,
verif_helper: VerifHelper,
mode_helper: ModeHelper,
) -> Self {
Self {
request_rx,
mgm_request_tx,
css_request_tx,
str_request_tx,
seq_count_provider,
verif_helper,
mode_helper,
mode: Idle,
}
}
pub fn periodic_op(&mut self) {
self.handle_requests();
}
fn handle_requests(&mut self) {
if let Ok(req) = self.request_rx.try_recv() {
let (req, start_token) = self
.verif_helper
.start_and_unwrap(req)
.expect("error sending start of execution");
match req {
Request::HkRequest(_) => {
if let Some(token) = start_token {
self.verif_helper
.completion_failure(token)
.expect("error sending completion failure");
}
}
Request::ModeRequest(req) => match req {
ModeRequest::SetMode(mode_submode) => {
if let Some(aocs_mode) = mode_submode_to_aocs_mode(mode_submode) {
self.mode = aocs_mode;
}
self.mgm_request_tx
.send(RequestWithToken(Request::ModeRequest(req), None))
.expect("error sending to mgm");
self.css_request_tx
.send(RequestWithToken(Request::ModeRequest(req), None))
.expect("error sending to css");
self.str_request_tx
.send(RequestWithToken(Request::ModeRequest(req), None))
.expect("error sending to str");
if let Some(token) = start_token {
self.verif_helper
.completion(token)
.expect("error sending completion success");
}
}
ModeRequest::ReadMode => {
self.mode_helper
.make_and_send_mode_reply(aocs_mode_to_mode_submode(self.mode))
.expect("error sending mode reply");
if let Some(token) = start_token {
self.verif_helper
.completion(token)
.expect("error sending completion success");
}
}
ModeRequest::AnnounceMode => {
if let Some(token) = start_token {
self.verif_helper
.completion_failure(token)
.expect("error sending completion failure");
}
}
ModeRequest::AnnounceModeRecursive => {
self.mode_helper
.make_and_send_mode_reply(aocs_mode_to_mode_submode(self.mode))
.expect("error sending mode reply");
self.mgm_request_tx
.send(RequestWithToken(Request::ModeRequest(req), None))
.expect("error sending mode request to mgm handler");
self.css_request_tx
.send(RequestWithToken(Request::ModeRequest(req), None))
.expect("error sending mode request to css handler");
self.str_request_tx
.send(RequestWithToken(Request::ModeRequest(req), None))
.expect("error sending mode request to str handler");
if let Some(token) = start_token {
self.verif_helper
.completion(token)
.expect("error sending completion failure");
}
}
},
Request::ActionRequest(_) => {
if let Some(token) = start_token {
self.verif_helper
.completion_failure(token)
.expect("error sending completion failure");
}
}
}
}
}
}
fn aocs_mode_to_mode_submode(sensor_mode: AOCSSensorMode) -> ModeAndSubmode {
match sensor_mode {
Idle => ModeAndSubmode::new(0, 0),
SendingData => ModeAndSubmode::new(1, 0),
}
}
fn mode_submode_to_aocs_mode(mode_submode: ModeAndSubmode) -> Option<AOCSSensorMode> {
match mode_submode.mode() {
0 => Some(Idle),
1 => Some(SendingData),
_ => None,
}
}
pub fn core_aocs_loop() {}

157
src/helpers.rs Normal file
View File

@ -0,0 +1,157 @@
use crate::requests::{Request, RequestWithToken};
use crate::tmtc::TmStore;
use eurosim_obsw::tmtc_err;
use satrs_core::mode::ModeAndSubmode;
use satrs_core::pool::StoreAddr;
use satrs_core::pus::mode::Subservice::TmModeReply;
use satrs_core::pus::verification::{
FailParams, TcStateAccepted, TcStateStarted, VerificationReporterWithSender, VerificationToken,
};
use satrs_core::pus::MpscPusInStoreSendError;
use satrs_core::seq_count::{SeqCountProviderSyncClonable, SequenceCountProviderCore};
use satrs_core::spacepackets::tc::PusTc;
use satrs_core::spacepackets::time::cds::TimeProvider;
use satrs_core::spacepackets::time::TimeWriter;
use satrs_core::spacepackets::tm::{PusTm, PusTmSecondaryHeader};
use satrs_core::spacepackets::SpHeader;
use std::sync::mpsc::Sender;
#[derive(Clone)]
pub struct VerifHelper {
verif_reporter: VerificationReporterWithSender<MpscPusInStoreSendError>,
time_stamp_buf: [u8; 7],
}
impl VerifHelper {
pub fn new(verif_reporter: VerificationReporterWithSender<MpscPusInStoreSendError>) -> Self {
Self {
verif_reporter,
time_stamp_buf: [0; 7],
}
}
pub fn start_and_unwrap(
&mut self,
request_with_token: RequestWithToken,
) -> Result<(Request, Option<VerificationToken<TcStateStarted>>), ()> {
match request_with_token.1 {
None => Ok((request_with_token.0, None)),
Some(token) => {
self.update_time_stamp();
if let Ok(start_token) = self //implement this for verification
.verif_reporter
.start_success(token, Some(&self.time_stamp_buf))
{
return Ok((request_with_token.0, Some(start_token)));
} else {
return Err(());
}
}
}
}
pub fn start_failure(&mut self, request_with_token: RequestWithToken) -> Result<(), ()> {
if let Some(token) = request_with_token.1 {
self.update_time_stamp();
if let Ok(()) = self.verif_reporter.start_failure(
token,
FailParams::new(
Some(&mut self.time_stamp_buf),
&tmtc_err::INVALID_PUS_SUBSERVICE,
None,
),
) {
return Ok(());
} else {
return Err(());
}
}
Ok(())
}
pub fn completion(&mut self, start_token: VerificationToken<TcStateStarted>) -> Result<(), ()> {
self.update_time_stamp();
if let Ok(()) = self
.verif_reporter
.completion_success(start_token, Some(&self.time_stamp_buf))
{
return Ok(());
} else {
return Err(());
}
}
pub fn completion_failure(
&mut self,
start_token: VerificationToken<TcStateStarted>,
) -> Result<(), ()> {
self.update_time_stamp();
if let Ok(()) = self.verif_reporter.completion_failure(
start_token,
FailParams::new(
Some(&mut self.time_stamp_buf),
&tmtc_err::INVALID_PUS_SUBSERVICE,
None,
),
) {
return Ok(());
} else {
return Err(());
}
}
fn update_time_stamp(&mut self) {
let cds_stamp = TimeProvider::from_now_with_u16_days().unwrap();
cds_stamp.write_to_bytes(&mut self.time_stamp_buf).unwrap();
}
}
pub struct ModeHelper {
apid: u16,
seq_count_provider: SeqCountProviderSyncClonable,
tm_store: TmStore,
tm_funnel_tx: Sender<StoreAddr>,
buf: [u8; 6],
time_stamp_buf: [u8; 7],
}
impl ModeHelper {
pub fn new(
apid: u16,
seq_count_provider: SeqCountProviderSyncClonable,
tm_store: TmStore,
tm_funnel_tx: Sender<StoreAddr>,
) -> Self {
Self {
apid,
seq_count_provider,
tm_store,
tm_funnel_tx,
buf: [0; 6],
time_stamp_buf: [0; 7],
}
}
fn update_time_stamp(&mut self) {
let cds_stamp = TimeProvider::from_now_with_u16_days().unwrap();
cds_stamp.write_to_bytes(&mut self.time_stamp_buf).unwrap();
}
pub fn make_and_send_mode_reply(
&mut self,
mode_submode: ModeAndSubmode,
) -> Result<(), std::sync::mpsc::SendError<StoreAddr>> {
self.update_time_stamp();
let mut sp_header =
SpHeader::tm_unseg(self.apid, self.seq_count_provider.get_and_increment(), 0).unwrap();
self.buf[0..4].copy_from_slice(&mode_submode.mode().to_be_bytes());
self.buf[4..6].copy_from_slice(&mode_submode.submode().to_be_bytes());
let mut len = 6;
let data = self.buf[0..len].to_vec();
let tm_sec_header =
PusTmSecondaryHeader::new_simple(200, TmModeReply as u8, &self.time_stamp_buf);
let tm = PusTm::new(&mut sp_header, tm_sec_header, Some(&self.buf[0..len]), true);
let addr = self.tm_store.add_pus_tm(&tm);
self.tm_funnel_tx.send(addr)
}
}

185
src/hk.rs
View File

@ -1,13 +1,14 @@
use std::collections::HashMap;
use strum::IntoEnumIterator;
use crate::aocs::{CSSData, MGMData};
use crate::hk::AocsDataType::float_value;
use crate::requests::Request;
use crate::requests::RequestWithToken;
use crate::tmtc::{TmStore, AOCS_HK_APID};
use byteorder::{ByteOrder, LittleEndian};
use byteorder::{BigEndian, ByteOrder, LittleEndian};
use chrono::Duration;
use eurosim_obsw::hk_err;
use log::debug;
use num_enum::FromPrimitive;
use satrs_core::hk::{HkRequest, UniqueId};
use satrs_core::pool::StoreAddr;
@ -15,12 +16,13 @@ use satrs_core::pus::hk::Subservice;
use satrs_core::pus::verification::{FailParams, VerificationReporterWithSender};
use satrs_core::pus::MpscPusInStoreSendError;
use satrs_core::seq_count::{SeqCountProviderSyncClonable, SequenceCountProviderCore};
use satrs_core::spacepackets::time::cds::TimeProvider;
use satrs_core::spacepackets::time::TimeWriter;
use satrs_core::spacepackets::time::cds::{CdsCommon, TimeProvider};
use satrs_core::spacepackets::time::{CcsdsTimeProvider, TimeWriter};
use satrs_core::spacepackets::tm::{PusTm, PusTmSecondaryHeader};
use satrs_core::spacepackets::SpHeader;
use satrs_core::tmtc::AddressableId;
use serde::{Deserialize, Serialize};
use serde_json::from_slice;
use std::ops::Deref;
use std::sync::mpsc::{Receiver, Sender};
use std::sync::{Arc, Mutex};
@ -42,6 +44,8 @@ pub const STR_QUATERNION_2: UniqueId = 11;
pub const STR_QUATERNION_3: UniqueId = 12;
pub const STR_QUATERNION_4: UniqueId = 13;
pub const AOCS_HK_NUM_OF_ELEMENTS: usize = 13 * 8;
#[derive(Debug, Copy, Clone, PartialEq, Eq)]
pub enum AocsHkIds {
TestAocsSet = 1,
@ -85,6 +89,24 @@ impl AocsDataMap {
pub fn get_value(&self, id: UniqueId) -> Option<&AocsDataType> {
self.map.get(&id)
}
pub fn all_values_as_bytes(&self) -> [u8; AOCS_HK_NUM_OF_ELEMENTS] {
let mut buf: [u8; AOCS_HK_NUM_OF_ELEMENTS] = [0; AOCS_HK_NUM_OF_ELEMENTS];
let mut i = 0;
let map = self.map.clone();
let mut map_as_vec = map.keys().collect::<Vec<&UniqueId>>();
map_as_vec.sort();
for element in map_as_vec {
match map.get(element).unwrap() {
float_value(val) => {
let val_as_byte = BigEndian::write_f64(&mut (buf[i..i + 8]), *val);
}
}
i += 8;
}
debug!("{:?}", buf);
buf
}
}
#[derive(Debug, EnumIter, PartialEq, Copy, Clone)]
@ -92,52 +114,6 @@ pub enum AocsDataType {
float_value(f64),
}
#[derive(Serialize, Deserialize)]
pub struct AOCSSensorData {
mgm_data: MGMData, // Voltage for 3 axis
css_data: CSSData, // Voltage for 18 sun sensors
str_data: [f64; 4], // Quaternion for position of satellite
}
impl AOCSSensorData {
pub fn new() -> AOCSSensorData {
let mgm_data = MGMData::default();
let css_data = CSSData::default();
let str_data = [0.0; 4];
AOCSSensorData {
mgm_data,
css_data,
str_data,
}
}
pub fn update_mgm_data(&mut self, mgm_data: &Arc<Mutex<MGMData>>) {
let data = mgm_data.lock().unwrap();
self.mgm_data = *data;
}
pub fn update_css_data(&mut self, css_data: &Arc<Mutex<CSSData>>) {
let data = css_data.lock().unwrap();
self.css_data = *data;
}
pub fn write_str_data(&mut self, str_data: [f64; 4]) {
self.str_data = str_data;
}
pub fn get_mgm_data(&mut self) -> MGMData {
self.mgm_data
}
pub fn read_css_data(&mut self) -> CSSData {
self.css_data
}
pub fn read_str_data(&mut self) -> [f64; 4] {
self.str_data
}
}
pub struct AocsHousekeeper {
data_map: Arc<Mutex<AocsDataMap>>,
id_list: Vec<UniqueId>,
@ -147,6 +123,9 @@ pub struct AocsHousekeeper {
aocs_tm_funnel_tx: Sender<StoreAddr>,
verif_reporter: VerificationReporterWithSender<MpscPusInStoreSendError>,
periodic_hk_ids: Option<Vec<UniqueId>>,
periodic_on: bool,
prev_time_step: TimeProvider,
collection_interval: Duration,
}
impl AocsHousekeeper {
@ -183,9 +162,61 @@ impl AocsHousekeeper {
aocs_tm_funnel_tx,