977 lines
35 KiB
Rust
977 lines
35 KiB
Rust
use crate::aocs::AOCSSensorMode::{Idle, SendingData};
|
|
use crate::can_ids::{DeviceId, MessageId, MessageModel};
|
|
use crate::helpers::{ModeHelper, VerifHelper};
|
|
use crate::hk::{AocsDataMap, AocsDataType, CSS_SUN_VECTOR_1, CSS_SUN_VECTOR_2, CSS_SUN_VECTOR_3, CSS_VOLTAGE_4, CSS_VOLTAGE_5, CSS_VOLTAGE_6, GPS_ALTITUDE, GPS_LATITUDE, GPS_LONGITUDE, MGM_FIELD_1, MGM_FIELD_2, MGM_FIELD_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 byteorder::{ByteOrder, LittleEndian};
|
|
use num_derive::ToPrimitive;
|
|
use satrs_core::mode::{ModeAndSubmode, ModeRequest};
|
|
use satrs_core::power::{PowerSwitcherCommandSender, SwitchId};
|
|
use satrs_core::seq_count::SeqCountProviderSyncClonable;
|
|
use std::sync::mpsc::{Receiver, Sender};
|
|
use std::sync::{Arc, Mutex};
|
|
use log::debug;
|
|
|
|
#[derive(ToPrimitive, PartialEq, Copy, Clone)]
|
|
pub enum AOCSSensorMode {
|
|
Idle = 0,
|
|
SendingData = 1,
|
|
}
|
|
|
|
pub trait AocsSensorHandler {
|
|
type Error;
|
|
|
|
fn get_package_id(&mut self) -> Result<MessageId, Self::Error>;
|
|
fn send_message(&mut self, id: MessageId, buf: &[u8]) -> Result<(), Self::Error>;
|
|
|
|
fn enable_sensor_data_generation(&mut self) -> Result<(), Self::Error> {
|
|
let id = self.get_package_id()?;
|
|
self.send_message(id, &[1])
|
|
}
|
|
|
|
fn disable_sensor_data_generation(&mut self) -> Result<(), Self::Error> {
|
|
let id = self.get_package_id()?;
|
|
self.send_message(id, &[0])
|
|
}
|
|
|
|
fn request_sensor_data_oneshot(&mut self) -> Result<(), Self::Error> {
|
|
let id = self.get_package_id()?;
|
|
self.send_message(id, &[2])
|
|
}
|
|
}
|
|
|
|
pub struct CSSHandler {
|
|
power_switcher: PowerSwitcher,
|
|
device_id: DeviceId,
|
|
switch_id: SwitchId,
|
|
device_state: DeviceState,
|
|
mode: AOCSSensorMode,
|
|
aocs_data: Arc<Mutex<AocsDataMap>>,
|
|
can_tx: Sender<MessageModel>,
|
|
can_rx: Receiver<MessageModel>,
|
|
request_rx: Receiver<RequestWithToken>,
|
|
verif_helper: VerifHelper,
|
|
mode_helper: ModeHelper,
|
|
}
|
|
|
|
impl AocsSensorHandler for CSSHandler {
|
|
type Error = ();
|
|
|
|
fn get_package_id(&mut self) -> Result<MessageId, Self::Error> {
|
|
Ok(MessageId::AOCSDataRequestSunSensor1)
|
|
}
|
|
|
|
fn send_message(&mut self, id: MessageId, buf: &[u8]) -> Result<(), Self::Error> {
|
|
self.can_tx
|
|
.send(MessageModel::new(id, buf).unwrap())
|
|
.unwrap();
|
|
return Ok(());
|
|
}
|
|
}
|
|
|
|
impl CSSHandler {
|
|
pub fn new(
|
|
power_switcher: PowerSwitcher,
|
|
device_id: DeviceId,
|
|
device_state: DeviceState,
|
|
aocs_data: Arc<Mutex<AocsDataMap>>,
|
|
can_tx: Sender<MessageModel>,
|
|
can_rx: Receiver<MessageModel>,
|
|
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.update_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 update_mode(&mut self, mode: AOCSSensorMode) {
|
|
if mode != self.mode {
|
|
match mode {
|
|
Idle => { self.set_mode_idle() }
|
|
SendingData => { self.set_mode_reading() }
|
|
}
|
|
}
|
|
}
|
|
|
|
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.message_id() {
|
|
MessageId::AOCSDataSunSensor1 => {
|
|
map.update_value(CSS_SUN_VECTOR_1, AocsDataType::FloatValue(float_data))
|
|
.expect("error updating value");
|
|
}
|
|
MessageId::AOCSDataSunSensor2 => {
|
|
map.update_value(CSS_SUN_VECTOR_2, AocsDataType::FloatValue(float_data))
|
|
.expect("error updating value");
|
|
}
|
|
MessageId::AOCSDataSunSensor3 => {
|
|
map.update_value(CSS_SUN_VECTOR_3, AocsDataType::FloatValue(float_data))
|
|
.expect("error updating value");
|
|
}
|
|
MessageId::AOCSDataSunSensor4 => {
|
|
map.update_value(CSS_VOLTAGE_4, AocsDataType::FloatValue(float_data))
|
|
.expect("error updating value");
|
|
}
|
|
MessageId::AOCSDataSunSensor5 => {
|
|
map.update_value(CSS_VOLTAGE_5, AocsDataType::FloatValue(float_data))
|
|
.expect("error updating value");
|
|
}
|
|
MessageId::AOCSDataSunSensor6 => {
|
|
map.update_value(CSS_VOLTAGE_6, AocsDataType::FloatValue(float_data))
|
|
.expect("error updating value");
|
|
}
|
|
_ => {}
|
|
}
|
|
drop(map);
|
|
}
|
|
}
|
|
|
|
fn decode_sensor_data(&self, buf: &[u8]) -> f64 {
|
|
LittleEndian::read_f64(&buf)
|
|
}
|
|
}
|
|
|
|
pub struct MGMHandler {
|
|
power_switcher: PowerSwitcher,
|
|
device_id: DeviceId,
|
|
switch_id: SwitchId,
|
|
device_state: DeviceState,
|
|
mode: AOCSSensorMode,
|
|
aocs_data: Arc<Mutex<AocsDataMap>>,
|
|
can_tx: Sender<MessageModel>,
|
|
can_rx: Receiver<MessageModel>,
|
|
request_rx: Receiver<RequestWithToken>,
|
|
verif_helper: VerifHelper,
|
|
mode_helper: ModeHelper,
|
|
}
|
|
|
|
impl AocsSensorHandler for MGMHandler {
|
|
type Error = ();
|
|
|
|
fn get_package_id(&mut self) -> Result<MessageId, Self::Error> {
|
|
Ok(MessageId::AOCSDataRequestMGM1)
|
|
}
|
|
|
|
fn send_message(&mut self, id: MessageId, buf: &[u8]) -> Result<(), Self::Error> {
|
|
self.can_tx
|
|
.send(MessageModel::new(id, buf).unwrap())
|
|
.unwrap();
|
|
return Ok(());
|
|
}
|
|
}
|
|
|
|
impl MGMHandler {
|
|
pub fn new(
|
|
power_switcher: PowerSwitcher,
|
|
device_id: DeviceId,
|
|
device_state: DeviceState,
|
|
aocs_data: Arc<Mutex<AocsDataMap>>,
|
|
can_tx: Sender<MessageModel>,
|
|
can_rx: Receiver<MessageModel>,
|
|
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() {
|
|
debug!("request received");
|
|
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.update_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 update_mode(&mut self, mode: AOCSSensorMode) {
|
|
if mode != self.mode {
|
|
match mode {
|
|
Idle => { self.set_mode_idle() }
|
|
SendingData => { self.set_mode_reading() }
|
|
}
|
|
}
|
|
}
|
|
|
|
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.message_id() {
|
|
MessageId::AOCSDataMGM1 => {
|
|
map.update_value(MGM_FIELD_1, AocsDataType::FloatValue(float_data))
|
|
.expect("error updating value");
|
|
}
|
|
MessageId::AOCSDataMGM2 => {
|
|
map.update_value(MGM_FIELD_2, AocsDataType::FloatValue(float_data))
|
|
.expect("error updating value");
|
|
}
|
|
MessageId::AOCSDataMGM3 => {
|
|
map.update_value(MGM_FIELD_3, AocsDataType::FloatValue(float_data))
|
|
.expect("error updating value");
|
|
}
|
|
_ => {}
|
|
}
|
|
drop(map);
|
|
}
|
|
}
|
|
|
|
fn decode_sensor_data(&self, buf: &[u8]) -> f64 {
|
|
LittleEndian::read_f64(&buf)
|
|
}
|
|
}
|
|
|
|
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<MessageModel>,
|
|
can_rx: Receiver<MessageModel>,
|
|
request_rx: Receiver<RequestWithToken>,
|
|
verif_helper: VerifHelper,
|
|
mode_helper: ModeHelper,
|
|
}
|
|
|
|
impl AocsSensorHandler for STRHandler {
|
|
type Error = ();
|
|
|
|
fn get_package_id(&mut self) -> Result<MessageId, Self::Error> {
|
|
Ok(MessageId::AOCSDataRequestStarTracker)
|
|
}
|
|
|
|
fn send_message(&mut self, id: MessageId, buf: &[u8]) -> Result<(), Self::Error> {
|
|
self.can_tx
|
|
.send(MessageModel::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<MessageModel>,
|
|
can_rx: Receiver<MessageModel>,
|
|
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.update_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 update_mode(&mut self, mode: AOCSSensorMode) {
|
|
if mode != self.mode {
|
|
match mode {
|
|
Idle => { self.set_mode_idle() }
|
|
SendingData => { self.set_mode_reading() }
|
|
}
|
|
}
|
|
}
|
|
|
|
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.message_id() {
|
|
MessageId::AOCSDataStarTracker1 => {
|
|
map.update_value(STR_QUATERNION_1, AocsDataType::FloatValue(float_data))
|
|
.expect("error updating value");
|
|
}
|
|
MessageId::AOCSDataStarTracker2 => {
|
|
map.update_value(STR_QUATERNION_2, AocsDataType::FloatValue(float_data))
|
|
.expect("error updating value");
|
|
}
|
|
MessageId::AOCSDataStarTracker3 => {
|
|
map.update_value(STR_QUATERNION_3, AocsDataType::FloatValue(float_data))
|
|
.expect("error updating value");
|
|
}
|
|
MessageId::AOCSDataStarTracker4 => {
|
|
map.update_value(STR_QUATERNION_4, AocsDataType::FloatValue(float_data))
|
|
.expect("error updating value");
|
|
}
|
|
_ => {}
|
|
}
|
|
drop(map);
|
|
}
|
|
}
|
|
|
|
fn decode_sensor_data(&self, buf: &[u8]) -> f64 {
|
|
LittleEndian::read_f64(&buf)
|
|
}
|
|
}
|
|
|
|
pub struct GPSHandler {
|
|
power_switcher: PowerSwitcher,
|
|
device_id: DeviceId,
|
|
switch_id: SwitchId,
|
|
device_state: DeviceState,
|
|
mode: AOCSSensorMode,
|
|
aocs_data: Arc<Mutex<AocsDataMap>>,
|
|
can_tx: Sender<MessageModel>,
|
|
can_rx: Receiver<MessageModel>,
|
|
request_rx: Receiver<RequestWithToken>,
|
|
verif_helper: VerifHelper,
|
|
mode_helper: ModeHelper,
|
|
}
|
|
|
|
impl AocsSensorHandler for GPSHandler {
|
|
type Error = ();
|
|
|
|
fn get_package_id(&mut self) -> Result<MessageId, Self::Error> {
|
|
Ok(MessageId::AOCSDataRequestGPS)
|
|
}
|
|
|
|
fn send_message(&mut self, id: MessageId, buf: &[u8]) -> Result<(), Self::Error> {
|
|
self.can_tx
|
|
.send(MessageModel::new(id, buf).unwrap())
|
|
.unwrap();
|
|
return Ok(());
|
|
}
|
|
}
|
|
|
|
impl GPSHandler {
|
|
pub fn new(
|
|
power_switcher: PowerSwitcher,
|
|
device_id: DeviceId,
|
|
device_state: DeviceState,
|
|
aocs_data: Arc<Mutex<AocsDataMap>>,
|
|
can_tx: Sender<MessageModel>,
|
|
can_rx: Receiver<MessageModel>,
|
|
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.update_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 update_mode(&mut self, mode: AOCSSensorMode) {
|
|
if mode != self.mode {
|
|
match mode {
|
|
Idle => { self.set_mode_idle() }
|
|
SendingData => { self.set_mode_reading() }
|
|
}
|
|
}
|
|
}
|
|
|
|
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.message_id() {
|
|
MessageId::AOCSDataGPSLatitude => {
|
|
map.update_value(GPS_LATITUDE, AocsDataType::FloatValue(float_data))
|
|
.expect("error updating value");
|
|
}
|
|
MessageId::AOCSDataGPSLongitude => {
|
|
map.update_value(GPS_LONGITUDE, AocsDataType::FloatValue(float_data))
|
|
.expect("error updating value");
|
|
}
|
|
MessageId::AOCSDataGPSAltitude => {
|
|
map.update_value(GPS_ALTITUDE, AocsDataType::FloatValue(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>,
|
|
mgm_request_tx: Sender<RequestWithToken>,
|
|
css_request_tx: Sender<RequestWithToken>,
|
|
str_request_tx: Sender<RequestWithToken>,
|
|
gps_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>,
|
|
gps_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,
|
|
gps_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");
|
|
self.gps_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");
|
|
self.gps_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() {}
|