This repository has been archived on 2021-11-24. You can view files and clone it, but cannot push or open issues or pull requests.
fsfw_example_public/mission/assemblies/TestAssembly.cpp

201 lines
5.5 KiB
C++
Raw Normal View History

2021-04-27 17:22:34 +02:00
#include <mission/assemblies/TestAssembly.h>
#include <common/config/commonSystemObjects.h>
2021-04-29 15:58:00 +02:00
2021-04-27 17:22:34 +02:00
#include <fsfw/objectmanager/ObjectManagerIF.h>
2021-04-29 15:58:00 +02:00
TestAssembly::TestAssembly(object_id_t objectId, object_id_t parentId):
AssemblyBase(objectId, parentId) {
2021-04-27 17:22:34 +02:00
ModeListEntry newModeListEntry;
newModeListEntry.setObject(objects::TEST_DEVICE_HANDLER_0);
newModeListEntry.setMode(MODE_OFF);
newModeListEntry.setSubmode(SUBMODE_NONE);
commandTable.insert(newModeListEntry);
newModeListEntry.setObject(objects::TEST_DEVICE_HANDLER_1);
newModeListEntry.setMode(MODE_OFF);
newModeListEntry.setSubmode(SUBMODE_NONE);
commandTable.insert(newModeListEntry);
}
TestAssembly::~TestAssembly() {
}
ReturnValue_t TestAssembly::commandChildren(Mode_t mode,
Submode_t submode) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
2021-04-29 15:58:00 +02:00
sif::info << "TestAssembly: Received command to go to mode " << mode <<
" submode " << (int) submode << std::endl;
2021-04-27 17:22:34 +02:00
#else
2021-04-29 15:58:00 +02:00
sif::printInfo("TestAssembly: Received command to go to mode %d submode %d\n", mode, submode);
2021-04-27 17:22:34 +02:00
#endif
ReturnValue_t result = RETURN_OK;
if(mode == MODE_OFF){
commandTable[0].setMode(MODE_OFF);
commandTable[0].setSubmode(SUBMODE_NONE);
commandTable[1].setMode(MODE_OFF);
commandTable[1].setSubmode(SUBMODE_NONE);
2021-04-29 15:58:00 +02:00
}
else if(mode == DeviceHandlerIF::MODE_NORMAL) {
2021-04-27 17:22:34 +02:00
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
2021-04-29 15:58:00 +02:00
if(isDeviceAvailable(objects::TEST_DEVICE_HANDLER_0)) {
if (childrenMap[objects::TEST_DEVICE_HANDLER_0].mode == MODE_ON) {
2021-04-27 17:22:34 +02:00
commandTable[0].setMode(mode);
commandTable[0].setSubmode(SUBMODE_NONE);
2021-04-29 15:58:00 +02:00
}
else {
2021-04-27 17:22:34 +02:00
commandTable[0].setMode(MODE_ON);
commandTable[0].setSubmode(SUBMODE_NONE);
result = NEED_SECOND_STEP;
}
2021-04-29 15:58:00 +02:00
}
else {
if (childrenMap[objects::TEST_DEVICE_HANDLER_1].mode == MODE_ON) {
2021-04-27 17:22:34 +02:00
commandTable[1].setMode(mode);
commandTable[1].setSubmode(SUBMODE_NONE);
2021-04-29 15:58:00 +02:00
}
else{
2021-04-27 17:22:34 +02:00
commandTable[1].setMode(MODE_ON);
commandTable[1].setSubmode(SUBMODE_NONE);
result = NEED_SECOND_STEP;
}
}
2021-04-29 15:58:00 +02:00
}
else{
2021-04-27 17:22:34 +02:00
// Dual Mode Normal
2021-04-29 15:58:00 +02:00
if (childrenMap[objects::TEST_DEVICE_HANDLER_0].mode == MODE_ON) {
2021-04-27 17:22:34 +02:00
commandTable[0].setMode(mode);
commandTable[0].setSubmode(SUBMODE_NONE);
2021-04-29 15:58:00 +02:00
}
else{
2021-04-27 17:22:34 +02:00
commandTable[0].setMode(MODE_ON);
commandTable[0].setSubmode(SUBMODE_NONE);
result = NEED_SECOND_STEP;
}
2021-04-29 15:58:00 +02:00
if (childrenMap[objects::TEST_DEVICE_HANDLER_1].mode == MODE_ON) {
2021-04-27 17:22:34 +02:00
commandTable[1].setMode(mode);
commandTable[1].setSubmode(SUBMODE_NONE);
2021-04-29 15:58:00 +02:00
}
else{
2021-04-27 17:22:34 +02:00
commandTable[1].setMode(MODE_ON);
commandTable[1].setSubmode(SUBMODE_NONE);
result = NEED_SECOND_STEP;
}
}
2021-04-29 15:58:00 +02:00
}
else{
2021-04-27 17:22:34 +02:00
//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(objects::TEST_DEVICE_HANDLER_0)){
commandTable[0].setMode(MODE_ON);
commandTable[0].setSubmode(SUBMODE_NONE);
2021-04-29 15:58:00 +02:00
}
else{
2021-04-27 17:22:34 +02:00
commandTable[1].setMode(MODE_ON);
commandTable[1].setSubmode(SUBMODE_NONE);
}
2021-04-29 15:58:00 +02:00
}
else{
2021-04-27 17:22:34 +02:00
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;
}
ReturnValue_t TestAssembly::isModeCombinationValid(Mode_t mode,
Submode_t submode) {
switch (mode) {
case MODE_OFF:
if (submode == SUBMODE_NONE) {
return RETURN_OK;
} else {
return INVALID_SUBMODE;
}
case DeviceHandlerIF::MODE_NORMAL:
case MODE_ON:
if (submode < 3) {
return RETURN_OK;
} else {
return INVALID_SUBMODE;
}
}
return INVALID_MODE;
}
ReturnValue_t TestAssembly::initialize() {
ReturnValue_t result = AssemblyBase::initialize();
if(result != RETURN_OK){
return result;
}
handler0 = objectManager->get<TestDevice>(objects::TEST_DEVICE_HANDLER_0);
handler1 = objectManager->get<TestDevice>(objects::TEST_DEVICE_HANDLER_1);
if((handler0 == nullptr) or (handler1 == nullptr)){
return HasReturnvaluesIF::RETURN_FAILED;
}
handler0->setParentQueue(this->getCommandQueue());
handler1->setParentQueue(this->getCommandQueue());
result = registerChild(objects::TEST_DEVICE_HANDLER_0);
if (result != HasReturnvaluesIF::RETURN_OK) {
return result;
}
result = registerChild(objects::TEST_DEVICE_HANDLER_1);
if (result != HasReturnvaluesIF::RETURN_OK) {
return result;
}
return result;
}
ReturnValue_t TestAssembly::checkChildrenStateOn(
Mode_t wantedMode, Submode_t wantedSubmode) {
if(submode == submodes::DUAL){
2021-04-29 15:58:00 +02:00
for(const auto& info:childrenMap) {
2021-04-27 17:22:34 +02:00
if(info.second.mode != wantedMode or info.second.mode != wantedSubmode){
return NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE;
}
}
return RETURN_OK;
2021-04-29 15:58:00 +02:00
}
else if(submode == submodes::SINGLE) {
for(const auto& info:childrenMap) {
2021-04-27 17:22:34 +02:00
if(info.second.mode == wantedMode and info.second.mode != wantedSubmode){
return RETURN_OK;
}
}
}
return INVALID_SUBMODE;
}
bool TestAssembly::isDeviceAvailable(object_id_t object) {
if(healthHelper.healthTable->getHealth(object) == HasHealthIF::HEALTHY){
return true;
2021-04-29 15:58:00 +02:00
}
else{
2021-04-27 17:22:34 +02:00
return false;
}
}