2021-10-17 23:27:31 +02:00
|
|
|
#include "TestAssembly.h"
|
|
|
|
|
|
|
|
#include <fsfw/objectmanager/ObjectManager.h>
|
|
|
|
|
|
|
|
TestAssembly::TestAssembly(object_id_t objectId, object_id_t parentId, object_id_t testDevice0,
|
2022-02-02 10:29:30 +01:00
|
|
|
object_id_t testDevice1)
|
|
|
|
: AssemblyBase(objectId, parentId),
|
|
|
|
deviceHandler0Id(testDevice0),
|
|
|
|
deviceHandler1Id(testDevice1) {
|
|
|
|
ModeListEntry newModeListEntry;
|
|
|
|
newModeListEntry.setObject(testDevice0);
|
|
|
|
newModeListEntry.setMode(MODE_OFF);
|
|
|
|
newModeListEntry.setSubmode(SUBMODE_NONE);
|
|
|
|
|
|
|
|
commandTable.insert(newModeListEntry);
|
|
|
|
|
|
|
|
newModeListEntry.setObject(testDevice1);
|
|
|
|
newModeListEntry.setMode(MODE_OFF);
|
|
|
|
newModeListEntry.setSubmode(SUBMODE_NONE);
|
|
|
|
|
|
|
|
commandTable.insert(newModeListEntry);
|
2021-10-17 23:27:31 +02:00
|
|
|
}
|
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
TestAssembly::~TestAssembly() {}
|
2021-10-17 23:27:31 +02:00
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
ReturnValue_t TestAssembly::commandChildren(Mode_t mode, Submode_t submode) {
|
2021-10-17 23:27:31 +02:00
|
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
2022-02-02 10:29:30 +01:00
|
|
|
sif::info << "TestAssembly: Received command to go to mode " << mode << " submode "
|
|
|
|
<< (int)submode << std::endl;
|
2021-10-17 23:27:31 +02:00
|
|
|
#else
|
2022-02-02 10:29:30 +01:00
|
|
|
sif::printInfo("TestAssembly: Received command to go to mode %d submode %d\n", mode, submode);
|
2021-10-17 23:27:31 +02:00
|
|
|
#endif
|
2022-08-16 01:08:26 +02:00
|
|
|
ReturnValue_t result = returnvalue::OK;
|
2022-02-02 10:29:30 +01:00
|
|
|
if (mode == MODE_OFF) {
|
|
|
|
commandTable[0].setMode(MODE_OFF);
|
|
|
|
commandTable[0].setSubmode(SUBMODE_NONE);
|
|
|
|
commandTable[1].setMode(MODE_OFF);
|
|
|
|
commandTable[1].setSubmode(SUBMODE_NONE);
|
|
|
|
} else if (mode == DeviceHandlerIF::MODE_NORMAL) {
|
|
|
|
if (submode == submodes::SINGLE) {
|
|
|
|
commandTable[0].setMode(MODE_OFF);
|
|
|
|
commandTable[0].setSubmode(SUBMODE_NONE);
|
|
|
|
commandTable[1].setMode(MODE_OFF);
|
|
|
|
commandTable[1].setSubmode(SUBMODE_NONE);
|
|
|
|
// We try to prefer 0 here but we try to switch to 1 even if it might fail
|
|
|
|
if (isDeviceAvailable(deviceHandler0Id)) {
|
|
|
|
if (childrenMap[deviceHandler0Id].mode == MODE_ON) {
|
|
|
|
commandTable[0].setMode(mode);
|
|
|
|
commandTable[0].setSubmode(SUBMODE_NONE);
|
|
|
|
} else {
|
|
|
|
commandTable[0].setMode(MODE_ON);
|
|
|
|
commandTable[0].setSubmode(SUBMODE_NONE);
|
|
|
|
result = NEED_SECOND_STEP;
|
|
|
|
}
|
|
|
|
} else {
|
|
|
|
if (childrenMap[deviceHandler1Id].mode == MODE_ON) {
|
|
|
|
commandTable[1].setMode(mode);
|
|
|
|
commandTable[1].setSubmode(SUBMODE_NONE);
|
|
|
|
} else {
|
|
|
|
commandTable[1].setMode(MODE_ON);
|
|
|
|
commandTable[1].setSubmode(SUBMODE_NONE);
|
|
|
|
result = NEED_SECOND_STEP;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
} else {
|
|
|
|
// Dual Mode Normal
|
|
|
|
if (childrenMap[deviceHandler0Id].mode == MODE_ON) {
|
|
|
|
commandTable[0].setMode(mode);
|
|
|
|
commandTable[0].setSubmode(SUBMODE_NONE);
|
|
|
|
} else {
|
|
|
|
commandTable[0].setMode(MODE_ON);
|
|
|
|
commandTable[0].setSubmode(SUBMODE_NONE);
|
|
|
|
result = NEED_SECOND_STEP;
|
|
|
|
}
|
|
|
|
if (childrenMap[deviceHandler1Id].mode == MODE_ON) {
|
|
|
|
commandTable[1].setMode(mode);
|
|
|
|
commandTable[1].setSubmode(SUBMODE_NONE);
|
|
|
|
} else {
|
|
|
|
commandTable[1].setMode(MODE_ON);
|
|
|
|
commandTable[1].setSubmode(SUBMODE_NONE);
|
|
|
|
result = NEED_SECOND_STEP;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
} else {
|
|
|
|
// Mode ON
|
|
|
|
if (submode == submodes::SINGLE) {
|
|
|
|
commandTable[0].setMode(MODE_OFF);
|
|
|
|
commandTable[0].setSubmode(SUBMODE_NONE);
|
|
|
|
commandTable[1].setMode(MODE_OFF);
|
|
|
|
commandTable[1].setSubmode(SUBMODE_NONE);
|
|
|
|
// We try to prefer 0 here but we try to switch to 1 even if it might fail
|
|
|
|
if (isDeviceAvailable(deviceHandler0Id)) {
|
|
|
|
commandTable[0].setMode(MODE_ON);
|
|
|
|
commandTable[0].setSubmode(SUBMODE_NONE);
|
|
|
|
} else {
|
|
|
|
commandTable[1].setMode(MODE_ON);
|
|
|
|
commandTable[1].setSubmode(SUBMODE_NONE);
|
|
|
|
}
|
|
|
|
} else {
|
|
|
|
commandTable[0].setMode(MODE_ON);
|
|
|
|
commandTable[0].setSubmode(SUBMODE_NONE);
|
|
|
|
commandTable[1].setMode(MODE_ON);
|
|
|
|
commandTable[1].setSubmode(SUBMODE_NONE);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
HybridIterator<ModeListEntry> iter(commandTable.begin(), commandTable.end());
|
|
|
|
executeTable(iter);
|
|
|
|
return result;
|
2021-10-17 23:27:31 +02:00
|
|
|
}
|
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
ReturnValue_t TestAssembly::isModeCombinationValid(Mode_t mode, Submode_t submode) {
|
|
|
|
switch (mode) {
|
|
|
|
case MODE_OFF:
|
|
|
|
if (submode == SUBMODE_NONE) {
|
2022-08-16 01:08:26 +02:00
|
|
|
return returnvalue::OK;
|
2022-02-02 10:29:30 +01:00
|
|
|
} else {
|
|
|
|
return INVALID_SUBMODE;
|
|
|
|
}
|
|
|
|
case DeviceHandlerIF::MODE_NORMAL:
|
|
|
|
case MODE_ON:
|
|
|
|
if (submode < 3) {
|
2022-08-16 01:08:26 +02:00
|
|
|
return returnvalue::OK;
|
2022-02-02 10:29:30 +01:00
|
|
|
} else {
|
|
|
|
return INVALID_SUBMODE;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
return INVALID_MODE;
|
2021-10-17 23:27:31 +02:00
|
|
|
}
|
|
|
|
|
|
|
|
ReturnValue_t TestAssembly::initialize() {
|
2022-02-02 10:29:30 +01:00
|
|
|
ReturnValue_t result = AssemblyBase::initialize();
|
2022-08-16 01:08:26 +02:00
|
|
|
if (result != returnvalue::OK) {
|
2022-02-02 10:29:30 +01:00
|
|
|
return result;
|
|
|
|
}
|
|
|
|
handler0 = ObjectManager::instance()->get<TestDevice>(deviceHandler0Id);
|
|
|
|
handler1 = ObjectManager::instance()->get<TestDevice>(deviceHandler1Id);
|
|
|
|
if ((handler0 == nullptr) or (handler1 == nullptr)) {
|
2022-08-16 01:08:26 +02:00
|
|
|
return returnvalue::FAILED;
|
2022-02-02 10:29:30 +01:00
|
|
|
}
|
|
|
|
|
|
|
|
handler0->setParentQueue(this->getCommandQueue());
|
|
|
|
handler1->setParentQueue(this->getCommandQueue());
|
|
|
|
|
|
|
|
result = registerChild(deviceHandler0Id);
|
2022-08-16 01:08:26 +02:00
|
|
|
if (result != returnvalue::OK) {
|
2022-02-02 10:29:30 +01:00
|
|
|
return result;
|
|
|
|
}
|
|
|
|
result = registerChild(deviceHandler1Id);
|
2022-08-16 01:08:26 +02:00
|
|
|
if (result != returnvalue::OK) {
|
2022-02-02 10:29:30 +01:00
|
|
|
return result;
|
|
|
|
}
|
|
|
|
return result;
|
2021-10-17 23:27:31 +02:00
|
|
|
}
|
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
ReturnValue_t TestAssembly::checkChildrenStateOn(Mode_t wantedMode, Submode_t wantedSubmode) {
|
|
|
|
if (submode == submodes::DUAL) {
|
|
|
|
for (const auto& info : childrenMap) {
|
|
|
|
if (info.second.mode != wantedMode or info.second.mode != wantedSubmode) {
|
|
|
|
return NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE;
|
|
|
|
}
|
|
|
|
}
|
2022-08-16 01:08:26 +02:00
|
|
|
return returnvalue::OK;
|
2022-02-02 10:29:30 +01:00
|
|
|
} else if (submode == submodes::SINGLE) {
|
|
|
|
for (const auto& info : childrenMap) {
|
|
|
|
if (info.second.mode == wantedMode and info.second.mode != wantedSubmode) {
|
2022-08-16 01:08:26 +02:00
|
|
|
return returnvalue::OK;
|
2022-02-02 10:29:30 +01:00
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
return INVALID_SUBMODE;
|
2021-10-17 23:27:31 +02:00
|
|
|
}
|
|
|
|
|
|
|
|
bool TestAssembly::isDeviceAvailable(object_id_t object) {
|
2022-02-02 10:29:30 +01:00
|
|
|
if (healthHelper.healthTable->getHealth(object) == HasHealthIF::HEALTHY) {
|
|
|
|
return true;
|
|
|
|
} else {
|
|
|
|
return false;
|
|
|
|
}
|
2021-10-17 23:27:31 +02:00
|
|
|
}
|