282 lines
11 KiB
Rust
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(_) => {}
|
|
}
|
|
}
|
|
}
|