eive/eive-obsw
eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 120 Wiki Activity

WIP: Fixes for Pointing Controller #616

Closed
meggert wants to merge 49 commits from acs-ptg-ctrl-fixes into v2.1.0-dev
pull from: acs-ptg-ctrl-fixes
merge into: eive:v2.1.0-dev
Conversation 1 Commits 49 Files Changed 30 +670 -350
30 changed files with 670 additions and 350 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
CHANGELOG.md
View File

@ -16,12 +16,43 @@ will consitute of a breaking change warranting a new major release:
# [unreleased] # [unreleased]
# [v2.2.0] to be released
# [v2.1.0] to be released
## Changed
- Adapt EM configuration to include all GomSpace PCDU devices except the ACU. For the ACU
(which broke), a dummy will still be used.
- Event Manager queue depth is configurable now.
- Do not construct and schedule broken TMP1075 device anymore.
- Do not track payload modes in system mode tables.
- ACS modes derived from system modes.
## Added
- Add the remaining system modes.
## Fixed
- Host build is working again. Added reduced live TM helper which schedules the PUS and CFDP
funnel.
# [v2.0.5] to be released
- The dual lane assembly transition failed handler started new transitions towards the current mode
instead of the target mode. This means that if the dual lane assembly never reached the initial
submode (e.g. mode normal and submode dual side), it will transition back to the current mode,
which miht be `MODE_OFF`. Furthermore, this can lead to invalid internal states, so the subsequent
recovery handling becomes stuck in the custom recovery sequence when swichting power back on.
- The dual lane custom recovery handling was adapted to always perform proper power switch handling
irrespective of current or target modes.
# [v2.0.4] 2023-04-19 # [v2.0.4] 2023-04-19
## Fixed ## Fixed
- The dual lane assembly device handlers did not properly set their datasets - The dual lane assembly datasets were not marked invalid properly on OFF transitions.
to invalid on off transitions
# [v2.0.3] 2023-04-17 # [v2.0.3] 2023-04-17

5
CMakeLists.txt
View File

@ -146,8 +146,11 @@ set(OBSW_ADD_TMP_DEVICES
${INIT_VAL} ${INIT_VAL}
CACHE STRING "Add TMP devices") CACHE STRING "Add TMP devices")
set(OBSW_ADD_GOMSPACE_PCDU set(OBSW_ADD_GOMSPACE_PCDU
${INIT_VAL} 1
CACHE STRING "Add GomSpace PCDU modules") CACHE STRING "Add GomSpace PCDU modules")
set(OBSW_ADD_GOMSPACE_ACU
${INIT_VAL}
CACHE STRING "Add GomSpace ACU submodule")
set(OBSW_ADD_RW set(OBSW_ADD_RW
${INIT_VAL} ${INIT_VAL}
CACHE STRING "Add RW modules") CACHE STRING "Add RW modules")

3
bsp_hosted/ObjectFactory.cpp
View File

