Merge remote-tracking branch 'origin/develop' into feature_imtq_assy
All checks were successful
EIVE/eive-obsw/pipeline/pr-develop This commit looks good

This commit is contained in:
Robin Müller 2023-03-02 18:40:11 +01:00
commit 04e3d2486d
No known key found for this signature in database
GPG Key ID: 11D4952C8CCEF814
33 changed files with 244 additions and 109 deletions

View File

@ -26,6 +26,11 @@ will consitute of a breaking change warranting a new major release:
- IMTQ: Sets were filled with wrong data, e.g. Raw MTM was filled with calibrated MTM measurements. - IMTQ: Sets were filled with wrong data, e.g. Raw MTM was filled with calibrated MTM measurements.
- Set RM3100 dataset to valid. - Set RM3100 dataset to valid.
- Fixed units in calculation of ACS control laws safe and detumble.
## Added
- Added Syrlinks Assembly object to allow recovery handling and to fix faulty FDIR behaviour.
## Added ## Added

View File

@ -1,7 +1,7 @@
/** /**
* @brief Auto-generated event translation file. Contains 267 translations. * @brief Auto-generated event translation file. Contains 267 translations.
* @details * @details
* Generated on: 2023-03-01 18:34:32 * Generated on: 2023-03-02 17:08:11
*/ */
#include "translateEvents.h" #include "translateEvents.h"

View File

@ -1,8 +1,8 @@
/** /**
* @brief Auto-generated object translation file. * @brief Auto-generated object translation file.
* @details * @details
* Contains 157 translations. * Contains 158 translations.
* Generated on: 2023-03-01 18:34:32 * Generated on: 2023-03-02 17:08:11
*/ */
#include "translateObjects.h" #include "translateObjects.h"
@ -142,8 +142,9 @@ const char *HEATER_7_HPA_STRING = "HEATER_7_HPA";
const char *ACS_BOARD_ASS_STRING = "ACS_BOARD_ASS"; const char *ACS_BOARD_ASS_STRING = "ACS_BOARD_ASS";
const char *SUS_BOARD_ASS_STRING = "SUS_BOARD_ASS"; const char *SUS_BOARD_ASS_STRING = "SUS_BOARD_ASS";
const char *TCS_BOARD_ASS_STRING = "TCS_BOARD_ASS"; const char *TCS_BOARD_ASS_STRING = "TCS_BOARD_ASS";
const char *RW_ASS_STRING = "RW_ASS"; const char *RW_ASSY_STRING = "RW_ASSY";
const char *CAM_SWITCHER_STRING = "CAM_SWITCHER"; const char *CAM_SWITCHER_STRING = "CAM_SWITCHER";
const char *SYRLINKS_ASSY_STRING = "SYRLINKS_ASSY";
const char *TM_FUNNEL_STRING = "TM_FUNNEL"; const char *TM_FUNNEL_STRING = "TM_FUNNEL";
const char *PUS_TM_FUNNEL_STRING = "PUS_TM_FUNNEL"; const char *PUS_TM_FUNNEL_STRING = "PUS_TM_FUNNEL";
const char *CFDP_TM_FUNNEL_STRING = "CFDP_TM_FUNNEL"; const char *CFDP_TM_FUNNEL_STRING = "CFDP_TM_FUNNEL";
@ -439,9 +440,11 @@ const char *translateObject(object_id_t object) {
case 0x73000003: case 0x73000003:
return TCS_BOARD_ASS_STRING; return TCS_BOARD_ASS_STRING;
case 0x73000004: case 0x73000004:
return RW_ASS_STRING; return RW_ASSY_STRING;
case 0x73000006: case 0x73000006:
return CAM_SWITCHER_STRING; return CAM_SWITCHER_STRING;
case 0x73000007:
return SYRLINKS_ASSY_STRING;
case 0x73000100: case 0x73000100:
return TM_FUNNEL_STRING; return TM_FUNNEL_STRING;
case 0x73000101: case 0x73000101:

View File

@ -9,6 +9,7 @@
#include <mission/devices/MgmRm3100CustomHandler.h> #include <mission/devices/MgmRm3100CustomHandler.h>
#include <mission/system/objects/CamSwitcher.h> #include <mission/system/objects/CamSwitcher.h>
#include <mission/system/objects/ImtqAssembly.h> #include <mission/system/objects/ImtqAssembly.h>
#include <mission/system/objects/SyrlinksAssembly.h>
#include "OBSWConfig.h" #include "OBSWConfig.h"
#include "bsp_q7s/boardtest/Q7STestTask.h" #include "bsp_q7s/boardtest/Q7STestTask.h"
@ -580,12 +581,14 @@ void ObjectFactory::createSyrlinksComponents(PowerSwitchIF* pwrSwitcher) {
syrlinks::MAX_REPLY_SIZE, UartModes::NON_CANONICAL); syrlinks::MAX_REPLY_SIZE, UartModes::NON_CANONICAL);
syrlinksUartCookie->setParityEven(); syrlinksUartCookie->setParityEven();
auto* syrlinksAssy = new SyrlinksAssembly(objects::SYRLINKS_ASSY);
syrlinksAssy->connectModeTreeParent(satsystem::com::SUBSYSTEM);
auto syrlinksFdir = new SyrlinksFdir(objects::SYRLINKS_HANDLER); auto syrlinksFdir = new SyrlinksFdir(objects::SYRLINKS_HANDLER);
auto syrlinksHandler = auto syrlinksHandler =
new SyrlinksHandler(objects::SYRLINKS_HANDLER, objects::UART_COM_IF, syrlinksUartCookie, new SyrlinksHandler(objects::SYRLINKS_HANDLER, objects::UART_COM_IF, syrlinksUartCookie,
pcdu::PDU1_CH1_SYRLINKS_12V, syrlinksFdir); pcdu::PDU1_CH1_SYRLINKS_12V, syrlinksFdir);
syrlinksHandler->setPowerSwitcher(pwrSwitcher); syrlinksHandler->setPowerSwitcher(pwrSwitcher);
syrlinksHandler->connectModeTreeParent(satsystem::com::SUBSYSTEM); syrlinksHandler->connectModeTreeParent(*syrlinksAssy);
#if OBSW_DEBUG_SYRLINKS == 1 #if OBSW_DEBUG_SYRLINKS == 1
syrlinksHandler->setDebugMode(true); syrlinksHandler->setDebugMode(true);
#endif #endif

View File

