473 lines
19 KiB
Rust
473 lines
19 KiB
Rust
mod action;
|
|
mod aocs;
|
|
mod cam;
|
|
mod can;
|
|
mod can_ids;
|
|
mod ccsds;
|
|
mod device_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,
|
|
};
|
|
use eurosim_obsw::{RequestTargetId, OBSW_SERVER_ADDR, SERVER_PORT};
|
|
use satrs_core::event_man::{
|
|
EventManagerWithMpscQueue, MpscEventReceiver, MpscEventU32SendProvider, SendEventProvider,
|
|
};
|
|
use satrs_core::events::EventU32;
|
|
use satrs_core::pool::{LocalPool, PoolCfg, StoreAddr};
|
|
use satrs_core::pus::event_man::{
|
|
DefaultPusMgmtBackendProvider, EventReporter, EventRequest, 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::seq_count::{
|
|
SeqCountProviderSyncClonable, SequenceCountProvider, SequenceCountProviderCore,
|
|
};
|
|
use satrs_core::{
|
|
spacepackets::time::cds::TimeProvider,
|
|
spacepackets::time::TimeWriter,
|
|
spacepackets::tm::{PusTm, PusTmSecondaryHeader},
|
|
spacepackets::{SequenceFlags, SpHeader},
|
|
};
|
|
|
|
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::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;
|
|
|
|
#[derive(Clone)]
|
|
struct EventTmSender {
|
|
store_helper: TmStore,
|
|
sender: mpsc::Sender<StoreAddr>,
|
|
}
|
|
|
|
impl EventTmSender {
|
|
fn new(store_helper: TmStore, sender: mpsc::Sender<StoreAddr>) -> Self {
|
|
Self {
|
|
store_helper,
|
|
sender,
|
|
}
|
|
}
|
|
}
|
|
|
|
impl EcssTmSenderCore for EventTmSender {
|
|
type Error = mpsc::SendError<StoreAddr>;
|
|
|
|
fn send_tm(&mut self, tm: PusTm) -> Result<(), EcssTmErrorWithSend<Self::Error>> {
|
|
let addr = self.store_helper.add_pus_tm(&tm);
|
|
self.sender
|
|
.send(addr)
|
|
.map_err(EcssTmErrorWithSend::SendError)
|
|
}
|
|
}
|
|
|
|
fn main() {
|
|
println!("Running SESPSat OBSW");
|
|
|
|
logger::setup_logger().unwrap();
|
|
|
|
let tm_pool = LocalPool::new(PoolCfg::new(vec![
|
|
(30, 32),
|
|
(15, 64),
|
|
(15, 128),
|
|
(15, 256),
|
|
(15, 1024),
|
|
(15, 2048),
|
|
]));
|
|
let tm_store = TmStore {
|
|
pool: Arc::new(RwLock::new(Box::new(tm_pool))),
|
|
};
|
|
let tc_pool = LocalPool::new(PoolCfg::new(vec![
|
|
(30, 32),
|
|
(15, 64),
|
|
(15, 128),
|
|
(15, 256),
|
|
(15, 1024),
|
|
(15, 2048),
|
|
]));
|
|
let tc_store = TcStore {
|
|
pool: Arc::new(RwLock::new(Box::new(tc_pool))),
|
|
};
|
|
|
|
let seq_count_provider = SeqCountProviderSyncClonable::default();
|
|
let aocs_seq_count_provider = seq_count_provider.clone();
|
|
|
|
let sock_addr = SocketAddr::new(IpAddr::V4(OBSW_SERVER_ADDR), SERVER_PORT);
|
|
let (tc_source_tx, tc_source_rx) = channel();
|
|
let (tm_funnel_tx, tm_funnel_rx) = channel();
|
|
let (tm_server_tx, tm_server_rx) = channel();
|
|
let verif_sender = MpscVerifSender::new(tm_store.pool.clone(), tm_funnel_tx.clone());
|
|
let verif_cfg = VerificationReporterCfg::new(
|
|
PUS_APID,
|
|
#[allow(clippy::box_default)]
|
|
Box::new(seq_count_provider.clone()),
|
|
1,
|
|
2,
|
|
8,
|
|
)
|
|
.unwrap();
|
|
|
|
let verif_reporter = VerificationReporterWithSender::new(&verif_cfg, Box::new(verif_sender));
|
|
|
|
// Create event handling components
|
|
let (event_request_tx, event_request_rx) = channel::<EventRequestWithToken>();
|
|
let (event_sender, event_man_rx) = channel();
|
|
let event_recv = MpscEventReceiver::<EventU32>::new(event_man_rx);
|
|
let mut event_man = EventManagerWithMpscQueue::new(Box::new(event_recv));
|
|
let event_reporter = EventReporter::new(PUS_APID, 128).unwrap();
|
|
let pus_tm_backend = DefaultPusMgmtBackendProvider::<EventU32>::default();
|
|
let mut pus_event_dispatcher =
|
|
PusEventDispatcher::new(event_reporter, Box::new(pus_tm_backend));
|
|
let (pus_event_man_tx, pus_event_man_rx) = channel();
|
|
let pus_event_man_send_provider = MpscEventU32SendProvider::new(1, pus_event_man_tx);
|
|
let mut reporter_event_handler = verif_reporter.clone();
|
|
let mut reporter_aocs = verif_reporter.clone();
|
|
let mut reporter_pld = verif_reporter.clone();
|
|
event_man.subscribe_all(pus_event_man_send_provider.id());
|
|
|
|
let mut request_map = HashMap::new();
|
|
let (acs_thread_tx, acs_thread_rx) = channel::<RequestWithToken>();
|
|
let (pld_thread_tx, pld_thread_rx) = channel::<RequestWithToken>();
|
|
request_map.insert(RequestTargetId::AcsSubsystem as u32, acs_thread_tx);
|
|
request_map.insert(RequestTargetId::PldSubsystem as u32, pld_thread_tx);
|
|
//add here receivers for tmtc task to send requests to
|
|
//request_map.insert(RequestTargetId::CanTask as u32, can_thread_tx);
|
|
|
|
let tc_source = PusTcSource {
|
|
tc_store,
|
|
tc_source: tc_source_tx,
|
|
};
|
|
|
|
// Create clones here to allow moving the values
|
|
let core_args = OtherArgs {
|
|
sock_addr,
|
|
verif_reporter,
|
|
event_sender,
|
|
event_request_tx,
|
|
request_map,
|
|
};
|
|
let tc_args = TcArgs {
|
|
tc_source,
|
|
tc_receiver: tc_source_rx,
|
|
};
|
|
let tm_args = TmArgs {
|
|
tm_store: tm_store.clone(),
|
|
tm_sink_sender: tm_funnel_tx.clone(),
|
|
tm_server_rx,
|
|
};
|
|
|
|
let (aocs_can_tx, aocs_can_rx) = mpsc::channel::<PackageModel>();
|
|
let (power_can_tx, power_can_rx) = mpsc::channel::<PackageModel>();
|
|
let (pld_can_tx, pld_can_rx) = mpsc::channel::<PackageModel>();
|
|
|
|
// make tx thread id hashmap
|
|
let mut can_senders = HashMap::new();
|
|
can_senders.insert(ThreadId::AOCSThread, aocs_can_tx);
|
|
can_senders.insert(ThreadId::PowerThread, power_can_tx);
|
|
can_senders.insert(ThreadId::PLDThread, pld_can_tx);
|
|
|
|
// 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");
|
|
let builder0 = thread::Builder::new().name("TMTCThread".into());
|
|
let jh0 = builder0.spawn(move || {
|
|
core_tmtc_task(core_args, tc_args, tm_args);
|
|
});
|
|
|
|
info!("Starting CAN Socket listening task");
|
|
let builder1 = thread::Builder::new().name("CanRxHandler".into());
|
|
let jh1 = builder1.spawn(move || loop {
|
|
let frame = socket0.rx_socket();
|
|
if let Some(frame) = frame {
|
|
let forward = socket0.forward_frame(frame);
|
|
}
|
|
});
|
|
|
|
let (pcdu_tx, pcdu_rx) = mpsc::channel::<(SwitchId, SwitchState)>();
|
|
let pcdu_can_tx =
|
|
can::CanTxHandler::new_socket("can0", ThreadId::PowerThread, load_package_ids()).unwrap();
|
|
|
|
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);
|
|
});
|
|
|
|
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();
|
|
|
|
/*
|
|
// AOCS Thread
|
|
let socket1 =
|
|
can::CanTxHandler::new_socket("can0", ThreadId::AOCSThread, package_map_aocs_tx).unwrap();
|
|
info!("Starting AOCS receiving thread");
|
|
let builder2 = thread::Builder::new().name("AOCSThread".into());
|
|
let jh2 = builder2.spawn(move || {
|
|
let mut time_stamp_buf: [u8; 7] = [0; 7];
|
|
let mut huge_buf: [u8; 8192] = [0; 8192];
|
|
let data: [u8; 3] = [1, 2, 3];
|
|
let current_mgm_data = MgmData::default();
|
|
//let current_mgt_data = MgtData::default();
|
|
//let current_
|
|
loop {
|
|
// device handling
|
|
//info!("Sending {:?}", PackageId::AOCSControlMGT1);
|
|
//socket1.tx_socket(PackageId::AOCSControlMGT1, &data);
|
|
//info!("Waiting for {:?}", PackageId::AOCSDataMGM1);
|
|
let msg = aocs_can_rx.try_recv();
|
|
//current_mgm_data.x = new_data
|
|
match aocs_can_rx.try_recv() {
|
|
Ok(package) => match package.package_id() {
|
|
_ => warn!("Incorrect Id"),
|
|
},
|
|
Err(_) => {}
|
|
}
|
|
|
|
// telecommand handling
|
|
match acs_thread_rx.try_recv() {
|
|
Ok(request_with_token) => {
|
|
match request_with_token.0 {
|
|
Request::HkRequest(hk_req) => {
|
|
match hk_req {
|
|
HkRequest::OneShot(id) => {
|
|
assert_eq!(id.target_id, RequestTargetId::AcsSubsystem as u32);
|
|
if id.unique_id == 0 {
|
|
let mut sp_header = SpHeader::tm_unseg(
|
|
0x02,
|
|
aocs_seq_count_provider.get_and_increment(),
|
|
0,
|
|
)
|
|
.unwrap();
|
|
let cds_stamp =
|
|
TimeProvider::from_now_with_u16_days().unwrap();
|
|
cds_stamp.write_to_bytes(&mut time_stamp_buf);
|
|
let mut len = id.write_to_be_bytes(&mut huge_buf).unwrap();
|
|
let json_string = "asdf";
|
|
huge_buf[8..json_string.len() + 8]
|
|
.copy_from_slice(json_string.as_bytes());
|
|
len += json_string.len();
|
|
let tm_sec_header = PusTmSecondaryHeader::new_simple(
|
|
3,
|
|
Subservice::TmHkPacket as u8,
|
|
&time_stamp_buf,
|
|
);
|
|
let hk_tm = PusTm::new(
|
|
&mut sp_header,
|
|
tm_sec_header,
|
|
Some(&huge_buf[0..len]),
|
|
true,
|
|
);
|
|
let addr = aocs_tm_store.add_pus_tm(&hk_tm);
|
|
aocs_tm_funnel_tx.send(addr).expect("sending failed");
|
|
/* let start_token = self //implement this for verification
|
|
.verif_reporter
|
|
.start_success(token, &self.time_stamp)
|
|
.expect("Error sending start success");
|
|
self.tm_tx
|
|
.send(addr)
|
|
.expect("Sending TM to TM funnel failed");
|
|
self.verif_reporter
|
|
.completion_success(start_token, &self.time_stamp)
|
|
.expect("Error sending completion success");
|
|
|
|
*/
|
|
}
|
|
}
|
|
HkRequest::Enable(_) => {}
|
|
HkRequest::Disable(_) => {}
|
|
HkRequest::ModifyCollectionInterval(_, _) => {}
|
|
}
|
|
}
|
|
_ => {}
|
|
}
|
|
}
|
|
Err(_) => {}
|
|
}
|
|
}
|
|
});
|
|
|
|
*/
|
|
|
|
let package_map_pld_tx = load_package_ids();
|
|
let pld_tm_funnel_tx = tm_funnel_tx.clone();
|
|
let mut pld_tm_store = tm_store.clone();
|
|
|
|
let PLDCanSocket =
|
|
can::CanTxHandler::new_socket("can0", ThreadId::PLDThread, package_map_pld_tx).unwrap();
|
|
let mut pcdu_tx_pld_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(_) => {}
|
|
}
|
|
}
|
|
});
|
|
|
|
println!("Starting TM funnel task");
|
|
let builder4 = thread::Builder::new().name("TMFunnelThread".into());
|
|
let jh4 = builder4.spawn(move || {
|
|
let tm_funnel = TmFunnel {
|
|
tm_server_tx,
|
|
tm_funnel_rx,
|
|
};
|
|
loop {
|
|
if let Ok(addr) = tm_funnel.tm_funnel_rx.recv() {
|
|
tm_funnel
|
|
.tm_server_tx
|
|
.send(addr)
|
|
.expect("Sending TM to server failed");
|
|
}
|
|
}
|
|
});
|
|
|
|
jh0.unwrap()
|
|
.join()
|
|
.expect("Joining UDP TMTC server thread failed");
|
|
jh1.unwrap()
|
|
.join()
|
|
.expect("Joining CAN Bus Listening 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 {
|
|
x: i16,
|
|
y: i16,
|
|
z: i16,
|
|
}
|