@ -64,8 +64,9 @@ void ObjectFactory::produce(void* args) {
PersistentTmStores persistentStores; PersistentTmStores persistentStores;
auto sdcMan = new DummySdCardManager("/tmp"); auto sdcMan = new DummySdCardManager("/tmp");
ObjectFactory::produceGenericObjects(nullptr, &pusFunnel, &cfdpFunnel, *sdcMan, &ipcStore, ObjectFactory::produceGenericObjects(nullptr, &pusFunnel, &cfdpFunnel, *sdcMan, &ipcStore,
&tmStore, persistentStores); &tmStore, persistentStores, 120);
new TmFunnelHandler(objects::LIVE_TM_TASK, *pusFunnel, *cfdpFunnel);
auto* dummyGpioIF = new DummyGpioIF(); auto* dummyGpioIF = new DummyGpioIF();
auto* dummySwitcher = new DummyPowerSwitcher(objects::PCDU_HANDLER, 18, 0); auto* dummySwitcher = new DummyPowerSwitcher(objects::PCDU_HANDLER, 18, 0);
std::vector<ReturnValue_t> switcherList; std::vector<ReturnValue_t> switcherList;

20
bsp_hosted/scheduling.cpp
View File

@ -59,19 +59,15 @@ void scheduling::initTasks() {
"DIST", 60, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.2, missedDeadlineFunc); "DIST", 60, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.2, missedDeadlineFunc);
ReturnValue_t result = tmtcDistributor->addComponent(objects::CCSDS_PACKET_DISTRIBUTOR); ReturnValue_t result = tmtcDistributor->addComponent(objects::CCSDS_PACKET_DISTRIBUTOR);
if (result != returnvalue::OK) { if (result != returnvalue::OK) {
sif::error << "adding CCSDS distributor failed" << std::endl; sif::error << "Adding CCSDS distributor failed" << std::endl;
} }
result = tmtcDistributor->addComponent(objects::PUS_PACKET_DISTRIBUTOR); result = tmtcDistributor->addComponent(objects::PUS_PACKET_DISTRIBUTOR);
if (result != returnvalue::OK) { if (result != returnvalue::OK) {
sif::error << "adding PUS distributor failed" << std::endl; sif::error << "Adding PUS distributor failed" << std::endl;
}
result = tmtcDistributor->addComponent(objects::TM_FUNNEL);
if (result != returnvalue::OK) {
sif::error << "adding TM funnel failed" << std::endl;
} }
result = tmtcDistributor->addComponent(objects::CFDP_DISTRIBUTOR); result = tmtcDistributor->addComponent(objects::CFDP_DISTRIBUTOR);
if (result != returnvalue::OK) { if (result != returnvalue::OK) {
sif::error << "adding CFDP distributor failed" << std::endl; sif::error << "Adding CFDP distributor failed" << std::endl;
} }
result = tmtcDistributor->addComponent(objects::UDP_TMTC_SERVER); result = tmtcDistributor->addComponent(objects::UDP_TMTC_SERVER);
if (result != returnvalue::OK) { if (result != returnvalue::OK) {
@ -95,6 +91,13 @@ void scheduling::initTasks() {
sif::error << "Add component UDP Polling failed" << std::endl; sif::error << "Add component UDP Polling failed" << std::endl;
} }
PeriodicTaskIF* liveTmTask = factory->createPeriodicTask(
"LIVE_TM", 55, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.4, nullptr, &RR_SCHEDULING);
result = liveTmTask->addComponent(objects::LIVE_TM_TASK);
if (result != returnvalue::OK) {
scheduling::printAddObjectError("LIVE_TM", objects::LIVE_TM_TASK);
}
PeriodicTaskIF* pusHighPrio = factory->createPeriodicTask( PeriodicTaskIF* pusHighPrio = factory->createPeriodicTask(
"PUS_HIGH_PRIO", 60, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.200, missedDeadlineFunc); "PUS_HIGH_PRIO", 60, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.200, missedDeadlineFunc);
result = pusHighPrio->addComponent(objects::PUS_SERVICE_1_VERIFICATION); result = pusHighPrio->addComponent(objects::PUS_SERVICE_1_VERIFICATION);
@ -149,7 +152,7 @@ void scheduling::initTasks() {
"THERMAL_CTL_TASK", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE, 1.0, missedDeadlineFunc); "THERMAL_CTL_TASK", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE, 1.0, missedDeadlineFunc);
result = thermalTask->addComponent(objects::CORE_CONTROLLER); result = thermalTask->addComponent(objects::CORE_CONTROLLER);
if (result != returnvalue::OK) { if (result != returnvalue::OK) {
scheduling::printAddObjectError("Core controller dummy", objects::CORE_CONTROLLER); scheduling::printAddObjectError("CORE_CTRL", objects::CORE_CONTROLLER);
} }
result = thermalTask->addComponent(objects::THERMAL_CONTROLLER); result = thermalTask->addComponent(objects::THERMAL_CONTROLLER);
if (result != returnvalue::OK) { if (result != returnvalue::OK) {
@ -217,6 +220,7 @@ void scheduling::initTasks() {
tmtcDistributor->startTask(); tmtcDistributor->startTask();
udpPollingTask->startTask(); udpPollingTask->startTask();
tcpPollingTask->startTask(); tcpPollingTask->startTask();
liveTmTask->startTask();
pusHighPrio->startTask(); pusHighPrio->startTask();
pusMedPrio->startTask(); pusMedPrio->startTask();

3
bsp_q7s/OBSWConfig.h.in
View File

@ -22,6 +22,9 @@
#define OBSW_COMMAND_SAFE_MODE_AT_STARTUP 1 #define OBSW_COMMAND_SAFE_MODE_AT_STARTUP 1
#define OBSW_ADD_GOMSPACE_PCDU @OBSW_ADD_GOMSPACE_PCDU@ #define OBSW_ADD_GOMSPACE_PCDU @OBSW_ADD_GOMSPACE_PCDU@
// This define is necessary because the EM setup has the P60 dock module, but no ACU on the P60
// module because it broke.
#define OBSW_ADD_GOMSPACE_ACU @OBSW_ADD_GOMSPACE_ACU@
#define OBSW_ADD_MGT @OBSW_ADD_MGT@ #define OBSW_ADD_MGT @OBSW_ADD_MGT@
#define OBSW_ADD_BPX_BATTERY_HANDLER @OBSW_ADD_BPX_BATTERY_HANDLER@ #define OBSW_ADD_BPX_BATTERY_HANDLER @OBSW_ADD_BPX_BATTERY_HANDLER@
#define OBSW_ADD_STAR_TRACKER @OBSW_ADD_STAR_TRACKER@ #define OBSW_ADD_STAR_TRACKER @OBSW_ADD_STAR_TRACKER@

7
bsp_q7s/core/ObjectFactory.cpp
View File

@ -189,7 +189,6 @@ void ObjectFactory::createCommunicationInterfaces(LinuxLibgpioIF** gpioComIF,
*i2cComIF = new I2cComIF(objects::I2C_COM_IF); *i2cComIF = new I2cComIF(objects::I2C_COM_IF);
*uartComIF = new SerialComIF(objects::UART_COM_IF); *uartComIF = new SerialComIF(objects::UART_COM_IF);
*spiMainComIF = new SpiComIF(objects::SPI_MAIN_COM_IF, q7s::SPI_DEFAULT_DEV, **gpioComIF); *spiMainComIF = new SpiComIF(objects::SPI_MAIN_COM_IF, q7s::SPI_DEFAULT_DEV, **gpioComIF);
//*spiRWComIF = new SpiComIF(objects::SPI_RW_COM_IF, q7s::SPI_RW_DEV, **gpioComIF);
} }
void ObjectFactory::createPcduComponents(LinuxLibgpioIF* gpioComIF, PowerSwitchIF** pwrSwitcher, void ObjectFactory::createPcduComponents(LinuxLibgpioIF* gpioComIF, PowerSwitchIF** pwrSwitcher,
@ -197,7 +196,6 @@ void ObjectFactory::createPcduComponents(LinuxLibgpioIF* gpioComIF, PowerSwitchI
CspCookie* p60DockCspCookie = new CspCookie(P60Dock::MAX_REPLY_SIZE, addresses::P60DOCK, 500); CspCookie* p60DockCspCookie = new CspCookie(P60Dock::MAX_REPLY_SIZE, addresses::P60DOCK, 500);
CspCookie* pdu1CspCookie = new CspCookie(PDU::MAX_REPLY_SIZE, addresses::PDU1, 500); CspCookie* pdu1CspCookie = new CspCookie(PDU::MAX_REPLY_SIZE, addresses::PDU1, 500);
CspCookie* pdu2CspCookie = new CspCookie(PDU::MAX_REPLY_SIZE, addresses::PDU2, 500); CspCookie* pdu2CspCookie = new CspCookie(PDU::MAX_REPLY_SIZE, addresses::PDU2, 500);
CspCookie* acuCspCookie = new CspCookie(ACU::MAX_REPLY_SIZE, addresses::ACU, 500);
auto p60Fdir = new GomspacePowerFdir(objects::P60DOCK_HANDLER); auto p60Fdir = new GomspacePowerFdir(objects::P60DOCK_HANDLER);
P60DockHandler* p60dockhandler = new P60DockHandler(objects::P60DOCK_HANDLER, objects::CSP_COM_IF, P60DockHandler* p60dockhandler = new P60DockHandler(objects::P60DOCK_HANDLER, objects::CSP_COM_IF,
@ -211,9 +209,12 @@ void ObjectFactory::createPcduComponents(LinuxLibgpioIF* gpioComIF, PowerSwitchI
Pdu2Handler* pdu2handler = new Pdu2Handler(objects::PDU2_HANDLER, objects::CSP_COM_IF, Pdu2Handler* pdu2handler = new Pdu2Handler(objects::PDU2_HANDLER, objects::CSP_COM_IF,
pdu2CspCookie, pdu2Fdir, enableHkSets); pdu2CspCookie, pdu2Fdir, enableHkSets);
#if OBSW_ADD_GOMSPACE_ACU == 1
CspCookie* acuCspCookie = new CspCookie(ACU::MAX_REPLY_SIZE, addresses::ACU, 500);
auto acuFdir = new GomspacePowerFdir(objects::ACU_HANDLER); auto acuFdir = new GomspacePowerFdir(objects::ACU_HANDLER);
ACUHandler* acuhandler = new ACUHandler(objects::ACU_HANDLER, objects::CSP_COM_IF, acuCspCookie, ACUHandler* acuhandler = new ACUHandler(objects::ACU_HANDLER, objects::CSP_COM_IF, acuCspCookie,
acuFdir, enableHkSets); acuFdir, enableHkSets);
#endif
auto pcduHandler = new PcduHandler(objects::PCDU_HANDLER, 50); auto pcduHandler = new PcduHandler(objects::PCDU_HANDLER, 50);
/** /**
@ -223,7 +224,9 @@ void ObjectFactory::createPcduComponents(LinuxLibgpioIF* gpioComIF, PowerSwitchI
p60dockhandler->setModeNormal(); p60dockhandler->setModeNormal();
pdu1handler->setModeNormal(); pdu1handler->setModeNormal();
pdu2handler->setModeNormal(); pdu2handler->setModeNormal();
#if OBSW_ADD_GOMSPACE_ACU == 1
acuhandler->setModeNormal(); acuhandler->setModeNormal();
#endif
if (pwrSwitcher != nullptr) { if (pwrSwitcher != nullptr) {
*pwrSwitcher = pcduHandler; *pwrSwitcher = pcduHandler;
} }

10
bsp_q7s/em/emObjectFactory.cpp
View File

@ -36,7 +36,8 @@ void ObjectFactory::produce(void* args) {
PersistentTmStores stores; PersistentTmStores stores;
ObjectFactory::produceGenericObjects(&healthTable, &pusFunnel, &cfdpFunnel, ObjectFactory::produceGenericObjects(&healthTable, &pusFunnel, &cfdpFunnel,
*SdCardManager::instance(), &ipcStore, &tmStore, stores); *SdCardManager::instance(), &ipcStore, &tmStore, stores,
200);
LinuxLibgpioIF* gpioComIF = nullptr; LinuxLibgpioIF* gpioComIF = nullptr;
SerialComIF* uartComIF = nullptr; SerialComIF* uartComIF = nullptr;
@ -56,6 +57,11 @@ void ObjectFactory::produce(void* args) {
#endif #endif
#if OBSW_ADD_GOMSPACE_PCDU == 1 #if OBSW_ADD_GOMSPACE_PCDU == 1
dummyCfg.addPowerDummies = false; dummyCfg.addPowerDummies = false;
// The ACU broke.
dummyCfg.addOnlyAcuDummy = true;
#endif
#if OBSW_ADD_BPX_BATTERY_HANDLER == 1
dummyCfg.addBpxBattDummy = false;
#endif #endif
#if OBSW_ADD_ACS_BOARD == 1 #if OBSW_ADD_ACS_BOARD == 1
dummyCfg.addAcsBoardDummies = false; dummyCfg.addAcsBoardDummies = false;
@ -89,7 +95,7 @@ void ObjectFactory::produce(void* args) {
// createRadSensorComponent(gpioComIF); // createRadSensorComponent(gpioComIF);
#if OBSW_ADD_ACS_BOARD == 1 #if OBSW_ADD_ACS_BOARD == 1
createAcsBoardComponents(*spiMainComIF, gpioComIF, uartComIF, *pwrSwitcher); createAcsBoardComponents(*spiMainComIF, gpioComIF, uartComIF, *pwrSwitcher, true);
#else #else
// Still add all GPIOs for EM. // Still add all GPIOs for EM.
GpioCookie* acsBoardGpios = new GpioCookie(); GpioCookie* acsBoardGpios = new GpioCookie();

3
bsp_q7s/fmObjectFactory.cpp
View File

@ -32,7 +32,8 @@ void ObjectFactory::produce(void* args) {
PersistentTmStores stores; PersistentTmStores stores;
ObjectFactory::produceGenericObjects(&healthTable, &pusFunnel, &cfdpFunnel, ObjectFactory::produceGenericObjects(&healthTable, &pusFunnel, &cfdpFunnel,
*SdCardManager::instance(), &ipcStore, &tmStore, stores); *SdCardManager::instance(), &ipcStore, &tmStore, stores,
200);
LinuxLibgpioIF* gpioComIF = nullptr; LinuxLibgpioIF* gpioComIF = nullptr;
SerialComIF* uartComIF = nullptr; SerialComIF* uartComIF = nullptr;

15
dummies/helperFactory.cpp
View File

@ -45,7 +45,9 @@
void dummy::createDummies(DummyCfg cfg, PowerSwitchIF& pwrSwitcher, GpioIF* gpioIF) { void dummy::createDummies(DummyCfg cfg, PowerSwitchIF& pwrSwitcher, GpioIF* gpioIF) {
new ComIFDummy(objects::DUMMY_COM_IF); new ComIFDummy(objects::DUMMY_COM_IF);
auto* comCookieDummy = new ComCookieDummy(); auto* comCookieDummy = new ComCookieDummy();
new BpxDummy(objects::BPX_BATT_HANDLER, objects::DUMMY_COM_IF, comCookieDummy); if (cfg.addBpxBattDummy) {
new BpxDummy(objects::BPX_BATT_HANDLER, objects::DUMMY_COM_IF, comCookieDummy);
}
if (cfg.addCoreCtrlCfg) { if (cfg.addCoreCtrlCfg) {
new CoreControllerDummy(objects::CORE_CONTROLLER); new CoreControllerDummy(objects::CORE_CONTROLLER);
} }
@ -75,8 +77,9 @@ void dummy::createDummies(DummyCfg cfg, PowerSwitchIF& pwrSwitcher, GpioIF* gpio
auto* imtqDummy = new ImtqDummy(objects::IMTQ_HANDLER, objects::DUMMY_COM_IF, comCookieDummy); auto* imtqDummy = new ImtqDummy(objects::IMTQ_HANDLER, objects::DUMMY_COM_IF, comCookieDummy);
imtqDummy->enableThermalModule(ThermalStateCfg()); imtqDummy->enableThermalModule(ThermalStateCfg());
imtqDummy->connectModeTreeParent(*imtqAssy); imtqDummy->connectModeTreeParent(*imtqAssy);
if (cfg.addPowerDummies) { if (cfg.addOnlyAcuDummy) {
new AcuDummy(objects::ACU_HANDLER, objects::DUMMY_COM_IF, comCookieDummy); new AcuDummy(objects::ACU_HANDLER, objects::DUMMY_COM_IF, comCookieDummy);
} else if (cfg.addPowerDummies) {
new PduDummy(objects::PDU1_HANDLER, objects::DUMMY_COM_IF, comCookieDummy); new PduDummy(objects::PDU1_HANDLER, objects::DUMMY_COM_IF, comCookieDummy);
new PduDummy(objects::PDU2_HANDLER, objects::DUMMY_COM_IF, comCookieDummy); new PduDummy(objects::PDU2_HANDLER, objects::DUMMY_COM_IF, comCookieDummy);
new P60DockDummy(objects::P60DOCK_HANDLER, objects::DUMMY_COM_IF, comCookieDummy); new P60DockDummy(objects::P60DOCK_HANDLER, objects::DUMMY_COM_IF, comCookieDummy);
@ -195,10 +198,10 @@ void dummy::createDummies(DummyCfg cfg, PowerSwitchIF& pwrSwitcher, GpioIF* gpio
objects::TMP1075_HANDLER_PLPCDU_0, objects::TMP1075_HANDLER_PLPCDU_0,
new Tmp1075Dummy(objects::TMP1075_HANDLER_PLPCDU_0, objects::DUMMY_COM_IF, comCookieDummy)); new Tmp1075Dummy(objects::TMP1075_HANDLER_PLPCDU_0, objects::DUMMY_COM_IF, comCookieDummy));
// damaged. // damaged.
// tmpSensorDummies.emplace( // tmpSensorDummies.emplace(
// objects::TMP1075_HANDLER_PLPCDU_1, // objects::TMP1075_HANDLER_PLPCDU_1,
// new Tmp1075Dummy(objects::TMP1075_HANDLER_PLPCDU_1, objects::DUMMY_COM_IF, // new Tmp1075Dummy(objects::TMP1075_HANDLER_PLPCDU_1, objects::DUMMY_COM_IF,
// comCookieDummy)); // comCookieDummy));
tmpSensorDummies.emplace( tmpSensorDummies.emplace(
objects::TMP1075_HANDLER_IF_BOARD, objects::TMP1075_HANDLER_IF_BOARD,
new Tmp1075Dummy(objects::TMP1075_HANDLER_IF_BOARD, objects::DUMMY_COM_IF, comCookieDummy)); new Tmp1075Dummy(objects::TMP1075_HANDLER_IF_BOARD, objects::DUMMY_COM_IF, comCookieDummy));

4
dummies/helperFactory.h
View File

@ -6,9 +6,13 @@ class GpioIF;
namespace dummy { namespace dummy {
// Default values targeted towards EM.
struct DummyCfg { struct DummyCfg {
bool addCoreCtrlCfg = true; bool addCoreCtrlCfg = true;
// Special variant because the ACU broke. Overrides addPowerDummies, only ACU dummy will be added.
bool addOnlyAcuDummy = false;
bool addPowerDummies = true; bool addPowerDummies = true;
bool addBpxBattDummy = true;
bool addSyrlinksDummies = true; bool addSyrlinksDummies = true;
bool addAcsBoardDummies = true; bool addAcsBoardDummies = true;
bool addSusDummies = true; bool addSusDummies = true;

13
mission/acs/defs.h
View File

@ -3,6 +3,7 @@
#include <eive/eventSubsystemIds.h> #include <eive/eventSubsystemIds.h>
#include <fsfw/modes/HasModesIF.h> #include <fsfw/modes/HasModesIF.h>
#include <mission/sysDefs.h>
namespace acs { namespace acs {
@ -11,12 +12,12 @@ enum class SimpleSensorMode { NORMAL = 0, OFF = 1 };
// These modes are the modes of the ACS controller and of the ACS subsystem. // These modes are the modes of the ACS controller and of the ACS subsystem.
enum AcsMode : Mode_t { enum AcsMode : Mode_t {
OFF = HasModesIF::MODE_OFF, OFF = HasModesIF::MODE_OFF,
SAFE = 10, SAFE = satsystem::Mode::SAFE,
PTG_IDLE = 11, PTG_IDLE = satsystem::Mode::PTG_IDLE,
PTG_NADIR = 12, PTG_NADIR = satsystem::Mode::PTG_NADIR,
PTG_TARGET = 13, PTG_TARGET = satsystem::Mode::PTG_TARGET,
PTG_TARGET_GS = 14, PTG_TARGET_GS = satsystem::Mode::PTG_TARGET_GS,
PTG_INERTIAL = 15, PTG_INERTIAL = satsystem::Mode::PTG_INERTIAL,
}; };
enum SafeSubmode : Submode_t { DEFAULT = 0, DETUMBLE = 1 }; enum SafeSubmode : Submode_t { DEFAULT = 0, DETUMBLE = 1 };

106
mission/controller/AcsController.cpp
View File

@ -169,7 +169,7 @@ void AcsController::performSafe() {
guidance.getTargetParamsSafe(sunTargetDir); guidance.getTargetParamsSafe(sunTargetDir);
double magMomMtq[3] = {0, 0, 0}, errAng = 0.0; double magMomMtq[3] = {0, 0, 0}, errAng = 0.0;
uint8_t safeCtrlStrat = safeCtrl.safeCtrlStrategy( acs::SafeModeStrategy safeCtrlStrat = safeCtrl.safeCtrlStrategy(
mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag, mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag,
gyrDataProcessed.gyrVecTot.isValid(), susDataProcessed.susVecTot.isValid(), gyrDataProcessed.gyrVecTot.isValid(), susDataProcessed.susVecTot.isValid(),
acsParameters.safeModeControllerParameters.useMekf, acsParameters.safeModeControllerParameters.useMekf,
@ -205,11 +205,13 @@ void AcsController::performSafe() {
case (acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL): case (acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL):
safeCtrlFailure(0, 1); safeCtrlFailure(0, 1);
break; break;
default:
sif::error << "AcsController: Invalid safe mode strategy for performSafe" << std::endl;
break;
} }
actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs, actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment,
*acsParameters.magnetorquerParameter.inverseAlignment, acsParameters.magnetorquerParameter.dipoleMax, magMomMtq, cmdDipoleMtqs);
acsParameters.magnetorquerParameter.dipolMax);
// detumble check and switch // detumble check and switch
if (mekfData.satRotRateMekf.isValid() && acsParameters.safeModeControllerParameters.useMekf && if (mekfData.satRotRateMekf.isValid() && acsParameters.safeModeControllerParameters.useMekf &&
@ -231,8 +233,8 @@ void AcsController::performSafe() {
} }
updateCtrlValData(errAng, safeCtrlStrat); updateCtrlValData(errAng, safeCtrlStrat);
updateActuatorCmdData(cmdDipolMtqs); updateActuatorCmdData(cmdDipoleMtqs);
commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2],
acsParameters.magnetorquerParameter.torqueDuration); acsParameters.magnetorquerParameter.torqueDuration);
} }
@ -259,7 +261,7 @@ void AcsController::performDetumble() {
triggerEvent(acs::MEKF_RECOVERY); triggerEvent(acs::MEKF_RECOVERY);
mekfInvalidFlag = false; mekfInvalidFlag = false;
} }
uint8_t safeCtrlStrat = detumble.detumbleStrategy( acs::SafeModeStrategy safeCtrlStrat = detumble.detumbleStrategy(
mgmDataProcessed.mgmVecTot.isValid(), gyrDataProcessed.gyrVecTot.isValid(), mgmDataProcessed.mgmVecTot.isValid(), gyrDataProcessed.gyrVecTot.isValid(),
mgmDataProcessed.mgmVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTotDerivative.isValid(),
acsParameters.detumbleParameter.useFullDetumbleLaw); acsParameters.detumbleParameter.useFullDetumbleLaw);
@ -279,11 +281,13 @@ void AcsController::performDetumble() {
case (acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL): case (acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL):
safeCtrlFailure(0, 1); safeCtrlFailure(0, 1);
break; break;
default:
sif::error << "AcsController: Invalid safe mode strategy for performDetumble" << std::endl;
break;
} }
actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs, actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment,
*acsParameters.magnetorquerParameter.inverseAlignment, acsParameters.magnetorquerParameter.dipoleMax, magMomMtq, cmdDipoleMtqs);
acsParameters.magnetorquerParameter.dipolMax);
if (mekfData.satRotRateMekf.isValid() && if (mekfData.satRotRateMekf.isValid() &&
VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) < VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) <
@ -304,8 +308,8 @@ void AcsController::performDetumble() {
} }
disableCtrlValData(); disableCtrlValData();
updateActuatorCmdData(cmdDipolMtqs); updateActuatorCmdData(cmdDipoleMtqs);
commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2],
acsParameters.magnetorquerParameter.torqueDuration); acsParameters.magnetorquerParameter.torqueDuration);
} }
@ -371,17 +375,17 @@ void AcsController::performPointingCtrl() {
targetSatRotRate, errorQuat, errorSatRotRate, errorAngle); targetSatRotRate, errorQuat, errorSatRotRate, errorAngle);
ptgCtrl.ptgLaw(&acsParameters.idleModeControllerParameters, errorQuat, errorSatRotRate, ptgCtrl.ptgLaw(&acsParameters.idleModeControllerParameters, errorQuat, errorSatRotRate,
*rwPseudoInv, torquePtgRws); *rwPseudoInv, torquePtgRws);
ptgCtrl.ptgNullspace( ptgCtrl.ptgNullspace(&acsParameters.idleModeControllerParameters,
&acsParameters.idleModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
&(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
&(sensorValues.rw4Set.currSpeed.value), rwTrqNs); rwTrqNs);
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4); VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation( ptgCtrl.ptgDesaturation(
&acsParameters.idleModeControllerParameters, mgmDataProcessed.mgmVecTot.value, &acsParameters.idleModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes); sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
enableAntiStiction = acsParameters.idleModeControllerParameters.enableAntiStiction; enableAntiStiction = acsParameters.idleModeControllerParameters.enableAntiStiction;
break; break;
@ -395,17 +399,17 @@ void AcsController::performPointingCtrl() {
errorSatRotRate, errorAngle); errorSatRotRate, errorAngle);
ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, errorQuat, errorSatRotRate, ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, errorQuat, errorSatRotRate,
*rwPseudoInv, torquePtgRws); *rwPseudoInv, torquePtgRws);
ptgCtrl.ptgNullspace( ptgCtrl.ptgNullspace(&acsParameters.targetModeControllerParameters,
&acsParameters.targetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
&(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
&(sensorValues.rw4Set.currSpeed.value), rwTrqNs); rwTrqNs);
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4); VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation( ptgCtrl.ptgDesaturation(
&acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value, &acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes); sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction; enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
break; break;
@ -416,17 +420,17 @@ void AcsController::performPointingCtrl() {
targetSatRotRate, errorQuat, errorSatRotRate, errorAngle); targetSatRotRate, errorQuat, errorSatRotRate, errorAngle);
ptgCtrl.ptgLaw(&acsParameters.gsTargetModeControllerParameters, errorQuat, errorSatRotRate, ptgCtrl.ptgLaw(&acsParameters.gsTargetModeControllerParameters, errorQuat, errorSatRotRate,
*rwPseudoInv, torquePtgRws); *rwPseudoInv, torquePtgRws);
ptgCtrl.ptgNullspace( ptgCtrl.ptgNullspace(&acsParameters.gsTargetModeControllerParameters,
&acsParameters.gsTargetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
&(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
&(sensorValues.rw4Set.currSpeed.value), rwTrqNs); rwTrqNs);
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4); VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation( ptgCtrl.ptgDesaturation(
&acsParameters.gsTargetModeControllerParameters, mgmDataProcessed.mgmVecTot.value, &acsParameters.gsTargetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes); sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
enableAntiStiction = acsParameters.gsTargetModeControllerParameters.enableAntiStiction; enableAntiStiction = acsParameters.gsTargetModeControllerParameters.enableAntiStiction;
break; break;
@ -440,17 +444,17 @@ void AcsController::performPointingCtrl() {
errorSatRotRate, errorAngle); errorSatRotRate, errorAngle);
ptgCtrl.ptgLaw(&acsParameters.nadirModeControllerParameters, errorQuat, errorSatRotRate, ptgCtrl.ptgLaw(&acsParameters.nadirModeControllerParameters, errorQuat, errorSatRotRate,
*rwPseudoInv, torquePtgRws); *rwPseudoInv, torquePtgRws);
ptgCtrl.ptgNullspace( ptgCtrl.ptgNullspace(&acsParameters.nadirModeControllerParameters,
&acsParameters.nadirModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
&(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
&(sensorValues.rw4Set.currSpeed.value), rwTrqNs); rwTrqNs);
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4); VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation( ptgCtrl.ptgDesaturation(
&acsParameters.nadirModeControllerParameters, mgmDataProcessed.mgmVecTot.value, &acsParameters.nadirModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes); sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
enableAntiStiction = acsParameters.nadirModeControllerParameters.enableAntiStiction; enableAntiStiction = acsParameters.nadirModeControllerParameters.enableAntiStiction;
break; break;
@ -463,40 +467,38 @@ void AcsController::performPointingCtrl() {
errorSatRotRate, errorAngle); errorSatRotRate, errorAngle);
ptgCtrl.ptgLaw(&acsParameters.inertialModeControllerParameters, errorQuat, errorSatRotRate, ptgCtrl.ptgLaw(&acsParameters.inertialModeControllerParameters, errorQuat, errorSatRotRate,
*rwPseudoInv, torquePtgRws); *rwPseudoInv, torquePtgRws);
ptgCtrl.ptgNullspace( ptgCtrl.ptgNullspace(&acsParameters.inertialModeControllerParameters,
&acsParameters.inertialModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
&(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
&(sensorValues.rw4Set.currSpeed.value), rwTrqNs); rwTrqNs);
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4); VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation( ptgCtrl.ptgDesaturation(
&acsParameters.inertialModeControllerParameters, mgmDataProcessed.mgmVecTot.value, &acsParameters.inertialModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes); sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction; enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction;
break; break;
default: default:
sif::error << "AcsController: Invalid mode for performPointingCtrl"; sif::error << "AcsController: Invalid mode for performPointingCtrl" << std::endl;
break; break;
} }
actuatorCmd.cmdSpeedToRws( actuatorCmd.cmdSpeedToRws(
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, torqueRws, sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
cmdSpeedRws, acsParameters.onBoardParams.sampleTime, acsParameters.onBoardParams.sampleTime, acsParameters.rwHandlingParameters.inertiaWheel,
acsParameters.rwHandlingParameters.maxRwSpeed, acsParameters.rwHandlingParameters.maxRwSpeed, torqueRws, cmdSpeedRws);
acsParameters.rwHandlingParameters.inertiaWheel);
if (enableAntiStiction) { if (enableAntiStiction) {
ptgCtrl.rwAntistiction(&sensorValues, cmdSpeedRws); ptgCtrl.rwAntistiction(&sensorValues, cmdSpeedRws);
} }
actuatorCmd.cmdDipolMtq(mgtDpDes, cmdDipolMtqs, actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment,
*acsParameters.magnetorquerParameter.inverseAlignment, acsParameters.magnetorquerParameter.dipoleMax, mgtDpDes, cmdDipoleMtqs);
acsParameters.magnetorquerParameter.dipolMax);
updateCtrlValData(targetQuat, errorQuat, errorAngle, targetSatRotRate); updateCtrlValData(targetQuat, errorQuat, errorAngle, targetSatRotRate);
updateActuatorCmdData(torqueRws, cmdSpeedRws, cmdDipolMtqs); updateActuatorCmdData(torqueRws, cmdSpeedRws, cmdDipoleMtqs);
commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2],
acsParameters.magnetorquerParameter.torqueDuration, cmdSpeedRws[0], acsParameters.magnetorquerParameter.torqueDuration, cmdSpeedRws[0],
cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3], cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3],
acsParameters.rwHandlingParameters.rampTime); acsParameters.rwHandlingParameters.rampTime);

2
mission/controller/AcsController.h
View File

@ -69,7 +69,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
bool mekfLost = false; bool mekfLost = false;
int32_t cmdSpeedRws[4] = {0, 0, 0, 0}; int32_t cmdSpeedRws[4] = {0, 0, 0, 0};
int16_t cmdDipolMtqs[3] = {0, 0, 0}; int16_t cmdDipoleMtqs[3] = {0, 0, 0};
#if OBSW_THREAD_TRACING == 1 #if OBSW_THREAD_TRACING == 1
uint32_t opCounter = 0; uint32_t opCounter = 0;

89
mission/controller/acs/AcsParameters.cpp
View File

@ -315,7 +315,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->setMatrix(rwMatrices.without4); parameterWrapper->setMatrix(rwMatrices.without4);
break; break;
case 0x6: case 0x6:
parameterWrapper->setVector(rwMatrices.nullspace); parameterWrapper->setVector(rwMatrices.nullspaceVector);
break; break;
default: default:
return INVALID_IDENTIFIER_ID; return INVALID_IDENTIFIER_ID;
@ -375,15 +375,18 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->set(idleModeControllerParameters.gainNullspace); parameterWrapper->set(idleModeControllerParameters.gainNullspace);
break; break;
case 0x5: case 0x5:
parameterWrapper->setVector(idleModeControllerParameters.desatMomentumRef); parameterWrapper->set(idleModeControllerParameters.nullspaceSpeed);
break; break;
case 0x6: case 0x6:
parameterWrapper->set(idleModeControllerParameters.deSatGainFactor); parameterWrapper->setVector(idleModeControllerParameters.desatMomentumRef);
break; break;
case 0x7: case 0x7:
parameterWrapper->set(idleModeControllerParameters.desatOn); parameterWrapper->set(idleModeControllerParameters.deSatGainFactor);
break; break;
case 0x8: case 0x8:
parameterWrapper->set(idleModeControllerParameters.desatOn);
break;
case 0x9:
parameterWrapper->set(idleModeControllerParameters.enableAntiStiction); parameterWrapper->set(idleModeControllerParameters.enableAntiStiction);
break; break;
default: default:
@ -408,48 +411,51 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->set(targetModeControllerParameters.gainNullspace); parameterWrapper->set(targetModeControllerParameters.gainNullspace);
break; break;
case 0x5: case 0x5:
parameterWrapper->setVector(targetModeControllerParameters.desatMomentumRef); parameterWrapper->set(targetModeControllerParameters.nullspaceSpeed);
break; break;
case 0x6: case 0x6:
parameterWrapper->set(targetModeControllerParameters.deSatGainFactor); parameterWrapper->setVector(targetModeControllerParameters.desatMomentumRef);
break; break;
case 0x7: case 0x7:
parameterWrapper->set(targetModeControllerParameters.desatOn); parameterWrapper->set(targetModeControllerParameters.deSatGainFactor);
break; break;
case 0x8: case 0x8:
parameterWrapper->set(targetModeControllerParameters.enableAntiStiction); parameterWrapper->set(targetModeControllerParameters.desatOn);
break; break;
case 0x9: case 0x9:
parameterWrapper->setVector(targetModeControllerParameters.refDirection); parameterWrapper->set(targetModeControllerParameters.enableAntiStiction);
break; break;
case 0xA: case 0xA:
parameterWrapper->setVector(targetModeControllerParameters.refRotRate); parameterWrapper->setVector(targetModeControllerParameters.refDirection);
break; break;
case 0xB: case 0xB:
parameterWrapper->setVector(targetModeControllerParameters.quatRef); parameterWrapper->setVector(targetModeControllerParameters.refRotRate);
break; break;
case 0xC: case 0xC:
parameterWrapper->set(targetModeControllerParameters.timeElapsedMax); parameterWrapper->setVector(targetModeControllerParameters.quatRef);
break; break;
case 0xD: case 0xD:
parameterWrapper->set(targetModeControllerParameters.latitudeTgt); parameterWrapper->set(targetModeControllerParameters.timeElapsedMax);
break; break;
case 0xE: case 0xE:
parameterWrapper->set(targetModeControllerParameters.longitudeTgt); parameterWrapper->set(targetModeControllerParameters.latitudeTgt);
break; break;
case 0xF: case 0xF:
parameterWrapper->set(targetModeControllerParameters.altitudeTgt); parameterWrapper->set(targetModeControllerParameters.longitudeTgt);
break; break;
case 0x10: case 0x10:
parameterWrapper->set(targetModeControllerParameters.avoidBlindStr); parameterWrapper->set(targetModeControllerParameters.altitudeTgt);
break; break;
case 0x11: case 0x11:
parameterWrapper->set(targetModeControllerParameters.blindAvoidStart); parameterWrapper->set(targetModeControllerParameters.avoidBlindStr);
break; break;
case 0x12: case 0x12:
parameterWrapper->set(targetModeControllerParameters.blindAvoidStop); parameterWrapper->set(targetModeControllerParameters.blindAvoidStart);
break; break;
case 0x13: case 0x13:
parameterWrapper->set(targetModeControllerParameters.blindAvoidStop);
break;
case 0x14:
parameterWrapper->set(targetModeControllerParameters.blindRotRate); parameterWrapper->set(targetModeControllerParameters.blindRotRate);
break; break;
default: default:
@ -474,30 +480,33 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->set(gsTargetModeControllerParameters.gainNullspace); parameterWrapper->set(gsTargetModeControllerParameters.gainNullspace);
break; break;
case 0x5: case 0x5:
parameterWrapper->setVector(gsTargetModeControllerParameters.desatMomentumRef); parameterWrapper->set(gsTargetModeControllerParameters.nullspaceSpeed);
break; break;
case 0x6: case 0x6:
parameterWrapper->set(gsTargetModeControllerParameters.deSatGainFactor); parameterWrapper->setVector(gsTargetModeControllerParameters.desatMomentumRef);
break; break;
case 0x7: case 0x7:
parameterWrapper->set(gsTargetModeControllerParameters.desatOn); parameterWrapper->set(gsTargetModeControllerParameters.deSatGainFactor);
break; break;
case 0x8: case 0x8:
parameterWrapper->set(gsTargetModeControllerParameters.enableAntiStiction); parameterWrapper->set(gsTargetModeControllerParameters.desatOn);
break; break;
case 0x9: case 0x9:
parameterWrapper->setVector(gsTargetModeControllerParameters.refDirection); parameterWrapper->set(gsTargetModeControllerParameters.enableAntiStiction);
break; break;
case 0xA: case 0xA:
parameterWrapper->set(gsTargetModeControllerParameters.timeElapsedMax); parameterWrapper->setVector(gsTargetModeControllerParameters.refDirection);
break; break;
case 0xB: case 0xB:
parameterWrapper->set(gsTargetModeControllerParameters.latitudeTgt); parameterWrapper->set(gsTargetModeControllerParameters.timeElapsedMax);
break; break;
case 0xC: case 0xC:
parameterWrapper->set(gsTargetModeControllerParameters.longitudeTgt); parameterWrapper->set(gsTargetModeControllerParameters.latitudeTgt);
break; break;
case 0xD: case 0xD:
parameterWrapper->set(gsTargetModeControllerParameters.longitudeTgt);
break;
case 0xE:
parameterWrapper->set(gsTargetModeControllerParameters.altitudeTgt); parameterWrapper->set(gsTargetModeControllerParameters.altitudeTgt);
break; break;
default: default:
@ -522,21 +531,24 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->set(nadirModeControllerParameters.gainNullspace); parameterWrapper->set(nadirModeControllerParameters.gainNullspace);
break; break;
case 0x5: case 0x5:
parameterWrapper->setVector(nadirModeControllerParameters.desatMomentumRef); parameterWrapper->set(nadirModeControllerParameters.nullspaceSpeed);
break; break;
case 0x6: case 0x6:
parameterWrapper->set(nadirModeControllerParameters.deSatGainFactor); parameterWrapper->setVector(nadirModeControllerParameters.desatMomentumRef);
break; break;
case 0x7: case 0x7:
parameterWrapper->set(nadirModeControllerParameters.desatOn); parameterWrapper->set(nadirModeControllerParameters.deSatGainFactor);
break; break;
case 0x8: case 0x8:
parameterWrapper->set(nadirModeControllerParameters.enableAntiStiction); parameterWrapper->set(nadirModeControllerParameters.desatOn);
break; break;
case 0x9: case 0x9:
parameterWrapper->setVector(nadirModeControllerParameters.refDirection); parameterWrapper->set(nadirModeControllerParameters.enableAntiStiction);
break; break;
case 0xA: case 0xA:
parameterWrapper->setVector(nadirModeControllerParameters.refDirection);
break;
case 0xB:
parameterWrapper->setVector(nadirModeControllerParameters.quatRef); parameterWrapper->setVector(nadirModeControllerParameters.quatRef);
break; break;
case 0xC: case 0xC:
@ -564,21 +576,24 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->set(inertialModeControllerParameters.gainNullspace); parameterWrapper->set(inertialModeControllerParameters.gainNullspace);
break; break;
case 0x5: case 0x5:
parameterWrapper->setVector(inertialModeControllerParameters.desatMomentumRef); parameterWrapper->set(inertialModeControllerParameters.nullspaceSpeed);
break; break;
case 0x6: case 0x6:
parameterWrapper->set(inertialModeControllerParameters.deSatGainFactor); parameterWrapper->setVector(inertialModeControllerParameters.desatMomentumRef);
break; break;
case 0x7: case 0x7:
parameterWrapper->set(inertialModeControllerParameters.desatOn); parameterWrapper->set(inertialModeControllerParameters.deSatGainFactor);
break; break;
case 0x8: case 0x8:
parameterWrapper->set(inertialModeControllerParameters.enableAntiStiction); parameterWrapper->set(inertialModeControllerParameters.desatOn);
break; break;
case 0x9: case 0x9:
parameterWrapper->setVector(inertialModeControllerParameters.tgtQuat); parameterWrapper->set(inertialModeControllerParameters.enableAntiStiction);
break; break;
case 0xA: case 0xA:
parameterWrapper->setVector(inertialModeControllerParameters.tgtQuat);
break;
case 0xB:
parameterWrapper->setVector(inertialModeControllerParameters.refRotRate); parameterWrapper->setVector(inertialModeControllerParameters.refRotRate);
break; break;
case 0xC: case 0xC:
@ -690,7 +705,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->setMatrix(magnetorquerParameter.inverseAlignment); parameterWrapper->setMatrix(magnetorquerParameter.inverseAlignment);
break; break;
case 0x5: case 0x5:
parameterWrapper->set(magnetorquerParameter.dipolMax); parameterWrapper->set(magnetorquerParameter.dipoleMax);
break; break;
case 0x6: case 0x6:
parameterWrapper->set(magnetorquerParameter.torqueDuration); parameterWrapper->set(magnetorquerParameter.torqueDuration);

6
mission/controller/acs/AcsParameters.h
View File

@ -814,7 +814,7 @@ class AcsParameters : public HasParametersIF {
{1.0864, 0, 0}, {-0.5432, -0.5432, 1.2797}, {0, 0, 0}, {-0.5432, 0.5432, 1.2797}}; {1.0864, 0, 0}, {-0.5432, -0.5432, 1.2797}, {0, 0, 0}, {-0.5432, 0.5432, 1.2797}};
double without4[4][3] = { double without4[4][3] = {
{0.5432, 0.5432, 1.2797}, {0, -1.0864, 0}, {-0.5432, 0.5432, 1.2797}, {0, 0, 0}}; {0.5432, 0.5432, 1.2797}, {0, -1.0864, 0}, {-0.5432, 0.5432, 1.2797}, {0, 0, 0}};
double nullspace[4] = {-0.5000, 0.5000, -0.5000, 0.5000}; double nullspaceVector[4] = {-1, 1, -1, 1};
} rwMatrices; } rwMatrices;
struct SafeModeControllerParameters { struct SafeModeControllerParameters {
@ -838,7 +838,9 @@ class AcsParameters : public HasParametersIF {
double om = 0.3; double om = 0.3;
double omMax = 1 * M_PI / 180; double omMax = 1 * M_PI / 180;
double qiMin = 0.1; double qiMin = 0.1;
double gainNullspace = 0.01; double gainNullspace = 0.01;
double nullspaceSpeed = 32500; // 0.1 RPM
double desatMomentumRef[3] = {0, 0, 0}; double desatMomentumRef[3] = {0, 0, 0};
double deSatGainFactor = 1000; double deSatGainFactor = 1000;
@ -931,7 +933,7 @@ class AcsParameters : public HasParametersIF {
double mtq2orientationMatrix[3][3] = {{0, 0, 1}, {0, 1, 0}, {-1, 0, 0}}; double mtq2orientationMatrix[3][3] = {{0, 0, 1}, {0, 1, 0}, {-1, 0, 0}};
double alignmentMatrixMtq[3][3] = {{0, 0, -1}, {-1, 0, 0}, {0, 1, 0}}; double alignmentMatrixMtq[3][3] = {{0, 0, -1}, {-1, 0, 0}, {0, 1, 0}};
double inverseAlignment[3][3] = {{0, -1, 0}, {0, 0, 1}, {-1, 0, 0}}; double inverseAlignment[3][3] = {{0, -1, 0}, {0, 0, 1}, {-1, 0, 0}};
double dipolMax = 0.2; // [Am^2] double dipoleMax = 0.2; // [Am^2]
uint16_t torqueDuration = 300; // [ms] uint16_t torqueDuration = 300; // [ms]
} magnetorquerParameter; } magnetorquerParameter;

61
mission/controller/acs/ActuatorCmd.cpp
View File

@ -5,11 +5,6 @@
#include <fsfw/globalfunctions/math/QuaternionOperations.h> #include <fsfw/globalfunctions/math/QuaternionOperations.h>
#include <fsfw/globalfunctions/math/VectorOperations.h> #include <fsfw/globalfunctions/math/VectorOperations.h>
#include <cmath>
#include "util/CholeskyDecomposition.h"
#include "util/MathOperations.h"
ActuatorCmd::ActuatorCmd() {} ActuatorCmd::ActuatorCmd() {}
ActuatorCmd::~ActuatorCmd() {} ActuatorCmd::~ActuatorCmd() {}
@ -25,24 +20,30 @@ void ActuatorCmd::scalingTorqueRws(double *rwTrq, double maxTorque) {
} }
} }
void ActuatorCmd::cmdSpeedToRws(int32_t speedRw0, int32_t speedRw1, int32_t speedRw2, void ActuatorCmd::cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1,
int32_t speedRw3, const double *rwTorque, int32_t *rwCmdSpeed, const int32_t speedRw2, const int32_t speedRw3,
double sampleTime, int32_t maxRwSpeed, double inertiaWheel) { const double sampleTime, const double inertiaWheel,
using namespace Math; const int32_t maxRwSpeed, const double *rwTorque,
int32_t *rwCmdSpeed) {
// Calculating the commanded speed in RPM for every reaction wheel // concentrate RW speed values (in 0.1 [RPM]) in vector
int32_t speedRws[4] = {speedRw0, speedRw1, speedRw2, speedRw3}; int32_t speedRws[4] = {speedRw0, speedRw1, speedRw2, speedRw3};
// calculate required RW speed as sum of current RW speed and RW speed delta
// delta w_rw = delta t / I_RW * torque_RW [rad/s]
double deltaSpeed[4] = {0, 0, 0, 0}; double deltaSpeed[4] = {0, 0, 0, 0};
double radToRpm = 60 / (2 * PI); // factor for conversion to RPM const double factor = sampleTime / inertiaWheel * RAD_PER_SEC_TO_RPM * 10;
// W_RW = Torque_RW / I_RW * delta t [rad/s]
double factor = sampleTime / inertiaWheel * radToRpm;
int32_t deltaSpeedInt[4] = {0, 0, 0, 0};
VectorOperations<double>::mulScalar(rwTorque, factor, deltaSpeed, 4); VectorOperations<double>::mulScalar(rwTorque, factor, deltaSpeed, 4);
// convert double to int32
int32_t deltaSpeedInt[4] = {0, 0, 0, 0};
for (int i = 0; i < 4; i++) { for (int i = 0; i < 4; i++) {
deltaSpeedInt[i] = std::round(deltaSpeed[i]); deltaSpeedInt[i] = std::round(deltaSpeed[i]);
} }
// sum of current RW speed and RW speed delta
VectorOperations<int32_t>::add(speedRws, deltaSpeedInt, rwCmdSpeed, 4); VectorOperations<int32_t>::add(speedRws, deltaSpeedInt, rwCmdSpeed, 4);
VectorOperations<int32_t>::mulScalar(rwCmdSpeed, 10, rwCmdSpeed, 4);
// crop values which would exceed the maximum possible RPM
for (uint8_t i = 0; i < 4; i++) { for (uint8_t i = 0; i < 4; i++) {
if (rwCmdSpeed[i] > maxRwSpeed) { if (rwCmdSpeed[i] > maxRwSpeed) {
rwCmdSpeed[i] = maxRwSpeed; rwCmdSpeed[i] = maxRwSpeed;
@ -52,24 +53,24 @@ void ActuatorCmd::cmdSpeedToRws(int32_t speedRw0, int32_t speedRw1, int32_t spee
} }
} }
void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator, void ActuatorCmd::cmdDipoleMtq(const double *inverseAlignment, const double maxDipole,
const double *inverseAlignment, double maxDipol) { const double *dipoleMoment, int16_t *dipoleMomentActuator) {
// Convert to actuator frame // convert to actuator frame
double dipolMomentActuatorDouble[3] = {0, 0, 0}; double dipoleMomentActuatorDouble[3] = {0, 0, 0};
MatrixOperations<double>::multiply(inverseAlignment, dipolMoment, dipolMomentActuatorDouble, 3, 3, MatrixOperations<double>::multiply(inverseAlignment, dipoleMoment, dipoleMomentActuatorDouble, 3, 3,
1); 1);
// Scaling along largest element if dipol exceeds maximum // scaling along largest element if dipole exceeds maximum
uint8_t maxIdx = 0; uint8_t maxIdx = 0;
VectorOperations<double>::maxAbsValue(dipolMomentActuatorDouble, 3, &maxIdx); VectorOperations<double>::maxAbsValue(dipoleMomentActuatorDouble, 3, &maxIdx);
double maxAbsValue = abs(dipolMomentActuatorDouble[maxIdx]); double maxAbsValue = abs(dipoleMomentActuatorDouble[maxIdx]);
if (maxAbsValue > maxDipol) { if (maxAbsValue > maxDipole) {
double scalingFactor = maxDipol / maxAbsValue; double scalingFactor = maxDipole / maxAbsValue;
VectorOperations<double>::mulScalar(dipolMomentActuatorDouble, scalingFactor, VectorOperations<double>::mulScalar(dipoleMomentActuatorDouble, scalingFactor,
dipolMomentActuatorDouble, 3); dipoleMomentActuatorDouble, 3);
} }
// scale dipole from 1 Am^2 to 1e^-4 Am^2 // scale dipole from 1 Am^2 to 1e^-4 Am^2
VectorOperations<double>::mulScalar(dipolMomentActuatorDouble, 1e4, dipolMomentActuatorDouble, 3); VectorOperations<double>::mulScalar(dipoleMomentActuatorDouble, 1e4, dipoleMomentActuatorDouble, 3);
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
dipolMomentActuator[i] = std::round(dipolMomentActuatorDouble[i]); dipoleMomentActuator[i] = std::round(dipoleMomentActuatorDouble[i]);
} }
} }

32
mission/controller/acs/ActuatorCmd.h
View File

@ -1,9 +1,8 @@
#ifndef ACTUATORCMD_H_ #ifndef ACTUATORCMD_H_
#define ACTUATORCMD_H_ #define ACTUATORCMD_H_
#include "MultiplicativeKalmanFilter.h" #include <cmath>
#include "SensorProcessing.h"
#include "SensorValues.h"
class ActuatorCmd { class ActuatorCmd {
public: public:
@ -19,29 +18,30 @@ class ActuatorCmd {
void scalingTorqueRws(double *rwTrq, double maxTorque); void scalingTorqueRws(double *rwTrq, double maxTorque);
/* /*
* @brief: cmdSpeedToRws() will set the maximum possible torque for the reaction * @brief: cmdSpeedToRws() Calculates the RPM for the reaction wheel configuration,
* wheels, also will calculate the needed revolutions per minute for the RWs, which will be given * given the required torque calculated by the controller. Will also scale down the RPM of the
* as Input to the RWs * wheels if they exceed the maximum possible RPM
* @param: rwTrqIn given torque from pointing controller * @param: rwTrq given torque from pointing controller
* rwTrqNS Nullspace torque
* rwCmdSpeed output revolutions per minute for every * rwCmdSpeed output revolutions per minute for every
* reaction wheel * reaction wheel
*/ */
void cmdSpeedToRws(int32_t speedRw0, int32_t speedRw1, int32_t speedRw2, int32_t speedRw3, void cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2,
const double *rwTorque, int32_t *rwCmdSpeed, double sampleTime, const int32_t speedRw3, const double sampleTime, const double inertiaWheel,
int32_t maxRwSpeed, double inertiaWheel); const int32_t maxRwSpeed, const double *rwTorque, int32_t *rwCmdSpeed);
/* /*
* @brief: cmdDipolMtq() gives the commanded dipol moment for the magnetorques * @brief: cmdDipoleMtq() gives the commanded dipole moment for the
* magnetorquer
* *
* @param: dipolMoment given dipol moment in spacecraft frame * @param: dipoleMoment given dipole moment in spacecraft frame
* dipolMomentActuator resulting dipol moment in actuator reference frame * dipoleMomentActuator resulting dipole moment in actuator reference frame
*/ */
void cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator, void cmdDipoleMtq(const double *inverseAlignment, const double maxDipole,
const double *inverseAlignment, double maxDipol); const double *dipoleMoment, int16_t *dipoleMomentActuator);
protected: protected:
private: private:
static constexpr double RAD_PER_SEC_TO_RPM = 60 / (2 * M_PI);
}; };
#endif /* ACTUATORCMD_H_ */ #endif /* ACTUATORCMD_H_ */

6
mission/controller/acs/control/Detumble.cpp
View File

@ -7,8 +7,10 @@ Detumble::Detumble() {}
Detumble::~Detumble() {} Detumble::~Detumble() {}
uint8_t Detumble::detumbleStrategy(const bool magFieldValid, const bool satRotRateValid, acs::SafeModeStrategy Detumble::detumbleStrategy(const bool magFieldValid,
const bool magFieldRateValid, const bool useFullDetumbleLaw) { const bool satRotRateValid,
const bool magFieldRateValid,
const bool useFullDetumbleLaw) {
if (not magFieldValid) { if (not magFieldValid) {
return acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL; return acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL;
} else if (satRotRateValid and useFullDetumbleLaw) { } else if (satRotRateValid and useFullDetumbleLaw) {

5
mission/controller/acs/control/Detumble.h
View File

@ -11,8 +11,9 @@ class Detumble {
Detumble(); Detumble();
virtual ~Detumble(); virtual ~Detumble();
uint8_t detumbleStrategy(const bool magFieldValid, const bool satRotRateValid, acs::SafeModeStrategy detumbleStrategy(const bool magFieldValid, const bool satRotRateValid,
const bool magFieldRateValid, const bool useFullDetumbleLaw); const bool magFieldRateValid,
const bool useFullDetumbleLaw);
void bDotLawFull(const double *satRotRateB, const double *magFieldB, double *magMomB, void bDotLawFull(const double *satRotRateB, const double *magFieldB, double *magMomB,
double gain); double gain);

140
mission/controller/acs/control/PtgCtrl.cpp
View File

@ -5,9 +5,6 @@
#include <fsfw/globalfunctions/math/QuaternionOperations.h> #include <fsfw/globalfunctions/math/QuaternionOperations.h>
#include <fsfw/globalfunctions/math/VectorOperations.h> #include <fsfw/globalfunctions/math/VectorOperations.h>
#include <fsfw/globalfunctions/sign.h> #include <fsfw/globalfunctions/sign.h>
#include <math.h>
#include "../util/MathOperations.h"
PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameters_; } PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameters_; }
@ -98,61 +95,88 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters
VectorOperations<double>::mulScalar(torqueRws, -1, torqueRws, 4); VectorOperations<double>::mulScalar(torqueRws, -1, torqueRws, 4);
} }
void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawParameters,
const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2,
const int32_t speedRw3, double *rwTrqNs) {
// concentrate RW speeds as vector and convert to double
double speedRws[4] = {static_cast<double>(speedRw0), static_cast<double>(speedRw1),
static_cast<double>(speedRw2), static_cast<double>(speedRw3)};
// calculate RPM offset utilizing the nullspace
double rpmOffset[4] = {0, 0, 0, 0};
double rpmOffsetSpeed = pointingLawParameters->nullspaceSpeed / 10 * RPM_TO_RAD_PER_SEC;
VectorOperations<double>::mulScalar(acsParameters->rwMatrices.nullspaceVector, rpmOffsetSpeed,
rpmOffset, 4);
// calculate resulting angular momentum
double rwAngMomentum[4] = {0, 0, 0, 0}, diffRwSpeed[4] = {0, 0, 0, 0};
VectorOperations<double>::subtract(speedRws, rpmOffset, diffRwSpeed, 4);
VectorOperations<double>::mulScalar(diffRwSpeed, acsParameters->rwHandlingParameters.inertiaWheel,
rwAngMomentum, 4);
// calculate resulting torque
double nullspaceMatrix[4][4] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MatrixOperations<double>::multiply(acsParameters->rwMatrices.nullspaceVector,
acsParameters->rwMatrices.nullspaceVector, *nullspaceMatrix, 4,
1, 4);
MatrixOperations<double>::multiply(*nullspaceMatrix, rwAngMomentum, rwTrqNs, 4, 4, 1);
VectorOperations<double>::mulScalar(rwTrqNs, -1 * pointingLawParameters->gainNullspace, rwTrqNs,
4);
}
void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters, void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters,
double *magFieldEst, bool magFieldEstValid, double *satRate, const double *magFieldB, const bool magFieldBValid,
int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2, const double *satRate, const int32_t speedRw0, const int32_t speedRw1,
int32_t *speedRw3, double *mgtDpDes) { const int32_t speedRw2, const int32_t speedRw3, double *mgtDpDes) {
if (!(magFieldEstValid) || !(pointingLawParameters->desatOn)) { if (not magFieldBValid or not pointingLawParameters->desatOn) {
mgtDpDes[0] = 0;
mgtDpDes[1] = 0;
mgtDpDes[2] = 0;
return; return;
} }
// calculating momentum of satellite and momentum of reaction wheels // concentrate RW speeds as vector and convert to double
double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3}; double speedRws[4] = {static_cast<double>(speedRw0), static_cast<double>(speedRw1),
double momentumRwU[4] = {0, 0, 0, 0}, momentumRw[3] = {0, 0, 0}; static_cast<double>(speedRw2), static_cast<double>(speedRw3)};
VectorOperations<double>::mulScalar(speedRws, acsParameters->rwHandlingParameters.inertiaWheel,
momentumRwU, 4);
MatrixOperations<double>::multiply(*(acsParameters->rwMatrices.alignmentMatrix), momentumRwU,
momentumRw, 3, 4, 1);
double momentumSat[3] = {0, 0, 0}, momentumTotal[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*(acsParameters->inertiaEIVE.inertiaMatrixDeployed), satRate,
momentumSat, 3, 3, 1);
VectorOperations<double>::add(momentumSat, momentumRw, momentumTotal, 3);
// calculating momentum error
double deltaMomentum[3] = {0, 0, 0};
VectorOperations<double>::subtract(momentumTotal, pointingLawParameters->desatMomentumRef,
deltaMomentum, 3);
// resulting magnetic dipole command
double crossMomentumMagField[3] = {0, 0, 0};
VectorOperations<double>::cross(deltaMomentum, magFieldEst, crossMomentumMagField);
double normMag = VectorOperations<double>::norm(magFieldEst, 3), factor = 0;
factor = (pointingLawParameters->deSatGainFactor) / normMag;
VectorOperations<double>::mulScalar(crossMomentumMagField, factor, mgtDpDes, 3);
}
void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawParameters, // convert magFieldB from uT to T
const int32_t *speedRw0, const int32_t *speedRw1, double magFieldBT[3] = {0, 0, 0};
const int32_t *speedRw2, const int32_t *speedRw3, double *rwTrqNs) { VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3};
double wheelMomentum[4] = {0, 0, 0, 0}; // calculate angular momentum of the satellite
double rpmOffset[4] = {1, 1, 1, -1}, factor = 350 * 2 * Math::PI / 60; double angMomentumSat[3] = {0, 0, 0};
// conversion to [rad/s] for further calculations MatrixOperations<double>::multiply(*(acsParameters->inertiaEIVE.inertiaMatrixDeployed), satRate,
VectorOperations<double>::mulScalar(rpmOffset, factor, rpmOffset, 4); angMomentumSat, 3, 3, 1);
VectorOperations<double>::mulScalar(speedRws, 2 * Math::PI / 60, speedRws, 4);
double diffRwSpeed[4] = {0, 0, 0, 0}; // calculate angular momentum of the reaction wheels with respect to the nullspace RW speed
VectorOperations<double>::subtract(speedRws, rpmOffset, diffRwSpeed, 4); // relocate RW speed zero to nullspace RW speed
VectorOperations<double>::mulScalar(diffRwSpeed, acsParameters->rwHandlingParameters.inertiaWheel, double refSpeedRws[4] = {0, 0, 0, 0};
wheelMomentum, 4); VectorOperations<double>::mulScalar(acsParameters->rwMatrices.nullspaceVector,
double gainNs = pointingLawParameters->gainNullspace; pointingLawParameters->nullspaceSpeed, refSpeedRws, 4);
double nullSpaceMatrix[4][4] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; VectorOperations<double>::subtract(speedRws, refSpeedRws, speedRws, 4);
MathOperations<double>::vecTransposeVecMatrix(acsParameters->rwMatrices.nullspace, // convert to rad/s
acsParameters->rwMatrices.nullspace, VectorOperations<double>::mulScalar(speedRws, RPM_TO_RAD_PER_SEC, speedRws, 4);
*nullSpaceMatrix, 4); // calculate angular momentum of each RW
MatrixOperations<double>::multiply(*nullSpaceMatrix, wheelMomentum, rwTrqNs, 4, 4, 1); double angMomentumRwU[4] = {0, 0, 0, 0};
VectorOperations<double>::mulScalar(rwTrqNs, gainNs, rwTrqNs, 4); VectorOperations<double>::mulScalar(speedRws, acsParameters->rwHandlingParameters.inertiaWheel,
VectorOperations<double>::mulScalar(rwTrqNs, -1, rwTrqNs, 4); angMomentumRwU, 4);
// convert RW angular momentum to body RF
double angMomentumRw[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*(acsParameters->rwMatrices.alignmentMatrix), angMomentumRwU,
angMomentumRw, 3, 4, 1);
// calculate total angular momentum
double angMomentumTotal[3] = {0, 0, 0};
VectorOperations<double>::add(angMomentumSat, angMomentumRw, angMomentumTotal, 3);
// calculating momentum error
double deltaAngMomentum[3] = {0, 0, 0};
VectorOperations<double>::subtract(angMomentumTotal, pointingLawParameters->desatMomentumRef,
deltaAngMomentum, 3);
// resulting magnetic dipole command
double crossAngMomentumMagField[3] = {0, 0, 0};
VectorOperations<double>::cross(deltaAngMomentum, magFieldBT, crossAngMomentumMagField);
double factor =
pointingLawParameters->deSatGainFactor / VectorOperations<double>::norm(magFieldBT, 3);
VectorOperations<double>::mulScalar(crossAngMomentumMagField, factor, mgtDpDes, 3);
} }
void PtgCtrl::rwAntistiction(ACS::SensorValues *sensorValues, int32_t *rwCmdSpeeds) { void PtgCtrl::rwAntistiction(ACS::SensorValues *sensorValues, int32_t *rwCmdSpeeds) {
@ -169,15 +193,9 @@ void PtgCtrl::rwAntistiction(ACS::SensorValues *sensorValues, int32_t *rwCmdSpee
if (rwCmdSpeeds[i] != 0) { if (rwCmdSpeeds[i] != 0) {
if (rwCmdSpeeds[i] > -acsParameters->rwHandlingParameters.stictionSpeed && if (rwCmdSpeeds[i] > -acsParameters->rwHandlingParameters.stictionSpeed &&
rwCmdSpeeds[i] < acsParameters->rwHandlingParameters.stictionSpeed) { rwCmdSpeeds[i] < acsParameters->rwHandlingParameters.stictionSpeed) {
if (currRwSpeed[i] == 0) { if (rwCmdSpeeds[i] > currRwSpeed[i]) {
if (rwCmdSpeeds[i] > 0) {
rwCmdSpeeds[i] = acsParameters->rwHandlingParameters.stictionSpeed;
} else if (rwCmdSpeeds[i] < 0) {
rwCmdSpeeds[i] = -acsParameters->rwHandlingParameters.stictionSpeed;
}
} else if (currRwSpeed[i] < -acsParameters->rwHandlingParameters.stictionSpeed) {
rwCmdSpeeds[i] = acsParameters->rwHandlingParameters.stictionSpeed; rwCmdSpeeds[i] = acsParameters->rwHandlingParameters.stictionSpeed;
} else if (currRwSpeed[i] > acsParameters->rwHandlingParameters.stictionSpeed) { } else if (rwCmdSpeeds[i] < currRwSpeed[i]) {
rwCmdSpeeds[i] = -acsParameters->rwHandlingParameters.stictionSpeed; rwCmdSpeeds[i] = -acsParameters->rwHandlingParameters.stictionSpeed;
} }
} }

24
mission/controller/acs/control/PtgCtrl.h
View File

@ -1,13 +1,10 @@
#ifndef PTGCTRL_H_ #ifndef PTGCTRL_H_
#define PTGCTRL_H_ #define PTGCTRL_H_
#include <math.h>
#include <mission/controller/acs/AcsParameters.h>
#include <mission/controller/acs/SensorValues.h>
#include <stdio.h> #include <stdio.h>
#include <string.h>
#include <time.h>
#include "../AcsParameters.h"
#include "../SensorValues.h"
#include "eive/resultClassIds.h"
class PtgCtrl { class PtgCtrl {
/* /*
@ -29,14 +26,14 @@ class PtgCtrl {
void ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, const double *qError, void ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, const double *qError,
const double *deltaRate, const double *rwPseudoInv, double *torqueRws); const double *deltaRate, const double *rwPseudoInv, double *torqueRws);
void ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters,
double *magFieldEst, bool magFieldEstValid, double *satRate,
int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2, int32_t *speedRw3,
double *mgtDpDes);
void ptgNullspace(AcsParameters::PointingLawParameters *pointingLawParameters, void ptgNullspace(AcsParameters::PointingLawParameters *pointingLawParameters,
const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2, const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2,
const int32_t *speedRw3, double *rwTrqNs); const int32_t speedRw3, double *rwTrqNs);
void ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters,
const double *magFieldB, const bool magFieldBValid, const double *satRate,
const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2,
const int32_t speedRw3, double *mgtDpDes);
/* @brief: Commands the stiction torque in case wheel speed is to low /* @brief: Commands the stiction torque in case wheel speed is to low
* torqueCommand modified torque after antistiction * torqueCommand modified torque after antistiction
@ -45,6 +42,7 @@ class PtgCtrl {
private: private:
const AcsParameters *acsParameters; const AcsParameters *acsParameters;
static constexpr double RPM_TO_RAD_PER_SEC = (2 * M_PI) / 60;
}; };
#endif /* ACS_CONTROL_PTGCTRL_H_ */ #endif /* ACS_CONTROL_PTGCTRL_H_ */

7
mission/controller/acs/control/SafeCtrl.cpp
View File

@ -9,9 +9,10 @@ SafeCtrl::SafeCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameter
SafeCtrl::~SafeCtrl() {} SafeCtrl::~SafeCtrl() {}
uint8_t SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const bool mekfValid, acs::SafeModeStrategy SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const bool mekfValid,
const bool satRotRateValid, const bool sunDirValid, const bool satRotRateValid, const bool sunDirValid,
const uint8_t mekfEnabled, const uint8_t dampingEnabled) { const uint8_t mekfEnabled,
const uint8_t dampingEnabled) {
if (not magFieldValid) { if (not magFieldValid) {
return acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL; return acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL;
} else if (mekfEnabled and mekfValid) { } else if (mekfEnabled and mekfValid) {

6
mission/controller/acs/control/SafeCtrl.h
View File

@ -12,9 +12,9 @@ class SafeCtrl {
SafeCtrl(AcsParameters *acsParameters_); SafeCtrl(AcsParameters *acsParameters_);
virtual ~SafeCtrl(); virtual ~SafeCtrl();
uint8_t safeCtrlStrategy(const bool magFieldValid, const bool mekfValid, acs::SafeModeStrategy safeCtrlStrategy(const bool magFieldValid, const bool mekfValid,
const bool satRotRateValid, const bool sunDirValid, const bool satRotRateValid, const bool sunDirValid,
const uint8_t mekfEnabled, const uint8_t dampingEnabled); const uint8_t mekfEnabled, const uint8_t dampingEnabled);
void safeMekf(const double *magFieldB, const double *satRotRateB, const double *sunDirModelI, void safeMekf(const double *magFieldB, const double *satRotRateB, const double *sunDirModelI,
const double *quatBI, const double *sunDirRefB, double *magMomB, const double *quatBI, const double *sunDirRefB, double *magMomB,

5
mission/genericFactory.cpp
View File

@ -96,9 +96,10 @@ std::atomic_bool tcs::TCS_BOARD_SHORTLY_UNAVAILABLE = false;
void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_, PusTmFunnel** pusFunnel, void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_, PusTmFunnel** pusFunnel,
CfdpTmFunnel** cfdpFunnel, SdCardMountedIF& sdcMan, CfdpTmFunnel** cfdpFunnel, SdCardMountedIF& sdcMan,
StorageManagerIF** ipcStore, StorageManagerIF** tmStore, StorageManagerIF** ipcStore, StorageManagerIF** tmStore,
PersistentTmStores& stores) { PersistentTmStores& stores,
uint32_t eventManagerQueueDepth) {
// Framework objects // Framework objects
new EventManager(objects::EVENT_MANAGER, 160); new EventManager(objects::EVENT_MANAGER, eventManagerQueueDepth);
auto healthTable = new HealthTable(objects::HEALTH_TABLE); auto healthTable = new HealthTable(objects::HEALTH_TABLE);
if (healthTable_ != nullptr) { if (healthTable_ != nullptr) {
*healthTable_ = healthTable; *healthTable_ = healthTable;

2
mission/genericFactory.h
View File

@ -45,7 +45,7 @@ namespace ObjectFactory {
void produceGenericObjects(HealthTableIF** healthTable, PusTmFunnel** pusFunnel, void produceGenericObjects(HealthTableIF** healthTable, PusTmFunnel** pusFunnel,
CfdpTmFunnel** cfdpFunnel, SdCardMountedIF& sdcMan, CfdpTmFunnel** cfdpFunnel, SdCardMountedIF& sdcMan,
StorageManagerIF** ipcStore, StorageManagerIF** tmStore, StorageManagerIF** ipcStore, StorageManagerIF** tmStore,
PersistentTmStores& stores); PersistentTmStores& stores, uint32_t eventManagerQueueDepth);
void createGenericHeaterComponents(GpioIF& gpioIF, PowerSwitchIF& pwrSwitcher, void createGenericHeaterComponents(GpioIF& gpioIF, PowerSwitchIF& pwrSwitcher,
HeaterHandler*& heaterHandler); HeaterHandler*& heaterHandler);

10
mission/scheduling.cpp
View File

@ -5,10 +5,12 @@
#include "fsfw/tasks/PeriodicTaskIF.h" #include "fsfw/tasks/PeriodicTaskIF.h"
void scheduling::scheduleTmpTempSensors(PeriodicTaskIF* tmpTask) { void scheduling::scheduleTmpTempSensors(PeriodicTaskIF* tmpTask) {
const std::array<object_id_t, 5> tmpIds = { const std::array<object_id_t, 4> tmpIds = {objects::TMP1075_HANDLER_TCS_0,
objects::TMP1075_HANDLER_TCS_0, objects::TMP1075_HANDLER_TCS_1, objects::TMP1075_HANDLER_TCS_1,
objects::TMP1075_HANDLER_PLPCDU_0, objects::TMP1075_HANDLER_PLPCDU_1, objects::TMP1075_HANDLER_PLPCDU_0,
objects::TMP1075_HANDLER_IF_BOARD}; // damaged.
// objects::TMP1075_HANDLER_PLPCDU_1,
objects::TMP1075_HANDLER_IF_BOARD};
for (const auto& tmpId : tmpIds) { for (const auto& tmpId : tmpIds) {
tmpTask->addComponent(tmpId, DeviceHandlerIF::PERFORM_OPERATION); tmpTask->addComponent(tmpId, DeviceHandlerIF::PERFORM_OPERATION);
tmpTask->addComponent(tmpId, DeviceHandlerIF::SEND_WRITE); tmpTask->addComponent(tmpId, DeviceHandlerIF::SEND_WRITE);

20
mission/sysDefs.h
View File

@ -1,16 +1,28 @@
#ifndef MISSION_SYSDEFS_H_ #ifndef MISSION_SYSDEFS_H_
#define MISSION_SYSDEFS_H_ #define MISSION_SYSDEFS_H_
#include <atomic> #include <fsfw/action/ActionMessage.h>
#include <fsfw/action/HasActionsIF.h>
#include <fsfw/modes/ModeMessage.h>
#include <fsfw/serialize/SerializeIF.h>
#include "acs/defs.h" #include <atomic>
#include <cstring>
extern std::atomic_uint16_t I2C_FATAL_ERRORS; extern std::atomic_uint16_t I2C_FATAL_ERRORS;
namespace satsystem { namespace satsystem {
enum Mode : Mode_t { BOOT = 5, SAFE = acs::AcsMode::SAFE, PTG_IDLE = acs::AcsMode::PTG_IDLE }; enum Mode : Mode_t {
BOOT = 5,
// DO NOT CHANGE THE ORDER starting from here, breaks ACS mode checks.
SAFE = 10,
PTG_IDLE = 11,
PTG_NADIR = 12,
PTG_TARGET = 13,
PTG_TARGET_GS = 14,
PTG_INERTIAL = 15,
};
} }
namespace xsc { namespace xsc {

12
mission/system/EiveSystem.cpp
View File

@ -39,18 +39,22 @@ void EiveSystem::announceMode(bool recursive) {
modeStr = "POINTING IDLE"; modeStr = "POINTING IDLE";
break; break;
} }
case (acs::AcsMode::PTG_INERTIAL): { case (satsystem::Mode::PTG_NADIR): {
modeStr = "POINTING INERTIAL"; modeStr = "POINTING NADIR";
break; break;
} }
case (acs::AcsMode::PTG_TARGET): { case (satsystem::Mode::PTG_TARGET): {
modeStr = "POINTING TARGET"; modeStr = "POINTING TARGET";
break; break;
} }
case (acs::AcsMode::PTG_TARGET_GS): { case (satsystem::Mode::PTG_TARGET_GS): {
modeStr = "POINTING TARGET GS"; modeStr = "POINTING TARGET GS";
break; break;
} }
case (satsystem::Mode::PTG_INERTIAL): {
modeStr = "POINTING INERTIAL";
break;
}
} }
sif::info << "EIVE system is now in " << modeStr << " mode" << std::endl; sif::info << "EIVE system is now in " << modeStr << " mode" << std::endl;
return Subsystem::announceMode(recursive); return Subsystem::announceMode(recursive);

9
mission/system/acs/DualLaneAssemblyBase.cpp
View File

@ -183,11 +183,11 @@ void DualLaneAssemblyBase::handleModeTransitionFailed(ReturnValue_t result) {
// transition to dual mode. // transition to dual mode.
if (not tryingOtherSide) { if (not tryingOtherSide) {
triggerEvent(CANT_KEEP_MODE, mode, submode); triggerEvent(CANT_KEEP_MODE, mode, submode);
startTransition(mode, nextSubmode); startTransition(targetMode, nextSubmode);
tryingOtherSide = true; tryingOtherSide = true;
} else { } else {
triggerEvent(transitionOtherSideFailedEvent, mode, targetSubmode); triggerEvent(transitionOtherSideFailedEvent, targetMode, targetSubmode);
startTransition(mode, Submodes::DUAL_MODE); startTransition(targetMode, Submodes::DUAL_MODE);
} }
} }
@ -205,7 +205,8 @@ bool DualLaneAssemblyBase::checkAndHandleRecovery() {
opCode = pwrStateMachine.fsm(); opCode = pwrStateMachine.fsm();
if (opCode == OpCodes::TO_OFF_DONE or opCode == OpCodes::TIMEOUT_OCCURED) { if (opCode == OpCodes::TO_OFF_DONE or opCode == OpCodes::TIMEOUT_OCCURED) {
customRecoveryStates = RecoveryCustomStates::POWER_SWITCHING_ON; customRecoveryStates = RecoveryCustomStates::POWER_SWITCHING_ON;
pwrStateMachine.start(targetMode, targetSubmode); // Command power back on in any case.
pwrStateMachine.start(HasModesIF::MODE_ON, targetSubmode);
} }
} }
if (customRecoveryStates == POWER_SWITCHING_ON) { if (customRecoveryStates == POWER_SWITCHING_ON) {

360
mission/system/systemTree.cpp
View File

@ -22,6 +22,10 @@ const auto check = subsystem::checkInsert;
void buildBootSequence(Subsystem& ss, ModeListEntry& eh); void buildBootSequence(Subsystem& ss, ModeListEntry& eh);
void buildSafeSequence(Subsystem& ss, ModeListEntry& eh); void buildSafeSequence(Subsystem& ss, ModeListEntry& eh);
void buildIdleSequence(Subsystem& ss, ModeListEntry& eh); void buildIdleSequence(Subsystem& ss, ModeListEntry& eh);
void buildPtgNadirSequence(Subsystem& ss, ModeListEntry& eh);
void buildPtgTargetSequence(Subsystem& ss, ModeListEntry& eh);
void buildPtgTargetGsSequence(Subsystem& ss, ModeListEntry& eh);
void buildPtgInertialSequence(Subsystem& ss, ModeListEntry& eh);
} // namespace } // namespace
static const auto OFF = HasModesIF::MODE_OFF; static const auto OFF = HasModesIF::MODE_OFF;
@ -40,6 +44,10 @@ void satsystem::init() {
buildBootSequence(EIVE_SYSTEM, entry); buildBootSequence(EIVE_SYSTEM, entry);
buildSafeSequence(EIVE_SYSTEM, entry); buildSafeSequence(EIVE_SYSTEM, entry);
buildIdleSequence(EIVE_SYSTEM, entry); buildIdleSequence(EIVE_SYSTEM, entry);
buildPtgNadirSequence(EIVE_SYSTEM, entry);
buildPtgTargetSequence(EIVE_SYSTEM, entry);
buildPtgTargetGsSequence(EIVE_SYSTEM, entry);
buildPtgInertialSequence(EIVE_SYSTEM, entry);
EIVE_SYSTEM.setInitialMode(satsystem::Mode::BOOT, 0); EIVE_SYSTEM.setInitialMode(satsystem::Mode::BOOT, 0);
} }
@ -68,88 +76,44 @@ auto EIVE_TABLE_IDLE_TRANS_0 =
auto EIVE_TABLE_IDLE_TRANS_1 = auto EIVE_TABLE_IDLE_TRANS_1 =
std::make_pair((satsystem::Mode::PTG_IDLE << 24) | 3, FixedArrayList<ModeListEntry, 5>()); std::make_pair((satsystem::Mode::PTG_IDLE << 24) | 3, FixedArrayList<ModeListEntry, 5>());
auto EIVE_SEQUENCE_PTG_NADIR =
std::make_pair(satsystem::Mode::PTG_NADIR, FixedArrayList<ModeListEntry, 5>());
auto EIVE_TABLE_PTG_NADIR_TGT =
std::make_pair((satsystem::Mode::PTG_NADIR << 24) | 1, FixedArrayList<ModeListEntry, 5>());
auto EIVE_TABLE_PTG_NADIR_TRANS_0 =
std::make_pair((satsystem::Mode::PTG_NADIR << 24) | 2, FixedArrayList<ModeListEntry, 5>());
auto EIVE_TABLE_PTG_NADIR_TRANS_1 =
std::make_pair((satsystem::Mode::PTG_NADIR << 24) | 3, FixedArrayList<ModeListEntry, 5>());
auto EIVE_SEQUENCE_PTG_TARGET =
std::make_pair(satsystem::Mode::PTG_TARGET, FixedArrayList<ModeListEntry, 5>());
auto EIVE_TABLE_PTG_TARGET_TGT =
std::make_pair((satsystem::Mode::PTG_TARGET << 24) | 1, FixedArrayList<ModeListEntry, 5>());
auto EIVE_TABLE_PTG_TARGET_TRANS_0 =
std::make_pair((satsystem::Mode::PTG_TARGET << 24) | 2, FixedArrayList<ModeListEntry, 5>());
auto EIVE_TABLE_PTG_TARGET_TRANS_1 =
std::make_pair((satsystem::Mode::PTG_TARGET << 24) | 3, FixedArrayList<ModeListEntry, 5>());
auto EIVE_SEQUENCE_PTG_TARGET_GS =
std::make_pair(satsystem::Mode::PTG_TARGET_GS, FixedArrayList<ModeListEntry, 5>());
auto EIVE_TABLE_PTG_TARGET_GS_TGT =
std::make_pair((satsystem::Mode::PTG_TARGET_GS << 24) | 1, FixedArrayList<ModeListEntry, 5>());
auto EIVE_TABLE_PTG_TARGET_GS_TRANS_0 =
std::make_pair((satsystem::Mode::PTG_TARGET_GS << 24) | 2, FixedArrayList<ModeListEntry, 5>());
auto EIVE_TABLE_PTG_TARGET_GS_TRANS_1 =
std::make_pair((satsystem::Mode::PTG_TARGET_GS << 24) | 3, FixedArrayList<ModeListEntry, 5>());
auto EIVE_SEQUENCE_PTG_INERTIAL =
std::make_pair(satsystem::Mode::PTG_INERTIAL, FixedArrayList<ModeListEntry, 5>());
auto EIVE_TABLE_PTG_INERTIAL_TGT =
std::make_pair((satsystem::Mode::PTG_INERTIAL << 24) | 1, FixedArrayList<ModeListEntry, 5>());
auto EIVE_TABLE_PTG_INERTIAL_TRANS_0 =
std::make_pair((satsystem::Mode::PTG_INERTIAL << 24) | 2, FixedArrayList<ModeListEntry, 5>());
auto EIVE_TABLE_PTG_INERTIAL_TRANS_1 =
std::make_pair((satsystem::Mode::PTG_INERTIAL << 24) | 3, FixedArrayList<ModeListEntry, 5>());
namespace { namespace {
void buildSafeSequence(Subsystem& ss, ModeListEntry& eh) {
std::string context = "satsystem::buildSafeSequence";
auto ctxc = context.c_str();
// Insert Helper Table
auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode, ArrayList<ModeListEntry>& table,
bool allowAllSubmodes = false) {
eh.setObject(obj);
eh.setMode(mode);
eh.setSubmode(submode);
if (allowAllSubmodes) {
eh.allowAllSubmodes();
}
check(table.insert(eh), ctxc);
};
// Insert Helper Sequence
auto ihs = [&](ArrayList<ModeListEntry>& sequence, Mode_t tableId, uint32_t waitSeconds,
bool checkSuccess) {
eh.setTableId(tableId);
eh.setWaitSeconds(waitSeconds);
eh.setCheckSuccess(checkSuccess);
check(sequence.insert(eh), ctxc);
};
// Do no track submode to allow transitions to DETUMBLE submode.
iht(objects::ACS_SUBSYSTEM, acs::AcsMode::SAFE, 0, EIVE_TABLE_SAFE_TGT.second, true);
iht(objects::PL_SUBSYSTEM, OFF, 0, EIVE_TABLE_SAFE_TGT.second);
check(ss.addTable(TableEntry(EIVE_TABLE_SAFE_TGT.first, &EIVE_TABLE_SAFE_TGT.second)), ctxc);
// Build SAFE transition 0.
iht(objects::TCS_SUBSYSTEM, NML, 0, EIVE_TABLE_SAFE_TRANS_0.second);
iht(objects::PL_SUBSYSTEM, OFF, 0, EIVE_TABLE_SAFE_TRANS_0.second);
iht(objects::ACS_SUBSYSTEM, acs::AcsMode::SAFE, 0, EIVE_TABLE_SAFE_TRANS_0.second, true);
check(ss.addTable(TableEntry(EIVE_TABLE_SAFE_TRANS_0.first, &EIVE_TABLE_SAFE_TRANS_0.second)),
ctxc);
// Build Safe sequence
ihs(EIVE_SEQUENCE_SAFE.second, EIVE_TABLE_SAFE_TGT.first, 0, false);
ihs(EIVE_SEQUENCE_SAFE.second, EIVE_TABLE_SAFE_TRANS_0.first, 0, false);
check(ss.addSequence(SequenceEntry(EIVE_SEQUENCE_SAFE.first, &EIVE_SEQUENCE_SAFE.second,
EIVE_SEQUENCE_SAFE.first)),
ctxc);
}
void buildIdleSequence(Subsystem& ss, ModeListEntry& eh) {
std::string context = "satsystem::buildIdleSequence";
auto ctxc = context.c_str();
// Insert Helper Table
auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode, ArrayList<ModeListEntry>& table) {
eh.setObject(obj);
eh.setMode(mode);
eh.setSubmode(submode);
check(table.insert(eh), ctxc);
};
// Insert Helper Sequence
auto ihs = [&](ArrayList<ModeListEntry>& sequence, Mode_t tableId, uint32_t waitSeconds,
bool checkSuccess) {
eh.setTableId(tableId);
eh.setWaitSeconds(waitSeconds);
eh.setCheckSuccess(checkSuccess);
check(sequence.insert(eh), ctxc);
};
iht(objects::ACS_SUBSYSTEM, acs::AcsMode::PTG_IDLE, 0, EIVE_TABLE_IDLE_TGT.second);
check(ss.addTable(TableEntry(EIVE_TABLE_IDLE_TGT.first, &EIVE_TABLE_IDLE_TGT.second)), ctxc);
// Build IDLE transition 0
iht(objects::TCS_SUBSYSTEM, NML, 0, EIVE_TABLE_IDLE_TRANS_0.second);
iht(objects::PL_SUBSYSTEM, OFF, 0, EIVE_TABLE_IDLE_TRANS_0.second);
iht(objects::ACS_SUBSYSTEM, acs::AcsMode::PTG_IDLE, 0, EIVE_TABLE_IDLE_TRANS_0.second);
check(ss.addTable(TableEntry(EIVE_TABLE_IDLE_TRANS_0.first, &EIVE_TABLE_IDLE_TRANS_0.second)),
ctxc);
// Build IDLE sequence
ihs(EIVE_SEQUENCE_IDLE.second, EIVE_TABLE_IDLE_TGT.first, 0, false);
ihs(EIVE_SEQUENCE_IDLE.second, EIVE_TABLE_IDLE_TRANS_0.first, 0, false);
check(ss.addSequence(SequenceEntry(EIVE_SEQUENCE_IDLE.first, &EIVE_SEQUENCE_IDLE.second,
EIVE_SEQUENCE_SAFE.first)),
ctxc);
}
void buildBootSequence(Subsystem& ss, ModeListEntry& eh) { void buildBootSequence(Subsystem& ss, ModeListEntry& eh) {
std::string context = "satsystem::buildBootSequence"; std::string context = "satsystem::buildBootSequence";
auto ctxc = context.c_str(); auto ctxc = context.c_str();
@ -194,4 +158,240 @@ void buildBootSequence(Subsystem& ss, ModeListEntry& eh) {
EIVE_SEQUENCE_SAFE.first)), EIVE_SEQUENCE_SAFE.first)),
ctxc); ctxc);
} }
void buildSafeSequence(Subsystem& ss, ModeListEntry& eh) {
std::string context = "satsystem::buildSafeSequence";
auto ctxc = context.c_str();
// Insert Helper Table
auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode, ArrayList<ModeListEntry>& table,
bool allowAllSubmodes = false) {
eh.setObject(obj);
eh.setMode(mode);
eh.setSubmode(submode);
if (allowAllSubmodes) {
eh.allowAllSubmodes();
}
check(table.insert(eh), ctxc);
};
// Insert Helper Sequence
auto ihs = [&](ArrayList<ModeListEntry>& sequence, Mode_t tableId, uint32_t waitSeconds,
bool checkSuccess) {
eh.setTableId(tableId);
eh.setWaitSeconds(waitSeconds);
eh.setCheckSuccess(checkSuccess);
check(sequence.insert(eh), ctxc);
};
// Do no track submode to allow transitions to DETUMBLE submode.
iht(objects::ACS_SUBSYSTEM, acs::AcsMode::SAFE, 0, EIVE_TABLE_SAFE_TGT.second, true);
check(ss.addTable(TableEntry(EIVE_TABLE_SAFE_TGT.first, &EIVE_TABLE_SAFE_TGT.second)), ctxc);
// Build SAFE transition 0.
iht(objects::TCS_SUBSYSTEM, NML, 0, EIVE_TABLE_SAFE_TRANS_0.second);
iht(objects::PL_SUBSYSTEM, OFF, 0, EIVE_TABLE_SAFE_TRANS_0.second);
iht(objects::ACS_SUBSYSTEM, acs::AcsMode::SAFE, 0, EIVE_TABLE_SAFE_TRANS_0.second, true);
check(ss.addTable(TableEntry(EIVE_TABLE_SAFE_TRANS_0.first, &EIVE_TABLE_SAFE_TRANS_0.second)),
ctxc);
// Build Safe sequence
ihs(EIVE_SEQUENCE_SAFE.second, EIVE_TABLE_SAFE_TGT.first, 0, false);
ihs(EIVE_SEQUENCE_SAFE.second, EIVE_TABLE_SAFE_TRANS_0.first, 0, false);
check(ss.addSequence(SequenceEntry(EIVE_SEQUENCE_SAFE.first, &EIVE_SEQUENCE_SAFE.second,
EIVE_SEQUENCE_SAFE.first)),
ctxc);
}
void buildIdleSequence(Subsystem& ss, ModeListEntry& eh) {
std::string context = "satsystem::buildIdleSequence";
auto ctxc = context.c_str();
// Insert Helper Table
auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode, ArrayList<ModeListEntry>& table) {
eh.setObject(obj);
eh.setMode(mode);
eh.setSubmode(submode);
check(table.insert(eh), ctxc);
};
// Insert Helper Sequence
auto ihs = [&](ArrayList<ModeListEntry>& sequence, Mode_t tableId, uint32_t waitSeconds,
bool checkSuccess) {
eh.setTableId(tableId);
eh.setWaitSeconds(waitSeconds);
eh.setCheckSuccess(checkSuccess);
check(sequence.insert(eh), ctxc);
};
iht(objects::ACS_SUBSYSTEM, acs::AcsMode::PTG_IDLE, 0, EIVE_TABLE_IDLE_TGT.second);
check(ss.addTable(TableEntry(EIVE_TABLE_IDLE_TGT.first, &EIVE_TABLE_IDLE_TGT.second)), ctxc);
// Build IDLE transition 0
iht(objects::TCS_SUBSYSTEM, NML, 0, EIVE_TABLE_IDLE_TRANS_0.second);
iht(objects::ACS_SUBSYSTEM, acs::AcsMode::PTG_IDLE, 0, EIVE_TABLE_IDLE_TRANS_0.second);
check(ss.addTable(TableEntry(EIVE_TABLE_IDLE_TRANS_0.first, &EIVE_TABLE_IDLE_TRANS_0.second)),
ctxc);
// Build IDLE sequence
ihs(EIVE_SEQUENCE_IDLE.second, EIVE_TABLE_IDLE_TGT.first, 0, false);
ihs(EIVE_SEQUENCE_IDLE.second, EIVE_TABLE_IDLE_TRANS_0.first, 0, false);
check(ss.addSequence(SequenceEntry(EIVE_SEQUENCE_IDLE.first, &EIVE_SEQUENCE_IDLE.second,
EIVE_SEQUENCE_SAFE.first)),
ctxc);
}
void buildPtgNadirSequence(Subsystem& ss, ModeListEntry& eh) {
std::string context = "satsystem::buildPtgNadirSequence";
auto ctxc = context.c_str();
// Insert Helper Table
auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode, ArrayList<ModeListEntry>& table) {
eh.setObject(obj);
eh.setMode(mode);
eh.setSubmode(submode);
check(table.insert(eh), ctxc);
};
// Insert Helper Sequence
auto ihs = [&](ArrayList<ModeListEntry>& sequence, Mode_t tableId, uint32_t waitSeconds,
bool checkSuccess) {
eh.setTableId(tableId);
eh.setWaitSeconds(waitSeconds);
eh.setCheckSuccess(checkSuccess);
check(sequence.insert(eh), ctxc);
};
iht(objects::ACS_SUBSYSTEM, acs::AcsMode::PTG_NADIR, 0, EIVE_TABLE_PTG_NADIR_TGT.second);
check(ss.addTable(TableEntry(EIVE_TABLE_PTG_NADIR_TGT.first, &EIVE_TABLE_PTG_NADIR_TGT.second)),
ctxc);
// Build PTG_NADIR transition 0
iht(objects::TCS_SUBSYSTEM, NML, 0, EIVE_TABLE_PTG_NADIR_TRANS_0.second);
iht(objects::ACS_SUBSYSTEM, acs::AcsMode::PTG_NADIR, 0, EIVE_TABLE_PTG_NADIR_TRANS_0.second);
check(ss.addTable(
TableEntry(EIVE_TABLE_PTG_NADIR_TRANS_0.first, &EIVE_TABLE_PTG_NADIR_TRANS_0.second)),
ctxc);
// Build PTG_NADIR sequence
ihs(EIVE_SEQUENCE_PTG_NADIR.second, EIVE_TABLE_PTG_NADIR_TGT.first, 0, false);
ihs(EIVE_SEQUENCE_PTG_NADIR.second, EIVE_TABLE_PTG_NADIR_TRANS_0.first, 0, false);
check(ss.addSequence(SequenceEntry(EIVE_SEQUENCE_PTG_NADIR.first, &EIVE_SEQUENCE_PTG_NADIR.second,
EIVE_SEQUENCE_IDLE.first)),
ctxc);
}
void buildPtgTargetSequence(Subsystem& ss, ModeListEntry& eh) {
std::string context = "satsystem::buildPtgTargetSequence";
auto ctxc = context.c_str();
// Insert Helper Table
auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode, ArrayList<ModeListEntry>& table) {
eh.setObject(obj);
eh.setMode(mode);
eh.setSubmode(submode);
check(table.insert(eh), ctxc);
};
// Insert Helper Sequence
auto ihs = [&](ArrayList<ModeListEntry>& sequence, Mode_t tableId, uint32_t waitSeconds,
bool checkSuccess) {
eh.setTableId(tableId);
eh.setWaitSeconds(waitSeconds);
eh.setCheckSuccess(checkSuccess);
check(sequence.insert(eh), ctxc);
};
iht(objects::ACS_SUBSYSTEM, acs::AcsMode::PTG_TARGET, 0, EIVE_TABLE_PTG_TARGET_TGT.second);
check(ss.addTable(TableEntry(EIVE_TABLE_PTG_TARGET_TGT.first, &EIVE_TABLE_PTG_TARGET_TGT.second)),
ctxc);
// Build PTG_TARGET transition 0
iht(objects::TCS_SUBSYSTEM, NML, 0, EIVE_TABLE_PTG_TARGET_TRANS_0.second);
iht(objects::ACS_SUBSYSTEM, acs::AcsMode::PTG_TARGET, 0, EIVE_TABLE_PTG_TARGET_TRANS_0.second);
check(ss.addTable(
TableEntry(EIVE_TABLE_PTG_TARGET_TRANS_0.first, &EIVE_TABLE_PTG_TARGET_TRANS_0.second)),
ctxc);
// Build PTG_TARGET sequence
ihs(EIVE_SEQUENCE_PTG_TARGET.second, EIVE_TABLE_PTG_TARGET_TGT.first, 0, false);
ihs(EIVE_SEQUENCE_PTG_TARGET.second, EIVE_TABLE_PTG_TARGET_TRANS_0.first, 0, false);
check(ss.addSequence(SequenceEntry(EIVE_SEQUENCE_PTG_TARGET.first,
&EIVE_SEQUENCE_PTG_TARGET.second, EIVE_SEQUENCE_IDLE.first)),
ctxc);
}
void buildPtgTargetGsSequence(Subsystem& ss, ModeListEntry& eh) {
std::string context = "satsystem::buildPtgTargetGsSequence";
auto ctxc = context.c_str();
// Insert Helper Table
auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode, ArrayList<ModeListEntry>& table) {
eh.setObject(obj);
eh.setMode(mode);
eh.setSubmode(submode);
check(table.insert(eh), ctxc);
};
// Insert Helper Sequence
auto ihs = [&](ArrayList<ModeListEntry>& sequence, Mode_t tableId, uint32_t waitSeconds,
bool checkSuccess) {
eh.setTableId(tableId);
eh.setWaitSeconds(waitSeconds);
eh.setCheckSuccess(checkSuccess);
check(sequence.insert(eh), ctxc);
};
iht(objects::ACS_SUBSYSTEM, acs::AcsMode::PTG_TARGET_GS, 0, EIVE_TABLE_PTG_TARGET_GS_TGT.second);
check(ss.addTable(
TableEntry(EIVE_TABLE_PTG_TARGET_GS_TGT.first, &EIVE_TABLE_PTG_TARGET_GS_TGT.second)),
ctxc);
// Build PTG_TARGET_GS transition 0
iht(objects::TCS_SUBSYSTEM, NML, 0, EIVE_TABLE_PTG_TARGET_GS_TRANS_0.second);
iht(objects::ACS_SUBSYSTEM, acs::AcsMode::PTG_TARGET_GS, 0,
EIVE_TABLE_PTG_TARGET_GS_TRANS_0.second);
check(ss.addTable(TableEntry(EIVE_TABLE_PTG_TARGET_GS_TRANS_0.first,
&EIVE_TABLE_PTG_TARGET_GS_TRANS_0.second)),
ctxc);
// Build PTG_TARGET_GS sequence
ihs(EIVE_SEQUENCE_PTG_TARGET_GS.second, EIVE_TABLE_PTG_TARGET_GS_TGT.first, 0, false);
ihs(EIVE_SEQUENCE_PTG_TARGET_GS.second, EIVE_TABLE_PTG_TARGET_GS_TRANS_0.first, 0, false);
check(
ss.addSequence(SequenceEntry(EIVE_SEQUENCE_PTG_TARGET_GS.first,
&EIVE_SEQUENCE_PTG_TARGET_GS.second, EIVE_SEQUENCE_IDLE.first)),
ctxc);
}
void buildPtgInertialSequence(Subsystem& ss, ModeListEntry& eh) {
std::string context = "satsystem::buildPtgInertialSequence";
auto ctxc = context.c_str();
// Insert Helper Table
auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode, ArrayList<ModeListEntry>& table) {
eh.setObject(obj);
eh.setMode(mode);
eh.setSubmode(submode);
check(table.insert(eh), ctxc);
};
// Insert Helper Sequence
auto ihs = [&](ArrayList<ModeListEntry>& sequence, Mode_t tableId, uint32_t waitSeconds,
bool checkSuccess) {
eh.setTableId(tableId);
eh.setWaitSeconds(waitSeconds);
eh.setCheckSuccess(checkSuccess);
check(sequence.insert(eh), ctxc);
};
iht(objects::ACS_SUBSYSTEM, acs::AcsMode::PTG_INERTIAL, 0, EIVE_TABLE_PTG_INERTIAL_TGT.second);
check(ss.addTable(
TableEntry(EIVE_TABLE_PTG_INERTIAL_TGT.first, &EIVE_TABLE_PTG_INERTIAL_TGT.second)),
ctxc);
// Build PTG_INERTIAL transition 0
iht(objects::TCS_SUBSYSTEM, NML, 0, EIVE_TABLE_PTG_INERTIAL_TRANS_0.second);
iht(objects::ACS_SUBSYSTEM, acs::AcsMode::PTG_INERTIAL, 0,
EIVE_TABLE_PTG_INERTIAL_TRANS_0.second);
check(ss.addTable(TableEntry(EIVE_TABLE_PTG_INERTIAL_TRANS_0.first,
&EIVE_TABLE_PTG_INERTIAL_TRANS_0.second)),
ctxc);
// Build PTG_INERTIAL sequence
ihs(EIVE_SEQUENCE_PTG_INERTIAL.second, EIVE_TABLE_PTG_INERTIAL_TGT.first, 0, false);
ihs(EIVE_SEQUENCE_PTG_INERTIAL.second, EIVE_TABLE_PTG_INERTIAL_TRANS_0.first, 0, false);
check(ss.addSequence(SequenceEntry(EIVE_SEQUENCE_PTG_INERTIAL.first,
&EIVE_SEQUENCE_PTG_INERTIAL.second, EIVE_SEQUENCE_IDLE.first)),
ctxc);
}
} // namespace } // namespace