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

added gps handler

Browse Source
This commit is contained in:
lkoester 2023-03-25 20:22:07 +01:00
parent 51471c0a2d
commit 7f4c62a855
8 changed files with 447 additions and 116 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

278
src/aocs.rs
View File

@ -3,7 +3,7 @@ use crate::aocs::AOCSSensorMode::{Idle, SendingData};
use crate::can_ids::PackageId::AOCSDataStarTracker1;
use crate::can_ids::{DeviceId, PackageId, PackageModel};
use crate::helpers::{ModeHelper, VerifHelper};
use crate::hk::{AocsDataMap, AocsDataType, AocsHousekeeper, CSS_VOLTAGE_1, CSS_VOLTAGE_2, CSS_VOLTAGE_3, CSS_VOLTAGE_4, CSS_VOLTAGE_5, CSS_VOLTAGE_6, MGM_VOLTAGE_1, MGM_VOLTAGE_2, MGM_VOLTAGE_3, STR_QUATERNION_1, STR_QUATERNION_2, STR_QUATERNION_3, STR_QUATERNION_4};
use crate::hk::{AocsDataMap, AocsDataType, AocsHousekeeper, 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 crate::tmtc::TmStore;
@ -23,6 +23,7 @@ use serde::{Deserialize, Serialize};
use std::sync::mpsc::{channel, Receiver, Sender, TryRecvError};
use std::sync::{Arc, Mutex};
use std::u32;
use log::debug;
#[derive(ToPrimitive, PartialEq, Copy, Clone)]
pub enum AOCSSensorMode {
@ -131,7 +132,7 @@ impl CSSHandler {
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.update_mode(aocs_mode);
}
if let Some(token) = start_token {
@ -179,6 +180,16 @@ impl CSSHandler {
}
}
}
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;
@ -212,15 +223,15 @@ impl CSSHandler {
let float_data = self.decode_sensor_data(package.data());
match package.package_id() {
PackageId::AOCSDataSunSensor1 => {
map.update_value(CSS_VOLTAGE_1, AocsDataType::float_value(float_data))
map.update_value(CSS_SUN_VECTOR_1, AocsDataType::float_value(float_data))
.expect("error updating value");
}
PackageId::AOCSDataSunSensor2 => {
map.update_value(CSS_VOLTAGE_2, AocsDataType::float_value(float_data))
map.update_value(CSS_SUN_VECTOR_2, AocsDataType::float_value(float_data))
.expect("error updating value");
}
PackageId::AOCSDataSunSensor3 => {
map.update_value(CSS_VOLTAGE_3, AocsDataType::float_value(float_data))
map.update_value(CSS_SUN_VECTOR_3, AocsDataType::float_value(float_data))
.expect("error updating value");
}
PackageId::AOCSDataSunSensor4 => {
@ -310,6 +321,7 @@ impl MGMHandler {
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)
@ -325,7 +337,7 @@ impl MGMHandler {
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.update_mode(aocs_mode);
}
if let Some(token) = start_token {
@ -373,6 +385,16 @@ impl MGMHandler {
}
}
}
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;
@ -405,20 +427,16 @@ impl MGMHandler {
let mut map = self.aocs_data.lock().expect("error locking data map");
let float_data = self.decode_sensor_data(package.data());
match package.package_id() {
PackageId::AOCSDataStarTracker1 => {
map.update_value(STR_QUATERNION_1, AocsDataType::float_value(float_data))
PackageId::AOCSDataMGM1 => {
map.update_value(MGM_FIELD_1, AocsDataType::float_value(float_data))
.expect("error updating value");
}
PackageId::AOCSDataStarTracker2 => {
map.update_value(STR_QUATERNION_2, AocsDataType::float_value(float_data))
PackageId::AOCSDataMGM2 => {
map.update_value(MGM_FIELD_2, AocsDataType::float_value(float_data))
.expect("error updating value");
}
PackageId::AOCSDataStarTracker3 => {
map.update_value(STR_QUATERNION_3, AocsDataType::float_value(float_data))
.expect("error updating value");
}
PackageId::AOCSDataStarTracker4 => {
map.update_value(STR_QUATERNION_4, AocsDataType::float_value(float_data))
PackageId::AOCSDataMGM3 => {
map.update_value(MGM_FIELD_3, AocsDataType::float_value(float_data))
.expect("error updating value");
}
_ => {}
@ -511,7 +529,7 @@ impl STRHandler {
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.update_mode(aocs_mode);
}
if let Some(token) = start_token {
@ -560,6 +578,15 @@ impl STRHandler {
}
}
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;
@ -592,16 +619,212 @@ impl STRHandler {
let mut map = self.aocs_data.lock().expect("error locking data map");
let float_data = self.decode_sensor_data(package.data());
match package.package_id() {
PackageId::AOCSDataMGM1 => {
map.update_value(MGM_VOLTAGE_1, AocsDataType::float_value(float_data))
PackageId::AOCSDataStarTracker1 => {
map.update_value(STR_QUATERNION_1, AocsDataType::float_value(float_data))
.expect("error updating value");
}
PackageId::AOCSDataMGM2 => {
map.update_value(MGM_VOLTAGE_2, AocsDataType::float_value(float_data))
PackageId::AOCSDataStarTracker2 => {
map.update_value(STR_QUATERNION_2, AocsDataType::float_value(float_data))
.expect("error updating value");
}
PackageId::AOCSDataMGM2 => {
map.update_value(MGM_VOLTAGE_3, AocsDataType::float_value(float_data))
PackageId::AOCSDataStarTracker3 => {
map.update_value(STR_QUATERNION_3, AocsDataType::float_value(float_data))
.expect("error updating value");
}
PackageId::AOCSDataStarTracker4 => {
map.update_value(STR_QUATERNION_4, AocsDataType::float_value(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<PackageModel>,
can_rx: Receiver<PackageModel>,
request_rx: Receiver<RequestWithToken>,
verif_helper: VerifHelper,
mode_helper: ModeHelper,
}
impl AocsSensorHandler for GPSHandler {
type Error = ();
fn get_package_id(&mut self) -> Result<PackageId, Self::Error> {
Ok(PackageId::AOCSDataRequestGPS)
}
fn send_message(&mut self, id: PackageId, buf: &[u8]) -> Result<(), Self::Error> {
self.can_tx
.send(PackageModel::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<PackageModel>,
can_rx: Receiver<PackageModel>,
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.package_id() {
PackageId::AOCSDataGPSLatitude => {
map.update_value(GPS_LATITUDE, AocsDataType::float_value(float_data))
.expect("error updating value");
}
PackageId::AOCSDataGPSLongitude => {
map.update_value(GPS_LONGITUDE, AocsDataType::float_value(float_data))
.expect("error updating value");
}
PackageId::AOCSDataGPSAltitude => {
map.update_value(GPS_ALTITUDE, AocsDataType::float_value(float_data))
.expect("error updating value");
}
_ => {}
@ -620,6 +843,7 @@ pub struct AocsController {
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,
@ -632,6 +856,7 @@ impl AocsController {
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,
@ -641,6 +866,7 @@ impl AocsController {
mgm_request_tx,
css_request_tx,
str_request_tx,
gps_request_tx,
seq_count_provider,
verif_helper,
mode_helper,
@ -680,6 +906,9 @@ impl AocsController {
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
@ -718,6 +947,9 @@ impl AocsController {
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

16
src/can_ids.rs
View File

@ -2,7 +2,7 @@ use embedded_can::{Id, StandardId};
use log::warn;
use std::collections::HashMap;
use crate::tmtc::{CSS_APID, MGM_APID, MGT_APID, PLD_APID, PWR_APID, RWL_APID, STR_APID};
use crate::tmtc::{CSS_APID, GPS_APID, MGM_APID, MGT_APID, PLD_APID, PWR_APID, RWL_APID, STR_APID};
pub use num_derive::{FromPrimitive, ToPrimitive};
pub use num_traits::{FromPrimitive, ToPrimitive};
pub use strum::IntoEnumIterator; // 0.17.1
@ -47,6 +47,7 @@ pub enum PackageId {
AOCSDataRequestSunSensor5 = 49,
AOCSDataRequestSunSensor6 = 50,
AOCSDataRequestStarTracker = 51,
AOCSDataRequestGPS = 55,
AOCSDataMGM1 = 61,
AOCSDataMGM2 = 62,
AOCSDataMGM3 = 63,
@ -61,6 +62,9 @@ pub enum PackageId {
AOCSDataStarTracker2 = 72,
AOCSDataStarTracker3 = 73,
AOCSDataStarTracker4 = 74,
AOCSDataGPSLatitude = 81,
AOCSDataGPSLongitude = 82,
AOCSDataGPSAltitude = 83,
CameraImageRequest = 101,
CameraImageRequestConfirmation = 102,
CameraImageExecutionStart = 103,
@ -92,6 +96,7 @@ pub enum DeviceId {
RWL4 = 21,
Camera = 22,
All = 23,
GPS = 24,
}
impl TryFrom<u16> for DeviceId {
@ -122,6 +127,7 @@ impl TryFrom<u16> for DeviceId {
x if x == DeviceId::RWL4 as u16 => Ok(DeviceId::RWL4),
x if x == DeviceId::Camera as u16 => Ok(DeviceId::Camera),
x if x == DeviceId::All as u16 => Ok(DeviceId::All),
x if x == DeviceId::GPS as u16 => Ok(DeviceId::GPS),
_ => Err(()),
}
}
@ -216,6 +222,7 @@ pub fn load_package_id_to_threads() -> HashMap<PackageId, SenderReceiverThread>
SenderReceiverThread::new(DeviceId::OBC, DeviceId::SunSensor5, ThreadId::AOCSThread),
SenderReceiverThread::new(DeviceId::OBC, DeviceId::SunSensor6, ThreadId::AOCSThread),
SenderReceiverThread::new(DeviceId::OBC, DeviceId::StarTracker, ThreadId::AOCSThread),
SenderReceiverThread::new(DeviceId::OBC, DeviceId::GPS, ThreadId::AOCSThread),
SenderReceiverThread::new(DeviceId::MGM1, DeviceId::OBC, ThreadId::AOCSThread),
SenderReceiverThread::new(DeviceId::MGM2, DeviceId::OBC, ThreadId::AOCSThread),
SenderReceiverThread::new(DeviceId::MGM3, DeviceId::OBC, ThreadId::AOCSThread),
@ -230,6 +237,9 @@ pub fn load_package_id_to_threads() -> HashMap<PackageId, SenderReceiverThread>
SenderReceiverThread::new(DeviceId::StarTracker, DeviceId::OBC, ThreadId::AOCSThread),
SenderReceiverThread::new(DeviceId::StarTracker, DeviceId::OBC, ThreadId::AOCSThread),
SenderReceiverThread::new(DeviceId::StarTracker, DeviceId::OBC, ThreadId::AOCSThread),
SenderReceiverThread::new(DeviceId::GPS, DeviceId::OBC, ThreadId::AOCSThread),
SenderReceiverThread::new(DeviceId::GPS, DeviceId::OBC, ThreadId::AOCSThread),
SenderReceiverThread::new(DeviceId::GPS, DeviceId::OBC, ThreadId::AOCSThread),
SenderReceiverThread::new(DeviceId::OBC, DeviceId::Camera, ThreadId::PLDThread),
SenderReceiverThread::new(DeviceId::Camera, DeviceId::OBC, ThreadId::PLDThread),
SenderReceiverThread::new(DeviceId::Camera, DeviceId::OBC, ThreadId::PLDThread),
@ -306,6 +316,7 @@ pub fn load_package_id_to_apids() -> HashMap<PackageId, SenderReceiverApid> {
SenderReceiverApid::new(DeviceId::OBC, DeviceId::SunSensor5, CSS_APID),
SenderReceiverApid::new(DeviceId::OBC, DeviceId::SunSensor6, CSS_APID),
SenderReceiverApid::new(DeviceId::OBC, DeviceId::StarTracker, STR_APID),
SenderReceiverApid::new(DeviceId::OBC, DeviceId::GPS, GPS_APID),
SenderReceiverApid::new(DeviceId::MGM1, DeviceId::OBC, MGM_APID),
SenderReceiverApid::new(DeviceId::MGM2, DeviceId::OBC, MGM_APID),
SenderReceiverApid::new(DeviceId::MGM3, DeviceId::OBC, MGM_APID),
@ -320,6 +331,9 @@ pub fn load_package_id_to_apids() -> HashMap<PackageId, SenderReceiverApid> {
SenderReceiverApid::new(DeviceId::StarTracker, DeviceId::OBC, STR_APID),
SenderReceiverApid::new(DeviceId::StarTracker, DeviceId::OBC, STR_APID),
SenderReceiverApid::new(DeviceId::StarTracker, DeviceId::OBC, STR_APID),
SenderReceiverApid::new(DeviceId::GPS, DeviceId::OBC, GPS_APID),
SenderReceiverApid::new(DeviceId::GPS, DeviceId::OBC, GPS_APID),
SenderReceiverApid::new(DeviceId::GPS, DeviceId::OBC, GPS_APID),
SenderReceiverApid::new(DeviceId::OBC, DeviceId::Camera, PLD_APID),
SenderReceiverApid::new(DeviceId::Camera, DeviceId::OBC, PLD_APID),
SenderReceiverApid::new(DeviceId::Camera, DeviceId::OBC, PLD_APID),

2
src/helpers.rs
View File

@ -15,6 +15,7 @@ use satrs_core::spacepackets::time::TimeWriter;
use satrs_core::spacepackets::tm::{PusTm, PusTmSecondaryHeader};
use satrs_core::spacepackets::SpHeader;
use std::sync::mpsc::Sender;
use log::info;
#[derive(Clone)]
pub struct VerifHelper {
@ -141,6 +142,7 @@ impl ModeHelper {
&mut self,
mode_submode: ModeAndSubmode,
) -> Result<(), std::sync::mpsc::SendError<StoreAddr>> {
info!("mode reply generated from apid {}", self.apid);
self.update_time_stamp();
let mut sp_header =
SpHeader::tm_unseg(self.apid, self.seq_count_provider.get_and_increment(), 0).unwrap();

65
src/hk.rs
View File

@ -30,12 +30,12 @@ use strum_macros::EnumIter;
pub type CollectionIntervalFactor = u32;
pub const MGM_VOLTAGE_1: UniqueId = 1;
pub const MGM_VOLTAGE_2: UniqueId = 2;
pub const MGM_VOLTAGE_3: UniqueId = 3;
pub const CSS_VOLTAGE_1: UniqueId = 4;
pub const CSS_VOLTAGE_2: UniqueId = 5;
pub const CSS_VOLTAGE_3: UniqueId = 6;
pub const MGM_FIELD_1: UniqueId = 1;
pub const MGM_FIELD_2: UniqueId = 2;
pub const MGM_FIELD_3: UniqueId = 3;
pub const CSS_SUN_VECTOR_1: UniqueId = 4;
pub const CSS_SUN_VECTOR_2: UniqueId = 5;
pub const CSS_SUN_VECTOR_3: UniqueId = 6;
pub const CSS_VOLTAGE_4: UniqueId = 7;
pub const CSS_VOLTAGE_5: UniqueId = 8;
pub const CSS_VOLTAGE_6: UniqueId = 9;
@ -43,8 +43,11 @@ pub const STR_QUATERNION_1: UniqueId = 10;
pub const STR_QUATERNION_2: UniqueId = 11;
pub const STR_QUATERNION_3: UniqueId = 12;
pub const STR_QUATERNION_4: UniqueId = 13;
pub const GPS_LATITUDE: UniqueId = 14;
pub const GPS_LONGITUDE: UniqueId = 15;
pub const GPS_ALTITUDE: UniqueId = 16;
pub const AOCS_HK_NUM_OF_ELEMENTS: usize = 13 * 8;
pub const AOCS_HK_NUM_OF_ELEMENTS: usize = 16 * 8;
#[derive(Debug, Copy, Clone, PartialEq, Eq)]
pub enum AocsHkIds {
@ -52,7 +55,7 @@ pub enum AocsHkIds {
TestMgmSet = 2,
}
#[derive(Clone)]
#[derive(Clone, Debug)]
pub struct AocsDataMap {
map: HashMap<UniqueId, AocsDataType>,
}
@ -61,12 +64,12 @@ impl AocsDataMap {
pub fn new() -> Self {
let mut data_map = HashMap::new();
data_map.insert(MGM_VOLTAGE_1, float_value(0.0));
data_map.insert(MGM_VOLTAGE_2, float_value(0.0));
data_map.insert(MGM_VOLTAGE_3, float_value(0.0));
data_map.insert(CSS_VOLTAGE_1, float_value(0.0));
data_map.insert(CSS_VOLTAGE_2, float_value(0.0));
data_map.insert(CSS_VOLTAGE_3, float_value(0.0));
data_map.insert(MGM_FIELD_1, float_value(0.0));
data_map.insert(MGM_FIELD_2, float_value(0.0));
data_map.insert(MGM_FIELD_3, float_value(0.0));
data_map.insert(CSS_SUN_VECTOR_1, float_value(0.0));
data_map.insert(CSS_SUN_VECTOR_2, float_value(0.0));
data_map.insert(CSS_SUN_VECTOR_3, float_value(0.0));
data_map.insert(CSS_VOLTAGE_4, float_value(0.0));
data_map.insert(CSS_VOLTAGE_5, float_value(0.0));
data_map.insert(CSS_VOLTAGE_6, float_value(0.0));
@ -74,6 +77,9 @@ impl AocsDataMap {
data_map.insert(STR_QUATERNION_2, float_value(0.0));
data_map.insert(STR_QUATERNION_3, float_value(0.0));
data_map.insert(STR_QUATERNION_4, float_value(0.0));
data_map.insert(GPS_LATITUDE, float_value(0.0));
data_map.insert(GPS_LONGITUDE, float_value(0.0));
data_map.insert(GPS_ALTITUDE, float_value(0.0));
Self { map: data_map }
}
@ -138,12 +144,12 @@ impl AocsHousekeeper {
verif_reporter: VerificationReporterWithSender<MpscPusInStoreSendError>,
) -> AocsHousekeeper {
let id_list = vec![
MGM_VOLTAGE_1,
MGM_VOLTAGE_2,
MGM_VOLTAGE_3,
CSS_VOLTAGE_1,
CSS_VOLTAGE_2,
CSS_VOLTAGE_3,
MGM_FIELD_1,
MGM_FIELD_2,
MGM_FIELD_3,
CSS_SUN_VECTOR_1,
CSS_SUN_VECTOR_2,
CSS_SUN_VECTOR_3,
CSS_VOLTAGE_4,
CSS_VOLTAGE_5,
CSS_VOLTAGE_6,
@ -151,6 +157,9 @@ impl AocsHousekeeper {
STR_QUATERNION_2,
STR_QUATERNION_3,
STR_QUATERNION_4,
GPS_LATITUDE,
GPS_LONGITUDE,
GPS_ALTITUDE,
];
AocsHousekeeper {
@ -178,12 +187,12 @@ impl AocsHousekeeper {
collection_interval: Duration,
) -> AocsHousekeeper {
let id_list = vec![
MGM_VOLTAGE_1,
MGM_VOLTAGE_2,
MGM_VOLTAGE_3,
CSS_VOLTAGE_1,
CSS_VOLTAGE_2,
CSS_VOLTAGE_3,
MGM_FIELD_1,
MGM_FIELD_2,
MGM_FIELD_3,
CSS_SUN_VECTOR_1,
CSS_SUN_VECTOR_2,
CSS_SUN_VECTOR_3,
CSS_VOLTAGE_4,
CSS_VOLTAGE_5,
CSS_VOLTAGE_6,
@ -191,6 +200,9 @@ impl AocsHousekeeper {
STR_QUATERNION_2,
STR_QUATERNION_3,
STR_QUATERNION_4,
GPS_LATITUDE,
GPS_LONGITUDE,
GPS_ALTITUDE,
];
AocsHousekeeper {
@ -269,6 +281,7 @@ impl AocsHousekeeper {
self.prev_time_step = TimeProvider::from_now_with_u16_days().unwrap();
let map = self.data_map.lock().unwrap();
let mut data_as_bytes = map.all_values_as_bytes();
debug!("{:?}", &map);
drop(map);
self.send_hk_packet(1, &mut data_as_bytes);
}

148
src/main.rs
View File

@ -20,10 +20,7 @@ mod requests;
mod tmtc;
use crate::requests::RequestWithToken;
use crate::tmtc::{
core_tmtc_task, OtherArgs, PusTcSource, TcArgs, TcStore, TmArgs, TmFunnel, TmStore, AOCS_APID,
AOCS_HK_APID, CSS_APID, MGM_APID, MGT_APID, PLD_APID, PUS_APID, PWR_APID, RWL_APID, STR_APID,
};
use crate::tmtc::{core_tmtc_task, OtherArgs, PusTcSource, TcArgs, TcStore, TmArgs, TmFunnel, TmStore, AOCS_APID, AOCS_HK_APID, CSS_APID, MGM_APID, MGT_APID, PLD_APID, PUS_APID, PWR_APID, RWL_APID, STR_APID, GPS_APID};
use eurosim_obsw::{RequestTargetId, OBSW_SERVER_ADDR, SERVER_PORT};
use satrs_core::event_man::{
EventManagerWithMpscQueue, MpscEventReceiver, MpscEventU32SendProvider, SendEventProvider,
@ -53,13 +50,13 @@ use std::sync::mpsc::channel;
use std::sync::{mpsc, Arc, Mutex, RwLock};
use std::thread;
//use libc::time64_t;
use crate::aocs::{core_aocs_loop, AocsController, MGMHandler};
use crate::aocs::{core_aocs_loop, AocsController, MGMHandler, STRHandler, GPSHandler, CSSHandler};
#[cfg(feature = "can")]
use crate::can::CanTxHandler;
use crate::helpers::{ModeHelper, VerifHelper};
use crate::hk::{AocsDataMap, AocsDataType, AocsHousekeeper};
use crate::pld_handler::core_pld_task;
use crate::power_handler::{core_power_task, PowerSwitcher};
use crate::power_handler::{core_power_task, DeviceState, PowerSwitcher};
fn main() {
logger::setup_logger().unwrap();
@ -90,7 +87,12 @@ fn main() {
let seq_count_provider = SeqCountProviderSyncClonable::default();
let msg_count_provider = SeqCountProviderSyncClonable::default();
let aocs_seq_count_provider = SeqCountProviderSyncClonable::default();
let aocs_hk_seq_count_provider = SeqCountProviderSyncClonable::default();
let aocs_ctrl_seq_count_provider = SeqCountProviderSyncClonable::default();
let mgm_seq_count_provider = SeqCountProviderSyncClonable::default();
let css_seq_count_provider = SeqCountProviderSyncClonable::default();
let str_seq_count_provider = SeqCountProviderSyncClonable::default();
let gps_seq_count_provider = SeqCountProviderSyncClonable::default();
let verif_seq_count_provider = seq_count_provider.clone();
let tmtc_seq_count_provider = seq_count_provider.clone();
@ -135,6 +137,7 @@ fn main() {
let (mgm_tx, mgm_rx) = channel::<RequestWithToken>();
let (css_tx, css_rx) = channel::<RequestWithToken>();
let (str_tx, str_rx) = channel::<RequestWithToken>();
let (gps_tx, gps_rx) = channel::<RequestWithToken>();
let (mgt_tx, mgt_rx) = channel::<RequestWithToken>();
let (rwl_tx, rwl_rx) = channel::<RequestWithToken>();
request_map.insert(RequestTargetId::AcsSubsystem as u32, aocs_thread_tx.clone());
@ -148,7 +151,6 @@ fn main() {
};
let mut apid_map = HashMap::new();
apid_map.insert(PLD_APID, pld_thread_tx.clone());
apid_map.insert(PWR_APID, pwr_thread_tx.clone());
apid_map.insert(AOCS_APID, aocs_thread_tx.clone());
@ -156,6 +158,7 @@ fn main() {
apid_map.insert(MGM_APID, mgm_tx.clone());
apid_map.insert(CSS_APID, css_tx.clone());
apid_map.insert(STR_APID, str_tx.clone());
apid_map.insert(GPS_APID, gps_tx.clone());
apid_map.insert(MGT_APID, mgt_tx.clone());
apid_map.insert(RWL_APID, rwl_tx.clone());
@ -184,6 +187,7 @@ fn main() {
let (css_can_tx, css_can_rx) = mpsc::channel::<PackageModel>();
let (rwl_can_tx, rwl_can_rx) = mpsc::channel::<PackageModel>();
let (str_can_tx, str_can_rx) = mpsc::channel::<PackageModel>();
let (gps_can_tx, gps_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>();
@ -196,6 +200,7 @@ fn main() {
can_senders.insert(RWL_APID, rwl_can_tx);
can_senders.insert(CSS_APID, css_can_tx);
can_senders.insert(STR_APID, str_can_tx);
can_senders.insert(GPS_APID, gps_can_tx);
// get package id hashmap
let package_ids_rx = load_package_id_to_apids();
@ -297,6 +302,7 @@ fn main() {
let aocs_tm_store = tm_store.clone();
let mut aocs_data_not_threadsafe = AocsDataMap::new();
/*
aocs_data_not_threadsafe
.update_value(1, AocsDataType::float_value(1.0))
.unwrap();
@ -336,73 +342,125 @@ fn main() {
aocs_data_not_threadsafe
.update_value(13, AocsDataType::float_value(13.0))
.unwrap();
*/
let aocs_data = Arc::new(Mutex::new(aocs_data_not_threadsafe));
let power_switcher_aocs = power_switcher.clone();
info!("Starting AOCS task");
let builder5 = thread::Builder::new().name("AOCSThread".into());
let jh5 = builder5.spawn(move || {
let mut aocs_housekeeper = AocsHousekeeper::new_with_collection_interval(
aocs_data.clone(),
aocs_hk_rx,
aocs_seq_count_provider.clone(),
aocs_hk_seq_count_provider,
aocs_tm_store.clone(),
aocs_tm_funnel_tx.clone(),
reporter_aocs.clone(),
Duration::seconds(1),
);
let aocs_controller_verif_helper = VerifHelper::new(verif_reporter.clone());
let aocs_verif_helper = VerifHelper::new(verif_reporter.clone());
let aocs_controller_mode_helper = ModeHelper::new(
AOCS_APID,
aocs_seq_count_provider.clone(),
aocs_ctrl_seq_count_provider.clone(),
aocs_tm_store.clone(),
aocs_tm_funnel_tx.clone(),
);
let mut aocs_controller = AocsController::new(
aocs_thread_rx,
mgm_tx,
css_tx,
str_tx,
aocs_seq_count_provider.clone(),
aocs_controller_verif_helper,
mgm_tx.clone(),
css_tx.clone(),
str_tx.clone(),
gps_tx.clone(),
aocs_ctrl_seq_count_provider.clone(),
aocs_verif_helper.clone(),
aocs_controller_mode_helper,
);
let mgm_mode_helper = ModeHelper::new(
MGM_APID,
mgm_seq_count_provider,
aocs_tm_store.clone(),
aocs_tm_funnel_tx.clone(),
);
let mut mgm_handler = MGMHandler::new(
power_switcher.clone(),
DeviceId::MGM1,
DeviceState::Off,
aocs_data.clone(),
can_tx_sender.clone(),
mgm_can_rx,
mgm_rx,
aocs_verif_helper.clone(),
mgm_mode_helper,
);
let str_mode_helper = ModeHelper::new(
STR_APID,
str_seq_count_provider,
aocs_tm_store.clone(),
aocs_tm_funnel_tx.clone(),
);
let mut str_handler = STRHandler::new(
power_switcher.clone(),
DeviceId::StarTracker,
DeviceState::Off,
aocs_data.clone(),
can_tx_sender.clone(),
str_can_rx,
str_rx,
aocs_verif_helper.clone(),
str_mode_helper,
);
let css_mode_helper = ModeHelper::new(
CSS_APID,
css_seq_count_provider,
aocs_tm_store.clone(),
aocs_tm_funnel_tx.clone(),
);
let mut css_handler = CSSHandler::new(
power_switcher.clone(),
DeviceId::SunSensor1,
DeviceState::Off,
aocs_data.clone(),
can_tx_sender.clone(),
css_can_rx,
css_rx,
aocs_verif_helper.clone(),
css_mode_helper,
);
let gps_mode_helper = ModeHelper::new(
GPS_APID,
gps_seq_count_provider,
aocs_tm_store.clone(),
aocs_tm_funnel_tx.clone(),
);
let mut gps_handler = GPSHandler::new(
power_switcher.clone(),
DeviceId::GPS,
DeviceState::Off,
aocs_data.clone(),
can_tx_sender.clone(),
gps_can_rx,
gps_rx,
aocs_verif_helper.clone(),
gps_mode_helper,
);
loop {
aocs_housekeeper.periodic_op();
aocs_controller.periodic_op();
}
/*let mut mgm_handler = MGMHandler::new(
power_switcher_aocs.clone(),
DeviceId::MGM1,
can_tx_sender.clone(),
mgm_can_rx,
mgm_action_rx,
);
let aocs_sensor_data = Arc::new(Mutex::new(AocsDataMap::new()));
let mut aocs_housekeeper = AocsHousekeeper::new(
aocs_sensor_data.clone(),
aocs_thread_rx,
aocs_seq_count_provider,
aocs_tm_store,
aocs_tm_funnel_tx,
reporter_aocs,
);
loop {
mgm_handler.periodic_op();
let mut locked_sensor_data = aocs_sensor_data.lock().unwrap();
locked_sensor_data.update_mgm_data(&mgm_handler.get_data_ref());
drop(locked_sensor_data);
aocs_housekeeper.handle_hk_request();
str_handler.periodic_op();
css_handler.periodic_op();
gps_handler.periodic_op();
}
*/
core_aocs_loop();
});
jh0.unwrap()

25
src/power_handler.rs
View File

@ -108,8 +108,6 @@ impl PCDU {
let mut map_lock = self.device_state_map.lock().unwrap();
*map_lock.get_mut(&dev_id).unwrap() = SwitchState::Unknown;
// TODO: potentially change bus logic -> remove acceptance and verification of power off/on, since status is simply called in next step anyway
self.can_rx.recv().unwrap();
self.can_rx.recv().unwrap();
Ok(())
} else {
Err(())
@ -125,8 +123,6 @@ impl PCDU {
.unwrap();
let mut map_lock = self.device_state_map.lock().unwrap();
*map_lock.get_mut(&dev_id).unwrap() = SwitchState::Unknown;
self.can_rx.recv().unwrap();
self.can_rx.recv().unwrap();
Ok(())
} else {
Err(())
@ -134,7 +130,7 @@ impl PCDU {
}
pub fn update_states_helper(&mut self, dev_id: &DeviceId) -> Result<(), ()> {
let _switch_id: SwitchId = *dev_id as u16;
/*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
@ -165,6 +161,9 @@ impl PCDU {
}
Err(_) => Err(()),
}
*/
Ok(())
}
pub fn update_all_states_helper(&mut self) -> Result<(), ()> {
@ -180,11 +179,18 @@ impl PCDU {
Ok(())
}
pub fn update_switch_states(&mut self, switch_id: SwitchId) -> Result<(), ()> {
pub fn update_switch_states(&mut self, switch_id: SwitchId, mut switch_state: SwitchState) -> 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),
//DeviceId::All => self.update_all_states_helper(),
DeviceId::All => {Ok(())}
_ => {
let mut map = self.device_state_map.lock().expect("error locking data map");
if let Some(mut val) = map.get_mut(&dev_id) {
val = &mut switch_state;
}
Ok(())
},
}
} else {
Err(())
@ -197,6 +203,7 @@ impl PCDU {
match switch_state {
SwitchState::Off => match self.send_power_off(switch_id) {
Ok(_) => {
self.update_switch_states(switch_id, switch_state).expect("error updating switch states");
i = i + 1;
}
Err(_) => {
@ -205,6 +212,7 @@ impl PCDU {
},
SwitchState::On => match self.send_power_on(switch_id) {
Ok(_) => {
self.update_switch_states(switch_id, switch_state).expect("error updating switch states");
i = i + 1;
}
Err(_) => {
@ -213,7 +221,6 @@ impl PCDU {
},
_ => {}
}
self.update_switch_states(switch_id).unwrap();
}
return Ok(i);
}

28
src/pus.rs
View File

@ -596,7 +596,7 @@ impl PusReceiver {
}
*/
let app_data = app_data.unwrap();
let mut invalid_subservice_handler = || {
self.tm_args
.verif_reporter
@ -650,17 +650,21 @@ impl PusReceiver {
)
.expect("Sending start failure TM failed");
}
// Should never fail after size check
let mode_submode = ModeAndSubmode::from_be_bytes(
app_data[0..ModeAndSubmode::raw_len()].try_into().unwrap(),
)
.unwrap();
info!("{:?}", mode_submode);
//forward_mode_request(target_id, ModeRequest::SetMode(mode_submode));
Some(RequestWithToken(
Request::ModeRequest(ModeRequest::SetMode(mode_submode)),
Some(token),
))
if let Some(app_data) = app_data {
// Should never fail after size check
let mode_submode = ModeAndSubmode::from_be_bytes(
app_data[0..ModeAndSubmode::raw_len()].try_into().unwrap(),
)
.unwrap();
info!("{:?}", mode_submode);
//forward_mode_request(target_id, ModeRequest::SetMode(mode_submode));
Some(RequestWithToken(
Request::ModeRequest(ModeRequest::SetMode(mode_submode)),
Some(token),
))
} else {
None
}
}
Subservice::TcReadMode => {
//let target_id = u32::from_be_bytes(app_data[0..4].try_into().unwrap());

1
src/tmtc.rs
View File

@ -36,6 +36,7 @@ pub const CSS_APID: u16 = 0x53;
pub const STR_APID: u16 = 0x54;
pub const MGT_APID: u16 = 0x55;
pub const RWL_APID: u16 = 0x56;
pub const GPS_APID: u16 = 0x57;
pub struct OtherArgs {
pub sock_addr: SocketAddr,