@ -150,6 +150,10 @@ void scheduling::initTasks() {
if (result != returnvalue::OK) { if (result != returnvalue::OK) {
scheduling::printAddObjectError("COM_SUBSYSTEM", objects::COM_SUBSYSTEM); scheduling::printAddObjectError("COM_SUBSYSTEM", objects::COM_SUBSYSTEM);
} }
result = genericSysTask->addComponent(objects::SYRLINKS_ASSY);
if (result != returnvalue::OK) {
scheduling::printAddObjectError("SYRLINKS_ASSY", objects::SYRLINKS_ASSY);
}
result = genericSysTask->addComponent(objects::PL_SUBSYSTEM); result = genericSysTask->addComponent(objects::PL_SUBSYSTEM);
if (result != returnvalue::OK) { if (result != returnvalue::OK) {
scheduling::printAddObjectError("PL_SUBSYSTEM", objects::PL_SUBSYSTEM); scheduling::printAddObjectError("PL_SUBSYSTEM", objects::PL_SUBSYSTEM);
@ -249,9 +253,9 @@ void scheduling::initTasks() {
} }
#endif /* OBSW_ADD_ACS_HANDLERS */ #endif /* OBSW_ADD_ACS_HANDLERS */
#if OBSW_ADD_RW == 1 #if OBSW_ADD_RW == 1
result = acsSysTask->addComponent(objects::RW_ASS); result = acsSysTask->addComponent(objects::RW_ASSY);
if (result != returnvalue::OK) { if (result != returnvalue::OK) {
scheduling::printAddObjectError("RW_ASS", objects::RW_ASS); scheduling::printAddObjectError("RW_ASS", objects::RW_ASSY);
} }
#endif #endif
#if OBSW_ADD_SUS_BOARD_ASS == 1 #if OBSW_ADD_SUS_BOARD_ASS == 1

View File

@ -142,8 +142,9 @@ enum commonObjects : uint32_t {
ACS_BOARD_ASS = 0x73000001, ACS_BOARD_ASS = 0x73000001,
SUS_BOARD_ASS = 0x73000002, SUS_BOARD_ASS = 0x73000002,
TCS_BOARD_ASS = 0x73000003, TCS_BOARD_ASS = 0x73000003,
RW_ASS = 0x73000004, RW_ASSY = 0x73000004,
CAM_SWITCHER = 0x73000006, CAM_SWITCHER = 0x73000006,
SYRLINKS_ASSY = 0x73000007,
IMTQ_ASSY = 0x73000008, IMTQ_ASSY = 0x73000008,
EIVE_SYSTEM = 0x73010000, EIVE_SYSTEM = 0x73010000,
ACS_SUBSYSTEM = 0x73010001, ACS_SUBSYSTEM = 0x73010001,

View File

@ -134,8 +134,9 @@
0x73000001;ACS_BOARD_ASS 0x73000001;ACS_BOARD_ASS
0x73000002;SUS_BOARD_ASS 0x73000002;SUS_BOARD_ASS
0x73000003;TCS_BOARD_ASS 0x73000003;TCS_BOARD_ASS
0x73000004;RW_ASS 0x73000004;RW_ASSY
0x73000006;CAM_SWITCHER 0x73000006;CAM_SWITCHER
0x73000007;SYRLINKS_ASSY
0x73000100;TM_FUNNEL 0x73000100;TM_FUNNEL
0x73000101;PUS_TM_FUNNEL 0x73000101;PUS_TM_FUNNEL
0x73000102;CFDP_TM_FUNNEL 0x73000102;CFDP_TM_FUNNEL

1 0x42694269 TEST_TASK
134 0x73000001 ACS_BOARD_ASS
135 0x73000002 SUS_BOARD_ASS
136 0x73000003 TCS_BOARD_ASS
137 0x73000004 RW_ASS RW_ASSY
138 0x73000006 CAM_SWITCHER
139 0x73000007 SYRLINKS_ASSY
140 0x73000100 TM_FUNNEL
141 0x73000101 PUS_TM_FUNNEL
142 0x73000102 CFDP_TM_FUNNEL

View File

@ -139,8 +139,9 @@
0x73000001;ACS_BOARD_ASS 0x73000001;ACS_BOARD_ASS
0x73000002;SUS_BOARD_ASS 0x73000002;SUS_BOARD_ASS
0x73000003;TCS_BOARD_ASS 0x73000003;TCS_BOARD_ASS
0x73000004;RW_ASS 0x73000004;RW_ASSY
0x73000006;CAM_SWITCHER 0x73000006;CAM_SWITCHER
0x73000007;SYRLINKS_ASSY
0x73000100;TM_FUNNEL 0x73000100;TM_FUNNEL
0x73000101;PUS_TM_FUNNEL 0x73000101;PUS_TM_FUNNEL
0x73000102;CFDP_TM_FUNNEL 0x73000102;CFDP_TM_FUNNEL

1 0x00005060 P60DOCK_TEST_TASK
139 0x73000001 ACS_BOARD_ASS
140 0x73000002 SUS_BOARD_ASS
141 0x73000003 TCS_BOARD_ASS
142 0x73000004 RW_ASS RW_ASSY
143 0x73000006 CAM_SWITCHER
144 0x73000007 SYRLINKS_ASSY
145 0x73000100 TM_FUNNEL
146 0x73000101 PUS_TM_FUNNEL
147 0x73000102 CFDP_TM_FUNNEL

View File

@ -1,7 +1,7 @@
/** /**
* @brief Auto-generated event translation file. Contains 267 translations. * @brief Auto-generated event translation file. Contains 267 translations.
* @details * @details
* Generated on: 2023-03-01 18:34:32 * Generated on: 2023-03-02 17:08:11
*/ */
#include "translateEvents.h" #include "translateEvents.h"

View File

@ -1,8 +1,8 @@
/** /**
* @brief Auto-generated object translation file. * @brief Auto-generated object translation file.
* @details * @details
* Contains 161 translations. * Contains 162 translations.
* Generated on: 2023-03-01 18:34:32 * Generated on: 2023-03-02 17:08:11
*/ */
#include "translateObjects.h" #include "translateObjects.h"
@ -147,8 +147,9 @@ const char *HEATER_7_HPA_STRING = "HEATER_7_HPA";
const char *ACS_BOARD_ASS_STRING = "ACS_BOARD_ASS"; const char *ACS_BOARD_ASS_STRING = "ACS_BOARD_ASS";
const char *SUS_BOARD_ASS_STRING = "SUS_BOARD_ASS"; const char *SUS_BOARD_ASS_STRING = "SUS_BOARD_ASS";
const char *TCS_BOARD_ASS_STRING = "TCS_BOARD_ASS"; const char *TCS_BOARD_ASS_STRING = "TCS_BOARD_ASS";
const char *RW_ASS_STRING = "RW_ASS"; const char *RW_ASSY_STRING = "RW_ASSY";
const char *CAM_SWITCHER_STRING = "CAM_SWITCHER"; const char *CAM_SWITCHER_STRING = "CAM_SWITCHER";
const char *SYRLINKS_ASSY_STRING = "SYRLINKS_ASSY";
const char *TM_FUNNEL_STRING = "TM_FUNNEL"; const char *TM_FUNNEL_STRING = "TM_FUNNEL";
const char *PUS_TM_FUNNEL_STRING = "PUS_TM_FUNNEL"; const char *PUS_TM_FUNNEL_STRING = "PUS_TM_FUNNEL";
const char *CFDP_TM_FUNNEL_STRING = "CFDP_TM_FUNNEL"; const char *CFDP_TM_FUNNEL_STRING = "CFDP_TM_FUNNEL";
@ -453,9 +454,11 @@ const char *translateObject(object_id_t object) {
case 0x73000003: case 0x73000003:
return TCS_BOARD_ASS_STRING; return TCS_BOARD_ASS_STRING;
case 0x73000004: case 0x73000004:
return RW_ASS_STRING; return RW_ASSY_STRING;
case 0x73000006: case 0x73000006:
return CAM_SWITCHER_STRING; return CAM_SWITCHER_STRING;
case 0x73000007:
return SYRLINKS_ASSY_STRING;
case 0x73000100: case 0x73000100:
return TM_FUNNEL_STRING; return TM_FUNNEL_STRING;
case 0x73000101: case 0x73000101:

View File

@ -1,7 +1,7 @@
/** /**
* @brief Auto-generated event translation file. Contains 267 translations. * @brief Auto-generated event translation file. Contains 267 translations.
* @details * @details
* Generated on: 2023-03-01 18:34:32 * Generated on: 2023-03-02 17:08:11
*/ */
#include "translateEvents.h" #include "translateEvents.h"

View File

@ -1,8 +1,8 @@
/** /**
* @brief Auto-generated object translation file. * @brief Auto-generated object translation file.
* @details * @details
* Contains 161 translations. * Contains 162 translations.
* Generated on: 2023-03-01 18:34:32 * Generated on: 2023-03-02 17:08:11
*/ */
#include "translateObjects.h" #include "translateObjects.h"
@ -147,8 +147,9 @@ const char *HEATER_7_HPA_STRING = "HEATER_7_HPA";
const char *ACS_BOARD_ASS_STRING = "ACS_BOARD_ASS"; const char *ACS_BOARD_ASS_STRING = "ACS_BOARD_ASS";
const char *SUS_BOARD_ASS_STRING = "SUS_BOARD_ASS"; const char *SUS_BOARD_ASS_STRING = "SUS_BOARD_ASS";
const char *TCS_BOARD_ASS_STRING = "TCS_BOARD_ASS"; const char *TCS_BOARD_ASS_STRING = "TCS_BOARD_ASS";
const char *RW_ASS_STRING = "RW_ASS"; const char *RW_ASSY_STRING = "RW_ASSY";
const char *CAM_SWITCHER_STRING = "CAM_SWITCHER"; const char *CAM_SWITCHER_STRING = "CAM_SWITCHER";
const char *SYRLINKS_ASSY_STRING = "SYRLINKS_ASSY";
const char *TM_FUNNEL_STRING = "TM_FUNNEL"; const char *TM_FUNNEL_STRING = "TM_FUNNEL";
const char *PUS_TM_FUNNEL_STRING = "PUS_TM_FUNNEL"; const char *PUS_TM_FUNNEL_STRING = "PUS_TM_FUNNEL";
const char *CFDP_TM_FUNNEL_STRING = "CFDP_TM_FUNNEL"; const char *CFDP_TM_FUNNEL_STRING = "CFDP_TM_FUNNEL";
@ -453,9 +454,11 @@ const char *translateObject(object_id_t object) {
case 0x73000003: case 0x73000003:
return TCS_BOARD_ASS_STRING; return TCS_BOARD_ASS_STRING;
case 0x73000004: case 0x73000004:
return RW_ASS_STRING; return RW_ASSY_STRING;
case 0x73000006: case 0x73000006:
return CAM_SWITCHER_STRING; return CAM_SWITCHER_STRING;
case 0x73000007:
return SYRLINKS_ASSY_STRING;
case 0x73000100: case 0x73000100:
return TM_FUNNEL_STRING; return TM_FUNNEL_STRING;
case 0x73000101: case 0x73000101:

View File

@ -157,20 +157,23 @@ void AcsController::performSafe() {
guidance.getTargetParamsSafe(sunTargetDir, satRateSafe); guidance.getTargetParamsSafe(sunTargetDir, satRateSafe);
// if MEKF is working // if MEKF is working
double magMomMtq[3] = {0, 0, 0}, errAng = 0.0; double magMomMtq[3] = {0, 0, 0}, errAng = 0.0;
bool magMomMtqValid = false;
if (result == MultiplicativeKalmanFilter::MEKF_RUNNING) { if (result == MultiplicativeKalmanFilter::MEKF_RUNNING) {
safeCtrl.safeMekf(now, mekfData.quatMekf.value, mekfData.quatMekf.isValid(), result = safeCtrl.safeMekf(now, mekfData.quatMekf.value, mekfData.quatMekf.isValid(),
mgmDataProcessed.magIgrfModel.value, mgmDataProcessed.magIgrfModel.isValid(), mgmDataProcessed.magIgrfModel.value,
susDataProcessed.sunIjkModel.value, susDataProcessed.isValid(), mgmDataProcessed.magIgrfModel.isValid(),
mekfData.satRotRateMekf.value, mekfData.satRotRateMekf.isValid(), susDataProcessed.sunIjkModel.value, susDataProcessed.isValid(),
sunTargetDir, satRateSafe, &errAng, magMomMtq, &magMomMtqValid); mekfData.satRotRateMekf.value, mekfData.satRotRateMekf.isValid(),
sunTargetDir, satRateSafe, &errAng, magMomMtq);
} else { } else {
safeCtrl.safeNoMekf( result = safeCtrl.safeNoMekf(
now, susDataProcessed.susVecTot.value, susDataProcessed.susVecTot.isValid(), now, susDataProcessed.susVecTot.value, susDataProcessed.susVecTot.isValid(),
susDataProcessed.susVecTotDerivative.value, susDataProcessed.susVecTotDerivative.isValid(), susDataProcessed.susVecTotDerivative.value, susDataProcessed.susVecTotDerivative.isValid(),
mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(),
mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTotDerivative.isValid(),
sunTargetDir, satRateSafe, &errAng, magMomMtq, &magMomMtqValid); sunTargetDir, satRateSafe, &errAng, magMomMtq);
}
if (result == returnvalue::FAILED) {
// ToDo: this should never ever happen or we are dead. prob add an event at least
} }
actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs); actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs);

View File

@ -1,11 +1,3 @@
/*
* Detumble.cpp
*
* Created on: 17 Aug 2022
* Author: Robin Marquardt
*/
#include "Detumble.h" #include "Detumble.h"
#include <fsfw/globalfunctions/constants.h> #include <fsfw/globalfunctions/constants.h>
@ -31,6 +23,12 @@ ReturnValue_t Detumble::bDotLaw(const double *magRate, const bool magRateValid,
if (!magRateValid || !magFieldValid) { if (!magRateValid || !magFieldValid) {
return DETUMBLE_NO_SENSORDATA; return DETUMBLE_NO_SENSORDATA;
} }
// change unit from uT to T
double magFieldT[3] = {0, 0, 0}, magRateT[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(magField, 1e-6, magFieldT, 3);
VectorOperations<double>::mulScalar(magRate, 1e-6, magRateT, 3);
double gain = detumbleParameter->gainD; double gain = detumbleParameter->gainD;
double factor = -gain / pow(VectorOperations<double>::norm(magField, 3), 2); double factor = -gain / pow(VectorOperations<double>::norm(magField, 3), 2);
VectorOperations<double>::mulScalar(magRate, factor, magMom, 3); VectorOperations<double>::mulScalar(magRate, factor, magMom, 3);

View File

@ -32,15 +32,13 @@ ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid,
double *magFieldModel, bool magFieldModelValid, double *magFieldModel, bool magFieldModelValid,
double *sunDirModel, bool sunDirModelValid, double *satRateMekf, double *sunDirModel, bool sunDirModelValid, double *satRateMekf,
bool rateMekfValid, double *sunDirRef, double *satRatRef, bool rateMekfValid, double *sunDirRef, double *satRatRef,
double *outputAngle, double *outputMagMomB, bool *outputValid) { double *outputAngle, double *outputMagMomB) {
if (!quatBJValid || !magFieldModelValid || !sunDirModelValid || !rateMekfValid) { if (!quatBJValid || !magFieldModelValid || !sunDirModelValid || !rateMekfValid) {
*outputValid = false;
return SAFECTRL_MEKF_INPUT_INVALID; return SAFECTRL_MEKF_INPUT_INVALID;
} }
double kRate = 0, kAlign = 0; double kRate = safeModeControllerParameters->k_rate_mekf;
kRate = safeModeControllerParameters->k_rate_mekf; double kAlign = safeModeControllerParameters->k_align_mekf;
kAlign = safeModeControllerParameters->k_align_mekf;
// Calc sunDirB ,magFieldB with mekf output and model // Calc sunDirB ,magFieldB with mekf output and model
double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
@ -49,22 +47,22 @@ ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid,
MatrixOperations<double>::multiply(*dcmBJ, sunDirModel, sunDirB, 3, 3, 1); MatrixOperations<double>::multiply(*dcmBJ, sunDirModel, sunDirB, 3, 3, 1);
MatrixOperations<double>::multiply(*dcmBJ, magFieldModel, magFieldB, 3, 3, 1); MatrixOperations<double>::multiply(*dcmBJ, magFieldModel, magFieldB, 3, 3, 1);
double crossSun[3] = {0, 0, 0}; // change unit from uT to T
VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldB, 3);
double crossSun[3] = {0, 0, 0};
VectorOperations<double>::cross(sunDirRef, sunDirB, crossSun); VectorOperations<double>::cross(sunDirRef, sunDirB, crossSun);
double normCrossSun = VectorOperations<double>::norm(crossSun, 3); double normCrossSun = VectorOperations<double>::norm(crossSun, 3);
// calc angle alpha between sunDirRef and sunDIr // calc angle alpha between sunDirRef and sunDIr
double alpha = 0, dotSun = 0; double dotSun = VectorOperations<double>::dot(sunDirRef, sunDirB);
dotSun = VectorOperations<double>::dot(sunDirRef, sunDirB); double alpha = acos(dotSun);
alpha = acos(dotSun);
// Law Torque calculations // Law Torque calculations
double torqueCmd[3] = {0, 0, 0}, torqueAlign[3] = {0, 0, 0}, torqueRate[3] = {0, 0, 0}, double torqueCmd[3] = {0, 0, 0}, torqueAlign[3] = {0, 0, 0}, torqueRate[3] = {0, 0, 0},
torqueAll[3] = {0, 0, 0}; torqueAll[3] = {0, 0, 0};
double scalarFac = 0; double scalarFac = kAlign * alpha / normCrossSun;
scalarFac = kAlign * alpha / normCrossSun;
VectorOperations<double>::mulScalar(crossSun, scalarFac, torqueAlign, 3); VectorOperations<double>::mulScalar(crossSun, scalarFac, torqueAlign, 3);
double rateSafeMode[3] = {0, 0, 0}; double rateSafeMode[3] = {0, 0, 0};
@ -82,23 +80,22 @@ ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid,
VectorOperations<double>::mulScalar(torqueMgt, 1 / pow(normMag, 2), outputMagMomB, 3); VectorOperations<double>::mulScalar(torqueMgt, 1 / pow(normMag, 2), outputMagMomB, 3);
*outputAngle = alpha; *outputAngle = alpha;
*outputValid = true;
return returnvalue::OK; return returnvalue::OK;
} }
// Will be the version in worst case scenario in event of no working MEKF (nor GYRs) // Will be the version in worst case scenario in event of no working MEKF (nor GYRs)
void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, double *sunRateB, ReturnValue_t SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, double *sunRateB,
bool sunRateBValid, double *magFieldB, bool magFieldBValid, bool sunRateBValid, double *magFieldB, bool magFieldBValid,
double *magRateB, bool magRateBValid, double *sunDirRef, double *magRateB, bool magRateBValid, double *sunDirRef,
double *satRateRef, double *outputAngle, double *outputMagMomB, double *satRateRef, double *outputAngle, double *outputMagMomB) {
bool *outputValid) {
// Check for invalid Inputs // Check for invalid Inputs
if (!susDirBValid || !magFieldBValid || !magRateBValid) { if (!susDirBValid || !magFieldBValid || !magRateBValid) {
*outputValid = false; return returnvalue::FAILED;
return;
} }
// change unit from uT to T
VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldB, 3);
// normalize sunDir and magDir // normalize sunDir and magDir
double magDirB[3] = {0, 0, 0}; double magDirB[3] = {0, 0, 0};
VectorOperations<double>::normalize(magFieldB, magDirB, 3); VectorOperations<double>::normalize(magFieldB, magDirB, 3);
@ -108,13 +105,11 @@ void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, doubl
double cosAngleSunMag = VectorOperations<double>::dot(magDirB, susDirB); double cosAngleSunMag = VectorOperations<double>::dot(magDirB, susDirB);
// Rate parallel to sun direction and magnetic field direction // Rate parallel to sun direction and magnetic field direction
double rateParaSun = 0, rateParaMag = 0; double dotSunRateMag = VectorOperations<double>::dot(sunRateB, magDirB);
double dotSunRateMag = 0, dotmagRateSun = 0, rateFactor = 0; double dotmagRateSun = VectorOperations<double>::dot(magRateB, susDirB);
dotSunRateMag = VectorOperations<double>::dot(sunRateB, magDirB); double rateFactor = 1 - pow(cosAngleSunMag, 2);
dotmagRateSun = VectorOperations<double>::dot(magRateB, susDirB); double rateParaSun = (dotmagRateSun + cosAngleSunMag * dotSunRateMag) / rateFactor;
rateFactor = 1 - pow(cosAngleSunMag, 2); double rateParaMag = (dotSunRateMag + cosAngleSunMag * dotmagRateSun) / rateFactor;
rateParaSun = (dotmagRateSun + cosAngleSunMag * dotSunRateMag) / rateFactor;
rateParaMag = (dotSunRateMag + cosAngleSunMag * dotmagRateSun) / rateFactor;
// Full rate or estimate // Full rate or estimate
double estSatRate[3] = {0, 0, 0}; double estSatRate[3] = {0, 0, 0};
@ -130,7 +125,7 @@ void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, doubl
* is sufficiently large */ * is sufficiently large */
double angleSunMag = acos(cosAngleSunMag); double angleSunMag = acos(cosAngleSunMag);
if (angleSunMag < safeModeControllerParameters->sunMagAngleMin) { if (angleSunMag < safeModeControllerParameters->sunMagAngleMin) {
return; return returnvalue::FAILED;
} }
// Rate for Torque Calculation // Rate for Torque Calculation
@ -138,9 +133,8 @@ void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, doubl
VectorOperations<double>::subtract(estSatRate, satRateRef, diffRate, 3); VectorOperations<double>::subtract(estSatRate, satRateRef, diffRate, 3);
// Torque Align calculation // Torque Align calculation
double kRateNoMekf = 0, kAlignNoMekf = 0; double kRateNoMekf = safeModeControllerParameters->k_rate_no_mekf;
kRateNoMekf = safeModeControllerParameters->k_rate_no_mekf; double kAlignNoMekf = safeModeControllerParameters->k_align_no_mekf;
kAlignNoMekf = safeModeControllerParameters->k_align_no_mekf;
double cosAngleAlignErr = VectorOperations<double>::dot(sunDirRef, susDirB); double cosAngleAlignErr = VectorOperations<double>::dot(sunDirRef, susDirB);
double crossSusSunRef[3] = {0, 0, 0}; double crossSusSunRef[3] = {0, 0, 0};
@ -171,5 +165,5 @@ void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, doubl
std::memcpy(outputMagMomB, magMomB, 3 * sizeof(double)); std::memcpy(outputMagMomB, magMomB, 3 * sizeof(double));
*outputAngle = angleAlignErr; *outputAngle = angleAlignErr;
*outputValid = true; return returnvalue::OK;
} }

