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

added power and payload handler

Browse Source
This commit is contained in:
lkoester 2023-02-09 15:38:32 +01:00
parent 251757ff7f
commit 42f30cd9a0
5 changed files with 360 additions and 237 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

0
src/device_handler.rs
View File

149
src/main.rs
View File

@ -4,7 +4,7 @@ mod cam;
mod can;
mod can_ids;
mod ccsds;
mod device_handler;
mod pld_handler;
mod hk;
mod logger;
mod pus;
@ -12,7 +12,6 @@ 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,
@ -24,39 +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, 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::pcdu::core_power_task;
use crate::pld_handler::{CameraHandler, core_pld_task};
use crate::pcdu::{core_power_task, PowerSwitcher};
#[derive(Clone)]
struct EventTmSender {
@ -192,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");
@ -224,12 +210,22 @@ fn main() {
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);
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();
@ -332,109 +328,12 @@ fn main() {
let PLDCanSocket =
can::CanTxHandler::new_socket("can0", ThreadId::PLDThread, package_map_pld_tx).unwrap();
let mut pcdu_tx_pld_clone = pcdu_tx.clone();
//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) => {
pcdu_tx_pld_clone.send((DeviceId::Camera as SwitchId, SwitchState::On)).unwrap();
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.");
}
}
}
pcdu_tx_pld_clone.send((DeviceId::Camera as SwitchId, SwitchState::Off)).unwrap();
}
ActionRequest::OrientationRequest(_) => {}
_ => {}
}
}
_ => {}
}
}
Err(_) => {}
}
}
core_pld_task(power_switcher.clone(), pld_thread_rx, pld_can_rx, &mut reporter_pld);
});
println!("Starting TM funnel task");

275
src/pcdu.rs
View File

@ -1,12 +1,13 @@
use std::collections::HashMap;
use std::sync::mpsc::Receiver;
use satrs_core::power::{PowerSwitch, PowerSwitcher, SwitchId, SwitchState};
use crate::can::{CanRxHandler, CanTxHandler};
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::ops::Deref;
use std::sync::{Arc, Mutex};
use std::time::Duration;
use log::warn;
use std::vec;
use log::info;
pub use strum::IntoEnumIterator; // 0.17.1
pub use strum_macros::EnumIter; // 0.17.1
@ -19,21 +20,169 @@ pub enum DeviceState {
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: HashMap<DeviceId, SwitchState>,
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>) -> PCDU{
let mut device_state_map: HashMap<DeviceId, SwitchState> = HashMap::new();
for id in DeviceId::iter() {
device_state_map.insert(id, SwitchState::Off);
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(())
}
}
PCDU{switch_rx, can_tx, can_rx, device_state_map}
}
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, ()>{
@ -41,7 +190,7 @@ impl PCDU {
while let Ok((switch_id, switch_state)) = self.switch_rx.recv() {
match switch_state {
SwitchState::Off => {
match self.send_switch_off_cmd(switch_id) {
match self.send_power_off(switch_id) {
Ok(_) => {
i = i + 1;
}
@ -51,7 +200,7 @@ impl PCDU {
}
}
SwitchState::On => {
match self.send_switch_on_cmd(switch_id) {
match self.send_power_on(switch_id) {
Ok(_) => {
i = i + 1;
}
@ -62,108 +211,14 @@ impl PCDU {
}
_ => {}
}
self.update_switch_states(switch_id);
}
return Ok(i);
}
}
impl PowerSwitcher for PCDU {
type Error = ();
fn send_switch_on_cmd(&mut self, switch_id: SwitchId) -> Result<(), Self::Error> {
return match DeviceId::try_from(switch_id) {
Ok(device_id) => {
let buf: &[u8] = &switch_id.to_be_bytes();
if buf.len() == 1 {
self.can_tx.tx_socket(PackageId::DevicePowerOnRequest, buf);
let mut i = 0;
while i < 10 {
match self.can_rx.recv() {
Ok(msg) => {
if msg.package_id()
== PackageId::DevicePowerOnConfirmation {
return if msg.data()[0] == device_id as u8
&& msg.data()[1] == 1 {
*self.device_state_map.get_mut(&(device_id)).unwrap() = SwitchState::On;
Ok(())
} else {
Err(())
}
}
}
Err(_) => {
warn!("Error receiving Can Bus Message.");
}
}
i = i + 1;
}
Err(())
} else {
Err(())
}
}
Err(_) => { Err(()) }
}
}
fn send_switch_off_cmd(&mut self, switch_id: SwitchId) -> Result<(), Self::Error> {
return match DeviceId::try_from(switch_id) {
Ok(device_id) => {
let buf: &[u8] = &switch_id.to_be_bytes();
if buf.len() == 1 {
self.can_tx.tx_socket(PackageId::DevicePowerOffRequest, buf);
let mut i = 0;
while i < 10 {
match self.can_rx.recv() {
Ok(msg) => {
if msg.package_id()
== PackageId::DevicePowerOffConfirmation {
return if msg.data()[0] == device_id as u8
&& msg.data()[1] == 1 {
*self.device_state_map.get_mut(&(device_id)).unwrap() = SwitchState::Off;
Ok(())
} else {
Err(())
}
}
}
Err(_) => {
warn!("Error receiving Can Bus Message.");
}
}
i = i + 1;
}
Err(())
} else {
Err(())
}
}
Err(_) => { Err(()) }
}
}
fn get_switch_state(&mut self, switch_id: SwitchId) -> Result<SwitchState, Self::Error> {
if let Ok(device_id) = DeviceId::try_from(switch_id) {
return match self.device_state_map.get_mut(&device_id) {
None => { Err(()) }
Some(state) => {
let return_state = state.clone();
Ok(return_state)
}
}
} else {
Err(())
}
}
fn switch_delay_ms(&self) -> u32 {
return 0;
}
}
pub fn core_power_task(switch_rx: Receiver<(SwitchId, SwitchState)>, can_tx: CanTxHandler, can_rx: Receiver<PackageModel>) {
let mut pcdu = PCDU::new(switch_rx, can_tx, can_rx);
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
View 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(_) => {}
}
}
}

2
src/pus.rs
View File

@ -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