eurosim-obsw/src/pld_handler.rs
lkoester 322fc70ac7 ...
2023-04-26 17:17:35 +02:00

282 lines
11 KiB
Rust

use crate::action::ActionRequest;
use crate::can_ids::{DeviceId, MessageId, MessageModel};
use crate::power_handler::{DeviceState, PowerSwitcher};
use crate::requests::{Request, RequestWithToken};
use eurosim_obsw::{RequestTargetId};
use log::debug;
use satrs_core::power::{PowerSwitchInfo, PowerSwitcherCommandSender, SwitchId};
use satrs_core::pus::verification::{VerificationReporterWithSender};
use satrs_core::pus::MpscPusInStoreSendError;
use satrs_core::spacepackets::time::cds::TimeProvider;
use satrs_core::spacepackets::time::TimeWriter;
use std::sync::mpsc;
use std::sync::mpsc::{Receiver, Sender};
use std::thread::sleep;
use std::time::Duration;
#[derive(Debug, PartialEq, Copy, Clone)]
pub enum CameraMode {
Idle,
PictureRequest,
Verification,
Start,
End,
Broken,
}
pub struct CameraHandler {
power_switcher: PowerSwitcher,
camera_device_id: DeviceId,
camera_switch_id: SwitchId,
device_state: DeviceState,
can_tx: Sender<MessageModel>,
can_rx: Receiver<MessageModel>,
mode: CameraMode,
mode_rx: Receiver<CameraMode>,
action_rx: Receiver<RequestWithToken>,
}
impl CameraHandler {
pub fn new(
power_switcher: PowerSwitcher,
camera_device_id: DeviceId,
can_tx: Sender<MessageModel>,
can_rx: Receiver<MessageModel>,
mode_rx: Receiver<CameraMode>,
action_rx: Receiver<RequestWithToken>,
) -> 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,
action_rx,
}
}
pub fn set_mode(&mut self, mode: CameraMode) {
if self.mode == CameraMode::Idle {
if mode == CameraMode::PictureRequest {
self.mode = CameraMode::PictureRequest;
}
}
}
pub fn get_mode(&self) -> CameraMode {
self.mode
}
pub fn handle_mode_requests(&mut self) {
match self.mode_rx.try_recv() {
Ok(mode) => {
self.set_mode(mode);
}
Err(_) => {}
}
}
/*pub fn handle_action_requests(&mut self) {
match self.action_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.");
debug!("{:?}", camera_handler.get_mode());
while camera_handler.get_mode() != CameraMode::Idle {
camera_handler.periodic_op();
}
let cds_stamp = TimeProvider::from_now_with_u16_days().unwrap();
cds_stamp.write_to_bytes(&mut time_stamp_buf).unwrap();
// 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(_) => {}
}
}
*/
pub fn take_picture(&mut self) {}
pub fn periodic_op(&mut self) {
// Camera Device Handler State Machine
match self.mode {
CameraMode::Broken => {}
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;
debug!("switching power");
}
if self.device_state == DeviceState::SwitchingPower {
let allowed_cycles = 10;
for i in 0..allowed_cycles {
if self
.power_switcher
.get_is_switch_on(self.camera_switch_id)
.expect("reading switch state failed")
{
self.device_state = DeviceState::On;
debug!("device on");
}
}
self.mode = CameraMode::Broken;
}
if self.device_state == DeviceState::On {
self.can_tx
.send(MessageModel::new(MessageId::CameraImageRequest, &[1]).unwrap())
.unwrap();
debug!("sent camera request");
self.mode = CameraMode::Verification;
}
}
CameraMode::Verification => {
if self.device_state == DeviceState::On {
debug!("waiting for image request confirmation");
if let Ok(msg) = self.can_rx.recv() {
if msg.message_id() == MessageId::CameraImageRequestConfirmation {
self.mode = CameraMode::Start;
}
}
}
}
CameraMode::Start => {
if self.device_state == DeviceState::On {
debug!("waiting for image start confirmation");
if let Ok(msg) = self.can_rx.recv() {
if msg.message_id() == MessageId::CameraImageExecutionStart {
self.mode = CameraMode::End;
}
}
}
}
CameraMode::End => {
if self.device_state == DeviceState::On {
debug!("waiting for image end confirmation");
if let Ok(msg) = self.can_rx.recv() {
if msg.message_id() == MessageId::CameraImageExectutionEnd {
self.power_switcher
.send_switch_off_cmd(self.camera_switch_id)
.expect("sending switch command failed");
self.device_state = DeviceState::SwitchingPower;
debug!("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;
debug!("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<MessageModel>,
pld_can_tx: Sender<MessageModel>,
reporter_pld: &mut VerificationReporterWithSender<MpscPusInStoreSendError>,
) {
let (_camera_mode_tx, camera_mode_rx) = mpsc::channel();
let (_action_tx, action_rx) = mpsc::channel();
let mut camera_handler = CameraHandler::new(
power_switcher,
DeviceId::Camera,
pld_can_tx,
pld_can_rx,
camera_mode_rx,
action_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.unwrap(),
Some(&time_stamp_buf),
)
.expect("Error sending start success.");
debug!("{:?}", camera_handler.get_mode());
while camera_handler.get_mode() != CameraMode::Idle {
camera_handler.periodic_op();
debug!("{:?}", camera_handler.get_mode());
sleep(Duration::from_millis(1000));
}
let cds_stamp = TimeProvider::from_now_with_u16_days().unwrap();
cds_stamp.write_to_bytes(&mut time_stamp_buf).unwrap();
// 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(_) => {}
}
}
}