View File

@ -23,14 +23,12 @@ class SafeCtrl {
bool magFieldModelValid, double *sunDirModel, bool sunDirModelValid, bool magFieldModelValid, double *sunDirModel, bool sunDirModelValid,
double *satRateMekf, bool rateMekfValid, double *sunDirRef, double *satRateMekf, bool rateMekfValid, double *sunDirRef,
double *satRatRef, // From Guidance (!) double *satRatRef, // From Guidance (!)
double *outputAngle, double *outputMagMomB, bool *outputValid); double *outputAngle, double *outputMagMomB);
void safeNoMekf(timeval now, double *susDirB, bool susDirBValid, double *sunRateB, ReturnValue_t safeNoMekf(timeval now, double *susDirB, bool susDirBValid, double *sunRateB,
bool sunRateBValid, double *magFieldB, bool magFieldBValid, double *magRateB, bool sunRateBValid, double *magFieldB, bool magFieldBValid,
bool magRateBValid, double *sunDirRef, double *satRateRef, double *outputAngle, double *magRateB, bool magRateBValid, double *sunDirRef,
double *outputMagMomB, bool *outputValid); double *satRateRef, double *outputAngle, double *outputMagMomB);
void idleSunPointing(); // with reaction wheels
protected: protected:
private: private:

View File

@ -139,9 +139,10 @@ void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_, PusTmFun
new CcsdsDistributor(config::EIVE_PUS_APID, objects::CCSDS_PACKET_DISTRIBUTOR); new CcsdsDistributor(config::EIVE_PUS_APID, objects::CCSDS_PACKET_DISTRIBUTOR);
new PusDistributor(config::EIVE_PUS_APID, objects::PUS_PACKET_DISTRIBUTOR, ccsdsDistrib); new PusDistributor(config::EIVE_PUS_APID, objects::PUS_PACKET_DISTRIBUTOR, ccsdsDistrib);
PusTmFunnel::FunnelCfg cfdpFunnelCfg(objects::CFDP_TM_FUNNEL, *tmStore, *ipcStore, 50); PusTmFunnel::FunnelCfg cfdpFunnelCfg(objects::CFDP_TM_FUNNEL, "CfdpTmFunnel", *tmStore, *ipcStore,
50);
*cfdpFunnel = new CfdpTmFunnel(cfdpFunnelCfg, config::EIVE_CFDP_APID); *cfdpFunnel = new CfdpTmFunnel(cfdpFunnelCfg, config::EIVE_CFDP_APID);
PusTmFunnel::FunnelCfg pusFunnelCfg(objects::PUS_TM_FUNNEL, *tmStore, *ipcStore, PusTmFunnel::FunnelCfg pusFunnelCfg(objects::PUS_TM_FUNNEL, "PusTmFunnel", *tmStore, *ipcStore,
config::MAX_PUS_FUNNEL_QUEUE_DEPTH); config::MAX_PUS_FUNNEL_QUEUE_DEPTH);
*pusFunnel = new PusTmFunnel(pusFunnelCfg, *timeStamper, sdcMan); *pusFunnel = new PusTmFunnel(pusFunnelCfg, *timeStamper, sdcMan);
#if OBSW_ADD_TCPIP_SERVERS == 1 #if OBSW_ADD_TCPIP_SERVERS == 1
@ -236,7 +237,7 @@ void ObjectFactory::createRwAssy(PowerSwitchIF& pwrSwitcher, power::Switch_t the
std::array<DeviceHandlerBase*, 4> rws, std::array<DeviceHandlerBase*, 4> rws,
std::array<object_id_t, 4> rwIds) { std::array<object_id_t, 4> rwIds) {
RwHelper rwHelper(rwIds); RwHelper rwHelper(rwIds);
auto* rwAss = new RwAssembly(objects::RW_ASS, &pwrSwitcher, theSwitch, rwHelper); auto* rwAss = new RwAssembly(objects::RW_ASSY, &pwrSwitcher, theSwitch, rwHelper);
for (size_t idx = 0; idx < rwIds.size(); idx++) { for (size_t idx = 0; idx < rwIds.size(); idx++) {
ReturnValue_t result = rws[idx]->connectModeTreeParent(*rwAss); ReturnValue_t result = rws[idx]->connectModeTreeParent(*rwAss);
if (result != returnvalue::OK) { if (result != returnvalue::OK) {

View File

@ -395,7 +395,7 @@ LocalPoolDataSetBase* ImtqHandler::getDataSetHandle(sid_t sid) {
} else if (sid == negZselfTestDataset.getSid()) { } else if (sid == negZselfTestDataset.getSid()) {
return &negZselfTestDataset; return &negZselfTestDataset;
} else { } else {
sif::error << "IMTQHandler::getDataSetHandle: Invalid sid" << std::endl; sif::error << "ImtqHandler::getDataSetHandle: Invalid SID" << std::endl;
return nullptr; return nullptr;
} }
} }

View File

@ -37,7 +37,7 @@ void SusHandler::doShutDown() {
updatePeriodicReply(false, REPLY); updatePeriodicReply(false, REPLY);
commandExecuted = false; commandExecuted = false;
internalState = InternalState::NONE; internalState = InternalState::NONE;
setMode(_MODE_POWER_DOWN); setMode(MODE_OFF);
} }
} }

View File

@ -47,8 +47,8 @@ ReturnValue_t SyrlinksFdir::eventReceived(EventMessage* event) {
} }
// else // else
if (missedReplyCount.incrementAndCheck()) { if (missedReplyCount.incrementAndCheck()) {
// handleRecovery(event->getEvent()); handleRecovery(event->getEvent());
triggerEvent(syrlinks::FDIR_REACTION_IGNORED, event->getEvent(), 0); // triggerEvent(syrlinks::FDIR_REACTION_IGNORED, event->getEvent(), 0);
} }
break; break;
case StorageManagerIF::GET_DATA_FAILED: case StorageManagerIF::GET_DATA_FAILED:
@ -80,7 +80,7 @@ ReturnValue_t SyrlinksFdir::eventReceived(EventMessage* event) {
break; break;
case Fuse::POWER_BELOW_LOW_LIMIT: case Fuse::POWER_BELOW_LOW_LIMIT:
// Device might got stuck during boot, retry. // Device might got stuck during boot, retry.
// handleRecovery(event->getEvent()); handleRecovery(event->getEvent());
triggerEvent(syrlinks::FDIR_REACTION_IGNORED, event->getEvent(), 0); triggerEvent(syrlinks::FDIR_REACTION_IGNORED, event->getEvent(), 0);
break; break;
//****Thermal***** //****Thermal*****

View File

@ -8,6 +8,7 @@ target_sources(
PayloadSubsystem.cpp PayloadSubsystem.cpp
AcsBoardAssembly.cpp AcsBoardAssembly.cpp
ImtqAssembly.cpp ImtqAssembly.cpp
SyrlinksAssembly.cpp
Stack5VHandler.cpp Stack5VHandler.cpp
SusAssembly.cpp SusAssembly.cpp
RwAssembly.cpp RwAssembly.cpp

View File

@ -30,6 +30,7 @@ void ComSubsystem::performChildOperation() {
if (countdownActive) { if (countdownActive) {
checkTransmitterCountdown(); checkTransmitterCountdown();
} }
Subsystem::performChildOperation(); Subsystem::performChildOperation();
} }

View File

@ -235,3 +235,8 @@ void DualLaneAssemblyBase::setPreferredSide(duallane::Submodes submode) {
} }
this->defaultSubmode = submode; this->defaultSubmode = submode;
} }
ReturnValue_t DualLaneAssemblyBase::checkAndHandleHealthState(Mode_t deviceMode,
Submode_t deviceSubmode) {
return returnvalue::OK;
}

View File

@ -74,6 +74,7 @@ class DualLaneAssemblyBase : public AssemblyBase, public ConfirmsFailuresIF {
MessageQueueId_t getEventReceptionQueue() override; MessageQueueId_t getEventReceptionQueue() override;
bool sideSwitchTransition(Mode_t mode, Submode_t submode); bool sideSwitchTransition(Mode_t mode, Submode_t submode);
ReturnValue_t checkAndHandleHealthState(Mode_t deviceMode, Submode_t deviceSubmode);
/** /**
* Implemented by user. Will be called if a full mode operation has finished. * Implemented by user. Will be called if a full mode operation has finished.

View File

@ -24,6 +24,12 @@ ReturnValue_t SusAssembly::commandChildren(Mode_t mode, Submode_t submode) {
modeTable[idx].setMode(MODE_OFF); modeTable[idx].setMode(MODE_OFF);
modeTable[idx].setSubmode(SUBMODE_NONE); modeTable[idx].setSubmode(SUBMODE_NONE);
} }
if (recoveryState == RecoveryState::RECOVERY_IDLE) {
result = checkAndHandleHealthStates(mode, submode);
if (result == NEED_TO_CHANGE_HEALTH) {
return returnvalue::OK;
}
}
if (recoveryState != RecoveryState::RECOVERY_STARTED) { if (recoveryState != RecoveryState::RECOVERY_STARTED) {
if (mode == DeviceHandlerIF::MODE_NORMAL or mode == MODE_ON) { if (mode == DeviceHandlerIF::MODE_NORMAL or mode == MODE_ON) {
result = handleNormalOrOnModeCmd(mode, submode); result = handleNormalOrOnModeCmd(mode, submode);
@ -148,3 +154,23 @@ void SusAssembly::refreshHelperModes() {
helper.susModes[idx] = childrenMap[helper.susIds[idx]].mode; helper.susModes[idx] = childrenMap[helper.susIds[idx]].mode;
} }
} }
ReturnValue_t SusAssembly::checkAndHandleHealthStates(Mode_t deviceMode, Submode_t deviceSubmode) {
using namespace returnvalue;
ReturnValue_t status = returnvalue::OK;
auto overwriteHealthForOneDev = [&](object_id_t dev) {
HealthState health = healthHelper.healthTable->getHealth(dev);
if (health == FAULTY or health == PERMANENT_FAULTY) {
overwriteDeviceHealth(dev, health);
status = NEED_TO_CHANGE_HEALTH;
} else if (health == EXTERNAL_CONTROL) {
modeHelper.setForced(true);
}
};
if (deviceSubmode == duallane::DUAL_MODE) {
for (uint8_t idx = 0; idx < 12; idx++) {
overwriteHealthForOneDev(helper.susIds[idx]);
}
}
return status;
}

View File

@ -66,6 +66,7 @@ class SusAssembly : public DualLaneAssemblyBase {
void powerStateMachine(Mode_t mode, Submode_t submode); void powerStateMachine(Mode_t mode, Submode_t submode);
ReturnValue_t handleNormalOrOnModeCmd(Mode_t mode, Submode_t submode); ReturnValue_t handleNormalOrOnModeCmd(Mode_t mode, Submode_t submode);
void refreshHelperModes(); void refreshHelperModes();
ReturnValue_t checkAndHandleHealthStates(Mode_t deviceMode, Submode_t deviceSubmode);
}; };
#endif /* MISSION_SYSTEM_SUSASSEMBLY_H_ */ #endif /* MISSION_SYSTEM_SUSASSEMBLY_H_ */

View File

@ -0,0 +1,57 @@
#include "SyrlinksAssembly.h"
#include <eive/objects.h>
using namespace returnvalue;
SyrlinksAssembly::SyrlinksAssembly(object_id_t objectId) : AssemblyBase(objectId) {
ModeListEntry entry;
entry.setObject(objects::SYRLINKS_HANDLER);
entry.setMode(MODE_OFF);
entry.setSubmode(SUBMODE_NONE);
commandTable.insert(entry);
}
ReturnValue_t SyrlinksAssembly::commandChildren(Mode_t mode, Submode_t submode) {
commandTable[0].setMode(mode);
commandTable[0].setSubmode(submode);
HybridIterator<ModeListEntry> iter(commandTable.begin(), commandTable.end());
if (recoveryState == RECOVERY_IDLE) {
ReturnValue_t result = checkAndHandleHealthState(mode, submode);
if (result == NEED_TO_CHANGE_HEALTH) {
return OK;
}
}
executeTable(iter);
return returnvalue::OK;
}
ReturnValue_t SyrlinksAssembly::checkChildrenStateOn(Mode_t wantedMode, Submode_t wantedSubmode) {
if (childrenMap[objects::SYRLINKS_HANDLER].mode != wantedMode) {
return NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE;
}
return returnvalue::OK;
}
ReturnValue_t SyrlinksAssembly::isModeCombinationValid(Mode_t mode, Submode_t submode) {
if (mode == MODE_ON or mode == DeviceHandlerIF::MODE_NORMAL or mode == MODE_OFF) {
return returnvalue::OK;
}
return returnvalue::FAILED;
}
ReturnValue_t SyrlinksAssembly::checkAndHandleHealthState(Mode_t deviceMode,
Submode_t deviceSubmode) {
HealthState health = healthHelper.healthTable->getHealth(objects::SYRLINKS_HANDLER);
if (health == FAULTY or health == PERMANENT_FAULTY) {
overwriteDeviceHealth(objects::SYRLINKS_HANDLER, health);
return NEED_TO_CHANGE_HEALTH;
} else if (health == EXTERNAL_CONTROL) {
modeHelper.setForced(true);
}
return OK;
}
void SyrlinksAssembly::handleChildrenLostMode(ReturnValue_t result) {
startTransition(mode, submode);
}

View File

@ -0,0 +1,20 @@
#ifndef MISSION_SYSTEM_OBJECTS_SYRLINKSASSEMBLY_H_
#define MISSION_SYSTEM_OBJECTS_SYRLINKSASSEMBLY_H_
#include <fsfw/devicehandlers/AssemblyBase.h>
class SyrlinksAssembly : public AssemblyBase {
public:
SyrlinksAssembly(object_id_t objectId);
private:
FixedArrayList<ModeListEntry, 1> commandTable;
ReturnValue_t commandChildren(Mode_t mode, Submode_t submode) override;
ReturnValue_t checkChildrenStateOn(Mode_t wantedMode, Submode_t wantedSubmode) override;
ReturnValue_t isModeCombinationValid(Mode_t mode, Submode_t submode) override;
void handleChildrenLostMode(ReturnValue_t result) override;
ReturnValue_t checkAndHandleHealthState(Mode_t deviceMode, Submode_t deviceSubmode);
};
#endif /* MISSION_SYSTEM_OBJECTS_SYRLINKSASSEMBLY_H_ */

View File

@ -110,7 +110,7 @@ Subsystem& satsystem::acs::init() {
iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_PTG_TRANS_0.second); iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_PTG_TRANS_0.second);
iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TRANS_0.second); iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TRANS_0.second);
iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TRANS_0.second); iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TRANS_0.second);
iht(objects::RW_ASS, NML, 0, ACS_TABLE_PTG_TRANS_0.second); iht(objects::RW_ASSY, NML, 0, ACS_TABLE_PTG_TRANS_0.second);
iht(objects::STAR_TRACKER, NML, 0, ACS_TABLE_PTG_TRANS_0.second); iht(objects::STAR_TRACKER, NML, 0, ACS_TABLE_PTG_TRANS_0.second);
check(ACS_SUBSYSTEM.addTable( check(ACS_SUBSYSTEM.addTable(
TableEntry(ACS_TABLE_PTG_TRANS_0.first, &ACS_TABLE_PTG_TRANS_0.second)), TableEntry(ACS_TABLE_PTG_TRANS_0.first, &ACS_TABLE_PTG_TRANS_0.second)),
@ -165,7 +165,7 @@ void buildOffSequence(Subsystem& ss, ModeListEntry& eh) {
iht(objects::IMTQ_ASSY, OFF, 0, ACS_TABLE_OFF_TRANS_1.second); iht(objects::IMTQ_ASSY, OFF, 0, ACS_TABLE_OFF_TRANS_1.second);
iht(objects::STAR_TRACKER, OFF, 0, ACS_TABLE_OFF_TRANS_1.second); iht(objects::STAR_TRACKER, OFF, 0, ACS_TABLE_OFF_TRANS_1.second);
iht(objects::ACS_BOARD_ASS, OFF, 0, ACS_TABLE_OFF_TRANS_1.second); iht(objects::ACS_BOARD_ASS, OFF, 0, ACS_TABLE_OFF_TRANS_1.second);
iht(objects::RW_ASS, OFF, 0, ACS_TABLE_OFF_TRANS_1.second); iht(objects::RW_ASSY, OFF, 0, ACS_TABLE_OFF_TRANS_1.second);
check(ss.addTable(TableEntry(ACS_TABLE_OFF_TRANS_1.first, &ACS_TABLE_OFF_TRANS_1.second)), ctxc); check(ss.addTable(TableEntry(ACS_TABLE_OFF_TRANS_1.first, &ACS_TABLE_OFF_TRANS_1.second)), ctxc);
// Build OFF sequence // Build OFF sequence
@ -207,7 +207,7 @@ void buildSafeSequence(Subsystem& ss, ModeListEntry& eh) {
iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_SAFE_TRANS_0.second); iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_SAFE_TRANS_0.second);
iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_SAFE_TRANS_0.second); iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_SAFE_TRANS_0.second);
iht(objects::STAR_TRACKER, OFF, 0, ACS_TABLE_SAFE_TRANS_0.second); iht(objects::STAR_TRACKER, OFF, 0, ACS_TABLE_SAFE_TRANS_0.second);
iht(objects::RW_ASS, OFF, 0, ACS_TABLE_SAFE_TRANS_0.second); iht(objects::RW_ASSY, OFF, 0, ACS_TABLE_SAFE_TRANS_0.second);
check(ss.addTable(&ACS_TABLE_SAFE_TRANS_0.second, ACS_TABLE_SAFE_TRANS_0.first, false, true), check(ss.addTable(&ACS_TABLE_SAFE_TRANS_0.second, ACS_TABLE_SAFE_TRANS_0.first, false, true),
ctxc); ctxc);
@ -262,7 +262,7 @@ void buildDetumbleSequence(Subsystem& ss, ModeListEntry& eh) {
iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_DETUMBLE_TRANS_0.second); iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_DETUMBLE_TRANS_0.second);
iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_DETUMBLE_TRANS_0.second); iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_DETUMBLE_TRANS_0.second);
iht(objects::STAR_TRACKER, OFF, 0, ACS_TABLE_DETUMBLE_TRANS_0.second); iht(objects::STAR_TRACKER, OFF, 0, ACS_TABLE_DETUMBLE_TRANS_0.second);
iht(objects::RW_ASS, OFF, 0, ACS_TABLE_DETUMBLE_TRANS_0.second); iht(objects::RW_ASSY, OFF, 0, ACS_TABLE_DETUMBLE_TRANS_0.second);
check(ss.addTable(&ACS_TABLE_DETUMBLE_TRANS_0.second, ACS_TABLE_DETUMBLE_TRANS_0.first, false, check(ss.addTable(&ACS_TABLE_DETUMBLE_TRANS_0.second, ACS_TABLE_DETUMBLE_TRANS_0.first, false,
true), true),
ctxc); ctxc);
@ -305,7 +305,7 @@ void buildIdleSequence(Subsystem& ss, ModeListEntry& eh) {
// Build IDLE target // Build IDLE target
iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_IDLE, ACS_TABLE_IDLE_TGT.second); iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_IDLE, ACS_TABLE_IDLE_TGT.second);
iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_IDLE_TGT.second); iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_IDLE_TGT.second);
iht(objects::RW_ASS, NML, 0, ACS_TABLE_IDLE_TGT.second); iht(objects::RW_ASSY, NML, 0, ACS_TABLE_IDLE_TGT.second);
iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_IDLE_TGT.second); iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_IDLE_TGT.second);
iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_IDLE_TGT.second); iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_IDLE_TGT.second);
ss.addTable(&ACS_TABLE_IDLE_TGT.second, ACS_TABLE_IDLE_TGT.first, false, true); ss.addTable(&ACS_TABLE_IDLE_TGT.second, ACS_TABLE_IDLE_TGT.first, false, true);
@ -316,7 +316,7 @@ void buildIdleSequence(Subsystem& ss, ModeListEntry& eh) {
iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_IDLE_TRANS_0.second); iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_IDLE_TRANS_0.second);
iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_IDLE_TRANS_0.second); iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_IDLE_TRANS_0.second);
iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_IDLE_TRANS_0.second); iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_IDLE_TRANS_0.second);
iht(objects::RW_ASS, NML, 0, ACS_TABLE_IDLE_TRANS_0.second); iht(objects::RW_ASSY, NML, 0, ACS_TABLE_IDLE_TRANS_0.second);
iht(objects::STAR_TRACKER, NML, 0, ACS_TABLE_IDLE_TRANS_0.second); iht(objects::STAR_TRACKER, NML, 0, ACS_TABLE_IDLE_TRANS_0.second);
ss.addTable(&ACS_TABLE_IDLE_TRANS_0.second, ACS_TABLE_IDLE_TRANS_0.first, false, true); ss.addTable(&ACS_TABLE_IDLE_TRANS_0.second, ACS_TABLE_IDLE_TRANS_0.first, false, true);
@ -358,7 +358,7 @@ void buildTargetPtSequence(Subsystem& ss, ModeListEntry& eh) {
iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second); iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second);
iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second); iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second);
iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second); iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second);
iht(objects::RW_ASS, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second); iht(objects::RW_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second);
iht(objects::STAR_TRACKER, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second); iht(objects::STAR_TRACKER, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second);
check(ss.addTable(&ACS_TABLE_PTG_TARGET_TGT.second, ACS_TABLE_PTG_TARGET_TGT.first, false, true), check(ss.addTable(&ACS_TABLE_PTG_TARGET_TGT.second, ACS_TABLE_PTG_TARGET_TGT.first, false, true),
ctxc); ctxc);
@ -407,7 +407,7 @@ void buildTargetPtNadirSequence(Subsystem& ss, ModeListEntry& eh) {
iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second); iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second);
iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second); iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second);
iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second); iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second);
iht(objects::RW_ASS, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second); iht(objects::RW_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second);
iht(objects::STAR_TRACKER, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second); iht(objects::STAR_TRACKER, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second);
check(ss.addTable(TableEntry(ACS_TABLE_PTG_TARGET_NADIR_TGT.first, check(ss.addTable(TableEntry(ACS_TABLE_PTG_TARGET_NADIR_TGT.first,
&ACS_TABLE_PTG_TARGET_NADIR_TGT.second)), &ACS_TABLE_PTG_TARGET_NADIR_TGT.second)),
@ -458,7 +458,7 @@ void buildTargetPtGsSequence(Subsystem& ss, ModeListEntry& eh) {
iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second); iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second);
iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second); iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second);
iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second); iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second);
iht(objects::RW_ASS, NML, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second); iht(objects::RW_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second);
iht(objects::STAR_TRACKER, NML, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second); iht(objects::STAR_TRACKER, NML, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second);
check(ss.addTable( check(ss.addTable(
TableEntry(ACS_TABLE_PTG_TARGET_GS_TGT.first, &ACS_TABLE_PTG_TARGET_GS_TGT.second)), TableEntry(ACS_TABLE_PTG_TARGET_GS_TGT.first, &ACS_TABLE_PTG_TARGET_GS_TGT.second)),
@ -508,7 +508,7 @@ void buildTargetPtInertialSequence(Subsystem& ss, ModeListEntry& eh) {
iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second); iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second);
iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second); iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second);
iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second); iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second);
iht(objects::RW_ASS, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second); iht(objects::RW_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second);
iht(objects::STAR_TRACKER, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second); iht(objects::STAR_TRACKER, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second);
check(ss.addTable(TableEntry(ACS_TABLE_PTG_TARGET_INERTIAL_TGT.first, check(ss.addTable(TableEntry(ACS_TABLE_PTG_TARGET_INERTIAL_TGT.first,
&ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second)), &ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second)),

View File

@ -105,11 +105,11 @@ void buildRxOnlySequence(Subsystem& ss, ModeListEntry& eh) {
// Build RX Only table. We could track the state of the CCSDS IP core handler // Build RX Only table. We could track the state of the CCSDS IP core handler
// as well but I do not think this is necessary because enabling that should // as well but I do not think this is necessary because enabling that should
// not interfere with the Syrlinks Handler. // not interfere with the Syrlinks Handler.
iht(objects::SYRLINKS_HANDLER, NML, ::com::Submode::RX_ONLY, COM_TABLE_RX_ONLY_TGT.second); iht(objects::SYRLINKS_ASSY, NML, ::com::Submode::RX_ONLY, COM_TABLE_RX_ONLY_TGT.second);
check(ss.addTable(TableEntry(COM_TABLE_RX_ONLY_TGT.first, &COM_TABLE_RX_ONLY_TGT.second)), ctxc); check(ss.addTable(TableEntry(COM_TABLE_RX_ONLY_TGT.first, &COM_TABLE_RX_ONLY_TGT.second)), ctxc);
// Build RX Only transition 0 // Build RX Only transition 0
iht(objects::SYRLINKS_HANDLER, NML, ::com::Submode::RX_ONLY, COM_TABLE_RX_ONLY_TRANS_0.second); iht(objects::SYRLINKS_ASSY, NML, ::com::Submode::RX_ONLY, COM_TABLE_RX_ONLY_TRANS_0.second);
check(ss.addTable(TableEntry(COM_TABLE_RX_ONLY_TRANS_0.first, &COM_TABLE_RX_ONLY_TRANS_0.second)), check(ss.addTable(TableEntry(COM_TABLE_RX_ONLY_TRANS_0.first, &COM_TABLE_RX_ONLY_TRANS_0.second)),
ctxc); ctxc);
@ -147,7 +147,7 @@ void buildTxAndRxLowRateSequence(Subsystem& ss, ModeListEntry& eh) {
}; };
// Build RX and TX low datarate table. // Build RX and TX low datarate table.
iht(objects::SYRLINKS_HANDLER, NML, ::com::Submode::RX_AND_TX_LOW_DATARATE, iht(objects::SYRLINKS_ASSY, NML, ::com::Submode::RX_AND_TX_LOW_DATARATE,
COM_TABLE_RX_AND_TX_LOW_RATE_TGT.second); COM_TABLE_RX_AND_TX_LOW_RATE_TGT.second);
iht(objects::CCSDS_HANDLER, ON, static_cast<Submode_t>(::com::CcsdsSubmode::DATARATE_LOW), iht(objects::CCSDS_HANDLER, ON, static_cast<Submode_t>(::com::CcsdsSubmode::DATARATE_LOW),
COM_TABLE_RX_AND_TX_LOW_RATE_TGT.second); COM_TABLE_RX_AND_TX_LOW_RATE_TGT.second);
@ -163,7 +163,7 @@ void buildTxAndRxLowRateSequence(Subsystem& ss, ModeListEntry& eh) {
ctxc); ctxc);
// Build TX and RX low transition 1 // Build TX and RX low transition 1
iht(objects::SYRLINKS_HANDLER, NML, ::com::Submode::RX_AND_TX_LOW_DATARATE, iht(objects::SYRLINKS_ASSY, NML, ::com::Submode::RX_AND_TX_LOW_DATARATE,
COM_TABLE_RX_AND_TX_LOW_RATE_TRANS_1.second); COM_TABLE_RX_AND_TX_LOW_RATE_TRANS_1.second);
check(ss.addTable(TableEntry(COM_TABLE_RX_AND_TX_LOW_RATE_TRANS_1.first, check(ss.addTable(TableEntry(COM_TABLE_RX_AND_TX_LOW_RATE_TRANS_1.first,
&COM_TABLE_RX_AND_TX_LOW_RATE_TRANS_1.second)), &COM_TABLE_RX_AND_TX_LOW_RATE_TRANS_1.second)),
@ -199,7 +199,7 @@ void buildTxAndRxHighRateSequence(Subsystem& ss, ModeListEntry& eh) {
}; };
// Build RX and TX high datarate table. // Build RX and TX high datarate table.
iht(objects::SYRLINKS_HANDLER, NML, ::com::Submode::RX_AND_TX_HIGH_DATARATE, iht(objects::SYRLINKS_ASSY, NML, ::com::Submode::RX_AND_TX_HIGH_DATARATE,
COM_TABLE_RX_AND_TX_HIGH_RATE_TGT.second); COM_TABLE_RX_AND_TX_HIGH_RATE_TGT.second);
iht(objects::CCSDS_HANDLER, ON, static_cast<Submode_t>(::com::CcsdsSubmode::DATARATE_HIGH), iht(objects::CCSDS_HANDLER, ON, static_cast<Submode_t>(::com::CcsdsSubmode::DATARATE_HIGH),
COM_TABLE_RX_AND_TX_HIGH_RATE_TGT.second); COM_TABLE_RX_AND_TX_HIGH_RATE_TGT.second);
@ -215,7 +215,7 @@ void buildTxAndRxHighRateSequence(Subsystem& ss, ModeListEntry& eh) {
ctxc); ctxc);
// Build TX and RX high transition 1 // Build TX and RX high transition 1
iht(objects::SYRLINKS_HANDLER, NML, ::com::Submode::RX_AND_TX_HIGH_DATARATE, iht(objects::SYRLINKS_ASSY, NML, ::com::Submode::RX_AND_TX_HIGH_DATARATE,
COM_TABLE_RX_AND_TX_HIGH_RATE_TRANS_1.second); COM_TABLE_RX_AND_TX_HIGH_RATE_TRANS_1.second);
check(ss.addTable(TableEntry(COM_TABLE_RX_AND_TX_HIGH_RATE_TRANS_1.first, check(ss.addTable(TableEntry(COM_TABLE_RX_AND_TX_HIGH_RATE_TRANS_1.first,
&COM_TABLE_RX_AND_TX_HIGH_RATE_TRANS_1.second)), &COM_TABLE_RX_AND_TX_HIGH_RATE_TRANS_1.second)),
@ -253,7 +253,7 @@ void buildTxAndRxDefaultRateSequence(Subsystem& ss, ModeListEntry& eh) {
}; };
// Build RX and TX default datarate table. // Build RX and TX default datarate table.
iht(objects::SYRLINKS_HANDLER, NML, ::com::Submode::RX_AND_TX_DEFAULT_DATARATE, iht(objects::SYRLINKS_ASSY, NML, ::com::Submode::RX_AND_TX_DEFAULT_DATARATE,
COM_TABLE_RX_AND_TX_DEFAULT_RATE_TGT.second); COM_TABLE_RX_AND_TX_DEFAULT_RATE_TGT.second);
iht(objects::CCSDS_HANDLER, ON, static_cast<Submode_t>(::com::CcsdsSubmode::DATARATE_DEFAULT), iht(objects::CCSDS_HANDLER, ON, static_cast<Submode_t>(::com::CcsdsSubmode::DATARATE_DEFAULT),
COM_TABLE_RX_AND_TX_DEFAULT_RATE_TGT.second); COM_TABLE_RX_AND_TX_DEFAULT_RATE_TGT.second);
@ -269,7 +269,7 @@ void buildTxAndRxDefaultRateSequence(Subsystem& ss, ModeListEntry& eh) {
ctxc); ctxc);
// Build TX and RX default transition 1 // Build TX and RX default transition 1
iht(objects::SYRLINKS_HANDLER, NML, ::com::Submode::RX_AND_TX_DEFAULT_DATARATE, iht(objects::SYRLINKS_ASSY, NML, ::com::Submode::RX_AND_TX_DEFAULT_DATARATE,
COM_TABLE_RX_AND_TX_DEFAULT_RATE_TRANS_1.second); COM_TABLE_RX_AND_TX_DEFAULT_RATE_TRANS_1.second);
check(ss.addTable(TableEntry(COM_TABLE_RX_AND_TX_DEFAULT_RATE_TRANS_1.first, check(ss.addTable(TableEntry(COM_TABLE_RX_AND_TX_DEFAULT_RATE_TRANS_1.first,
&COM_TABLE_RX_AND_TX_DEFAULT_RATE_TRANS_1.second)), &COM_TABLE_RX_AND_TX_DEFAULT_RATE_TRANS_1.second)),

View File

@ -5,7 +5,7 @@
#include "fsfw/ipc/QueueFactory.h" #include "fsfw/ipc/QueueFactory.h"
TmFunnelBase::TmFunnelBase(FunnelCfg cfg) TmFunnelBase::TmFunnelBase(FunnelCfg cfg)
: SystemObject(cfg.objectId), tmStore(cfg.tmStore), ipcStore(cfg.ipcStore) { : SystemObject(cfg.objectId), name(cfg.name), tmStore(cfg.tmStore), ipcStore(cfg.ipcStore) {
tmQueue = QueueFactory::instance()->createMessageQueue(cfg.tmMsgDepth); tmQueue = QueueFactory::instance()->createMessageQueue(cfg.tmMsgDepth);
} }
@ -37,8 +37,7 @@ ReturnValue_t TmFunnelBase::sendPacketToDestinations(store_address_t origStoreId
message.setStorageId(storeId); message.setStorageId(storeId);
} else { } else {
#if FSFW_CPP_OSTREAM_ENABLED == 1 #if FSFW_CPP_OSTREAM_ENABLED == 1
sif::error << "PusTmFunnel::handlePacket: Store too full to create data copy" sif::error << name << "::handlePacket: Store too full to create data copy" << std::endl;
<< std::endl;
#endif #endif
} }
} else { } else {
@ -48,7 +47,8 @@ ReturnValue_t TmFunnelBase::sendPacketToDestinations(store_address_t origStoreId
result = tmQueue->sendMessage(dest.queueId, &message); result = tmQueue->sendMessage(dest.queueId, &message);
if (result != returnvalue::OK) { if (result != returnvalue::OK) {
#if FSFW_CPP_OSTREAM_ENABLED == 1 #if FSFW_CPP_OSTREAM_ENABLED == 1
sif::error << "PusTmFunnel::handlePacket: Error sending TM to downlink handler" << std::endl; sif::error << name << "::handlePacket: Error sending TM to downlink handler " << dest.name
<< std::endl;
#endif #endif
tmStore.deleteData(message.getStorageId()); tmStore.deleteData(message.getStorageId());
} }

View File

@ -12,10 +12,15 @@
class TmFunnelBase : public AcceptsTelemetryIF, public SystemObject { class TmFunnelBase : public AcceptsTelemetryIF, public SystemObject {
public: public:
struct FunnelCfg { struct FunnelCfg {
FunnelCfg(object_id_t objId, StorageManagerIF& tmStore, StorageManagerIF& ipcStore, FunnelCfg(object_id_t objId, const char* name, StorageManagerIF& tmStore,
uint32_t tmMsgDepth) StorageManagerIF& ipcStore, uint32_t tmMsgDepth)
: objectId(objId), tmStore(tmStore), ipcStore(ipcStore), tmMsgDepth(tmMsgDepth) {} : objectId(objId),
name(name),
tmStore(tmStore),
ipcStore(ipcStore),
tmMsgDepth(tmMsgDepth) {}
object_id_t objectId; object_id_t objectId;
const char* name;
StorageManagerIF& tmStore; StorageManagerIF& tmStore;
StorageManagerIF& ipcStore; StorageManagerIF& ipcStore;
uint32_t tmMsgDepth; uint32_t tmMsgDepth;
@ -30,6 +35,7 @@ class TmFunnelBase : public AcceptsTelemetryIF, public SystemObject {
~TmFunnelBase() override; ~TmFunnelBase() override;
protected: protected:
const char* name;
StorageManagerIF& tmStore; StorageManagerIF& tmStore;
StorageManagerIF& ipcStore; StorageManagerIF& ipcStore;
@ -43,7 +49,6 @@ class TmFunnelBase : public AcceptsTelemetryIF, public SystemObject {
}; };
std::vector<Destination> destinations; std::vector<Destination> destinations;
MessageQueueIF* tmQueue = nullptr; MessageQueueIF* tmQueue = nullptr;
}; };

2
tmtc

@ -1 +1 @@
Subproject commit 2dd850f0725d37256c17576bf7d3ae4423184044 Subproject commit 77fbcede10d44fd15dc7d5d1b3965f06c6a8e7fc